diff --git a/.gitignore b/.gitignore index 4e3ccbcb4c8..e3f96f3a77a 100644 --- a/.gitignore +++ b/.gitignore @@ -21,6 +21,10 @@ 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/switchkins_setup.c +share/linuxcnc/kins_util.c +share/linuxcnc/kins_single.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 38e706fac22..4c987aba743 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini +++ b/configs/sim/axis/vismach/5axis/bridgemill/5axis.ini @@ -4,8 +4,13 @@ MACHINE = Sim-5Axis Bridge Mill (xyzbcw) DEBUG = 0 [DISPLAY] - GEOMETRY = XYZCBW - OPEN_FILE = ./5axisgui.ngc +# GEOMETRY tells the AXIS live plot how to guess the tool position from +# the nine axis values: letters translate, ABC rotate the point. W is +# deliberately absent: the plot would add the W value as a Z offset after +# the rotations, while the kinematics spends W along the tool axis, so +# the guess never matches. + GEOMETRY = XYZCB + OPEN_FILE = ./g532-fused-orient-move.ngc INCREMENTS = 10 mm, 1 mm, .1 mm JOG_AXES = XYZC DISPLAY = axis diff --git a/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.hal b/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.hal index c45e7c5765b..3780f9ce3d0 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.hal +++ b/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.hal @@ -17,7 +17,6 @@ net :gui-pivot-len <= 5axisgui.pivot_len net :gui-pivot-len => 5axiskins.pivot-length net :tool-len <= motion.tooloffset.z -net :tool-len => 5axiskins.tool-length net :tool-len => 5axisgui.tool_length net :tool-diam <= halui.tool.diameter diff --git a/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.ngc b/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.ngc index 7b2c5a8f188..cbc165137d7 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.ngc +++ b/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.ngc @@ -1,3 +1,17 @@ +; 5axisgui.ngc - the historical demo program, unchanged. +; It drills a sphere pattern with W words. W was never a physical +; quill: no motor is connected to its joint, the kinematics folds +; the word into the XYZ slides, and the head sliding along the tool +; axis is what plunges the rigidly mounted tool. Watch the slides +; in vismach do the stroke. +; +; The quirk, then and now: the controller keeps W as a separate +; world coordinate, so the programmed XYZ point does not move and +; the preview shows nothing of the stroke; only the W DRO tracks it. +; Drilling the controller can see, check and preview is what the +; tilted work plane family is for (g532-fused-orient-move.ngc, +; g536-orient-then-move.ngc). + # = 60 ; sphere radius # = 5 ; safe distance # = -5 @@ -13,6 +27,7 @@ # = [90/#] g49 +g12.1 p0 ; the TCP kinematics, in case a previous run left the identity one active t#m6g43 g53 g0 x0y0z#b0c0 w0 diff --git a/configs/sim/axis/vismach/5axis/bridgemill/README b/configs/sim/axis/vismach/5axis/bridgemill/README index 2adf5599795..4a92c6a2078 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/README +++ b/configs/sim/axis/vismach/5axis/bridgemill/README @@ -1,6 +1,46 @@ -This is a simulation of an XYZBCWY 5 axis bridge mill. +This is a simulation of an XYZBCWY 5 axis bridge mill with a +tilting head (B, C) and a W axis. -Example demo: +W is not a physical axis of the machine and never was: no motor is +connected to its joint. The kinematics folds a W word into the +XYZ joints, so the head slides along the tool axis and the +rigidly mounted tool goes with it: that is the whole trick, and it +is what vismach shows. There is no quill to draw because there is +no quill. + +The quirk: the controller keeps W as a separate world coordinate, +so a W word leaves the programmed XYZ point untouched and the +preview cannot show the stroke; the W DRO tracks it. It also +means the planner does not see the tool-axis motion, so for +drilling the controller can check and preview, use the tilted work +plane family. + +Because the W joint is virtual, a point-to-point move on it +(g53.5/g53.7 j5=) moves no motor on a real machine while the +controller believes the world moved, the opposite of a W word. +Do not drill that way. + +Demo programs: + + g532-fused-orient-move.ngc -- drill a sphere pattern with tilted work + planes (g68.2), using g53.2 to solve each + orientation without moving (STAY), then + one fused g0 that turns the head and + travels to the hole at the same time + (TCP motion) + g536-orient-then-move.ngc -- the same pattern with g53.6 (Heidenhain + MOVE style): reorient about the fixed + tip, then travel in the tilted plane + 5axisgui.ngc --------------- the historical demo program, unchanged: + it drills the same pattern with W words, + the slides doing the stroke while the + preview stays blind to it + +The tool table provides tool 100 (length 100). Load it with +t100m6g43 so the vismach tool and the kinematics pivot length +(pivotsum: 250 + tool length) match. + +Example MDI: 1) $ linuxcnc 5axis.ini 2) F1 ---------- Estop off @@ -8,12 +48,12 @@ Example demo: CTRL-HOME --- home all F5 ---------- MDI tab 3) orient vismach gui as required - 4) g0w10 ; retract w + 4) g0w10 ; head slides up the tool axis, tip with it 5) g43h100 ; tool offset (100) 6) g0b45 ; tilt 45 deg wrt z 7) g0c30 ; rotate 30 deg in xy - 8) g0w-10 ; simulate drill - 9) g0w10 ; retract drill + 8) g0w-10 ; head slides back, tip plunges 10 along the tool axis + 9) g0w10 ; and back 10) etc Note: Motion for the W coordinate is incorporated diff --git a/configs/sim/axis/vismach/5axis/bridgemill/g532-fused-orient-move.ngc b/configs/sim/axis/vismach/5axis/bridgemill/g532-fused-orient-move.ngc new file mode 100644 index 00000000000..bd193299758 --- /dev/null +++ b/configs/sim/axis/vismach/5axis/bridgemill/g532-fused-orient-move.ngc @@ -0,0 +1,50 @@ +; g532-fused-orient-move.ngc - drill a sphere pattern with tilted work planes: +; g68.2 tilts the plane onto each hole normal, g53.2 solves the head +; orientation without moving (STAY), and one fused g0 turns the head +; and travels to the hole at the same time (TCP motion). See README +; for the other demos. + + # = 60 ; sphere radius + # = 5 ; safe distance +# = -5 + # = 20 ; clearance above the ball for start and stop + # = 8 + # = 16 + # = 100 + # = 1000 + +# = 0 +# = 0 +# = [360/#] +# = [90/#] + +g49 +g12.1 p0 ; the TCP kinematics, in case a previous run left the identity one active +t#m6g43 + +g53 g0 x0 y0 z0 b0 c0 +g10 l20 p0 x0 y0 z[#+#+#] b0 c0 ; ball center at program origin, we park above it +f# +o100 while [# lt #] + # = [[#-1-#]*#] ; top ring first, the way in stays outside the ball + # = 0 +o200 while [# lt #] +o210 if [[# mod 2] eq 0] + # = [# * #] +o210 else + # = [360 - [1+ #] * #] +o210 endif + g68.2 p1 j[90-#] k# ; plane Z is the sphere radius at b, c + g53.2 ; solve the orientation, stay put + g0 x0 y0 z[#+#] b#<_orient_b> c#<_orient_c> ; one TCP turn and travel + g1 z[#+#] + g0 z[#+#] + # = [#+1] +o200 endwhile + # = [#+1] +o100 endwhile + +g69 +g53 g0 z0 ; up, clear of the ball +g53 g0 x0 y0 b0 c0 +m2 diff --git a/configs/sim/axis/vismach/5axis/bridgemill/g536-orient-then-move.ngc b/configs/sim/axis/vismach/5axis/bridgemill/g536-orient-then-move.ngc new file mode 100644 index 00000000000..2354e7e0625 --- /dev/null +++ b/configs/sim/axis/vismach/5axis/bridgemill/g536-orient-then-move.ngc @@ -0,0 +1,49 @@ +; g536-orient-then-move.ngc - same sphere as g532-fused-orient-move.ngc, but with g53.6 +; (Heidenhain MOVE style): reorient about the fixed tip, a TCP move, +; then travel to the next hole in the tilted plane with a separate g0. +; g532-fused-orient-move.ngc fuses the turn and the travel into one move instead. + + # = 60 ; sphere radius + # = 5 ; safe distance +# = -5 + # = 20 ; clearance above the ball for start and stop + # = 8 + # = 16 + # = 100 + # = 1000 + +# = 0 +# = 0 +# = [360/#] +# = [90/#] + +g49 +g12.1 p0 ; the TCP kinematics, in case a previous run left the identity one active +t#m6g43 + +g53 g0 x0 y0 z0 b0 c0 +g10 l20 p0 x0 y0 z[#+#+#] b0 c0 ; ball center at program origin, we park above it +f# +o100 while [# lt #] + # = [[#-1-#]*#] ; top ring first, the way in stays outside the ball + # = 0 +o200 while [# lt #] +o210 if [[# mod 2] eq 0] + # = [# * #] +o210 else + # = [360 - [1+ #] * #] +o210 endif + g68.2 p1 j[90-#] k# ; plane Z is the sphere radius at b, c + g53.6 ; reorient about the tip, a TCP move + g0 x0 y0 z[#+#] + g1 z[#+#] + g0 z[#+#] + # = [#+1] +o200 endwhile + # = [#+1] +o100 endwhile + +g69 +g53 g0 z0 ; up, clear of the ball +g53 g0 x0 y0 b0 c0 +m2 diff --git a/configs/sim/axis/vismach/5axis/max5/max5kins.hal b/configs/sim/axis/vismach/5axis/max5/max5kins.hal index 85b9d37c2be..04b5dc47843 100644 --- a/configs/sim/axis/vismach/5axis/max5/max5kins.hal +++ b/configs/sim/axis/vismach/5axis/max5/max5kins.hal @@ -16,9 +16,7 @@ loadusr -W ./max5gui.py # set a visible tool setp max5gui.tool-radius 3 -# the tool length is applied along the tool, not along Z, so the tip stays -# on the programmed point as B tilts -net tool-len motion.tooloffset.z max5gui.tool-length maxkins.tool-length +net tool-len motion.tooloffset.z max5gui.tool-length # add motion controller functions to servo thread addf motion-command-handler servo-thread diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/README b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/README index fea7c5d4882..ba7fafdbe6d 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/README +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/README @@ -1,27 +1,20 @@ This is a simulation configuration for a 6 axis machine with one table rotary and two spindle rotary joints -This simulation also includes a python remap of Gcodes for tilted workplane (TWP) functionality. -Both the kinematic and the twp remap support nutation of the secondary rotary joints (ie A or B ) form 0 to 90°. -Hence this also works for the 'usual' orthogonal spindle rotary-tilt type machines by setting the nutation angle to 90°. +This simulation uses the interpreter's tilted work plane codes, documented in the G-code section of the manual: -Implemented TWP functionality: -G68.2 : defines twp using euler-angles, pitch-roll-yaw, 2-vectors, 3 points, optionally with offset in XYZ and rotation in XY -G68.3 : defines twp from current tool orientation, optionally with offset in XYZ and rotation in XY -G68.4 : same as G68.2 but as an incremental definition from an active TWP plane -G69 : cancels the current twp (resets all parameters, moves to G54 and sets Identity kinematics) -G53.1 (P) : spindle orientation without tcp, switches to G59 and activates tool kinematics -G53.3 (P XYZ) : same as G53.1 but with simultaneous move the the XYZ coords on the twp plane -G53.6 (P) : same as G53.1 but spindle orientation with tcp +G68.2 : defines the plane by three angles, three points or two vectors, with an origin in XYZ and a turn R about the plane's Z +G68.3 : defines the plane from the current tool direction, with an origin in XYZ and a turn R +G68.4 : any G68.2 form, composed onto the active plane +G69 : cancels the plane +G53.1 (P Q) : orients the tool to the plane, rotaries only, the linear joints stay where they are +G53.3 (P Q XYZ) : orients the tool and moves to XYZ in the plane, interpolated in joint space +G53.6 (P Q) : orients the tool with the tool centre point held -- Spindle is C primary, A secondary or B secondary as defined in the [TWP] section of the ini file -- All G53.x commands will respect axis limits as set in the ini file for the respective primary and secondary spindle joints. -- The P word sets the orientation strategy: 0(default)=shortest distance, - 1=positive rotation only, - 2=negative rotation only - (this applies to the primary rotary, the secondary moves the shortest distance) +The orientation codes need the TCP kinematics (G12.1 P1). P picks the solution, nearest to the present rotary position first. Q0 holds the table and lets the head do it, Q1 lets the table take part as well. + +The kinematic supports nutation of the secondary rotary joint (A or B) from 0 to 90 degrees, so this also covers the usual orthogonal spindle rotary-tilt machines by setting the nutation angle to 90 degrees. + +The python maths this configuration used to carry as a remap lives on in tests/kins-twp, where the kinematics module is checked against it. For more: https://forum.linuxcnc.org/show-your-stuff/49103-kinematic-model-for-a-5axis-mill-with-universal-nutating-head?start=0#271334 - -Full Documentation can be found at: -https://github.com/Sigma1912/LinuxCNC_Demo_Configs/tree/main/table-rotary_spindle-rotary-nutating/Documentation diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition.ngc index 31feddcff12..4dc18624cf2 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition.ngc @@ -1,4 +1,5 @@ G69 +g12.1 p1 (the TCP kinematics: the plane codes need it) g10 l2 p0 x1000 y-1000 z-1000 m6 t3 g43 h3 g68.2 q121 i25 j-10 @@ -13,5 +14,6 @@ o100 REPEAT[100] g0 y50 o100 ENDREPEAT g69 +g13.1 M2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_back_and_forth.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_back_and_forth.ngc index 37b6f85032d..e1d5bb5d34b 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_back_and_forth.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_back_and_forth.ngc @@ -1,4 +1,5 @@ g69 +g12.1 p1 (the TCP kinematics: the plane codes need it) g10 l2 p0 x1300 y-200 z-1400 m6 t3 g43 h3 g68.2 q121 i0 j5 @@ -20,4 +21,5 @@ o100 REPEAT[1000] o100 ENDREPEAT g69 +g13.1 M2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_g533.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_g533.ngc index f01324916b5..0a2e4731539 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_g533.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_g533.ngc @@ -1,4 +1,5 @@ G69 +g12.1 p1 (the TCP kinematics: the plane codes need it) g10 l2 p0 x1000 y-1000 z-1000 m6 t3 g43 h3 g68.2 q121 i25 j-10 @@ -7,7 +8,7 @@ o100 REPEAT[100] g68.4 q131 i-35 j-35 k0 ;g53.3 p0 x50y50z150 g53.6 - x50y50z150 + g0 x50y50z150 g0 z100 g0 x-50 g0 y-50 @@ -16,5 +17,6 @@ o100 REPEAT[100] g0 x0y0z120 o100 ENDREPEAT g69 +g13.1 M2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/simple_example.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/simple_example.ngc index 7586f2f1d3c..bc2f46c6b6b 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/simple_example.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/simple_example.ngc @@ -1,4 +1,6 @@ g69 +g13.1 +g12.1 p1 (the TCP kinematics: the plane codes need it) g10 l2 p0 x1300 y-200 z-1400 m6 t3 g43 h3 g0 x0y0z100 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/square.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/square.ngc index c7e263ce601..c1f2bbf2abe 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/square.ngc +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/square.ngc @@ -5,5 +5,4 @@ osub g0 x50 g0 y50 g0 x0y0z120 - g52 x0y0z0 oendsub diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py deleted file mode 100755 index f4f9506a846..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py +++ /dev/null @@ -1,1676 +0,0 @@ -# This is a python remap for LinuxCNC implementing 'Tilted Work Plane' -# G68.2, G68.3, G68.4 and related Gcodes G53.1, G53.3, G53.6, G69 -# -# Copyright ()c) 2023 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 traceback -import numpy as np -from math import sin,cos,tan,asin,acos,atan,atan2,sqrt,pi,degrees,radians,fabs -from interpreter import * -import emccanon -from util import lineno, call_pydevd -import hal - - -# logging -import logging -# this name will be printed first on each log message -log = logging.getLogger('remap.py; TWP') -# we have to setup a handler to be able to set the log level for this module -handler = logging.StreamHandler() -formatter = logging.Formatter('%(name)s %(levelname)s: %(message)s') -handler.setFormatter(formatter) -log.addHandler(handler) -# 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") -# instantiate the LinuxCNC ini-parser -config = linuxcnc.ini(inifile) - -## SPINDLE ROTARY JOINT LETTERS -# spindle primary joint -joint_letter_primary = config.getstring('TWP', 'PRIMARY', fallback="").capitalize() -# spindle secondary joint (ie the one closer to the tool) -joint_letter_secondary = config.getstring('TWP', 'SECONDARY', fallback="").capitalize() - -if not joint_letter_primary in ('A','B','C') or not joint_letter_secondary in ('A','B','C'): - log.error("Unable to parse joint letters given in INI [TWP].") -elif joint_letter_primary == joint_letter_secondary: - log.error("Letters for primary and secondary joints in INI [TWP] must not be the same.") -else: - # get the MIN/MAX limits of the respective rotary joint letters - category = 'AXIS_' + joint_letter_primary - primary_min_limit = 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) - 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' - -## 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' - - -# raise InterpreterException if execute() or read() fail -throw_exceptions = 1 - -## VALUE INITIALIZATION -# we start with the identity matrix (ie the twp is equal to the world coordinates) -twp_matrix = np.asmatrix(np.identity(4)) - -# some g68.2 p-word modes require several calls to enter all the required parameters so we -# need a flag that indicates when the twp has been defined and is ready for g53.x -# [current p-word, number of calls required, (state of calls required for that p mode added by g68.2)] -# note that we use string since boolean True == 1, which gives wrong results if we want -# to count the elements that are True because it is counted as integer '1' -# eg: twp_flag = [0, 1, 'empty'] -twp_flag = [] -# we need a place to store the twp-build-parameters if the mode needs more than one call -twp_build_params = {} -# container to store the current work offset during twp operations -current_work_offset_number = 1 -saved_work_offset = [0,0,0] -# orientation mode refers to the strategy used to choose from the different rotary angles for a given -# tool-z 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 - - -# 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 - - -# 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) - # 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) - 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 - -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)) - 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)) - 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 - 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 - - -# 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)) - # 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)) - theta = pos + dist - else: # if positive limits would be exceeded we need to go the longer wey in the other direction - if mode == 0: - log.debug('Max_limit reached, target remains: %s', trgt) - theta = trgt - else: # if the rotation direction was set by the operator then we can not change direction - 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)) - theta = pos + dist - else: # if negative limits would be exceeded we need to go the longer way int the other direction - if mode == 0: - log.debug('Min_limit reached, target remains: %s', trgt) - theta = trgt - else: # if the rotation direction was set by the operator then we can not change direction - theta = None - dist = None - # we also attach the distance for this particular move and mode - log.debug('Angle and distance returned: %s, %s', theta, dist) - return theta, dist - - -# 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): - 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) - # 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 - 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) - sec_move, sec_dist = calc_rotary_move_with_joint_limits(sec_pos, sec_trgt, - secondary_max_limit, secondary_min_limit, - 0) - # 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 - - -# 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): - 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) - # now we need to pick and return the (primary angle, secondary angle) that results in the - # shortest move of the primary joint - (theta_1, theta_2) = (None, None) - dist = 3600 - for trgt_angles, dists in valid_joint_moves_and_distances: - if orient_mode == 0 and fabs(dists[0]) < 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 - (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 - (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 - 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 - # 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) - # 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 - 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) - # 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: - 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) - # calculate the difference of the respective elements - tool_x_diff = tool_x_req - tool_x_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) - 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]]) - - -# 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 - - -# The tilted-work-plane is created in identity mode and must NOT be updated after a switch -def gui_update_twp(self): - 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])) - # publish the twp offset coordinates in world coordinates (ie identity) - [work_offset_x, work_offset_y, work_offset_z] = saved_work_offset - log.debug("Setting work_offsets in the simulation: %s", (work_offset_x, work_offset_y, work_offset_z)) - # this is used to translate the rotated twp to the correct position - # care must be taken that only the work_offsets in identity mode are sent as that is - # what the model uses. The visuals for the offsets are created 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)) - - -# 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): - # get which offset is active (g54=1 .. g59.3=9) - active_offset = int(self.params[5220]) - current_work_offset_number = active_offset - # set the relevant parameter numbers that hold the active offset values - # (G54_x: #5221, G55_x:#[5221+20], G56_x:#[5221+40] ....) - work_offset_x = (active_offset-1)*20 + 5221 - work_offset_y = work_offset_x + 1 - work_offset_z = work_offset_x + 2 - co_x = self.params[work_offset_x] - co_y = self.params[work_offset_y] - co_z = self.params[work_offset_z] - current_work_offset = [co_x, co_y, co_z] - return [current_work_offset_number, current_work_offset] - - -def get_current_rotary_positions(self): - global joint_letter_primary, joint_letter_secondary - if joint_letter_primary == 'A': - theta_1 = radians(self.AA_current) - elif joint_letter_primary == 'B': - theta_1 = radians(self.BB_current) - elif joint_letter_primary == 'C': - theta_1 = radians(self.CC_current) - log.debug('Current position Primary joint: %s', degrees(theta_1)) - # read current spindle rotary angles and convert to radians - if joint_letter_secondary == 'A': - theta_2 = radians(self.AA_current) - elif joint_letter_secondary == 'B': - theta_2 = radians(self.BB_current) - elif joint_letter_secondary == 'C': - theta_2 = radians(self.CC_current) - log.debug('Current position Secondary joint: %s', degrees(theta_2)) - 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 - # 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)! - twp_flag = [] - twp_build_params = {} - 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 - global joint_letter_primary, joint_letter_secondary, twp_error_status - global orient_mode - if self.task == 0: # ignore the preview interpreter - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - if not hal.get_value(twp_is_defined): - # reset the twp parameters - reset_twp_params(self) - msg = "G53.x: 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) - 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 - 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)) - 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) - 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 - 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 - 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) - 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: - # reset the twp parameters - reset_twp_params(self) - msg = ("G53.x ERROR: Requested tool orientation not reachable -> aborting G53.x") - 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) - # 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 - 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) - # 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))) - 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()) - # switch to the dedicated TWP work offsets - self.execute("G59", lineno()) - # activate TOOL 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()) - # set twp-state to 'active' (2) - self.execute("M68 E2 Q2") - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - -# Cancel an active TWP definition and reset the parameters to zero -# Note: To avoid that this python code is run prematurely by the read ahead we need a quebuster at the beginning but -# because we need self.execute() to switch the WCS properly this remap needs to be called from -# an ngc that contains a quebuster before calling this code -def g69_core(self): - 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) - # set twp-state to 'undefined' (0) - self.execute("M68 E2 Q0") - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - -# define a virtual tilted-work-plane (twp) that is perpendicular to the current -# tool-orientation -def g683(self, **words): - global twp_matrix, pre_rot, twp_flag, saved_work_offset_number, saved_work_offset - - if self.task == 0: # ignore the preview interpreter - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - # ! IMPORTANT ! - # We need to use 'yield INTERP_EXECUTE_FINISH' here to stop the read ahead - # and avoid it executing the rest of the remap ahead of time - ## NOTE: No 'self.execute(..)' command can be used after 'yield INTERP_EXECUTE_FINISH' - yield INTERP_EXECUTE_FINISH - - if hal.get_value(twp_is_defined): - # reset the twp parameters - reset_twp_params(self) - msg =("G68.3 ERROR: TWP already defined.") - log.debug(msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # NOTE: Due to easier abort handling we currently restrict the use of twp to G54 - # as LinuxCNC seems to revert to G54 as the default system - # get which offset is active (g54=1 .. g59.3=9) - (n, offsets) = get_current_work_offset(self) - if n != 1: - # reset the twp parameters - reset_twp_params(self) - msg = "G68.3 ERROR: Must be in G54 to define TWP." - log.debug(msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - c = self.blocks[self.remap_level] - # parse the requested origin - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = 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 - 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) - # put the requested origin into the twp_matrix - (twp_matrix[0,3], twp_matrix[1,3], twp_matrix[2,3]) = (x, y, z) - # update the build state of the twp call - twp_flag[2] = 'done' - log.info("G68.3: Built twp-transformation-matrix: \n%s", twp_matrix) - # collect the currently active work offset values (ie g54, g55 or other) - saved_work_offset = offsets - saved_work_offset_number = n - log.debug("G68.3: Saved work offsets: %s", (n, saved_work_offset)) - # set twp-state to 'defined' (1) - self.execute("M68 E2 Q1") - yield INTERP_EXECUTE_FINISH - - gui_update_twp(self) - 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 - - if self.task == 0: # ignore the preview interpreter - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - # ! IMPORTANT ! - # We need to use 'yield INTERP_EXECUTE_FINISH' here to stop the read ahead - # and avoid it executing the rest of the remap ahead of time - ## NOTE: No 'self.execute(..)' command can be used after 'yield INTERP_EXECUTE_FINISH' - yield INTERP_EXECUTE_FINISH - - if hal.get_value(twp_is_defined): # ie TWP has already been defined - # reset the twp parameters - reset_twp_params(self) - msg = ("G68.2: TWP already defined.") - log.debug(msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # NOTE: Due to easier abort handling we currently restrict the use of twp to G54 - # as LinuxCNC seems to revert to G54 as the default system - (n, offsets) = get_current_work_offset(self) - if n != 1: - # reset the twp parameters - reset_twp_params(self) - msg = "G68.2 ERROR: Must be in G54 to define TWP." - log.debug(msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # collect the currently active work offset values (ie g54, g55 or other) - saved_work_offset_number = n - saved_work_offset = offsets - log.debug("G68.2: Saved work offsets %s", (n, saved_work_offset)) - - c = self.blocks[self.remap_level] - p = c.p_number if c.p_flag else 0 - if p == 0: # true euler angles (this is the default mode) - twp_flag = [int(p), 1, 'empty'] # one call to define the twp in this mode - # parse requested order of rotations (default is '313' ie: ZXZ) - q = str(int(c.q_number if c.q_flag else 313)) - if q not in ['121','131','212','232','313','323']: - # reset the twp parameters - reset_twp_params(self) - msg = ("G68.2 (P0): No recognised Q-Word found.") - log.debug(msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # parse the requested origin - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = 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 - - # 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) - # 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) - # 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 - twp_matrix = np.hstack((twp_rotation, twp_origin)) - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - # update the build state of the twp call - twp_flag[2] = 'done' - - elif p == 1: # non-true euler angles, eg: 'pitch,roll,yaw' - twp_flag = [int(p), 1, 'empty'] # one call to define the twp in this mode - # parse requested order of rotations (default is '123' ie: XYZ) - q = str(int(c.q_number if c.q_flag else 123)) - - if q not in ['123','132','213','231','312','321']: - # reset the twp parameters - reset_twp_params(self) - msg = ("G68.2 P1: No recognised Q-Word found.") - log.debug(msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - - # parse the requested origin - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = 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 - - # 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) - # 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) - # 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 - twp_matrix = np.hstack((twp_rotation, twp_origin)) - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - # update the build state of the twp call - twp_flag[2] = 'done' - - elif p == 2: # twp defined py 3 points on the plane - # 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) - 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 - twp_build_params['q0'] = [x,y,z,r] - twp_flag[2] = 'done' - elif q == 1: # define point 1 - x1 = c.x_number if c.x_flag else 0 - y1 = c.y_number if c.y_flag else 0 - z1 = c.z_number if c.z_flag else 0 - twp_build_params['q1'] = [x1,y1,z1] - twp_flag[3] = 'done' - elif q == 2: # define point 2 - x2 = c.x_number if c.x_flag else 0 - y2 = c.y_number if c.y_flag else 0 - z2 = c.z_number if c.z_flag else 0 - twp_build_params['q2'] = [x2,y2,z2] - twp_flag[4] = 'done' - elif q == 3: # define point 3 - x3 = c.x_number if c.x_flag else 0 - y3 = c.y_number if c.y_flag else 0 - z3 = c.z_number if c.z_flag else 0 - twp_build_params['q3'] = [x3,y3,z3] - twp_flag[5] = 'done' - else: - # reset the twp parameters - reset_twp_params(self) - msg = ("G68.2 P2: No recognised Q-Word found.") - log.debug(msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # only start calculations once all the parameters have been passed - if twp_flag.count('done') == twp_flag[1]: - [x, y, z, r] = twp_build_params['q0'][0:4] - # build the translation vector of the twp_matrix - twp_origin = [[x], [y], [z]] - p1 = twp_build_params['q1'][0:3] - p2 = twp_build_params['q2'] - p3 = twp_build_params['q3'] - log.debug("G68.2 P2: Point 1: %s",p1) - log.debug("G68.2 P2: Point 2: %s",p2) - log.debug("G68.2 P2: Point 3: %s",p3) - # build vectors x:P1->P2 and v2:P1->P3 - twp_vect_x = [p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]] - log.debug("G68.2 P2: Twp_vect_x: \n%s",twp_vect_x) - v2 = [p3[0]-p1[0], p3[1]-p1[1], p3[2]-p1[2]] - log.debug("G68.2 P2 (v2): %s",v2) - # normalize the two vectors - twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) - v2 = v2 / np.linalg.norm(v2) - # we can use the cross product to calculate the tool-z 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) - 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 - 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 - # 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) - # 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 - twp_matrix = np.hstack((twp_rotation, twp_origin)) - # expand to 4x4 array and make into a matrix - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - log.debug("G68.2 P2: Built twp-transformation-matrix: \n%s", twp_matrix) - - elif p == 3: # two vectors (vector 1 defines the tool-x and vector 2 defines the tool-z) - q = int(c.q_number if c.q_flag else 0) - # if this is the first call for this mode reset the twp_flag flag - if not twp_flag: - log.info('first call') - twp_flag = [int(p), 2 , 'empty', 'empty'] # two calls needed - twp_build_params = {'q0':[], 'q1':[]} - 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 - # first vector (direction of x in the twp) - i = c.i_number if c.i_flag else 0 - j = c.j_number if c.j_flag else 0 - k = c.k_number if c.k_flag else 0 - twp_build_params['q0'] = [x,y,z,i,j,k,r] - twp_flag[2] = 'done' - elif q == 1: # define second vector (the normal vector of the twp - i1 = c.i_number if c.i_flag else 0 - j1 = c.j_number if c.j_flag else 0 - k1 = c.k_number if c.k_flag else 0 - twp_build_params['q1'] = [i1,j1,k1] - twp_flag[3] = 'done' - else: - # reset the twp parameters - reset_twp_params(self) - msg = ("G68.2 P3: No recognised Q-Word found.") - log.debug(msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # only start calculations once all the parameters have been passed - if twp_flag.count('done') == twp_flag[1]: - twp_origin = (x ,y, z) = twp_build_params['q0'][0:3] - r = twp_build_params['q0'][6] - (i, j, k) = twp_build_params['q0'][3:6] - (i1, j1, k1) = twp_build_params['q1'] - 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 - 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) - # the two vectors must be orthogonal - if orth != 0: - reset_twp_params(self) - msg = ("G68.2 P3: Vectors are not orthogonal.") - 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 - 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 - # 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 - 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) - # 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 - twp_origin = [[x], [y], [z]] - twp_matrix = np.hstack((twp_rotation, twp_origin)) - # expand to 4x4 array and make into a matrix - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - log.debug("G68.2 P3: Built twp-transformation-matrix: \n%s", twp_matrix) - - else: - # reset the twp parameters - reset_twp_params(self) - msg = ("G68.2: No recognised P-Word found.") - log.debug(msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - log.debug("G68.2: twp_flag: %s", twp_flag) - log.debug("G68.2: calls required: %s", twp_flag.count('done')) - log.debug("G68.2: number of calls made: %s", twp_flag.count('done')) - - if twp_flag.count('done') == twp_flag[1]: - log.info('G68.2: requested rotation: %s', radians(r)) - log.info("G68.2: twp-tranformation-matrix: \n%s",twp_matrix) - twp_origin = [twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]] - log.info("G68.2: twp origin: %s", twp_origin) - twp_vect_x = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] - log.info("G68.2: twp vector-x: %s", twp_vect_x) - twp_vect_z = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] - log.info("G68.2: twp vector-z: %s", twp_vect_z) - # set twp-state to 'defined' (1) - self.execute("M68 E2 Q1") - yield INTERP_EXECUTE_FINISH - - gui_update_twp(self) - 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 - - if self.task == 0: # ignore the preview interpreter - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - # ! IMPORTANT ! - # We need to use 'yield INTERP_EXECUTE_FINISH' here to stop the read ahead - # and avoid it executing the rest of the remap ahead of time - ## NOTE: No 'self.execute(..)' command can be used after 'yield INTERP_EXECUTE_FINISH' - yield INTERP_EXECUTE_FINISH - - if not hal.get_value(twp_is_active): # ie there is currently no TWP defined - # reset the twp parameters - reset_twp_params(self) - msg = ("G68.4: No TWP active to increment from. Run G68.2 or G68.3 first.") - log.debug(msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # collect the currently active work offset values (ie g54, g55 or other) - n = get_current_work_offset(self)[0] - # Must be in one of the dedicated offset systems for TWP - if False: #n < 6: - # reset the twp parameters - reset_twp_params(self) - msg = ("G68.4 ERROR: Must be in G59, G59.x to increment TWP.") - log.debug(msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - - # store the current TWP to - twp_matrix_current = np.matrix.copy(twp_matrix) - - c = self.blocks[self.remap_level] - p = c.p_number if c.p_flag else 0 - - if p == 0: # true euler angles (this is the default mode) - twp_flag = [int(p), 1, 'empty'] # one call to define the twp in this mode - # parse requested order of rotations (default is '313' ie: ZXZ) - q = str(int(c.q_number if c.q_flag else 313)) - - if q not in ['121','131','212','232','313','323']: - # reset the twp parameters - reset_twp_params(self) - msg = ("G68.4 (P0): No recognised Q-Word found.") - log.debug(msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # parse the requested origin - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested 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 - - # 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) - # 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) - # 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 - twp_matrix = np.hstack((twp_rotation, twp_origin)) - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - # update the build state of the twp call - twp_flag[2] = 'done' - - elif p == 1: # non-true euler angles, eg: 'pitch,roll,yaw' - twp_flag = [int(p), 1, 'empty'] # one call to define the twp in this mode - # parse requested order of rotations (default is '123' ie: XYZ) - q = str(int(c.q_number if c.q_flag else 123)) - - if q not in ['123','132','213','231','312','321']: - # reset the twp parameters - reset_twp_params(self) - msg = ("G68.4 P1: No recognised Q-Word found.") - log.debug(msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # parse the requested origin - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = 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 - - # 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) - # 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) - # 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 - twp_matrix = np.hstack((twp_rotation, twp_origin)) - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - # update the build state of the twp call - twp_flag[2] = 'done' - - elif p == 2: # twp defined py 3 points on the plane - # 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) - 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 - twp_build_params['q0'] = [x,y,z,r] - twp_flag[2] = 'done' - elif q == 1: # define point 1 - x1 = c.x_number if c.x_flag else 0 - y1 = c.y_number if c.y_flag else 0 - z1 = c.z_number if c.z_flag else 0 - twp_build_params['q1'] = [x1,y1,z1] - twp_flag[3] = 'done' - elif q == 2: # define point 2 - x2 = c.x_number if c.x_flag else 0 - y2 = c.y_number if c.y_flag else 0 - z2 = c.z_number if c.z_flag else 0 - twp_build_params['q2'] = [x2,y2,z2] - twp_flag[4] = 'done' - elif q == 3: # define point 3 - x3 = c.x_number if c.x_flag else 0 - y3 = c.y_number if c.y_flag else 0 - z3 = c.z_number if c.z_flag else 0 - twp_build_params['q3'] = [x3,y3,z3] - twp_flag[5] = 'done' - else: - # reset the twp parameters - reset_twp_params(self) - msg = ("G68.4 P2: No recognised Q-Word found.") - log.debug(msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # only start calculations once all the parameters have been passed - if twp_flag.count('done') == twp_flag[1]: - [x, y, z, r] = twp_build_params['q0'][0:4] - # build the translation vector of the twp_matrix - twp_origin = [[x], [y], [z]] - p1 = twp_build_params['q1'][0:3] - p2 = twp_build_params['q2'] - p3 = twp_build_params['q3'] - log.debug("G68.4 P2: Point 1: %s",p1) - log.debug("G68.4 P2: Point 2: %s",p2) - log.debug("G68.4 P2: Point 3: %s",p3) - # build vectors x:P1->P2 and v2:P1->P3 - twp_vect_x = [p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]] - log.debug("G68.4 P2: Twp_vect_x: \n%s",twp_vect_x) - v2 = [p3[0]-p1[0], p3[1]-p1[1], p3[2]-p1[2]] - log.debug("G68.4 P2: (v2) %s", v2) - # normalize the two vectors - twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) - v2 = v2 / np.linalg.norm(v2) - # we can use the cross product to calculate the tool-z 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) - 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 - 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 - # 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 - 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) - # 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 - twp_matrix = np.hstack((twp_rotation, twp_origin)) - # expand to 4x4 array and make into a matrix - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - log.debug("G68.4 P2: Built twp-transformation-matrix: \n%s", twp_matrix) - - elif p == 3: # two vectors (vector 1 defines the tool-x and vector 2 defines the tool-z) - q = int(c.q_number if c.q_flag else 0) - # if this is the first call for this mode reset the twp_flag flag - if not twp_flag: - twp_flag = [int(p), 2 , 'empty', 'empty'] # two calls needed - twp_build_params = {'q0':[], 'q1':[]} - if q == 0: # define new origin and first vector (direction of x in the twp) - 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 - # first vector (direction of x in the twp) - i = c.i_number if c.i_flag else 0 - j = c.j_number if c.j_flag else 0 - k = c.k_number if c.k_flag else 0 - twp_build_params['q0'] = [x,y,z,i,j,k,r] - twp_flag[2] = 'done' - elif q == 1: # define second vector (the normal vector of the twp - i1 = c.i_number if c.i_flag else 0 - j1 = c.j_number if c.j_flag else 0 - k1 = c.k_number if c.k_flag else 0 - twp_build_params['q1'] = [i1,j1,k1] - twp_flag[3] = 'done' - else: - # reset the twp parameters - reset_twp_params(self) - msg = ("G68.4 P3: No recognised Q-Word found.") - log.debug(msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # only start calculations once all the parameters have been passed - if twp_flag.count('done') == twp_flag[1]: - twp_origin = (x ,y, z) = twp_build_params['q0'][0:3] - r = twp_build_params['q0'][6] - (i, j, k) = twp_build_params['q0'][3:6] - (i1, j1, k1) = twp_build_params['q1'] - 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 - 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) - # the two vectors must be orthogonal - if orth != 0: - # reset the twp parameters - reset_twp_params(self) - ## 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) - 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 - 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 - # 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) - # 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 - twp_origin = [[x], [y], [z]] - twp_matrix = np.hstack((twp_rotation, twp_origin)) - # expand to 4x4 array and make into a matrix - twp_row_4 = [0,0,0,1] - twp_matrix = np.vstack((twp_matrix, twp_row_4)) - twp_matrix = np.asmatrix(twp_matrix) - log.debug("G68.4 P3: Built twp-transformation-matrix: \n%s", twp_matrix) - - else: - # reset the twp parameters - reset_twp_params(self) - msg = ("G68.4: No recognised P-Word found.") - log.debug(msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - log.debug("G68.4: twp_flag: %s", twp_flag) - log.debug("G68.4: calls required: %s", twp_flag.count('done')) - log.debug("G68.4: number of calls made: %s", twp_flag.count('done')) - - if twp_flag.count('done') == twp_flag[1]: - log.info('G68.4: requested rotation %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...") - twp_matrix_new = twp_matrix_current * twp_matrix - log.info("G68.4: twp_matrix_new: \n%s",twp_matrix_new) - twp_origin = [twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]] - log.info("G68.4: twp origin: %s", twp_origin) - twp_vect_x = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] - log.info("G68.4: twp vector-x: %s", twp_vect_x) - twp_vect_z = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] - log.info("G68.4: twp vector-z: %s", twp_vect_z) - log.info("G68.4: incremented twp_matrix: \n%s", twp_matrix_new) - twp_matrix = twp_matrix_new - # set twp-state to 'defined' (1) - self.execute("M68 E2 Q1") - yield INTERP_EXECUTE_FINISH - - gui_update_twp(self) - return INTERP_OK diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/toplevel.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/toplevel.py deleted file mode 100755 index c7ce432a045..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/toplevel.py +++ /dev/null @@ -1,20 +0,0 @@ -# This is a component of LinuxCNC -# Copyright 2011, 2012, 2013 Dewey Garrett , -# Michael Haberler -# -# This program is free software; you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation; either version 2 of the License, or -# (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. -# -import remap - diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/twp-helper-comp.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/twp-helper-comp.py index 44748312c4e..ee6994e7196 100755 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/twp-helper-comp.py +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/twp-helper-comp.py @@ -1,87 +1,47 @@ #!/usr/bin/env python3 +# Publishes the tilted work plane for the vismach model: the origin and the +# X and Z directions in the coordinate system the plane was defined in, +# read from status where the interpreter keeps them (G68.2, G68.3, G68.4, +# G69), and the active work offset the model translates the plane by. import hal import linuxcnc +import time h = hal.component("twp-helper-comp") -# this pin reflects the machine.analog pin used for the -# twp-status -h.newpin("twp-status", hal.Type.REAL, hal.Dir.IN) -# these pins are created here from 'twp-status'' -h.newpin("twp-is-redefined", hal.Type.BOOL, hal.Dir.OUT) +h.newpin("twp-status", hal.Type.REAL, hal.Dir.OUT) # 0 undefined, 1 defined h.newpin("twp-is-defined", hal.Type.BOOL, hal.Dir.OUT) h.newpin("twp-is-active", hal.Type.BOOL, hal.Dir.OUT) -# twp origin vector -h.newpin("twp-ox-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-oy-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-oz-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-ox", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-oy", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-oz", hal.Type.REAL, hal.Dir.OUT) -# twp x-orientation vector -h.newpin("twp-xx-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-xy-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-xz-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-xx", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-xy", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-xz", hal.Type.REAL, hal.Dir.OUT) -# twp z-orientation vector -h.newpin("twp-zx-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-zy-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-zz-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-zx", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-zy", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-zz", hal.Type.REAL, hal.Dir.OUT) -# twp origin vector in machine coordinate system -h.newpin("twp-ox-world-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-oy-world-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-oz-world-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-ox-world", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-oy-world", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-oz-world", hal.Type.REAL, hal.Dir.OUT) +for name in ("twp-ox", "twp-oy", "twp-oz", + "twp-xx", "twp-xy", "twp-xz", + "twp-zx", "twp-zy", "twp-zz", + "twp-ox-world", "twp-oy-world", "twp-oz-world"): + h.newpin(name, hal.Type.REAL, hal.Dir.OUT) h.ready() -# create a connection to the status channel s = linuxcnc.stat() try: while 1: - # publish twp-status - if h['twp-status'] == 1: - h['twp-is-defined'] = 1 - h['twp-is-active'] = 0 - elif h['twp-status'] == 2: - h['twp-is-defined'] = 1 - h['twp-is-active'] = 1 - else: - h['twp-is-defined'] = 0 - h['twp-is-active'] = 0 - - # passthrough the twp arguments - h['twp-ox'] = h['twp-ox-in'] - h['twp-oy'] = h['twp-oy-in'] - h['twp-oz'] = h['twp-oz-in'] - h['twp-xx'] = h['twp-xx-in'] - h['twp-xy'] = h['twp-xy-in'] - h['twp-xz'] = h['twp-xz-in'] - h['twp-zx'] = h['twp-zx-in'] - h['twp-zy'] = h['twp-zy-in'] - h['twp-zz'] = h['twp-zz-in'] - - # we only want to expose offsets when twp is not defined - if not h['twp-is-defined']: - s.poll() # get current values - g5x_offset = s.g5x_offset - h['twp-ox-world'] = g5x_offset[0] - h['twp-oy-world'] = g5x_offset[1] - h['twp-oz-world'] = g5x_offset[2] - else : # use the values from the remap - h['twp-ox-world'] = h['twp-ox-world-in'] - h['twp-oy-world'] = h['twp-oy-world-in'] - h['twp-oz-world'] = h['twp-oz-world-in'] + s.poll() + active = 1 if s.g68_active else 0 + h['twp-status'] = active + h['twp-is-defined'] = active + h['twp-is-active'] = active + + o = s.g68_offset + r = s.g68_rotation + h['twp-ox'], h['twp-oy'], h['twp-oz'] = o[0], o[1], o[2] + # columns of the rotation: the plane's X and Z + h['twp-xx'], h['twp-xy'], h['twp-xz'] = r[0], r[3], r[6] + h['twp-zx'], h['twp-zy'], h['twp-zz'] = r[2], r[5], r[8] + + g5x = s.g5x_offset + h['twp-ox-world'], h['twp-oy-world'], h['twp-oz-world'] = g5x[0], g5x[1], g5x[2] + time.sleep(0.05) except KeyboardInterrupt: raise SystemExit diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/util.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/util.py deleted file mode 100755 index 59b012058ad..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/util.py +++ /dev/null @@ -1,67 +0,0 @@ -# This is a component of LinuxCNC -# Copyright 2011, 2013 Dewey Garrett , Michael -# Haberler -# -# This program is free software; you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation; either version 2 of the License, or -# (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. -# -import inspect -import emccanon - -# O-word procedure to trap into the Pydevd debugger -# start debug server in Eclipse, then -# call as 'O call' from MDI - -# example setup for debugging embedded Python code -# see http://pydev.org/manual_adv_remote_debugger.html -# if this points to a valid directory, - -def call_pydevd(): - """ trap into the pydevd debugger""" - - import os,sys - - pydevdir= '/home/mah/.eclipse/org.eclipse.platform_3.5.0_155965261/plugins/org.python.pydev.debug_2.0.0.2011040403/pysrc/' - - # the 'emctask' module is present only in the milltask instance, otherwise both the UI and - # milltask would try to connect to the debug server. - - if os.path.isdir(pydevdir) and 'emctask' in sys.builtin_module_names: - sys.path.append(pydevdir) - sys.path.insert(0,pydevdir) - try: - import pydevd - emccanon.MESSAGE("pydevd imported, connecting to Eclipse debug server...") - pydevd.settrace() - except: - emccanon.MESSAGE("no pydevd module found") - pass - - - -def lineno(): - """ return line number in the current Python script """ - return inspect.currentframe().f_back.f_lineno - -def error_stack(self): - """ print the Interpreters error stack (function names) """ - print("error stack level=%d" % (self.stack_index)) - for s in self.stack(): - print("--'%s'" % (s)) - -def callstack(self): - """ print the O-Word call stack """ - for i in range(self.call_level): - c = self.sub_context[i] - print("%d: pos=%d seq=%d filename=%s sub=%s" % (i,c.position, c.sequence_number,c.filename,c.subname)) diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g531remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g531remap.ngc deleted file mode 100755 index 4b2fd293c61..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g531remap.ngc +++ /dev/null @@ -1,14 +0,0 @@ -; this is the wrapper remap to orient the spindle using IDENTITY kinematics G53.1 - -osub -M66 L0 E0 ;force sync, stop read ahead -o100 if [EXISTS [#

]] -o100 else - #

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

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

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

) -o105 else - #

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

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

]] -o100 else - #

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

;orient the spindle with P word -M66 L0 E0 -oendsub -m2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g69remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g69remap.ngc deleted file mode 100755 index 9efd1b7db29..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g69remap.ngc +++ /dev/null @@ -1,11 +0,0 @@ -; this is the wrapper remap to cancel TWP - -osub -M66 L0 E0 ; force sync, stop read ahead -M469 ; call the python G69_core code -G13.1 ; back to identity kins -M68 E2 Q0 ; reset twp-state to 'undefined' (0) -G54 ; switch to G54 -M66 L0 E0 ; force sync, stop read ahead -oendsub -m2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/on_abort_with_twp_reset.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/on_abort_with_twp_reset.ngc deleted file mode 100755 index 1cbf3d41db7..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/on_abort_with_twp_reset.ngc +++ /dev/null @@ -1,15 +0,0 @@ -;This is a workaround for a bug that leads to stat.gcodes and parameter[5250] -;to get out of sync after some program aborts -;in [RS274NGC] section of the ini add: ON_ABORT_COMMAND = o call -;save this to a path specified in SUBROUTINE_PATH = -;NOTE: we cannot run remapped codes here only custom Mcodes (ie M100..M199) - -o sub -;(msg, on_abort START) -M68 E2 Q0 ; reset twp-state to 'undefined' (0) -G13.1 ; back to identity kins -G64 P0.01 ; reset the toolpath tolerance as this sometimes gets set to zero on estop events -G54 ; switch to G54 -(msg, on_abort END) -o endsub -M2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini index 06b9cd5d23f..0c4389ca1a9 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini @@ -29,46 +29,18 @@ MAX_ANGULAR_VELOCITY = 360 [RS274NGC] RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 ON_ABORT_COMMAND = o call -#ON_ABORT_COMMAND = o call SUBROUTINE_PATH = ../remap_subs:../demos HAL_PIN_VARS = 1 REMAP = M428 modalgroup=10 ngc=428remap REMAP = M429 modalgroup=10 ngc=429remap REMAP = M430 modalgroup=10 ngc=430remap - REMAP = G53.1 modalgroup=1 argspec=p ngc=g531remap - REMAP = G53.3 modalgroup=1 argspec=pxyz ngc=g533remap - REMAP = G53.6 modalgroup=1 argspec=p ngc=g536remap - REMAP = M530 modalgroup=10 python=g53x_core - - REMAP = G68.2 modalgroup=1 argspec=pqxyzijkr python=g682 - REMAP = G68.3 modalgroup=1 argspec=xyzr python=g683 - REMAP = G68.4 modalgroup=1 argspec=pqxyzijkr python=g684 - - REMAP = G69 modalgroup=1 ngc=g69remap - REMAP = M469 modalgroup=10 python=g69_core - PARAMETER_FILE = xyzacb-trsrn.var -[PYTHON] -# where to find the Python code: -# code specific for this configuration -PATH_APPEND = ../python -# import the following Python module -TOPLEVEL = ../python/toplevel.py -# the higher the more verbose tracing of the Python plugin -LOG_LEVEL = 3 - [KINS] KINEMATICS = xyzacb_trsrn JOINTS = 6 -[TWP] -# this defines the primary spindle rotation -PRIMARY = C -# this defines the secnodary spindle rotation (ie the one closest to the tool) -SECONDARY = B - [HAL] HALUI = halui HALFILE = LIB:basic_sim.tcl @@ -76,11 +48,6 @@ POSTGUI_HALFILE = xyzacb-trsrn_postgui.hal #HALCMD = loadusr ../python/feed_zero.py - -# signal reflecting twp states (0=undefined, 1=defined, 2=active) -HALCMD = net twp-status <= motion.analog-out-02 - - # connections required for the kinematics component HALCMD = net :tool-offset motion.tooloffset.z xyzacb_trsrn_kins.tool-offset-z HALCMD = net :rot-axis-y xyzacb_trsrn_kins.y-rot-axis @@ -93,8 +60,6 @@ HALCMD = net :offset-y xyzacb_trsrn_kins.y- # load the required twp-helper component and its hal connections HALCMD = loadusr -W ../python/twp-helper-comp.py -#twp-status -HALCMD = net twp-status => twp-helper-comp.twp-status HALCMD = net twp-is-defined <= twp-helper-comp.twp-is-defined HALCMD = net twp-is-active <= twp-helper-comp.twp-is-active # current twp parameters @@ -150,7 +115,6 @@ HALCMD = net twp-status xyzacb-trsrn-gui.twp HALCMD = net twp-is-defined xyzacb-trsrn-gui.twp_defined HALCMD = net twp-is-active xyzacb-trsrn-gui.twp_active - [HALUI] # NOTE: kinstype==0 is identity kins because sparm=identityfirst # M428:identity kins (kinstype 0, startupDEFAULT) @@ -220,7 +184,6 @@ MAX_ACCELERATION = 302 MAX_VELOCITY = 30 MAX_ACCELERATION = 301 - [JOINT_0] TYPE = LINEAR HOME = 0 @@ -231,7 +194,6 @@ MAX_ACCELERATION = 301 HOME_SEARCH_VEL = 0 HOME_SEQUENCE = 0 - [JOINT_1] TYPE = LINEAR HOME = 0 @@ -252,7 +214,6 @@ MAX_ACCELERATION = 301 HOME_SEARCH_VEL = 0 HOME_SEQUENCE = 0 - #table rotary [JOINT_3] TYPE = ANGULAR diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini index d9ae382fefc..5223bc375f6 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini @@ -28,56 +28,24 @@ MAX_ANGULAR_VELOCITY = 360 [RS274NGC] RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 -#ON_ABORT_COMMAND = o call -ON_ABORT_COMMAND = o call +ON_ABORT_COMMAND = o call SUBROUTINE_PATH = ../remap_subs:../demos HAL_PIN_VARS = 1 REMAP = M428 modalgroup=10 ngc=428remap REMAP = M429 modalgroup=10 ngc=429remap REMAP = M430 modalgroup=10 ngc=430remap - REMAP = G53.1 modalgroup=1 argspec=p ngc=g531remap - REMAP = G53.3 modalgroup=1 argspec=pxyz ngc=g533remap - REMAP = G53.6 modalgroup=1 argspec=p ngc=g536remap - REMAP = M530 modalgroup=10 python=g53x_core - - REMAP = G68.2 modalgroup=1 argspec=pqxyzijkr python=g682 - REMAP = G68.3 modalgroup=1 argspec=xyzr python=g683 - REMAP = G68.4 modalgroup=1 argspec=pqxyzijkr python=g684 - - REMAP = G69 modalgroup=1 ngc=g69remap - REMAP = M469 modalgroup=10 python=g69_core - PARAMETER_FILE = xyzbca-trsrn.var -[PYTHON] -# where to find the Python code: -# code specific for this configuration -PATH_APPEND = ../python -# import the following Python module -TOPLEVEL = ../python/toplevel.py -# the higher the more verbose tracing of the Python plugin -LOG_LEVEL = 3 - [KINS] KINEMATICS = xyzbca_trsrn JOINTS = 6 -[TWP] -# this defines the primary spindle rotation -PRIMARY = C -# this defines the secnodary spindle rotation (ie the one closest to the tool) -SECONDARY = A - [HAL] HALUI = halui HALFILE = LIB:basic_sim.tcl POSTGUI_HALFILE = xyzbca-trsrn_postgui.hal -# signal reflecting twp states (0=undefined, 1=defined, 2=active) -HALCMD = net twp-status <= motion.analog-out-02 - - # connections required for the kinematics component HALCMD = net :tool-offset motion.tooloffset.z xyzbca_trsrn_kins.tool-offset-z HALCMD = net :rot-axis-x xyzbca_trsrn_kins.x-rot-axis @@ -90,8 +58,6 @@ HALCMD = net :offset-y xyzbca_trsrn_kins.y- # load the required twp-helper component and its hal connections HALCMD = loadusr -W ../python/twp-helper-comp.py -#twp-status -HALCMD = net twp-status => twp-helper-comp.twp-status HALCMD = net twp-is-defined <= twp-helper-comp.twp-is-defined HALCMD = net twp-is-active <= twp-helper-comp.twp-is-active # current twp parameters @@ -147,7 +113,6 @@ HALCMD = net twp-status xyzbca-trsrn-gui.twp HALCMD = net twp-is-defined xyzbca-trsrn-gui.twp_defined HALCMD = net twp-is-active xyzbca-trsrn-gui.twp_active - [HALUI] # NOTE: kinstype==0 is identity kins because sparm=identityfirst # M428:identity kins (kinstype 0, startupDEFAULT) @@ -217,7 +182,6 @@ MAX_ACCELERATION = 302 MAX_VELOCITY = 30 MAX_ACCELERATION = 301 - [JOINT_0] TYPE = LINEAR HOME = 0 @@ -228,7 +192,6 @@ MAX_ACCELERATION = 301 HOME_SEARCH_VEL = 0 HOME_SEQUENCE = 0 - [JOINT_1] TYPE = LINEAR HOME = 0 @@ -249,7 +212,6 @@ MAX_ACCELERATION = 301 HOME_SEARCH_VEL = 0 HOME_SEQUENCE = 0 - # spindle secondary joint [JOINT_3] TYPE = ANGULAR diff --git a/debian/linuxcnc-uspace-dev.install b/debian/linuxcnc-uspace-dev.install index 199dae9fcc0..e13585d482d 100644 --- a/debian/linuxcnc-uspace-dev.install +++ b/debian/linuxcnc-uspace-dev.install @@ -5,3 +5,7 @@ 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/switchkins_setup.c +usr/share/linuxcnc/kins_util.c +usr/share/linuxcnc/kins_single.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/code/code-notes.adoc b/docs/src/code/code-notes.adoc index 3874fd37002..064db167757 100644 --- a/docs/src/code/code-notes.adoc +++ b/docs/src/code/code-notes.adoc @@ -1312,8 +1312,7 @@ settings.tool_offset:: + * Used to compute position in various places. * Sent to Motion via the +EMCMOT_SET_OFFSET+ message. - All motion does with the offsets is export them to the HAL pins +motion.0.tooloffset.[xyzabcuvw]+. - FIXME: export these from someplace closer to the tool table (io or interp, probably) and remove the EMCMOT_SET_OFFSET message. + Motion exports the offsets to the HAL pins +motion.0.tooloffset.[xyzabcuvw]+ and hands them to the kinematics module through +kinematicsSetTool()+, for a module whose maths needs the tool length. settings.pockets_max:: Used interchangeably with +CANON_POCKETS_MAX+ (a #defined constant, set to 1000 as of April 2020). diff --git a/docs/src/config/python-interface.adoc b/docs/src/config/python-interface.adoc index 25d5762445b..0fce1c760ac 100644 --- a/docs/src/config/python-interface.adoc +++ b/docs/src/config/python-interface.adoc @@ -179,6 +179,15 @@ see <> for an example. *g5x_offset*:: '(returns tuple of floats)' - offset of the currently active coordinate system. +*g68_active*:: '(returns integer)' - + a tilted work plane (G68.2) is in effect. + +*g68_offset*:: '(returns tuple of floats)' - + origin of the tilted work plane, in the coordinate system it was defined in. + +*g68_rotation*:: '(returns tuple of floats)' - + rotation of the tilted work plane, nine values row by row; its columns are the plane's X, Y and Z. + *g92_offset*:: '(returns tuple of floats)' - pose of the current g92 offset. diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 12dbcdf599f..4e7f6738a7d 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -84,13 +84,18 @@ as the 'L number', and so on for any other letter. |<> |Use Tool Length Offset from Tool Table |<> |Dynamic Tool Length Offset |<> |Apply additional Tool Length Offset +|<> |Tool Length Offset on Primary Kinematics +|<> |Tool Length Offset with the Tool Axis as a Vector |<> |Cancel Tool Length Offset |<> |Local Coordinate System Offset |<> |Move in Machine Coordinates +|<> |Orient the Tool to the Tilted Work Plane +|<> |Point-to-Point Move |<> |Select Coordinate System (1 - 9) |<> |Exact Path Mode |<> |Exact Stop Mode |<> |Path Control Mode with Optional Tolerance +|<> |Tilted Work Plane |<> |Lathe finishing cycle |<> |Lathe roughing cycle |<> |Drilling Cycle with Chip Breaking @@ -1537,6 +1542,19 @@ in the numbered parameters '5081-5089'. The numbered parameters '5401-5409' instead hold the loaded tool's stored offset, refreshed on tool change ('M6') and 'G10 L1'/'L10'/'L11'. +Some kinematics modules apply the tool length themselves, along the tool +axis rather than along Z (5axiskins, maxkins, the trt modules, genhexkins, +pentakins; see the <> +chapter). 'G43' does not move the machine under those either. With the +tool axis tilted at the time, the programmed coordinates of the point the +tool stands on change by more than the offset difference: the interpreter +works them out through the kinematics, so the next move starts from where +the tool tip actually is. Motion reads the point from the joints the same +way and stops the program if the two disagree. A module that can only be +evaluated in realtime leaves the interpreter with the offset difference +alone; under one of those, change the tool length with the tool axis +untilted, motion stops the program otherwise. + [NOTE] 'G43 H0' is a little special. Its behavior is different on random tool changer machines and nonrandom tool changer machines (see the @@ -1641,11 +1659,125 @@ It is an error if: NOTE: G43.2 does not write to the tool table. +[[gcode:g43.4]] +== G43.4 Tool Length Offset on Primary Kinematics(((G43.4 Tool Length Offset on Primary Kinematics))) + +[source,ngc] +---- +G43.4 +---- + +* 'H' - tool number (optional) + +'G43.4' is 'G43' together with a switch to the kinematics the module +declares its working transform, so the program runs with tool length +compensation in the module's working kinematics. The switch happens +first and the offset applies after it, as if the two had been written +on consecutive lines. 'G49' is the matching cancel: it clears the +offset and switches back to identity kinematics, as long as the offset +in effect is still 'G43.4''s and no 'G12.1' or 'G13.1' has selected a +kinematics since. + +The H word, the offset itself and the parameters it lands in are +'G43''s. Which kinematics is the working one is declared by the module +(KINSTYPE_PRIMARY, see the <> chapter), the number is not the answer; a module that +declares none rejects 'G43.4' at read time. + +The switch is a queue synchronisation point like +'<>': when 'G43.4' or 'G49' changes the +kinematics, the interpreter waits for queued motion to finish first, so +both stop any blending in progress. Already on the target kinematics +they do not switch and blend normally. A plain 'G43', 'G43.1' or +'G43.2' never switches, whatever kinematics is selected, and the 'G49' +that cancels one of them does not switch either. + +On a machine without switchable kinematics there is nothing to switch: +'G43.4' is a plain 'G43' and 'G49' a plain cancel. + +It is an error if: + +* the kinematics module is switchable but declares no primary + kinematics, or +* any of the 'G43' error conditions holds. + +[[gcode:g43.5]] +== G43.5 Tool Length Offset with the Tool Axis as a Vector(((G43.5 Tool Length Offset with the Tool Axis as a Vector))) + +[source,ngc] +---- +G43.5 +G0 X- Y- Z- I- J- K- +G1 X- Y- Z- I- J- K- F- +---- + +* 'H' - tool number (optional) +* 'I J K' - the direction the tool axis is to point along, on a 'G0' or + 'G1' line while 'G43.5' is in effect + +'G43.5' is '<>' with one thing more: while it is in +effect, a 'G0' or 'G1' line may give the direction of the tool axis as +a vector 'I J K' in place of rotary words, and the interpreter works +out where the rotary joints have to go to point the tool that way. +This is the form of a five-axis program written for the tool rather +than for the machine, the one Fanuc calls tool center point control +type 2, and it runs on any machine whose kinematics module supplies its +tool frame (see the <> chapter): the same program orients a tilting head, a +tilting table and a robot. + +The vector points from the tool tip towards the holder, in the +coordinate system the line's 'X Y Z' are in: the active coordinate +system with its rotation, the <> where +one is active, and the machine's on a '<>' line. Its +length does not matter. A word left out is zero, so 'K1' alone is the +tool vertical. The words describe a direction, so they are never +incremental: 'G91' applies to the 'X Y Z' of the line, not to 'I J K', +and 'G91.1' does not apply to them either. A line with 'I J K' and no +axis word turns the tool where it stands. + +Where more than one pose of the rotaries points the tool along the +vector, two on a five-axis machine, the interpreter takes the one +nearest where the rotaries stand, every orienting joint free to take +part, so a path of vectors runs continuous from wherever it starts and +a rotary never swings the long way round. On the pole of the primary +rotary, the tool vertical on a mill, one joint no longer matters to the +direction and it stays where it is. + +The pose the interpreter finds goes into the move as ordinary rotary +words, in program coordinates, so the rotary offsets of the coordinate +system apply, and the move interpolates like any other move with rotary +words: the rotaries turn together with the linear axes between the +poses of consecutive lines. The vector itself is not interpolated +along the way; a program that needs the tool to sweep between two +directions writes the lines in between. + +'G2' and 'G3' keep 'I J K' as the arc centre; the tool direction on an +arc is given with rotary words. 'G43', 'G43.1', 'G43.2' and 'G49' end +the vector form along with the offset they replace; 'G49' cancels the +offset and undoes the kinematics switch, as after 'G43.4'. + +It is an error if: + +* 'I J K' are given together with a rotary word on the same line, +* the vector is zero, +* the kinematics selected is the identity one, or the module supplies + no tool frame or cannot be evaluated by the interpreter, +* the direction cannot be reached by the rotary joints, +* 'I J K' are given on a 'G53.5' or 'G53.7' line, or +* any of the 'G43.4' error conditions holds. + [[gcode:g49]] == G49 Cancel Tool Length Compensation(((G49 Cancel Tool Length Offset))) * 'G49' - cancels tool length compensation +'G49' also switches a switchable kinematics module back to its identity +kinematics when it cancels a 'G43.4' or 'G43.5', undoing the switch that made. It +leaves a kinematics selected by 'G12.1' or 'G13.1' alone, as it does +after a plain 'G43', and a module that declares no identity kinematics +gets the plain cancel. + It is OK to program using the same offset already in use. It is also OK to program using no tool length offset if none is currently being used. @@ -1691,6 +1823,229 @@ It is an error if: * G53 is used without G0 or G1 being active, * or G53 is used while cutter compensation is on. +[[gcode:g53.1]] +== G53.1, G53.2, G53.3, G53.6 Orient the Tool to the Work Plane(((G53.1 Orient the Tool))) + +[source,ngc] +---- +G53.1 +G53.2 +G53.3 X- Y- Z- +G53.6 +---- + +All four solve where the rotary joints have to go so that the tool axis is +normal to the active <>, the plane's Z. They +differ in what happens then: + +* 'G53.1' moves the rotaries alone. The linear joints stay where they are, + and the tool tip swings to wherever that carries it. It is a + point-to-point move, like <>. +* 'G53.6' keeps the tool centre point where it is. It is a Cartesian move of + the rotary words, so the kinematics compensates the linear joints all along. +* 'G53.3' moves the rotaries and takes the tool to 'X Y Z', given in the + plane, in one point-to-point move. A word left out keeps the present value. +* 'G53.2' moves nothing. It only publishes the solved pose on the named + parameters '#<_orient_x>', '#<_orient_y>', '#<_orient_z>', '#<_orient_a>', + '#<_orient_b>' and '#<_orient_c>', and on the numbered parameters + '#5071' to '#5079' ('X Y Z A B C U V W'), in program units in the plane, + and sets '#<_orient_valid>' and '#5080' to 1. The program can then reach + the pose with a move of its own making, for instance a single 'G0' that + names 'X Y Z' and the rotary words together, so the turn and the travel + are one Cartesian move under the TCP kinematics. This is what Heidenhain + calls `STAY`. Reading '#<_orient_x>' and kin before the first 'G53.2' is + an error; '#<_orient_valid>' and '#5080' say whether they hold a pose. + +The kinematics module answers where the rotaries have to go, with its tool +frame inverse (see the kinematics conventions chapter), so no configuration +carries the formula. That answer has more than one solution on most +machines, two on a five-axis one, and often a choice of which joints to use. + +'P' picks which of them. Without 'P', or with 'P0', the machine takes the +one nearest where its rotaries are standing, which is the shortest move and +depends on where that is. 'P1' and 'P2' name the pose instead, so a program +reaches the same one wherever it starts from: the two poses lean the head +opposite ways, and they differ in the sign of the secondary rotary, the one +whose axis the other carries. 'P1' is the pose with that rotary positive and +'P2' the pose with it negative. The interpreter works out which rotary that +is by asking the kinematics module, so nothing is configured for it. This is +the choice Heidenhain writes as `SEQ+` and `SEQ-`, with one difference: a +control offering `SEQ` falls back to the nearer pose when both lie the same +side of home, where 'P1' and 'P2' refuse and say so. + +The two poses become one where the tool direction asked for lies along the +primary rotary's axis, straight up on a vertical mill, and there every form +gives the same answer. + +'P' names a pose only where a rotary turns the tool. On a machine whose +rotaries all carry the work, the tilting-table configurations among them, +there is no such rotary and only the nearest form is available; the same +goes for a machine that is not of this shape at all, a robot among them. + +'Q' says whether the joints that carry the work, the table, take part. + +* 'Q0', the default, holds them and lets the head do the work. On a machine + whose head has only two rotaries the plane's X is then not something the + joints can place, and it is left to the coordinate system, which the plane + already carries; nothing else is done with it. This is what Heidenhain + calls `COORD ROT`. If nothing is reachable with the table held, the move + is tried again with every joint free. +* 'Q1' frees the table from the start. With the plane's X asked for as well, + a machine with the joints for it turns the table so that the plane's X is + reached by the machine, Heidenhain's `TABLE ROT`. + +Which joints carry the work is read off the kinematics module's work frame, +so 'Q' means the same thing on every module and needs no INI entry. + +The orientation is evaluated by the interpreter, ahead of the moves before +it, on the kinematics type the program is in. It needs a kinematics type +whose frames describe the machine: on a switchable module that is its TCP +kinematics, selected with <>, and not the identity +kinematics the module starts in. + +.G53.1 Example +[source,ngc] +---- +G12.1 P1 (the TCP kinematics) +G68.2 X50 Y50 Z0 I30 J20 K0 (a plane) +G53.1 (rotaries to its normal, the head does it) +G0 X0 Y0 Z10 (rapid to a point in the plane, above its origin) +G1 Z-2 F200 (down, along the plane's normal) +G69 +---- + +It is an error if: + +* No tilted work plane is active. +* The kinematics type in force is the identity kinematics, the module cannot + be evaluated by the interpreter, or it reports no frames, so it cannot say + where its joints point the tool. +* The plane's normal cannot be reached by the rotary joints. +* 'P' is anything but 0, 1 or 2, no reachable pose has the secondary rotary + the way 'P' asks for, or the machine has no pair of poses a tilting joint + tells apart. +* 'Q' is anything but 0 or 1. +* Axis words are used with 'G53.1', 'G53.2' or 'G53.6', or words other than + 'X', 'Y' and 'Z' with 'G53.3'. +* Cutter compensation is on. + +[[gcode:g53.4]] +== G53.4, G53.5, G53.7 Point-to-Point Move(((G53.4 Point-to-Point Move))) + +[source,ngc] +---- +G53.4 G0 +G53.4 G1 F- +G53.5 G0 +G53.5 G1 F- +G53.7 G0 J= ... +G53.7 G1 F- J= ... +---- + +A point-to-point move defines its two ends and leaves the path between them +to the joints. The inverse kinematics runs once, at the destination, and every +joint then travels from where it is to where it must be, all together, the +slowest setting the pace. The tool does not follow a straight line; on a +machine with rotary joints it swings. The joints are the motion controller's, +<>, the ones the kinematics module maps the axes +to; what HAL connects behind each of them is not part of the interpolation. + +All three are non-modal, like 'G53': they apply to the one block they are +written in, which must have 'G0' or 'G1' in force or on the line. Each strips +one more layer of interpretation from the destination than the one before: + +* 'G53.4' takes a destination in program coordinates, through the offsets and + the tilted work plane like any other move. This is the move a program uses + to cross a kinematic singularity, or to turn a rotary head right round, + without leaving its coordinate system and without the trajectory planner + trying to hold the tool tip on a line the joints cannot follow at speed. +* 'G53.5' takes the same axis letters and sends the slides there. The words + are read as though the kinematics were the identity: each letter names the + joints the module's identity mapping gives it, which the `coordinates=` + parameter and the `[TRAJ]COORDINATES` line describe, and the two joints of + a gantry take one value together. Values are in program units, so 'G20' + scales them like any other axis word. No offset, tool length, rotation or + work plane applies. On a mill whose joint 2 carries the Z slide, 'G53.5 G0 + Z0' puts that slide at zero whatever the head is doing, where 'G53 G0 Z0' + puts the tool tip at machine Z zero. This is the move for parking, tool + change and home positions, where the slides matter and the part does not. + It is not a joint jog: the machine stays in world mode and the position on + the screen stays true through the move. ++ +An axis letter carries a unit class and a joint does not, so 'G53.5' is +refused on a machine whose letters name joints of the other kind. A serial +robot answers X with its first rotary joint, which turns in degrees, and +there the whole code is refused rather than that one letter. Which joints +turn is what `[JOINT_n] TYPE` declares. + +* 'G53.7' takes joint values, one word per joint, and works on every machine: + 'J2=-5' sends joint 2 to -5. The number after 'J' is the joint number, the + `[JOINT_n]` section and the `joint.n` HAL pins, and the value after '=' is + the joint's own position, what `joint.n.pos-cmd` shows, in the joint's own + units. Nothing is converted, not even 'G20' and 'G21'. A joint left out + keeps its position, and the two joints of a gantry pair must both be given, + with one value. This is the form for a robot, and for any machine where the + letters do not name the joints they look like. + +With 'G0' the speed comes from the joint limits in the INI file, +`[JOINT_n] MAX_VELOCITY` and `MAX_ACCELERATION`, scaled so that no joint +exceeds its own; the rapid override applies. With 'G1' the move takes the time +the straight move to the same destination would take at the programmed feed: +under G94 the F word applies to the distance between the two ends by the same +rule as a straight move, XYZ if any of them move, else UVW, else the rotary +words; under G93 the move takes 1/F minutes exactly, which is the form to use +when the ends coincide or only the joints move. The feed override applies, and +the joint limits still cap it. Nothing blends into or out of a point-to-point +move: the move before it comes to a stop and so does the move itself. + +'G53.4' is also the move <> and <> make. + +.G53.4 Example +[source,ngc] +---- +G0 X0 Y0 Z100 A0 C0 +G53.4 G0 A90 C180 (swing the rotaries round, the tip goes where the joints take it) +G0 X0 Y0 Z100 (and back on a straight line) +---- + +.G53.5 Example +[source,ngc] +---- +G53.5 G0 Z0 (the Z slide to its zero, whatever the head's tilt) +G53.5 G0 X0 Y0 B0 C0 (park X, Y and both head joints; the Z slide stays) +G93 G53.5 G1 F0.5 C180 (turn the C joint to 180 in two minutes) +G94 +---- + +.G53.7 Example +[source,ngc] +---- +G53.7 G0 J2=0 (the Z slide, joint 2, to its zero) +G53.7 G0 J0=0 J1=0 J4=0 J5=0 (park those four joints; joint 2 stays) +G93 G53.7 G1 F0.5 J5=180 (turn joint 5 to 180 in two minutes) +G94 +---- + +It is an error if: + +* Neither 'G0' nor 'G1' is in force. +* An axis letter is without a real value, or one is used that is not + configured. +* With 'G53.5', no axis word is given, a letter is used that is not a joint of + the kinematics, or the machine's letters name joints of the other kind. +* With 'G53.7', an axis word or a plain 'J' word is used, no joint word is + given, a joint number is not a whole number from 0 to 15 or is beyond the + joints of the kinematics, or one joint of a gantry pair is given without the + other or with a different value. +* With 'G53.5' or 'G53.7', polar coordinates are used, incremental distance + mode is in force, or the kinematics module cannot be evaluated by the + interpreter. +* A joint word, 'J=', is used without 'G53.7'. +* With 'G1', the feed is zero, or under G94 the two ends coincide, or feed per + revolution (G95) is in force. +* Cutter compensation is on. +* An external offset is applied when the move reaches motion. + [[gcode:g54-g59.3]] == G54-G59.3 Select Coordinate System(((G54-G59.3 Select Coordinate System))) @@ -1865,6 +2220,147 @@ G64 P0.015 Q2 .G64 Heart image::images/G64_Heart_Q2.png["G64 Heart",align="center"] +[[gcode:g68.2]] +== G68.2, G68.3, G68.4, G69 Tilted Work Plane(((G68.2 Tilted Work Plane))) + +[source,ngc] +---- +G68.2 X- Y- Z- I- J- K- (three angles, Euler) +G68.2 P1 X- Y- Z- I- J- K- (three angles about fixed axes) +G68.2 P2 Q0 X- Y- Z- (three points: the origin, then) +G68.2 P2 Q1 X- Y- Z- (a first point,) +G68.2 P2 Q2 X- Y- Z- (a second point on the plane's +X,) +G68.2 P2 Q3 X- Y- Z- (a third point on its +Y side) +G68.2 P3 Q1 X- Y- Z- I- J- K- (two vectors: the origin and +X, then) +G68.2 P3 Q2 I- J- K- (+Z, the normal) +G68.3 X- Y- Z- (the plane from the tool direction) +G68.4 ... (any G68.2 form, on the active plane) +G69 (cancel) +---- + +A tilted work plane is a coordinate system composed on top of the active one: +the blocks between the definition and 'G69' are programmed in the plane, with +X and Y in it and Z along its normal, while the work offset underneath, 'G54' +say, is untouched. Positions on the display, probe results and +<> all take the plane into account. It is the same offset +chain as always with one more stage, applied first: + +---- +absolute = tool offset + G5x + XY rotation applied to (G92 + origin + rotation applied to program) +---- + +'X', 'Y' and 'Z' are the plane's origin and the plane's rotation is built +from the rest of the words, both in the coordinate system that is active when +the plane is defined: the work offset with 'G92' and the XY rotation in +place, which is what the position display shows at that moment. A rotary +word does not pass through the plane, since on a TCP kinematics the rotary +coordinates are the rotary joints and a plane does not change what a joint +is. Words left out are zero. + +'P' selects how the rotation is given: + +* 'P0', or no 'P': three angles 'I', 'J', 'K' applied one after another, + each about an axis of the plane as rotated so far (Euler angles). 'Q' + names the axes with three digits, 1 for X, 2 for Y and 3 for Z, no two + adjacent alike; the default is 'Q313', Z then X then Z. +* 'P1': three angles 'I', 'J', 'K', each about an axis of the coordinate + system the plane is defined in, in the order 'Q' gives; the default is + 'Q123', X then Y then Z. +* 'P2': three points, over up to four blocks with 'Q0' to 'Q3'. The + direction from the first point to the second is the plane's +X, the third + point lies on the +Y side. The 'Q0' block gives the origin and 'R'; without + it the origin is the first point. +* 'P3': two vectors, over two blocks. 'Q1' gives the origin and the +X + direction in 'I', 'J', 'K'; 'Q2' gives +Z, the normal, in 'I', 'J', 'K'. + The X direction need not be exactly at right angles to the normal; the + part of it along the normal is dropped. + +'R' turns the plane about its own Z after everything else, in degrees. + +The forms are numbered as Fanuc numbers them, and the two angle forms are the +conventions Heidenhain writes as `PLANE EULER` and `PLANE SPATIAL`. Fanuc has +a fifth form, projection angles, that is not implemented. See the +<> chapter. + +The blocks of a 'P2' or 'P3' definition have to follow one another; any +other block in between is an error. A definition with a plane already active +replaces it, with the words in the coordinate system underneath, not in the +old plane. + +'G68.3' takes the plane from the tool: Z is the tool axis as the rotary +joints have it at that moment, X is the default tool X of the kinematics +conventions, tool X turned about the tool axis by the smaller angle that +makes it parallel to the machine XY plane, and machine X when the tool is +vertical; 'R' turns the plane from there. It asks the kinematics module for +the tool direction, so it needs a kinematics type whose frames describe the +machine, as <> does. + +'G68.4' takes any 'G68.2' form and composes it onto the active plane: the +words are in the plane, and the result is a new plane relative to the old +one. It needs a plane to build on. + +The G-code preview draws every plane a program defines, so a program can +be checked for where its planes sit before it runs: a rectangle lying in +the plane, over the moves made under it with a margin around them, the +plane's own X, Y and Z at the centre of the rectangle in the colours of +the machine axes, and a cross at the plane's origin, which need not lie +in the rectangle: a program drilling a sphere puts every plane's origin +at the centre and works out on the surface. The rectangle is drawn at the plane's Z where the program comes +down to it, and otherwise at the end of the Z range the program worked +in that is nearest to it, so a drilling cycle that stays above the +origin shows its holes where they are cut; a plane nothing moved under +is drawn as a square sized from the program. A plane restated with the +same words is one plane, not one per restatement. The plane in effect +on the machine, the one the last executed 'G68.2' set, is drawn over +the others in its own colour, so as the program runs the plane it is +in stands out from the ones it has been in and will be in; a plane set +from MDI shows the same way, as a square with nothing under it. + +'G69' cancels the plane. So does the end of the program, 'M2' or 'M30', and +an abort: the plane is not persistent and nothing about it is written to the +parameter file. + +Defining the plane does not move anything. To bring the tool normal to it +use <>. + +While a plane is active the codes that define the coordinate system the +plane sits on are refused: 'G92', 'G92.1', 'G92.2', 'G92.3', 'G52', 'G10 L2', +'G10 L20', 'G10 L10', 'G10 L11', and a change of coordinate system +('G54' to 'G59.3'). Cancel the plane first. + +The active plane is reported in the modal G-code display as the code that +defined it. Status carries it as `g68_offset`, `g68_rotation` and +`g68_active`, next to the other offsets, for displays that want to show +plane coordinates or draw the plane. + +.G68.2 Example +[source,ngc] +---- +G54 +G68.2 X50 Y50 Z0 I30 J20 K0 (Euler: 30 about Z, 20 about the new X) +G53.1 (tool normal to it) +G0 X0 Y0 Z5 (5 above the plane's origin, along its normal) +G1 Z-3 F150 (a hole 3 deep, straight into the plane) +G0 Z5 +G68.4 X20 P1 I0 J0 K90 (a plane 20 along X in the old one, turned 90 about Z) +G0 X0 Y0 Z5 +G69 (back to G54 as it was) +---- + +It is an error if: + +* 'P' is not 0, 1, 2 or 3, or 'Q' with 'P0' or 'P1' is not three axis digits + with no two adjacent alike. +* A 'P2' or 'P3' definition is interrupted, or its 'Q' words come out of + order. +* The points of a 'P2' definition coincide or lie on one line, or a vector of + a 'P3' definition is zero or the X direction lies along the normal. +* 'G68.4' is used with no plane active. +* 'G68.3' is used where the kinematics cannot be evaluated, is the identity + kinematics, or reports no frames. +* Cutter compensation is on. +* Polar coordinates or a motion code are used on the same line. + [[gcode:g70]] == G70 Lathe finishing cycle(((G70 Lathe finishing cycle))) diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index 62c6fd87198..75988002d5d 100644 --- a/docs/src/gcode/overview.adoc +++ b/docs/src/gcode/overview.adoc @@ -121,7 +121,7 @@ The table includes N and O for completeness, even though, as defined above, line |G | General function (See table <>) |H | Tool length offset index |I | X offset for arcs and G87 canned cycles -|J | Y offset for arcs and G87 canned cycles +|J | Y offset for arcs and G87 canned cycles; as J=, the position of joint n for <> .2+|K | Z offset for arcs and G87 canned cycles. <| Spindle-Motion Ratio for G33 synchronized movements. |L | generic parameter word for G10, M66 and others @@ -304,6 +304,11 @@ example '##2' means the value of the parameter whose index is the which the G38 took place. Volatile. * '5070' - <> probe result: 1 if success, 0 if probe failed to close. Used with G38.3 and G38.5. Volatile. +* '5071-5079' - Pose last solved by <> for X, Y, Z, A, + B, C, U, V & W, in program units in the tilted work plane. Same values + as `#<_orient_x>` and kin. Read-only, volatile. +* '5080' - 'G53.2' result: 1 once a 'G53.2' has solved a pose, 0 before. + Same as `#<_orient_valid>`. Read-only, volatile. * '5081-5089' - Tool length offset currently applied to motion for X, Y, Z, A, B, C, U, V & W, in the current program units. Set by `G43`/`G43.1`/`G43.2`, and 0 when `G49` is in effect. These report the @@ -507,6 +512,13 @@ can be added easily without changes to the source code. 'P' number of the last 'G12.1', or 0 after 'G13.1' or when no kinematics has been selected. See <>. +* '#<_orient_valid>', '#<_orient_x>', '#<_orient_y>', '#<_orient_z>', + '#<_orient_a>', '#<_orient_b>', '#<_orient_c>' - The pose 'G53.2' last + solved, in program units in the tilted work plane. '#<_orient_valid>' is + 1 once a 'G53.2' has run; reading the others before that is an error. The + same values are on the numbered parameters '#5071' to '#5080'. See + <>. + * '#<_plane>' - returns the value designating the current plane: [width="20%",options="header"] @@ -964,7 +976,7 @@ The modal groups are shown in the following Table. [width="80%",cols="4,6",options="header"] |=== |Modal Group Meaning | Member Words -|Non-modal codes (Group 0) | G4, G10 G28, G30, G52, G53, G92, G92.1, G92.2, G92.3, +|Non-modal codes (Group 0) | G4, G10 G28, G30, G52, G53, G53.1, G53.2, G53.3, G53.4, G53.5, G53.6, G53.7, G92, G92.1, G92.2, G92.3, |Motion (Group 1) | G0, G1, G2, G3, G33, G38.n, G73, G76, G80, G81 G82, G83, G84, G85, G86, G87, G88, G89 |Plane selection (Group 2) | G17, G18, G19, G17.1, G18.1, G19.1 @@ -973,7 +985,8 @@ The modal groups are shown in the following Table. |Feed Rate Mode (Group 5) | G93, G94, G95 |Units (Group 6) | G20, G21 |Cutter Diameter Compensation (Group 7) | G40, G41, G42, G41.1, G42.1 -|Tool Length Offset (Group 8) | G43, G43.1, G49 +|Tool Length Offset (Group 8) | G43, G43.1, G43.2, G43.4, G43.5, G49 +|Tilted Work Plane (Group 9) | G68.2, G68.3, G68.4, G69 |Canned Cycles Return Mode (Group 10) | G98, G99 |Coordinate System (Group 12) | G54, G55, G56, G57, G58, G59, G59.1, G59.2, G59.3 |Control Mode (Group 13) | G61, G61.1, G64 diff --git a/docs/src/hal/components.adoc b/docs/src/hal/components.adoc index d4987f82b26..6fab8fb2038 100644 --- a/docs/src/hal/components.adoc +++ b/docs/src/hal/components.adoc @@ -336,6 +336,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/kins.9.adoc b/docs/src/man/man9/kins.9.adoc index f2bd1051d4c..01cef58cc73 100644 --- a/docs/src/man/man9/kins.9.adoc +++ b/docs/src/man/man9/kins.9.adoc @@ -207,7 +207,9 @@ documentation for more info) *genhexkins.tool-offset*:: TCP offset from platform origin along Z to implement RTCP function. - To avoid joints jump change tool offset only when the platform is not tilted. + Motion hands the module the offset in effect (G43, G49), so the pin needs + no connection. A tool offset change with the platform tilted moves the + programmed point, not the joints. === genserkins - generalized serial kinematics @@ -252,9 +254,10 @@ replacing it. Put a given length in one column or the other, not both. *maxkins.tool-length*:: Tool length, applied along the tool rather than along Z, so that the tip - stays on the programmed point as B tilts. Net it from *motion.tooloffset.z*. - Left unconnected, the tool length stays where canon put it, along machine - Z, which is only correct at B0. + stays on the programmed point as B tilts. Motion hands the module the + offset in effect (G43, G49), so the pin needs no connection; it is read + only until motion has sent anything. A tool offset change with B tilted + moves the programmed point, not the joints. === pentakins - Pentapod Kinematics @@ -290,7 +293,9 @@ The forward kinematics iteration is controlled by HAL pins. *pentakins.tool-offset*:: TCP offset from effector origin along Z to implement RTCP function. - To avoid joints jump change tool offset only when the platform is not tilted. + Motion hands the module the offset in effect (G43, G49), so the pin needs + no connection. A tool offset change with the platform tilted moves the + programmed point, not the joints. === pumakins - kinematics for puma typed robots @@ -413,12 +418,13 @@ expected by it (XYZBCW `->` joints 0..5) *5axiskins.tool-length*:: Tool length, applied along the tool rather than along Z, so that the tip - stays on the programmed point as B and C move. Net it from - *motion.tooloffset.z*. Left unconnected, the tool length stays where canon - put it, along machine Z, which is only correct at B0. A tool length in the - W column of the tool table reaches the same place, once a block commands W, - and adds to this pin rather than replacing it. Put a given length in one - column or the other, not both. + stays on the programmed point as B and C move. Motion hands the module the + offset in effect (G43, G49), so the pin needs no connection; it is read + only until motion has sent anything. A tool offset change with B tilted + moves the programmed point, not the joints. A tool length in the W column of the tool + table reaches the same place, once a block commands W, and adds to this + one rather than replacing it. Put a given length in one column or the + other, not both. == SEE ALSO diff --git a/docs/src/motion/5-axis-kinematics.adoc b/docs/src/motion/5-axis-kinematics.adoc index 9391f8611a9..1c6e0fe406c 100644 --- a/docs/src/motion/5-axis-kinematics.adoc +++ b/docs/src/motion/5-axis-kinematics.adoc @@ -317,23 +317,17 @@ See the simulation INI files for details of the HAL connections used for the vis === Tool-Length Compensation -In order to use tools from a tool table sequentially with tool-length compensation applied automatically, a further Z-offset is required. For a tool that is longer than the "master" tool, which typically has a tool length of zero, LinuxCNC has a variable called "motion.tooloffset.z". If this variable is passed on to the kinematic component (and vismach python script), then the necessary additional Z-offset for a new tool can be accounted for by adding the component statement, for example: +In order to use tools from a tool table sequentially with tool-length compensation applied automatically, a further Z-offset is required. For a tool that is longer than the "master" tool, which typically has a tool length of zero, the kinematics accounts for the tool length in effect, for example: image::5-axis-figures/equation__38.png[align="center"] -The required HAL connection (for xyzac-trt) is: +Motion hands the tool offset in effect (G43, G49) to the kinematics module directly, so the module sees the tool from the tool table with no HAL connection. The module's tool-offset pin (xyzac-trt-kins.tool-offset) remains for a configuration that connects it, and is read only until motion has sent an offset; a value set on it that disagrees with the tool table is reported once and not used. -[source,hal] ----- -net :tool-offset motion.tooloffset.z xyzac-trt-kins.tool-offset ----- - -where: +Motion also publishes the offset on the HAL pin "motion.tooloffset.z", which is what a vismach python script reads to draw the tool: +[source,hal] ---- -:tool-offset ---------------- signal name -motion.tooloffset.z --------- output HAL pin from LinuxCNC motion module -xyzac-trt-kins.tool-offset -- input HAL pin to xyzac-trt-kins +net :tool-offset motion.tooloffset.z xyzac-trt-gui.tool-offset ---- == Custom Kinematics Components @@ -383,17 +377,18 @@ KINEMATICS = kinsname where "kinsname" is the name of your kins program. Additional HAL pins may be created by the module for variable configuration items -such as the D~x~, D~y~, D~z~, tool-offset used in the xyzac-trt kinematics module. +such as the D~x~, D~y~, D~z~ used in the xyzac-trt kinematics module. These pins can be connected to a signal for dynamic control or set once with HAL connections like: [source,hal] ---- # set offset parameters -net :tool-offset motion.tooloffset.z xyzac-trt-kins.tool-offset setp xyzac-trt-kins.y-offset 0 setp xyzac-trt-kins.z-offset 20 ---- +The tool length is not among them: motion hands it to the module from the tool table. + == Figures .Table tilting/rotating configuration diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 063ee63e794..6a4cde19d67 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -63,6 +63,16 @@ against, and the two halves of this chapter use different pairs deliberately. Positions are measured in the work frame, which is what makes a program independent of how the table is set. +With every rotary joint at zero, the work frame position is what the linear +joints read. The shipped head and table modules fold the pivot lengths and +the offsets in, so that the tool tip and the slides agree at the zero pose +and the pivot shows only as the compensation, `L(1 - cos B)` on a head, +once a rotary leaves zero. A module with a switchable identity type keeps +this of necessity, since the identity and the geometric type must agree at +the zero pose or a switch would jump the reported position; a module without +one keeps it too, so that `G53.5` slide positions and `G53` positions coincide +with the rotaries at zero. + Orientations are measured against the machine frame, and there are two of them. A module reports the tool frame and the work frame separately, each in machine coordinates. A consumer that wants the tool in workpiece coordinates composes @@ -178,7 +188,7 @@ half turn about one of the two transverse axes, and which one is chosen decides where tool X lands. Because it is a rotation in its own right, a module declares it rather than -applying it by hand, as the last argument of `switchkinsRegisterFrames()`. +applying it by hand, in the `native` field of its ops table. Shared code applies it and checks once, at load, that it is orthonormal with determinant +1. `TOOL_FRAME_SPINDLE` is the identity, for a module whose maths is already in the convention; `TOOL_FRAME_FLANGE` is the half turn a @@ -279,17 +289,18 @@ The joint values that reach a requested orientation, through question a tilted work plane asks when it has to orient the machine. <> says what it answers. -The Jacobian, relating commanded velocity to joint velocity at a given pose, so -that a feed can be checked against the joint velocity, acceleration and limit -values it will actually demand, and so that proximity to a singularity is a -number rather than a surprise. A module with a closed form can supply it -directly. Otherwise it can be obtained by differencing `kinematicsInverse()` -about the pose, which needs no change to the module at all. +How joint motion follows world motion, through `kinematicsJacobian()`, so +that a feed can be checked against the joint velocity and acceleration it will +actually demand, and so that proximity to a singularity is a number rather +than a surprise. <> says what it answers and in +which units. All of these are functions of the joint values and the module's own geometry. None needs state carried between calls, and none needs the module to be running in a realtime thread to be useful: the interesting callers, a limit check before a move and a preview before a program runs, are not in the servo loop. +<> is how a module is written so that they +can call it. [[sec:orientation-inverse]] == The Orientation Inverse @@ -328,6 +339,13 @@ its X points, and `G53.1` moves the rotaries to align the tool axis. Neither refuses a program for naming both directions on a five-axis machine, and neither should this. +In tree the G-code side of that is `G68.2`, which defines the plane, and +`G53.1`, `G53.2`, `G53.3` and `G53.6`, which ask this inverse where the +rotaries go, with the plane's normal and its X as the request. Their `Q` word is the +`held` mask: `Q0` holds the joints the work frame survey finds and takes the +reported turn as the coordinate rotation it is, `Q1` holds nothing. See the +G-code chapter. + Some requests still do not pin the machine down. A five-axis machine asked to point the tool along the axis its primary rotary turns about can hold any primary angle; a machine with three orientation joints asked only for a tool @@ -377,6 +395,182 @@ The search is not a realtime routine. How long it takes depends on the machine and on the request, and the callers that want it, orienting a tilted work plane and previewing a program, are not in the servo loop. +[[sec:jacobian]] +== The Jacobian + +A feed is a speed in the work frame. What the machine has to deliver is a +speed at each joint, and on any kinematics that is not the identity the two +are related by where the machine is. The Jacobian is that relation at one +pose: how each joint responds to a unit rate of each pose coordinate. + + jac[j][a] = d joint[j] / d pose[a] + +Rows are joints. Columns are the pose coordinates in `EmcPose` order, X Y Z A +B C U V W. It is the derivative of `kinematicsInverse()`: multiplied by a pose +velocity it gives the joint velocity motion will command, which is what a feed +limit compares with the joint limits. Joint `j` binds when + + |jac[j] . tangent| * F + +exceeds that joint's velocity limit, `tangent` being the direction of the move +in pose coordinates and `F` the feed along it. The acceleration limit follows +from a second Jacobian taken further along the path, with no more from the +module. A row that grows without bound is a pose approaching a singularity, +where no world speed is slow enough for the joints to follow. + +=== Units + +Each entry is in joint units per pose unit, whatever units the module's own +forward and inverse already use. Nothing is converted: a caller that feeds +pose rates in `EmcPose` units gets joint rates in the units motion already +commands, and never has to know which unit a rotary joint is in. On every +module in the tree both are degrees, so a table rotary's own row is a 1 in its +own column, and a robot's rotary rows carry degrees per millimetre against the +linear columns. + +This is why the Jacobian, unlike the orientation inverse, does not need the +interface to name the rotary joint unit. Every number in it is a ratio of +quantities that already pass through `kinematicsForward()` and +`kinematicsInverse()`, and the caller never combines it with anything measured +in another unit. + +=== Frame + +The columns are pose coordinates, so the answer lives in the work frame, where +`kinematicsForward()` reports positions. The A, B and C columns are rates of +the pose words, the wrapped linear axes the planner already treats as +coordinates, and not an angular velocity vector: on a machine that carries the +work the forward writes the rotary joint into the pose word, and that column +says exactly that, a 1 for its own joint. + +That makes this a different object from the frames of +<>, and the two rules are kept apart deliberately. A frame +is an orientation, and a renderer placing two bodies needs each against +something fixed, so frames are reported against the machine. A Jacobian is a +derivative of the pose, and everything that uses it multiplies it by a pose +rate, so it is reported where the pose is. A module whose maths produces a +twist in the machine frame, which is what the Denavit-Hartenberg modules +produce, turns it into pose word rates through the matrix of the axes each +pose word turns about, once, inside the module. `genserkins` does this, and +having it written once there is worth more than the closed form itself, since +every consumer would otherwise guess it. + +=== What a module has to supply + +Nothing. The shared code takes central differences of the module's inverse +about the pose, eighteen inverse calls on the solution branch the inverse +flags select. That costs a few microseconds on a closed form inverse and +milliseconds on one that iterates, and it answers to the inverse's own +precision, which for an iterating inverse is its convergence tolerance divided +by the step. Modules built on `switchkins.c` answer this way for every type +whose ops table has no Jacobian; an identity type answers exactly. + +A module with a closed form puts it in the `jacobian` field of its ops table. +It is exact, it costs what the inverse costs, and it knows its own singular +poses rather than discovering them as an inverse that fails a step away from +the pose. Every module in the tree whose inverse is written out supplies one. +The two arms whose inverse is a chain of arc tangents, `pumakins` and +`three21kins`, answer through the differences. + +A module reading its rotary angles from the joint argument of the inverse +rather than from the pose, which the nutating heads do, has an inverse whose +derivative about the pose is not the coupling the machine has. Such a module +supplies the closed form, taken against the pose. + +[[sec:parameters]] +== The Parameter Block + +Everything above is a function of the joint values, the tool and the machine's +geometry. A module written the old way reads its geometry from HAL pins it +created, keeps its kinematics type and its iteration scratch in statics, and so +can only answer for the machine as it is now, from inside the realtime thread. +Anything else that needs the same maths, a planner evaluating poses the machine +has not reached, task checking a program at load, a tool asking what if, had to +carry a second copy of it, and the two copies drift. + +A module is written instead as functions of two blocks the caller supplies. +`kins_params` describes the machine: the kinematics type, the joint map from +`coordinates=`, the tool offset, and the geometry as an array of doubles. One +copy may be shared by any number of callers, since nothing writes it during a +call. `kins_scratch` is what one caller carries between its own calls: the pose +an iterating forward last found, which seeds the next, and what the module +reports about the call it just made. It is never shared between callers, so +motion and a planner evaluating the same module cannot disturb each other. + +=== The table + +A module declares its geometry as a table of named entries, one per value it +reads. The name is the pin name the config already uses, less the module +prefix, so nothing in a config changes. + +[source,c] +---- +static const kins_param_desc fiveaxis_params[] = { + { "pivot-length", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PIVOT_LENGTH }, +}; +enum { P_PIVOT_LENGTH }; +---- + +An entry is an input, an output, or an input that can be poked (`KINS_IO`, a +`HAL_IO` pin). The maths reads `p->geometry[P_PIVOT_LENGTH]` where it read a +pin, and writes an output into `s->out[]` at the same index. An entry flagged +as the tool is the tool length along the tool axis; the shared code puts its +value in `p->tool.tran.z` as well, which is what the maths reads, so that a +caller outside realtime can supply the tool from the tool table without there +being a pin. + +=== The ops table + +The maths of one kinematics type is a `kins_ops` table: the forward and inverse, +the optional work and tool frames with the native rotation that relates the +tool frame to the convention, and the optional Jacobian. A type whose forward +iterates from the pose it is handed says so, and the shared code seeds it with +the last answer after a switch. A type also says what it IS: `identity` marks +the no-transform type `G13.1` cancels to, `primary` the working transform +`G43.4` and `G43.5` switch to (see the Switchable Kinematics chapter). A module with +several types has one geometry table and one ops table per type, registered +with `switchkinsRegisterOps()`; a module with one type describes itself in a +`kins_module` and links `kins_single.c`. + +=== What the shared code does + +In realtime it makes one HAL pin per table entry, copies the pins into the +block before every call and the outputs back after it, and supplies the classic +entry points, `kinematicsForward()` and the rest, so that motion sees no +difference. It also exports `kinematicsSetTool()`, through which motion hands +the module the tool offset in effect whenever that changes, so the tool comes +from the tool table and not from a net the config had to remember. A table +entry flagged as the tool is read from its pin only until motion has sent +anything; after that the pin is overwritten with motion's value, and the shared +code says once if the pin is left disagreeing with it, which is a config +setting a tool length where the tool table should. Motion references the call +weakly, so a module written before it still loads and keeps its pin. + +Outside realtime a module exports `kinsDescribe()`, which hands a caller its +table and the ops of each type; the caller fills a block from wherever it likes +and asks the same functions through `kinsOpsForward()`, `kinsOpsInverse()`, +`kinsOpsJacobian()` and the frame calls, with the same defaults applied, so +both sides get the same answers. The non-realtime loader in +`kinematics_userspace/` binds the pins of the running module by the table's +names. Its tool is the caller's where the caller gives one through +`kinematicsUserSetTool()`, since a planner knows what a segment runs under +better than the machine does; otherwise it is motion's, from motion's own +offset pins, and the loader says once when the module's tool pin disagrees with +them. `kinslimits` is built on it. + +A module that does not provide the form keeps working as it did. It just cannot +be evaluated outside realtime, which the loader reports. + +=== What stays outside the block + +The kinematics type is in the block, so a caller evaluating a program that +switches type puts the type each block will run under in its own block, and +nothing is switched globally. The tool is in the block, from motion in realtime +and from the caller outside it. The joint map is in the block, from +`coordinates=`. Nothing else the maths needs exists, +and a module that finds it needs something else has found a parameter it should +declare. + [[sec:writing-a-module]] == Writing a Module @@ -386,6 +580,11 @@ Frames:: separately, each against the machine frame, so that a consumer placing both bodies can. +Zero pose:: + With every rotary joint at zero the work frame position equals the linear + joints. Pivots and offsets live in the module and show only as compensation + once a rotary moves. + Signs:: Positive A, B and C are counterclockwise about work X, Y and Z viewed from the positive end, describing the motion of the tool relative to the @@ -406,11 +605,24 @@ Orientation inverse:: do nothing. Register a closed form only where one exists, and where it does, say which poses it treats as degenerate. +Jacobian:: + Rows are joints, columns are pose coordinates, entries in the units the + forward and inverse already use, reported where the pose is. A module with + a closed form inverse differentiates it and registers the result; one + without lets the shared code difference the inverse. + Geometry stays in the module:: Whatever a consumer needs to know about the machine's shape is answered by the module. A consumer that restates it has taken a copy that nothing keeps in step, which is the situation this chapter exists to end. +No state:: + Write the maths as functions of the parameter block and the scratch, as + <> describes: geometry in the table, + the kinematics type and the tool from the block, and anything carried + between calls in the scratch. A static in a module is a second machine + that only the realtime thread can see. + Mount orientation is not this:: A tool or holder mount orientation is a different quantity: a right-angle head, a tool held at an angle, an end effector clocked on its flange. Those @@ -425,6 +637,34 @@ Mount orientation is not this:: determinant, rather than as three angles whose ordering convention is written down nowhere. +== Relation to Other Controls + +The tilted work plane borrows its shape from the controls that had one first, +so a program or a post moving either way reads the same. + +The two angle forms are the two those controls define. `G68.2 P0`, three +angles each about an axis of the plane as rotated so far, is what Heidenhain +calls `PLANE EULER`: a precession about Z, a nutation about the X the +precession has already turned, and a rotation about the tilted Z. `G68.2 P1`, +three angles about the axes of the system underneath, is `PLANE SPATIAL`, +whose three angles are each about a non-tilted axis. The numbering of the +forms follows Fanuc's `G68.2`, where P0 is Euler angles, P1 roll, pitch and +yaw, P2 three points and P3 two vectors. Fanuc has a fifth form, projection +angles, that is not implemented. + +Selecting one of the two poses that reach a plane, `P1` and `P2` on +<>, is Heidenhain's `SEQ+` and `SEQ-`, and takes the same +reference: the sign of the rotary measured from its home position. Two +differences are worth knowing. A control offering `SEQ` falls back to the +nearer pose when both lie the same side of home, where these refuse and say +so. And `SEQ` is offered on a machine whose rotaries all carry the work, +keyed to the tilting one, where `P` here names a pose only when a rotary +turns the tool, so on a table-table machine only the nearest form is +available. + +Whether the joints carrying the work take part, `Q0` and `Q1`, is +Heidenhain's `COORD ROT` and `TABLE ROT`. + == References * <>, for the axis nomenclature diff --git a/docs/src/motion/kinematics.adoc b/docs/src/motion/kinematics.adoc index 26f3e8a03d0..7fd97dd8eaf 100644 --- a/docs/src/motion/kinematics.adoc +++ b/docs/src/motion/kinematics.adoc @@ -48,6 +48,28 @@ typically refer to the usual Cartesian coordinates. The A B C axes refer to rotational coordinates about the X Y Z axes respectively. The U V W axes refer to additional coordinates that are commonly made colinear to the X Y Z axes respectively. +[[sec:joint-space]] +=== Joint Space + +The joints the motion controller works with are the ones the kinematics +module defines: the numbers `kinematicsInverse()` produces and +`kinematicsForward()` reads, numbered as the `[JOINT_n]` sections of the INI +file and the `joint.N` HAL pins. Their zero is where homing put it and their +limits are the `[JOINT_n]` limits. _Joint space_ is the set of these +positions taken together, one coordinate per joint, as opposed to the +Cartesian coordinates of the axes. + +What sits behind a joint is a HAL matter. On most machines +`joint.N.motor-pos-cmd` drives one motor, so a joint and an actuator are the +same thing, but a joint can also drive two motors, or one motor through a +ratio, or feed a linkage computed in HAL, and the motion controller does not +know the difference. A point-to-point move, <>, runs each +of the controller's joints on a straight line from its start to its end +position at a rate within that joint's limits; the actuators behind them +follow through whatever HAL puts in between. With switchable kinematics the +joints do not change when the kinematics type does, only the mapping between +them and the axes. + == Trivial Kinematics The simplest machines are those in which which each joint is placed @@ -310,7 +332,6 @@ between joint numbers and axis letters when in joint mode ---- int kinematicsSwitchable(void) int kinematicsSwitch(int switchkins_type) -KINS_NOT_SWITCHABLE ---- The function kinematicsSwitchable() returns 1 if multiple @@ -320,9 +341,9 @@ See <>. [NOTE] The majority of provided kinematics modules support a single -kinematics type and use the directive "*KINS_NOT_SWITCHABLE*" to -supply defaults for the required kinematicsSwitchable() and -kinematicsSwitch() functions. +kinematics type; they are written on the parameter block and link +'kins_single.c', which supplies these entry points and answers "not +switchable" for them. ---- int kinematicsHome(EmcPose *world, double *joint, diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 8c1fea13e24..6a7c7b09823 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -46,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) Every module listed above uses its own kinematics for type0 and identity kinematics for type1. Each accepts the module string @@ -218,8 +222,13 @@ A module that declares no identity kinstype refuses 'G13.1' with an error and can still be driven by number with 'G12.1'; see Code Notes for how a module declares its types. -See the G-code documentation for 'G12.1' and 'G13.1' for the full -description. +For tool length work there are spellings that name the kinematics by +what it is rather than by number: 'G43.4' applies the tool length +offset and switches to the kinstype the module declares its working +transform, 'G43.5' does the same and lets the lines after it give the +tool axis as a vector, and the 'G49' that cancels either switches back +to identity. See the G-code documentation for 'G43.4', 'G43.5' and +'G49', and for 'G12.1' and 'G13.1', for the full description. === M-code commands @@ -415,14 +424,25 @@ 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 directory and edited to supply custom kinematics with -kinstype==2. +kinstype==2: the in-tree modules register its USERK_OPS as that +kinstype, so the forward and inverse in the copy are what runs. The user custom kinematics file can be compiled from out-of-tree source locations for rt-preempt implementations or by replacing @@ -435,6 +455,52 @@ 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 +#include +---- + +A realtime module cannot link a library, so the implementation arrives +as source: switchkins.c, switchkins_setup.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 supplies switchkinsSetup() and calls switchkinsRunSetup() +and switchkinsInit() from EXTRA_SETUP(), which halcompile runs after +hal_init() and before hal_ready(). See <> for the 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 @@ -459,78 +525,164 @@ 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 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 switchkinsRegisterOps(), once per +kinstype, with the maths of that type written as functions of the +parameter block (see the Kinematics Conventions chapter): -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. +---- +int switchkinsRegisterOps(int ktype, const kins_ops *ops); +---- -A module can provide further kinstypes by calling -switchkinsRegister() from within switchkinsSetup(), once per -kinstype: +The geometry of the whole module is one table, named in the kparms +fields 'params' and 'nparams'; every kinstype reads it from the block. +The older form, switchkinsRegister() with a setup, forward and inverse +routine per kinstype that read pins of their own, is still accepted: ---- int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv); ---- 'ktype' runs from 0 to SWITCHKINS_MAX_TYPES-1 (defined in -kinematics.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 +kinematics.h as KINS_MAX_TYPES). Registering a kinstype twice, by +either route, 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. -A module should also declare what each kinstype IS, again from within +A module also declares what each kinstype IS, with flags from +kinematics.h: + +. *KINSTYPE_IDENTITY* no transform: the joints are the world +. *KINSTYPE_PRIMARY* the module's working transform + +A kinstype registered with switchkinsRegisterOps() carries its flag in +the ops table itself, as the 'identity' or 'primary' field; a kinstype +registered the older way gets it from a call, again from within switchkinsSetup(): ---- int switchkinsDeclare(int ktype, int flags); ---- -with flags from kinematics.h: +G-code reads these declarations: 'G13.1' cancels to the kinstype +declared KINSTYPE_IDENTITY, and 'G43.4' switches to the kinstype +declared KINSTYPE_PRIMARY, whatever their numbers, so a module whose +kinematics are not in the conventional order still gets working +spellings. At most one kinstype may be declared identity and at most +one primary, and declaring a kinstype the module does not provide +fails the module load. A module that declares nothing keeps working +exactly as before for 'G12.1 P-' and 'G49', but 'G13.1' and 'G43.4' +are an error, since the numbers of the identity and primary kinematics +are then a guess. + +When every kinstype is registered, the module calls: -. *KINSTYPE_IDENTITY* no transform: the joints are the world -. *KINSTYPE_PRIMARY* the module's working transform +---- +int switchkinsInit(const int comp_id, kparms* kp, const char* coordinates); +---- -G-code reads these declarations: 'G13.1' cancels to the kinstype -declared KINSTYPE_IDENTITY, whatever its number, so a module whose -identity kinematics is not kinstype 0 still gets a working 'G13.1'. -At most one kinstype may be declared identity, and declaring a -kinstype the module does not provide fails the module load. A module -that declares nothing keeps working exactly as before for 'G12.1 P-', -but 'G13.1' is an error, since the number of the identity kinematics -is then a guess. - -After calling switchkinsSetup(), rtapi_app_main() checks the -supplied parameters, creates a HAL component, and then invokes -the setup routine identified for each kinstype. - -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. +which checks the supplied parameters, creates the HAL pins, the +table's among them, selects kinstype 0, and then invokes the setup +routine of each kinstype registered the older way. The caller owns +the HAL component: it does hal_init() before switchkinsInit() and +hal_ready() after it. + +A module built this way also exports kinsDescribe(), through which a +copy of it loaded outside realtime learns its table and the maths of +each kinstype; the non-realtime loader and kinslimits use it. It +exports kinematicsSetTool() as well, through which motion hands it +the tool offset in effect whenever that changes. A table entry +flagged as the tool is overwritten with it, and the entry's pin only +matters until motion has sent anything, so a config need not net +motion.tooloffset.z to the module. When the offset changes, motion +keeps the joints where they are and reads the point back from them +under the new offset, so a tool length change with the tool axis +tilted moves the programmed point rather than the machine; the +interpreter reads the point the same way through the non-realtime +loader, and motion stops the program if the two disagree. A kinstype +registered the older way reads its own pins and is not affected. + +=== Module main program + +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 sets the configuration settings, names the geometry table and +registers the kinstypes with switchkinsRegisterOps(). The pointer +arguments are the older route for kinstypes 0,1,2; a module using +them leaves the rest alone. switchkinsRunSetup() in +switchkins_setup.c is what runs switchkinsSetup() and registers what +it returned, for the 'main' program and for kinsDescribe() alike. + +A module written as a halcompile component gets rtapi_app_main() +from halcompile instead. It supplies the same switchkinsSetup(), and +from its EXTRA_SETUP() routine, which halcompile runs after +hal_init() and before hal_ready(), calls switchkinsRunSetup() and +then switchkinsInit(). The component names the objects it needs in +hal/components/Submakefile: + +---- +millturn-extra-objs := emc/kinematics/switchkins.o emc/kinematics/switchkins_setup.o emc/kinematics/kins_util.o +---- === Outline -The two routes in one switchkinsSetup(), with the kinematics itself -left out. Types 0 to 2 are filled in through the pointer arguments as -they always were, and a fourth is registered: +A switchkinsSetup() with the kinematics itself left out: the table, +the ops table of the machine's own kinstype, and the shared identity +and userk ops for the other two: [source,c] ---- +static const kins_param_desc my_params[] = { + { "pivot-length", KINS_PARAM_FLOAT, KINS_IN, 0, 100.0 }, + { "tool-offset", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, // the tool length +}; +enum { P_PIVOT_LENGTH, P_TOOL_OFFSET }; + +static int my_forward(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + double pivot = p->geometry[P_PIVOT_LENGTH]; // where a pin was read + double tool = p->tool.tran.z; // the tool, from wherever the caller has it + // ... +} + +static int my_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); + +static const kins_ops my_ops = { + .forward = my_forward, + .inverse = my_inverse, + // .work, .tool, .native and .jacobian are optional, see kinematics.h +}; + int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, KF* kfwd0, KF* kfwd1, KF* kfwd2, @@ -541,37 +693,24 @@ int switchkinsSetup(kparms* kp, kp->halprefix = "mykins"; // hal pin names kp->required_coordinates = "xyzab"; kp->max_joints = strlen(kp->required_coordinates); + kp->params = my_params; + kp->nparams = sizeof(my_params)/sizeof(my_params[0]); // remaining kparms fields - *kset0 = identityKinematicsSetup; // kinstype 0 is the startup default - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = myKinematicsSetup; - *kfwd1 = myKinematicsForward; - *kinv1 = myKinematicsInverse; - - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; - - // any further kinstype comes from switchkinsRegister(), and the - // numbering carries on from the three above with no gaps - if (switchkinsRegister(3, myOtherKinematicsSetup, - myOtherKinematicsForward, - myOtherKinematicsInverse)) { return -1; } + switchkinsRegisterOps(0, &my_ops); // kinstype 0 is the startup default + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(2, &USERK_OPS); + // any further kinstype is registered the same way, and the + // numbering carries on with no gaps return 0; } // switchkinsSetup() ---- -A module wanting fewer than three kinstypes leaves the unused pointer -arguments alone and starts registering at the first free number. - For the surrounding shape, the in-tree switchkinsSetup() routines are in src/emc/kinematics: 5axiskins.c, xyzac-trt-kins.c, genserkins.c, scarakins.c and the others listed at the top of this document. None of -them registers a fourth kinstype yet, so the call above has no in-tree +them registers a fourth kinstype yet, so a call for one has no in-tree example to copy. // vim: set syntax=asciidoc: diff --git a/lib/python/rs274/glcanon.py b/lib/python/rs274/glcanon.py index 0c32c87b3ae..8ad25736ef5 100644 --- a/lib/python/rs274/glcanon.py +++ b/lib/python/rs274/glcanon.py @@ -168,6 +168,10 @@ def __init__(self, colors, geometry, is_foam=0, foam_w=1.5, foam_z=0.0): # fixture may hold several pieces. A canon is built per file load, so # nothing else ever clears this. self.workpieces = [] + # The tilted work planes the program defined, in order, and the one + # in effect, which the moves made under it extend. + self.workplanes = [] + self._workplane = None def comment(self, arg): if arg.startswith("WORKPIECE,"): @@ -315,6 +319,25 @@ def set_xy_rotation(self, theta): self._flush_moves() Translated.set_xy_rotation(self, theta) + def set_g68_frame(self, *args): + self._flush_moves() + Translated.set_g68_frame(self, *args) + self._workplane = None + if not self.g68_active: + return + # where the plane sits on the machine: its origin and axes through + # the same offsets a move endpoint gets + origin = self.rotate_and_translate(0, 0, 0, 0, 0, 0, 0, 0, 0)[:3] + axes = [] + for x, y, z in ((1, 0, 0), (0, 1, 0), (0, 0, 1)): + p = self.rotate_and_translate(x, y, z, 0, 0, 0, 0, 0, 0)[:3] + axes.append((p[0] - origin[0], p[1] - origin[1], p[2] - origin[2])) + if self.workplanes and self.workplanes[-1].same_as(origin, axes): + self._workplane = self.workplanes[-1] + return + self._workplane = glcanon_scene.WorkPlane(self.lineno, origin, axes) + self.workplanes.append(self._workplane) + def set_g5x_offset(self, *args, **kw): self._flush_moves() Translated.set_g5x_offset(self, *args, **kw) @@ -435,6 +458,7 @@ def change_tool(self, arg): def straight_traverse(self, x,y,z, a,b,c, u,v,w): if self.suppress > 0: return l = self.rotate_and_translate(x,y,z,a,b,c,u,v,w) + if self._workplane is not None: self._workplane.extend(l) if not self.first_move: self._stage_move(self.lineno, self.lo, l, 0.0, (self.xo, self.yo, self.zo), CAT_TRAVERSE) @@ -446,6 +470,7 @@ def rigid_tap(self, x, y, z): l = self.rotate_and_translate(x,y,z,0,0,0,0,0,0)[:3] l += (self.lo[3], self.lo[4], self.lo[5], self.lo[6], self.lo[7], self.lo[8]) + if self._workplane is not None: self._workplane.extend(l) offset = (self.xo, self.yo, self.zo) # self.dwells.append((self.lineno, self.colors['dwell'], x + self.offset_x, y + self.offset_y, z + self.offset_z, 0)) self._reserve_staging(2) @@ -475,11 +500,14 @@ def straight_arcsegments(self, segs): self._stage_move(lineno, lo, l, feedrate, to, CAT_ARC) lo = l self.lo = lo + if self._workplane is not None and len(segs): + self._workplane.extend(lo) def straight_feed(self, x,y,z, a,b,c, u,v,w): if self.suppress > 0: return self.first_move = False l = self.rotate_and_translate(x,y,z,a,b,c,u,v,w) + if self._workplane is not None: self._workplane.extend(l) self._stage_move(self.lineno, self.lo, l, self.feedrate, (self.xo, self.yo, self.zo), CAT_FEED) self.lo = l @@ -488,6 +516,7 @@ def straight_probe(self, x,y,z, a,b,c, u,v,w): if self.suppress > 0: return self.first_move = False l = self.rotate_and_translate(x,y,z,a,b,c,u,v,w) + if self._workplane is not None: self._workplane.extend(l) self._stage_move(self.lineno, self.lo, l, self.feedrate, (self.xo, self.yo, self.zo), CAT_FEED) self.lo = l @@ -669,6 +698,10 @@ class GlCanonDraw: 'limits': (1.0, 0.0, 0.0), 'workpiece': glcanon_scene.WORKPIECE_COLOR, 'workpiece_alpha': glcanon_scene.WORKPIECE_ALPHA, + 'workplane': glcanon_scene.WORKPLANE_COLOR, + 'workplane_alpha': glcanon_scene.WORKPLANE_ALPHA, + 'workplane_active': glcanon_scene.WORKPLANE_ACTIVE_COLOR, + 'workplane_active_alpha': glcanon_scene.WORKPLANE_ACTIVE_ALPHA, } def __init__(self, s=None, lp=None, g=None): self.stat = s @@ -1140,6 +1173,12 @@ def get_show_workpiece(self): and any host can toggle it by setting self.show_workpiece.""" return getattr(self, 'show_workpiece', True) + def get_show_workplane(self): + """Whether the tilted work planes the program defined are drawn. + Defaulted like get_show_workpiece, and toggled the same way, by + setting self.show_workplane.""" + return getattr(self, 'show_workplane', True) + def get_workpieces(self): """The stock the loaded program declared, as rs274.glcanon_scene .Workpiece records - the declared params, the outline in machine @@ -1279,6 +1318,7 @@ def frame_context(self) -> glcanon_scene.FrameContext: show_metric=self.get_show_metric(), show_small_origin=self.show_small_origin, show_workpiece=self.get_show_workpiece(), + show_workplane=self.get_show_workplane(), program_alpha=self.get_program_alpha(), grid_size=self.get_grid_size(), highlight_line=self.get_highlight_line(), @@ -1363,6 +1403,15 @@ def posstrs(self): positions[X] = _x * math.cos(t) - _y * math.sin(t) positions[Y] = _x * math.sin(t) + _y * math.cos(t) positions = [(i-j) for i, j in zip(positions, s.g92_offset)] + if s.g68_active: + # the tilted work plane sits inside G92 + r = s.g68_rotation + _x = positions[X] - s.g68_offset[X] + _y = positions[Y] - s.g68_offset[Y] + _z = positions[Z] - s.g68_offset[Z] + positions[X] = r[0]*_x + r[3]*_y + r[6]*_z + positions[Y] = r[1]*_x + r[4]*_y + r[7]*_z + positions[Z] = r[2]*_x + r[5]*_y + r[8]*_z else: positions = list(positions) diff --git a/lib/python/rs274/glcanon_scene.py b/lib/python/rs274/glcanon_scene.py index 664c23dfc0c..88b761362da 100644 --- a/lib/python/rs274/glcanon_scene.py +++ b/lib/python/rs274/glcanon_scene.py @@ -98,6 +98,20 @@ #: default black background. WORKPIECE_ALPHA = 0.4 +#: Colour of a tilted work plane the program defined with G68.2, drawn as a +#: rectangle lying in the plane over what the program did there, with the +#: plane's own axes at its origin in the machine-axis colours. +WORKPLANE_COLOR = (0.35, 0.75, 1.00) + +#: The rectangle sits under the path it frames, like the stock outline. +WORKPLANE_ALPHA = 0.5 + +#: The plane in effect on the machine, the one the last executed G68.2 set, +#: drawn over the others so the one the program is in can be told from the +#: ones it has been in or will be in. +WORKPLANE_ACTIVE_COLOR = (1.00, 0.80, 0.20) +WORKPLANE_ACTIVE_ALPHA = 0.9 + def minmax(*args: float) -> tuple[float, float]: return min(*args), max(*args) @@ -211,7 +225,7 @@ class FrameContext: 'view', 'width', 'height', 'show_program', 'show_rapids', 'show_extents', 'show_offsets', 'show_limits', 'show_tool', 'show_live_plot', 'show_relative', 'show_metric', 'show_small_origin', - 'show_workpiece', + 'show_workpiece', 'show_workplane', 'program_alpha', 'grid_size', 'highlight_line', 'enable_dro', 'cone_basesize', 'disable_cone_scaling', 'view_tool_min_dia', # callables: overridable hooks and lazily-needed values @@ -289,6 +303,7 @@ class FrameContext: show_metric: bool show_small_origin: bool show_workpiece: bool + show_workplane: bool program_alpha: bool #: Ground-grid spacing in internal units; ``0`` means "no grid", and is the #: grid part's visibility gate. @@ -2438,6 +2453,216 @@ def _to_display(machine_points: Float64Points, program.ro) +class WorkPlane: + """One tilted work plane the program defined, with G68.2, G68.3 or + G68.4: where it sits on the machine, and what the program did in it. + + :attr:`origin` and :attr:`axes` are in absolute machine coordinates, in + the canon's units, with the g92 offset, the g5x XY rotation and the g5x + offset that were active at the definition applied the way a move + endpoint on the same line gets them. The extents are in the plane's own + coordinates, the ones the program writes under it, accumulated from + every move made while it was in effect, so a reader can tell a plane + the program only oriented to from one it cut in; they are empty + (``min > max``) while nothing moved. + + Read them off the widget's canon, as the workpieces:: + + for plane in gremlin_widget.canon.workplanes: + print(plane.lineno, plane.origin, plane.extents) + """ + + __slots__ = ('lineno', 'origin', 'axes', 'min_extents', 'max_extents') + + def __init__(self, lineno: int, origin: Sequence[float], + axes: Sequence[Sequence[float]]) -> None: + #: Source line of the definition, or ``-1`` without one. + self.lineno = lineno + #: The plane's origin, machine coordinates. + self.origin = tuple(origin) + #: The plane's X, Y and Z as unit vectors in machine coordinates. + self.axes = tuple(tuple(a) for a in axes) + self.min_extents = [9e99, 9e99, 9e99] + self.max_extents = [-9e99, -9e99, -9e99] + + def __repr__(self) -> str: + return "" % (self.lineno, self.origin) + + def same_as(self, origin: Sequence[float], + axes: Sequence[Sequence[float]], tol: float = 1e-9) -> bool: + """Whether a definition names this plane again, so a program that + restates its plane in a loop is one plane, not one per pass.""" + for a, b in zip(self.origin, origin): + if abs(a - b) > tol: + return False + for u, v in zip(self.axes, axes): + for a, b in zip(u, v): + if abs(a - b) > tol: + return False + return True + + def extend(self, point: Sequence[float]) -> None: + """Take in a machine point the program reached under the plane.""" + o = self.origin + d = (point[0] - o[0], point[1] - o[1], point[2] - o[2]) + for i, a in enumerate(self.axes): + v = d[0]*a[0] + d[1]*a[1] + d[2]*a[2] + if v < self.min_extents[i]: + self.min_extents[i] = v + if v > self.max_extents[i]: + self.max_extents[i] = v + + @property + def has_moves(self) -> bool: + return self.max_extents[X] >= self.min_extents[X] + + @property + def extents(self) -> tuple[tuple[float, ...], tuple[float, ...]]: + """``(min_xyz, max_xyz)`` of the moves made under the plane, in the + plane's coordinates.""" + return tuple(self.min_extents), tuple(self.max_extents) + + def machine_point(self, x: float, y: float, z: float) -> tuple[float, float, float]: + """A point of the plane in machine coordinates.""" + o, (ax, ay, az) = self.origin, self.axes + return (o[0] + x*ax[0] + y*ay[0] + z*az[0], + o[1] + x*ax[1] + y*ay[1] + z*az[1], + o[2] + x*ax[2] + y*ay[2] + z*az[2]) + + +def active_workplane(ctx: FrameContext) -> "WorkPlane | None": + """The plane in effect on the machine, as status reports it: the one the + last executed G68.2 set, whether from the program or from MDI. + + Status carries it as the interpreter gave it, the origin in the + coordinate system the plane was defined in and in machine units, so it + goes through the g92 offset, the XY rotation and the g5x offset the way + the canon takes a definition, in the canon's units. Where it is one the + loaded program defined, that record is returned, extents and all; a + plane from MDI, or from a program no longer loaded, comes back as a + record of its own with nothing under it. + """ + s = ctx.stat + if s is None or not getattr(s, 'g68_active', 0): + return None + try: + o = ctx.to_internal_units(s.g68_offset) + r = s.g68_rotation + g92 = ctx.to_internal_units(s.g92_offset) + g5x = ctx.to_internal_units(s.g5x_offset) + t = math.radians(s.rotation_xy) + except (AttributeError, TypeError): + return None + c, sn = math.cos(t), math.sin(t) + + def through(x: float, y: float, z: float) -> tuple[float, float, float]: + x, y, z = (r[0]*x + r[1]*y + r[2]*z + o[X] + g92[X], + r[3]*x + r[4]*y + r[5]*z + o[Y] + g92[Y], + r[6]*x + r[7]*y + r[8]*z + o[Z] + g92[Z]) + return (x*c - y*sn + g5x[X], x*sn + y*c + g5x[Y], z + g5x[Z]) + + origin = through(0, 0, 0) + axes = [] + for unit in ((1, 0, 0), (0, 1, 0), (0, 0, 1)): + p = through(*unit) + axes.append((p[0] - origin[0], p[1] - origin[1], p[2] - origin[2])) + for plane in getattr(ctx.canon, 'workplanes', ()): + # status has been through machine units and back, so not to the bit + if plane.same_as(origin, axes, tol=1e-6): + return plane + return WorkPlane(-1, origin, axes) + + +class WorkPlanePart(Part): + """The tilted work planes the program defined, one drawing each, and + the one in effect on the machine over them in its own colour. + + A plane is drawn where the program worked in it: a rectangle lying in + the plane, over the moves made under it with a margin around them, at + the plane's own Z where the program reached that and otherwise at the + end of the Z range it worked in nearest to it, so a drilling cycle that + never comes down to the plane's origin still shows its holes; a plane + nothing moved under gets a square sized from the program. The plane's + axes stand at the centre of the rectangle in the machine-axis colours, + so the direction the program's X and Y took can be read where the work + is, and the plane's origin is marked with a cross, since the two need + not coincide: a program that drills a sphere puts every plane's origin + at the centre and works out on the surface. + + The active plane is whatever status says the last executed G68.2 set, + so it walks through the program's planes as the program runs, and an + MDI plane shows too, as a square with nothing under it. + """ + + #: Inner lines each way, to read as a surface rather than an outline. + SUBDIVISIONS = 4 + + def draw(self, ctx: FrameContext) -> None: + canon = ctx.canon + planes = list(getattr(canon, 'workplanes', ())) + active = active_workplane(ctx) + if not planes and active is None: + return + # the size a plane with nothing, or only a point, under it is drawn + # at: a tenth of the program, as the extents part spaces its labels + size = max(canon.max_extents[X] - canon.min_extents[X], + canon.max_extents[Y] - canon.min_extents[Y], + canon.max_extents[Z] - canon.min_extents[Z], 2) * .1 + others = [p for p in planes if p is not active] + if others: + self.draw_planes(ctx, others, size, + ctx.colors.get('workplane', WORKPLANE_COLOR), + ctx.colors.get('workplane_alpha', WORKPLANE_ALPHA)) + if active is not None: + self.draw_planes(ctx, [active], size, + ctx.colors.get('workplane_active', WORKPLANE_ACTIVE_COLOR), + ctx.colors.get('workplane_active_alpha', WORKPLANE_ACTIVE_ALPHA)) + + def draw_planes(self, ctx: FrameContext, planes: Sequence[WorkPlane], + size: float, color: Color, alpha: float) -> None: + canon = ctx.canon + outline, inner, cross = [], [], [] + axes = {'axis_x': [], 'axis_y': [], 'axis_z': []} + n = self.SUBDIVISIONS + for plane in planes: + if plane.has_moves: + lo, hi = plane.min_extents, plane.max_extents + cx, cy = (lo[X] + hi[X]) / 2, (lo[Y] + hi[Y]) / 2 + hw = max((hi[X] - lo[X]) * .6, size / 2) + hh = max((hi[Y] - lo[Y]) * .6, size / 2) + z = min(max(lo[Z], 0.0), hi[Z]) + else: + cx = cy = z = 0.0 + hw = hh = size / 2 + x0, x1, y0, y1 = cx - hw, cx + hw, cy - hh, cy + hh + corner = [plane.machine_point(x0, y0, z), plane.machine_point(x1, y0, z), + plane.machine_point(x1, y1, z), plane.machine_point(x0, y1, z)] + for i in range(4): + outline += [corner[i], corner[(i + 1) % 4]] + for i in range(1, n): + f = i / n + inner += [plane.machine_point(x0 + (x1 - x0) * f, y0, z), + plane.machine_point(x0 + (x1 - x0) * f, y1, z), + plane.machine_point(x0, y0 + (y1 - y0) * f, z), + plane.machine_point(x1, y0 + (y1 - y0) * f, z)] + length = max(hw, hh) * .5 + centre = plane.machine_point(cx, cy, z) + axes['axis_x'] += [centre, plane.machine_point(cx + length, cy, z)] + axes['axis_y'] += [centre, plane.machine_point(cx, cy + length, z)] + axes['axis_z'] += [centre, plane.machine_point(cx, cy, z + length)] + arm = length * .4 + cross += [plane.machine_point(-arm, 0, 0), plane.machine_point(arm, 0, 0), + plane.machine_point(0, -arm, 0), plane.machine_point(0, arm, 0), + plane.machine_point(0, 0, -arm), plane.machine_point(0, 0, arm)] + to_display = Workpiece._to_display + ctx.prim.draw_lines(ctx, to_display(np.array(outline), canon), color, alpha) + ctx.prim.draw_lines(ctx, to_display(np.array(inner), canon), color, alpha * .4) + ctx.prim.draw_lines(ctx, to_display(np.array(cross), canon), color, alpha) + for name, verts in axes.items(): + ctx.prim.draw_lines(ctx, to_display(np.array(verts), canon), + ctx.colors[name], alpha) + + class WorkpiecePart(Part): """Wireframe stock outlines declared by ``(WORKPIECE,...)`` comments. @@ -2504,6 +2729,7 @@ def __init__(self) -> None: self.limits_box = LimitsBoxPart() self.backplot = BackplotPart() self.workpiece = WorkpiecePart() + self.workplane = WorkPlanePart() self.tool = ToolPart() self.overlay = OverlayPart() super().__init__([ @@ -2520,6 +2746,8 @@ def __init__(self) -> None: (self.backplot, lambda ctx: ctx.show_live_plot), (self.workpiece, lambda ctx: ctx.show_workpiece and ctx.canon is not None), + (self.workplane, lambda ctx: ctx.show_workplane + and ctx.canon is not None), (self.tool, lambda ctx: ctx.show_tool), (self.overlay, lambda ctx: ctx.enable_dro), ]) diff --git a/lib/python/rs274/interpret.py b/lib/python/rs274/interpret.py index 8815f7f8696..055909ade05 100644 --- a/lib/python/rs274/interpret.py +++ b/lib/python/rs274/interpret.py @@ -24,8 +24,18 @@ class Translated: g5x_offset_a = g5x_offset_b = g5x_offset_c = 0 g5x_offset_u = g5x_offset_v = g5x_offset_w = 0 rotation_xy = 0 + g68_active = 0 + g68_offset = (0.0, 0.0, 0.0) + g68_rotation = (1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0) def rotate_and_translate(self, x,y,z,a,b,c,u,v,w): + if self.g68_active: + r = self.g68_rotation + o = self.g68_offset + x, y, z = (r[0]*x + r[1]*y + r[2]*z + o[0], + r[3]*x + r[4]*y + r[5]*z + o[1], + r[6]*x + r[7]*y + r[8]*z + o[2]) + x += self.g92_offset_x y += self.g92_offset_y z += self.g92_offset_z @@ -83,6 +93,10 @@ def set_xy_rotation(self, theta): t = math.radians(theta) self.rotation_sin = math.sin(t) self.rotation_cos = math.cos(t) + def set_g68_frame(self, x, y, z, r0, r1, r2, r3, r4, r5, r6, r7, r8, active): + self.g68_active = active + self.g68_offset = (x, y, z) + self.g68_rotation = (r0, r1, r2, r3, r4, r5, r6, r7, r8) class ArcsToSegmentsMixin: plane = 1 @@ -179,5 +193,11 @@ def get_axis_mask(self): def get_block_delete(self): return self.s.block_delete + def get_external_joint_positions(self): + return tuple(self.s.joint_actual_position[:self.s.joints]) + + def get_kinematics_type(self): + return self.s.kinematics_type + # vim:ts=8:sts=4:et: diff --git a/src/Makefile b/src/Makefile index 64d7d75c34a..fabf8b2c041 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/kins_rt.h \ + emc/kinematics_userspace/kinematics_user.h \ emc/nml_intf/emcmotcfg.h \ emc/ini/inifile.hh \ emc/ini/inifile.h \ @@ -784,6 +788,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/switchkins_setup.c ../src/emc/kinematics/kins_util.c ../src/emc/kinematics/kins_single.c $(DESTDIR)$(prefix)/share/linuxcnc/ install-kernel-indep: install-python install-python: install-dirs @@ -913,6 +918,9 @@ endif # "kbuild" system. $(BASEPWD) is used here, instead of relative paths, because # that's what kbuild seems to require +# A component built in tree includes the shared kinematics sources by the +# bare names the out-of-tree build resolves in share/linuxcnc +RTFLAGS += -I$(BASEPWD)/emc/kinematics EXTRA_CFLAGS := $(filter-out -ffast-math,$(RTFLAGS)) -D__MODULE__ \ -I$(BASEPWD)/../include -I$(BASEPWD) \ -DSEQUENTIAL_SUPPORT -DHAL_SUPPORT -DDYNAMIC_PLCSIZE -DRT_SUPPORT -DOLD_TIMERS_MONOS_SUPPORT -DMODBUS_IO_MASTER \ @@ -1127,37 +1135,56 @@ hal_lib-objs := hal/hal_lib.o $(MATHSTUB) obj-m += trivkins.o trivkins-objs := emc/kinematics/trivkins.o trivkins-objs += emc/kinematics/kins_util.o +trivkins-objs += emc/kinematics/kins_single.o obj-m += maxkins.o maxkins-objs := emc/kinematics/maxkins.o +maxkins-objs += emc/kinematics/kins_util.o +maxkins-objs += emc/kinematics/kins_single.o obj-m += rotatekins.o rotatekins-objs := emc/kinematics/rotatekins.o +rotatekins-objs += emc/kinematics/kins_util.o +rotatekins-objs += emc/kinematics/kins_single.o obj-m += tripodkins.o tripodkins-objs := emc/kinematics/tripodkins.o +tripodkins-objs += emc/kinematics/kins_util.o +tripodkins-objs += emc/kinematics/kins_single.o obj-m += corexykins.o corexykins-objs := emc/kinematics/corexykins.o +corexykins-objs += emc/kinematics/kins_util.o +corexykins-objs += emc/kinematics/kins_single.o obj-m += lineardeltakins.o lineardeltakins-objs := emc/kinematics/lineardeltakins.o +lineardeltakins-objs += emc/kinematics/kins_util.o +lineardeltakins-objs += emc/kinematics/kins_single.o obj-m += pentakins.o pentakins-objs := emc/kinematics/pentakins.o +pentakins-objs += emc/kinematics/kins_util.o +pentakins-objs += emc/kinematics/kins_single.o pentakins-objs += libposemath/_posemath.o pentakins-objs += $(MATHSTUB) obj-m += rotarydeltakins.o rotarydeltakins-objs := emc/kinematics/rotarydeltakins.o +rotarydeltakins-objs += emc/kinematics/kins_util.o +rotarydeltakins-objs += emc/kinematics/kins_single.o rotarydeltakins-objs += libposemath/_posemath.o rotarydeltakins-objs += $(MATHSTUB) obj-m += rosekins.o rosekins-objs := emc/kinematics/rosekins.o +rosekins-objs += emc/kinematics/kins_util.o +rosekins-objs += emc/kinematics/kins_single.o obj-m += scorbot-kins.o scorbot-kins-objs := emc/kinematics/scorbot-kins.o +scorbot-kins-objs += emc/kinematics/kins_util.o +scorbot-kins-objs += emc/kinematics/kins_single.o ifeq ($(origin userkfuncs), undefined) # use template: @@ -1181,6 +1208,8 @@ 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 += emc/kinematics/switchkins_setup.o genhexkins-objs += $(USERKFUNCS) obj-m += genserkins.o @@ -1190,6 +1219,8 @@ 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 += emc/kinematics/switchkins_setup.o genserkins-objs += $(USERKFUNCS) obj-m += xyzac-trt-kins.o @@ -1197,6 +1228,8 @@ 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 += emc/kinematics/switchkins_setup.o xyzac-trt-kins-objs += $(USERKFUNCS) obj-m += xyzbc-trt-kins.o @@ -1204,6 +1237,8 @@ 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 += emc/kinematics/switchkins_setup.o xyzbc-trt-kins-objs += $(USERKFUNCS) obj-m += scarakins.o @@ -1212,6 +1247,8 @@ 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 += emc/kinematics/switchkins_setup.o scarakins-objs += $(USERKFUNCS) obj-m += pumakins.o @@ -1220,6 +1257,8 @@ 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 += emc/kinematics/switchkins_setup.o pumakins-objs += $(USERKFUNCS) obj-m += three21kins.o @@ -1228,6 +1267,8 @@ 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 += emc/kinematics/switchkins_setup.o three21kins-objs += $(USERKFUNCS) obj-m += 5axiskins.o @@ -1236,6 +1277,8 @@ 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 += emc/kinematics/switchkins_setup.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 aa0af3cfb8b..9de19603720 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -11,10 +11,9 @@ * Copyright (c) 2007 Chris Radek * * Notes: -* 1) pivot_length hal pin must agree with mechanical -* design (including vismach simulation) and augmented -* with current tool z offset -* (typ: mechanical_pivot_length + motion.tooloffset.z) +* 1) pivot-length must agree with the mechanical design +* (including vismach simulation); the tool length is +* the offset motion applies, handed over by motion * 2) C axis: spherical coordinates aziumthal angle (t or theta) * projection of radius to xy plane * 3) B axis: spherical coordinates polar angle (p or phi) @@ -42,8 +41,8 @@ * 9) Coordinates XYZBCW are required, AUV may be used * if specified with the coordinates parameter and will * be mapped one-to-one with the assigned joint. -* 10) The direction of the tilt axis is the opposite of the -* conventional axis direction. See +* 10) The direction of the tilt axis is the opposite of the +* conventional axis direction. See * https://linuxcnc.org/docs/html/gcode/machining-center.html ********************************************************************/ @@ -56,18 +55,29 @@ #include #include #include -#include #include #include -#include -#include "switchkins.h" - -static struct haldata { - hal_real_t pivot_length; - hal_real_t tool_length; -} *haldata; -static int fiveaxis_max_joints; +#include + +// the geometry, one pin each; the maths reads it from the block +static const kins_param_desc fiveaxis_params[] = { + { "pivot-length", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PIVOT_LENGTH }, + { "tool-length", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, +}; +enum { P_PIVOT_LENGTH, P_TOOL_LENGTH }; + +// assignments of principal joints to axis letters, from the block +// (-1 means not defined) +#define JX (p->joint_of_axis[0]) +#define JY (p->joint_of_axis[1]) +#define JZ (p->joint_of_axis[2]) +#define JA (p->joint_of_axis[3]) +#define JB (p->joint_of_axis[4]) +#define JC (p->joint_of_axis[5]) +#define JU (p->joint_of_axis[6]) +#define JV (p->joint_of_axis[7]) +#define JW (p->joint_of_axis[8]) static PmCartesian s2r(double r, double t, double p) { // s2r: spherical coordinates to cartesian coordinates @@ -85,28 +95,18 @@ static PmCartesian s2r(double r, double t, double p) { return c; } //s2r() -// assignments of principal joints to axis letters: -// (-1 means not defined (yet)) -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; - -static int fiveaxis_KinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int fiveaxis_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); - rtapi_real tool_length = hal_get_real(haldata->tool_length); - PmCartesian r = s2r(pivot_length + joints[JW] + tool_length, + double pivot_length = p->geometry[P_PIVOT_LENGTH]; + double tool_length = p->geometry[P_TOOL_LENGTH]; + PmCartesian r = s2r(pivot_length + tool_length + joints[JW], joints[JC], 180.0 - joints[JB]); @@ -124,18 +124,20 @@ static int fiveaxis_KinematicsForward(const double *joints, pos->v = (JV != -1)? joints[JV] : 0; return 0; -} //fiveaxis_KinematicsForward() +} // fiveaxis_forward() -static int fiveaxis_KinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int fiveaxis_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); - rtapi_real tool_length = hal_get_real(haldata->tool_length); - PmCartesian r = s2r(pivot_length + pos->w + tool_length, + double pivot_length = p->geometry[P_PIVOT_LENGTH]; + double tool_length = p->geometry[P_TOOL_LENGTH]; + PmCartesian r = s2r(pivot_length + tool_length + pos->w, pos->c, 180.0 - pos->b); @@ -156,90 +158,77 @@ static int fiveaxis_KinematicsInverse(const EmcPose * pos, // update joints with support for // multiple-joints per-coordinate letter: // based on computed position - position_to_mapped_joints(fiveaxis_max_joints, - &P, - joints); - return 0; -} // fiveaxis_kinematicsInverse() - -int fiveaxis_KinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) + return kinsPoseToMappedJoints(p, &P, joints); +} // fiveaxis_inverse() + +static int fiveaxis_jacobian(const kins_params *p, + const double *joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { - int result=0; - int i,jno; - int axis_idx_for_jno[EMCMOT_MAX_JOINTS]; - int minjoints = strlen(kp->required_coordinates); - fiveaxis_max_joints = strlen(coordinates); // allow for dup coords - - if (fiveaxis_max_joints > kp->max_joints) { - rtapi_print_msg(RTAPI_MSG_ERR, - "ERROR %s: coordinates=%s requires %d joints, max joints=%d\n", - kp->kinsname, - coordinates, - fiveaxis_max_joints, - kp->max_joints); - goto error; - } - - if (map_coordinates_to_jnumbers(coordinates, - kp->max_joints, - kp->allow_duplicates, - axis_idx_for_jno)) { - goto error; - } - // require all chars in reqd_coordinates (order doesn't matter) - for (i=0; i < minjoints; i++) { - char reqd_char; - reqd_char = *(kp->required_coordinates + i); - if ( !strchr(coordinates,toupper(reqd_char)) - && !strchr(coordinates,tolower(reqd_char)) ) { - rtapi_print_msg(RTAPI_MSG_ERR, - "ERROR %s:\nrequired coordinates:%s\n" - "specified coordinates:%s\n", - kp->kinsname, kp->required_coordinates, coordinates); - goto error; - } - } - // assign principal joint numbers (first found in coordinates map) - // duplicates are handled by position_to_mapped_joints() - for (jno=0; jnogeometry[P_PIVOT_LENGTH] + p->geometry[P_TOOL_LENGTH] + pos->w; + const double sb = sin(TO_RAD*pos->b), cb = cos(TO_RAD*pos->b); + const double sc = sin(TO_RAD*pos->c), cc = cos(TO_RAD*pos->c); + double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; + int a, b; + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + for (b = 0; b < EMCMOT_MAX_AXIS; b++) { dP[a][b] = 0; } } - haldata = hal_malloc(sizeof(*haldata)); - if(!haldata) goto error; - - result = hal_pin_new_real(comp_id, HAL_IN, &(haldata->pivot_length), - DEFAULT_PIVOT_LENGTH, "%s.pivot-length", kp->halprefix); - if(result < 0) goto error; - - result = hal_pin_new_real(comp_id, HAL_IN, &(haldata->tool_length), - 0.0, "%s.tool-length", kp->halprefix); - if(result < 0) goto error; + // the computed position of the inverse is the pose less the pivot + // vector r = s2r(R, c, 180 - b), which is (R sin b cos c, R sin b sin c, + // -R cos b); each row is that coordinate differentiated + dP[0][0] = 1; + dP[0][4] = -R * cb * cc * TO_RAD; + dP[0][5] = R * sb * sc * TO_RAD; + dP[0][8] = -sb * cc; + + dP[1][1] = 1; + dP[1][4] = -R * cb * sc * TO_RAD; + dP[1][5] = -R * sb * cc * TO_RAD; + dP[1][8] = -sb * sc; + + dP[2][2] = 1; + dP[2][4] = -R * sb * TO_RAD; + dP[2][8] = cb; + + for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } + + return kinsJacobianFromMappedAxesP(p, (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // fiveaxis_jacobian() + +// The head carries the tool and nothing turns the work: the work frame is the +// machine frame and the tool frame Rz(C) Ry(-B), the tilt left handed about y +// (note 10), its third column the tool axis from the tip towards the holder. +static int fiveaxis_tool_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + const double sb = sin(joints[JB]*TO_RAD), cb = cos(joints[JB]*TO_RAD); + const double sc = sin(joints[JC]*TO_RAD), cc = cos(joints[JC]*TO_RAD); - rtapi_print("Kinematics Module %s\n",__FILE__); - rtapi_print(" module name = %s\n" - " coordinates = %s Requires: [KINS]JOINTS>=%d\n" - " sparm = %s\n", - kp->kinsname, - coordinates,fiveaxis_max_joints, - kp->sparm?kp->sparm:"NOTSPECIFIED"); - rtapi_print(" default pivot-length = %.3f\n", hal_get_real(haldata->pivot_length)); + rot->x.x = cb * cc; rot->y.x = -sc; rot->z.x = -sb * cc; + rot->x.y = cb * sc; rot->y.y = cc; rot->z.y = -sb * sc; + rot->x.z = sb; rot->y.z = 0; rot->z.z = cb; return 0; - -error: - return -1; -} // fiveaxis_KinematicsSetup() +} // fiveaxis_tool_frame() + +static const kins_ops fiveaxis_ops = { + .forward = fiveaxis_forward, + .inverse = fiveaxis_inverse, + .work = kinsIdentityFrame, + .tool = fiveaxis_tool_frame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = fiveaxis_jacobian, + .primary = 1, +}; int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -247,38 +236,27 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "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; + kp->params = fiveaxis_params; + kp->nparams = sizeof(fiveaxis_params)/sizeof(fiveaxis_params[0]); if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = fiveaxis_KinematicsSetup; - *kfwd1 = fiveaxis_KinematicsForward; - *kinv1 = fiveaxis_KinematicsInverse; - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &fiveaxis_ops); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = fiveaxis_KinematicsSetup; - *kfwd0 = fiveaxis_KinematicsForward; - *kinv0 = fiveaxis_KinematicsInverse; - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &fiveaxis_ops); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } // switchkinsSetup() diff --git a/src/emc/kinematics/Submakefile b/src/emc/kinematics/Submakefile index 77085c21c2e..89c6173d5be 100644 --- a/src/emc/kinematics/Submakefile +++ b/src/emc/kinematics/Submakefile @@ -2,6 +2,7 @@ GENSERKINSSRCS := emc/kinematics/ugenserkins.c GENSERKINSSRCS += emc/kinematics/genserfuncs.c +GENSERKINSSRCS += emc/kinematics/kins_util.c USERSRCS += $(GENSERKINSSRCS) DELTAMODULESRCS := emc/kinematics/lineardeltakins.cc @@ -33,3 +34,18 @@ $(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/switchkins_setup.c \ + ../share/linuxcnc/kins_util.c \ + ../share/linuxcnc/kins_single.c + +$(EMCKINEMATICSSRCS): ../share/linuxcnc/%.c: ./emc/kinematics/%.c + $(ECHO) Copying switchkins source $(notdir $@) + $(Q)cp -f $< $@ + +TARGETS += $(EMCKINEMATICSSRCS) diff --git a/src/emc/kinematics/corexykins.c b/src/emc/kinematics/corexykins.c index f592ff52e9f..353ae0f5a84 100644 --- a/src/emc/kinematics/corexykins.c +++ b/src/emc/kinematics/corexykins.c @@ -8,12 +8,15 @@ #include #include #include +#include -int kinematicsForward(const double *joints - ,EmcPose *pos - ,const KINEMATICS_FORWARD_FLAGS *fflags - ,KINEMATICS_INVERSE_FLAGS *iflags - ) { +static int corexy_forward(const kins_params *p, kins_scratch *s, + const double *joints, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)p; + (void)s; (void)fflags; (void)iflags; pos->tran.x = 0.5 * (joints[0] + joints[1]); @@ -29,11 +32,13 @@ int kinematicsForward(const double *joints return 0; } -int kinematicsInverse(const EmcPose *pos - ,double *joints - ,const KINEMATICS_INVERSE_FLAGS *iflags - ,KINEMATICS_FORWARD_FLAGS *fflags - ) { +static int corexy_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)p; + (void)s; (void)iflags; (void)fflags; joints[0] = pos->tran.x + pos->tran.y; @@ -49,22 +54,46 @@ int kinematicsInverse(const EmcPose *pos return 0; } -int kinematicsHome(EmcPose *world - ,double *joint - ,KINEMATICS_FORWARD_FLAGS *fflags - ,KINEMATICS_INVERSE_FLAGS *iflags - ) { - *fflags = 0; - *iflags = 0; - return kinematicsForward(joint, world, fflags, iflags); +static int corexy_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + int j, a; + (void)p; + (void)joints; + (void)pos; + (void)iflags; + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + // the two belt motors each carry x and y, in opposite senses for y + jac[0][0] = 1; jac[0][1] = 1; + jac[1][0] = 1; jac[1][1] = -1; + for (j = 2; j < 9; j++) { jac[j][j] = 1; } + return 0; } -KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; } +static const kins_ops corexy_ops = { + .forward = corexy_forward, + .inverse = corexy_inverse, + .jacobian = corexy_jacobian, +}; + +// no geometry: the belts are what they are. Joints 0..8 are the nine +// letters in order; the entry points come from kins_single.c +const kins_module_info kins_module = { + .name = "corexykins", + .halprefix = "corexykins", + .params = NULL, + .nparams = 0, + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &corexy_ops }, +}; -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); MODULE_LICENSE("GPL"); static int comp_id; @@ -72,6 +101,11 @@ int rtapi_app_main(void) { comp_id = hal_init("corexykins"); if(comp_id < 0) return comp_id; + if (kinsSingleInit(comp_id, "XYZABCUVW", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } + hal_ready(comp_id); return 0; } diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index 2966c377852..418dfc8f5dd 100644 --- a/src/emc/kinematics/genhexkins.c +++ b/src/emc/kinematics/genhexkins.c @@ -16,17 +16,17 @@ machines referred to as "Stewart Platforms". The functions are general enough to be configured for any platform - configuration. In the functions "genhexKinematicsForward" and - "genhexKinematicsInverse" are arrays "a[i]" and "b[i]". The values stored - in these arrays correspond to the positions of the ends of the i'th - strut. The value stored in a[i] is the position of the end of the i'th - strut attached to the platform, in platform coordinates. The value - stored in b[i] is the position of the end of the i'th strut attached - to the base, in base (world) coordinates. + configuration. In the functions "genhex_forward" and "genhex_inverse" + are arrays "a[i]" and "b[i]". The values stored in these arrays + correspond to the positions of the ends of the i'th strut. The value + stored in a[i] is the position of the end of the i'th strut attached + to the platform, in platform coordinates. The value stored in b[i] is + the position of the end of the i'th strut attached to the base, in + base (world) coordinates. The default values for base and platform joints positions are defined in the header file genhexkins.h. The actual values for a particular - machine can be adjusted by hal parameters: + machine can be adjusted by hal pins: genhexkins.base.N.x genhexkins.base.N.y @@ -67,18 +67,18 @@ genhexkins.correction.N - pins showing current values of strut length correction. - The genhexKinematicsInverse function solves the inverse kinematics using + The genhex_inverse function solves the inverse kinematics using a closed form algorithm. The inverse kinematics problem is given the pose of the platform and returns the strut lengths. For this problem there is only one solution that is always returned correctly. - The genhexKinematicsForward function solves the forward kinematics using + The genhex_forward function solves the forward kinematics using an iterative algorithm. Due to the iterative nature of this algorithm - the genhexKinematicsForward function requires an initial value to begin the + the genhex_forward function requires an initial value to begin the iterative routine and then converges to the "nearest" solution. The forward kinematics problem is given the strut lengths and returns the pose of the platform. For this problem there arein multiple - solutions. The genhexKinematicsForward function will return only one of + solutions. The genhex_forward function will return only one of these solutions which will be the solution nearest to the initial value given. It is possible that there are no solutions "near" the given initial value and the iteration will not converge and no @@ -103,6 +103,10 @@ genhexkins.max-iterations - maximum number of iterations spent for a converged solution during current session. + The maths is written as pure functions of the parameter block (see + kinematics.h): the pins above are the table below, read into the block + before every call and written from the scratch after it. + ----------------------------------------------------------------------------*/ #include @@ -110,54 +114,102 @@ #include #include #include -#include /* these decls, KINEMATICS_FORWARD_FLAGS */ #include "genhexkins.h" -#include "switchkins.h" - -static struct haldata { - hal_real_t basex[NUM_STRUTS]; - hal_real_t basey[NUM_STRUTS]; - hal_real_t basez[NUM_STRUTS]; - hal_real_t platformx[NUM_STRUTS]; - hal_real_t platformy[NUM_STRUTS]; - hal_real_t platformz[NUM_STRUTS]; - hal_real_t basenx[NUM_STRUTS]; - hal_real_t baseny[NUM_STRUTS]; - hal_real_t basenz[NUM_STRUTS]; - hal_real_t platformnx[NUM_STRUTS]; - hal_real_t platformny[NUM_STRUTS]; - hal_real_t platformnz[NUM_STRUTS]; - hal_real_t correction[NUM_STRUTS]; - hal_real_t screw_lead; - hal_uint_t last_iter; - hal_uint_t max_iter; - hal_uint_t iter_limit; - hal_real_t max_error; - hal_real_t conv_criterion; - hal_real_t tool_offset; - hal_real_t spindle_offset; - hal_bool_t fwd_kins_fail; - - 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; - -} *haldata; - -static int genhex_gui_forward_kins(EmcPose *pos) -{ - hal_set_real(haldata->gui_x, pos->tran.x); - hal_set_real(haldata->gui_y, pos->tran.y); - hal_set_real(haldata->gui_z, pos->tran.z); - hal_set_real(haldata->gui_a, pos->a); - hal_set_real(haldata->gui_b, pos->b); - hal_set_real(haldata->gui_c, pos->c); - return 0; -} // genhex_gui_forward_kins +#include + +// the table: thirteen entries per strut, then the iteration controls, +// the offsets and the reports. The macros index it. +#define STRUT_ENTRIES 13 +#define P_BASE_X(i) (STRUT_ENTRIES*(i) + 0) +#define P_BASE_Y(i) (STRUT_ENTRIES*(i) + 1) +#define P_BASE_Z(i) (STRUT_ENTRIES*(i) + 2) +#define P_PLAT_X(i) (STRUT_ENTRIES*(i) + 3) +#define P_PLAT_Y(i) (STRUT_ENTRIES*(i) + 4) +#define P_PLAT_Z(i) (STRUT_ENTRIES*(i) + 5) +#define P_BASE_NX(i) (STRUT_ENTRIES*(i) + 6) +#define P_BASE_NY(i) (STRUT_ENTRIES*(i) + 7) +#define P_BASE_NZ(i) (STRUT_ENTRIES*(i) + 8) +#define P_PLAT_NX(i) (STRUT_ENTRIES*(i) + 9) +#define P_PLAT_NY(i) (STRUT_ENTRIES*(i) + 10) +#define P_PLAT_NZ(i) (STRUT_ENTRIES*(i) + 11) +#define P_CORR(i) (STRUT_ENTRIES*(i) + 12) +enum { + P_LAST_ITER = STRUT_ENTRIES*NUM_STRUTS, + P_MAX_ITER, + P_MAX_ERROR, + P_CONV_CRITERION, + P_ITER_LIMIT, + P_TOOL_OFFSET, + P_SPINDLE_OFFSET, + P_SCREW_LEAD, + P_GUI_X, P_GUI_Y, P_GUI_Z, P_GUI_A, P_GUI_B, P_GUI_C, + P_FWD_FAIL, + P_COUNT +}; + +#define STRUT_ROWS(i, bx, by, bz, px, py, pz, bnx, bny, bnz, pnx, pny, pnz) \ + { "base." #i ".x", KINS_PARAM_FLOAT, KINS_IN, 0, bx }, \ + { "base." #i ".y", KINS_PARAM_FLOAT, KINS_IN, 0, by }, \ + { "base." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, bz }, \ + { "platform." #i ".x", KINS_PARAM_FLOAT, KINS_IN, 0, px }, \ + { "platform." #i ".y", KINS_PARAM_FLOAT, KINS_IN, 0, py }, \ + { "platform." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, pz }, \ + { "base-n." #i ".x", KINS_PARAM_FLOAT, KINS_IN, 0, bnx }, \ + { "base-n." #i ".y", KINS_PARAM_FLOAT, KINS_IN, 0, bny }, \ + { "base-n." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, bnz }, \ + { "platform-n." #i ".x", KINS_PARAM_FLOAT, KINS_IN, 0, pnx }, \ + { "platform-n." #i ".y", KINS_PARAM_FLOAT, KINS_IN, 0, pny }, \ + { "platform-n." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, pnz }, \ + { "correction." #i, KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 } + +static const kins_param_desc genhex_params[P_COUNT] = { + STRUT_ROWS(0, DEFAULT_BASE_0_X, DEFAULT_BASE_0_Y, DEFAULT_BASE_0_Z, + DEFAULT_PLATFORM_0_X, DEFAULT_PLATFORM_0_Y, DEFAULT_PLATFORM_0_Z, + DEFAULT_BASE_0_NX, DEFAULT_BASE_0_NY, DEFAULT_BASE_0_NZ, + DEFAULT_PLATFORM_0_NX, DEFAULT_PLATFORM_0_NY, DEFAULT_PLATFORM_0_NZ), + STRUT_ROWS(1, DEFAULT_BASE_1_X, DEFAULT_BASE_1_Y, DEFAULT_BASE_1_Z, + DEFAULT_PLATFORM_1_X, DEFAULT_PLATFORM_1_Y, DEFAULT_PLATFORM_1_Z, + DEFAULT_BASE_1_NX, DEFAULT_BASE_1_NY, DEFAULT_BASE_1_NZ, + DEFAULT_PLATFORM_1_NX, DEFAULT_PLATFORM_1_NY, DEFAULT_PLATFORM_1_NZ), + STRUT_ROWS(2, DEFAULT_BASE_2_X, DEFAULT_BASE_2_Y, DEFAULT_BASE_2_Z, + DEFAULT_PLATFORM_2_X, DEFAULT_PLATFORM_2_Y, DEFAULT_PLATFORM_2_Z, + DEFAULT_BASE_2_NX, DEFAULT_BASE_2_NY, DEFAULT_BASE_2_NZ, + DEFAULT_PLATFORM_2_NX, DEFAULT_PLATFORM_2_NY, DEFAULT_PLATFORM_2_NZ), + STRUT_ROWS(3, DEFAULT_BASE_3_X, DEFAULT_BASE_3_Y, DEFAULT_BASE_3_Z, + DEFAULT_PLATFORM_3_X, DEFAULT_PLATFORM_3_Y, DEFAULT_PLATFORM_3_Z, + DEFAULT_BASE_3_NX, DEFAULT_BASE_3_NY, DEFAULT_BASE_3_NZ, + DEFAULT_PLATFORM_3_NX, DEFAULT_PLATFORM_3_NY, DEFAULT_PLATFORM_3_NZ), + STRUT_ROWS(4, DEFAULT_BASE_4_X, DEFAULT_BASE_4_Y, DEFAULT_BASE_4_Z, + DEFAULT_PLATFORM_4_X, DEFAULT_PLATFORM_4_Y, DEFAULT_PLATFORM_4_Z, + DEFAULT_BASE_4_NX, DEFAULT_BASE_4_NY, DEFAULT_BASE_4_NZ, + DEFAULT_PLATFORM_4_NX, DEFAULT_PLATFORM_4_NY, DEFAULT_PLATFORM_4_NZ), + STRUT_ROWS(5, DEFAULT_BASE_5_X, DEFAULT_BASE_5_Y, DEFAULT_BASE_5_Z, + DEFAULT_PLATFORM_5_X, DEFAULT_PLATFORM_5_Y, DEFAULT_PLATFORM_5_Z, + DEFAULT_BASE_5_NX, DEFAULT_BASE_5_NY, DEFAULT_BASE_5_NZ, + DEFAULT_PLATFORM_5_NX, DEFAULT_PLATFORM_5_NY, DEFAULT_PLATFORM_5_NZ), + [P_LAST_ITER] = { "last-iterations", KINS_PARAM_U32, KINS_OUT, 0, 0 }, + [P_MAX_ITER] = { "max-iterations", KINS_PARAM_U32, KINS_OUT, 0, 0 }, + [P_MAX_ERROR] = { "max-error", KINS_PARAM_FLOAT, KINS_IN, 0, 500.0 }, + [P_CONV_CRITERION] = { "convergence-criterion", KINS_PARAM_FLOAT, KINS_IN, 0, 1e-9 }, + [P_ITER_LIMIT] = { "limit-iterations", KINS_PARAM_U32, KINS_IN, 0, 120 }, + [P_TOOL_OFFSET] = { "tool-offset", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + [P_SPINDLE_OFFSET] = { "spindle-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + [P_SCREW_LEAD] = { "screw-lead", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_SCREW_LEAD }, + // the pose the forward found, for a vismach gui; switchkins provides + // the skgui.* pins for the same purpose + [P_GUI_X] = { "x", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_GUI_Y] = { "y", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_GUI_Z] = { "z", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_GUI_A] = { "a", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_GUI_B] = { "b", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_GUI_C] = { "c", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_FWD_FAIL] = { "fwd-kins-fail", KINS_PARAM_BIT, KINS_OUT, 0, 0 }, +}; + +// the most iterations a converged solution has taken this session, kept +// in the caller's scratch so each caller reports its own +#define MAX_ITER_SEEN(s) ((s)->aux[0]) /******************************* MatInvert() ***************************/ @@ -260,45 +312,45 @@ static void MatMult(double J[][6], const double x[], double Ans[]) } } // MatMult() -/* declare arrays for base and platform coordinates */ -static PmCartesian b[NUM_STRUTS]; -static PmCartesian a[NUM_STRUTS]; - -/* declare base and platform joint axes vectors */ - -static PmCartesian nb1[NUM_STRUTS]; -static PmCartesian na0[NUM_STRUTS]; - -/************************genhex_read_hal_pins**************************/ - -static int genhex_read_hal_pins(void) { +/* the geometry of one call, taken from the block: base and platform + coordinates, the joint axes vectors and the screw lead */ +typedef struct { + PmCartesian b[NUM_STRUTS]; + PmCartesian a[NUM_STRUTS]; + PmCartesian nb1[NUM_STRUTS]; + PmCartesian na0[NUM_STRUTS]; + double screw_lead; +} genhex_geometry; + +static void geometry_of(const kins_params *p, genhex_geometry *g) { int t; - /* set the base and platform coordinates from hal pin values */ - rtapi_real spindle_offset = hal_get_real(haldata->spindle_offset); - rtapi_real tool_offset = hal_get_real(haldata->tool_offset); + /* set the base and platform coordinates from the block */ + const double spindle_offset = p->geometry[P_SPINDLE_OFFSET]; + const double tool_offset = p->tool.tran.z; for (t = 0; t < NUM_STRUTS; t++) { - b[t].x = hal_get_real(haldata->basex[t]); - b[t].y = hal_get_real(haldata->basey[t]); - b[t].z = hal_get_real(haldata->basez[t]) + spindle_offset + tool_offset; - a[t].x = hal_get_real(haldata->platformx[t]); - a[t].y = hal_get_real(haldata->platformy[t]); - a[t].z = hal_get_real(haldata->platformz[t]) + spindle_offset + tool_offset; - - nb1[t].x = hal_get_real(haldata->basenx[t]); - nb1[t].y = hal_get_real(haldata->baseny[t]); - nb1[t].z = hal_get_real(haldata->basenz[t]); - na0[t].x = hal_get_real(haldata->platformnx[t]); - na0[t].y = hal_get_real(haldata->platformny[t]); - na0[t].z = hal_get_real(haldata->platformnz[t]); + g->b[t].x = p->geometry[P_BASE_X(t)]; + g->b[t].y = p->geometry[P_BASE_Y(t)]; + g->b[t].z = p->geometry[P_BASE_Z(t)] + spindle_offset + tool_offset; + g->a[t].x = p->geometry[P_PLAT_X(t)]; + g->a[t].y = p->geometry[P_PLAT_Y(t)]; + g->a[t].z = p->geometry[P_PLAT_Z(t)] + spindle_offset + tool_offset; + + g->nb1[t].x = p->geometry[P_BASE_NX(t)]; + g->nb1[t].y = p->geometry[P_BASE_NY(t)]; + g->nb1[t].z = p->geometry[P_BASE_NZ(t)]; + g->na0[t].x = p->geometry[P_PLAT_NX(t)]; + g->na0[t].y = p->geometry[P_PLAT_NY(t)]; + g->na0[t].z = p->geometry[P_PLAT_NZ(t)]; } - return 0; -} // genhex_read_hal_pins() + g->screw_lead = p->geometry[P_SCREW_LEAD]; +} // geometry_of() /***************************StrutLengthCorrection***************************/ -static int StrutLengthCorrection(const PmCartesian * StrutVectUnit, +static int StrutLengthCorrection(const genhex_geometry *g, + const PmCartesian * StrutVectUnit, const PmRotationMatrix * RMatrix, const int strut_number, double * correction) @@ -307,32 +359,34 @@ static int StrutLengthCorrection(const PmCartesian * StrutVectUnit, double dotprod; /* define base joints axis vectors */ - pmCartCartCross(&nb1[strut_number], StrutVectUnit, &nb2); + pmCartCartCross(&g->nb1[strut_number], StrutVectUnit, &nb2); pmCartCartCross(StrutVectUnit, &nb2, &nb3); pmCartUnitEq(&nb3); /* define platform joints axis vectors */ - pmMatCartMult(RMatrix, &na0[strut_number], &na1); + pmMatCartMult(RMatrix, &g->na0[strut_number], &na1); pmCartCartCross(&na1, StrutVectUnit, &na2); pmCartUnitEq(&na2); /* define dot product */ pmCartCartDot(&nb3, &na2, &dotprod); - *correction = hal_get_real(haldata->screw_lead) * asin(dotprod) / PM_2_PI; + *correction = g->screw_lead * asin(dotprod) / PM_2_PI; return 0; } // StrutLengthCorrection() -/**************** genhexKinematicsForward() *****************/ -static int genhexKinematicsForward(const double * joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +/**************** genhex_forward() *****************/ +static int genhex_forward(const kins_params *p, kins_scratch *s, + const double * joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; (void)iflags; + genhex_geometry g; PmCartesian aw; PmCartesian InvKinStrutVect,InvKinStrutVectUnit; PmCartesian q_trans, RMatrix_a, RMatrix_a_cross_Strut; @@ -351,7 +405,7 @@ static int genhexKinematicsForward(const double * joints, int i; unsigned iteration = 0; - genhex_read_hal_pins(); + geometry_of(p, &g); /* abort on obvious problems, like joints <= 0 */ /* FIXME-- should check against triangle inequality, so that joints @@ -376,13 +430,16 @@ static int genhexKinematicsForward(const double * joints, q_trans.z = pos->tran.z; /* Enter Newton-Raphson iterative method */ - rtapi_real max_error = hal_get_real(haldata->max_error); + const double max_error = p->geometry[P_MAX_ERROR]; + const unsigned iter_limit = (unsigned)p->geometry[P_ITER_LIMIT]; + const double conv_criterion = p->geometry[P_CONV_CRITERION]; while (iterate) { /* check for large error and return error flag if no convergence */ if ((conv_err > +max_error) || (conv_err < -max_error)) { /* we can't converge */ - hal_set_bool(haldata->fwd_kins_fail, 1); + s->failed = 1; + s->out[P_FWD_FAIL] = 1; return -2; }; @@ -390,9 +447,10 @@ static int genhexKinematicsForward(const double * joints, /* check iteration to see if the kinematics can reach the convergence criterion and return error flag if it can't */ - if (iteration > hal_get_ui32(haldata->iter_limit)) { + if (iteration > iter_limit) { /* we can't converge */ - hal_set_bool(haldata->fwd_kins_fail, 1); + s->failed = 1; + s->out[P_FWD_FAIL] = 1; return -5; } @@ -403,18 +461,19 @@ static int genhexKinematicsForward(const double * joints, estimate to get joint estimate, subtract joints to get joint deltas, and compute inv J while we're at it */ for (i = 0; i < NUM_STRUTS; i++) { - pmMatCartMult(&RMatrix, &a[i], &RMatrix_a); + pmMatCartMult(&RMatrix, &g.a[i], &RMatrix_a); pmCartCartAdd(&q_trans, &RMatrix_a, &aw); - pmCartCartSub(&aw, &b[i], &InvKinStrutVect); + pmCartCartSub(&aw, &g.b[i], &InvKinStrutVect); if (0 != pmCartUnit(&InvKinStrutVect, &InvKinStrutVectUnit)) { - hal_set_bool(haldata->fwd_kins_fail, 1); + s->failed = 1; + s->out[P_FWD_FAIL] = 1; return -1; } pmCartMag(&InvKinStrutVect, &InvKinStrutLength); - if (hal_get_real(haldata->screw_lead) != 0.0) { + if (g.screw_lead != 0.0) { /* enable strut length correction */ - StrutLengthCorrection(&InvKinStrutVectUnit, &RMatrix, i, &corr); + StrutLengthCorrection(&g, &InvKinStrutVectUnit, &RMatrix, i, &corr); /* define corrected joint lengths */ InvKinStrutLength += corr; } @@ -455,7 +514,6 @@ static int genhexKinematicsForward(const double * joints, /* enter loop to determine if a strut needs another iteration */ iterate = 0; /*assume iteration is done */ - rtapi_real conv_criterion = hal_get_real(haldata->conv_criterion); for (i = 0; i < NUM_STRUTS; i++) { if (fabs(StrutLengthDiff[i]) > conv_criterion) { iterate = 1; @@ -473,33 +531,42 @@ static int genhexKinematicsForward(const double * joints, pos->tran.y = q_trans.y; pos->tran.z = q_trans.z; - hal_set_ui32(haldata->last_iter, iteration); - - if (iteration > hal_get_ui32(haldata->max_iter)){ - hal_set_ui32(haldata->max_iter, iteration); + s->iterations = iteration; + s->failed = 0; + s->out[P_LAST_ITER] = iteration; + if (iteration > MAX_ITER_SEEN(s)) { + MAX_ITER_SEEN(s) = iteration; } - hal_set_bool(haldata->fwd_kins_fail, 0); + s->out[P_MAX_ITER] = MAX_ITER_SEEN(s); + s->out[P_FWD_FAIL] = 0; - genhex_gui_forward_kins(pos); + s->out[P_GUI_X] = pos->tran.x; + s->out[P_GUI_Y] = pos->tran.y; + s->out[P_GUI_Z] = pos->tran.z; + s->out[P_GUI_A] = pos->a; + s->out[P_GUI_B] = pos->b; + s->out[P_GUI_C] = pos->c; return 0; -} // genhexKinematicsForward() +} // genhex_forward() -/************************ genhexKinematicsInverse() ************************/ +/************************ genhex_inverse() ************************/ /* the inverse kinematics take world coordinates and determine joint values, given the inverse kinematics flags to resolve any ambiguities. The forward flags are set to indicate their value appropriate to the world coordinates passed in. */ -static int genhexKinematicsInverse(const EmcPose * pos, - double * joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int genhex_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double * joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { (void)iflags; (void)fflags; + genhex_geometry g; PmCartesian aw, temp; PmCartesian InvKinStrutVect, InvKinStrutVectUnit; PmRotationMatrix RMatrix; @@ -507,7 +574,7 @@ static int genhexKinematicsInverse(const EmcPose * pos, int i; double InvKinStrutLength, corr; - genhex_read_hal_pins(); + geometry_of(p, &g); /* define Rotation Matrix */ rpy.r = pos->a * PM_PI / 180.0; @@ -519,22 +586,22 @@ static int genhexKinematicsInverse(const EmcPose * pos, for (i = 0; i < NUM_STRUTS; i++) { /* convert location of platform strut end from platform to world coordinates */ - pmMatCartMult(&RMatrix, &a[i], &temp); + pmMatCartMult(&RMatrix, &g.a[i], &temp); pmCartCartAdd(&pos->tran, &temp, &aw); /* define strut lengths */ - pmCartCartSub(&aw, &b[i], &InvKinStrutVect); + pmCartCartSub(&aw, &g.b[i], &InvKinStrutVect); pmCartMag(&InvKinStrutVect, &InvKinStrutLength); - if (hal_get_real(haldata->screw_lead) != 0.0) { + if (g.screw_lead != 0.0) { /* enable strut length correction */ /* define unit strut vector */ if (0 != pmCartUnit(&InvKinStrutVect, &InvKinStrutVectUnit)) { return -1; } /* define correction value and corrected joint lengths */ - StrutLengthCorrection(&InvKinStrutVectUnit, &RMatrix, i, &corr); - hal_set_real(haldata->correction[i], corr); + StrutLengthCorrection(&g, &InvKinStrutVectUnit, &RMatrix, i, &corr); + s->out[P_CORR(i)] = corr; InvKinStrutLength += corr; } @@ -542,145 +609,94 @@ static int genhexKinematicsInverse(const EmcPose * pos, } return 0; -} //genhexKinematicsInverse() - -// HAL pin initializaion values. In small arrays so we can easily -// address them in the pin creation loop. -static const rtapi_real init_basex[NUM_STRUTS] = { - DEFAULT_BASE_0_X, DEFAULT_BASE_1_X, DEFAULT_BASE_2_X, - DEFAULT_BASE_3_X, DEFAULT_BASE_4_X, DEFAULT_BASE_5_X, -}; -static const rtapi_real init_basey[NUM_STRUTS] = { - DEFAULT_BASE_0_Y, DEFAULT_BASE_1_Y, DEFAULT_BASE_2_Y, - DEFAULT_BASE_3_Y, DEFAULT_BASE_4_Y, DEFAULT_BASE_5_Y, -}; -static const rtapi_real init_basez[NUM_STRUTS] = { - DEFAULT_BASE_0_Z, DEFAULT_BASE_1_Z, DEFAULT_BASE_2_Z, - DEFAULT_BASE_3_Z, DEFAULT_BASE_4_Z, DEFAULT_BASE_5_Z, -}; -static const rtapi_real init_platformx[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_X, DEFAULT_PLATFORM_1_X, DEFAULT_PLATFORM_2_X, - DEFAULT_PLATFORM_3_X, DEFAULT_PLATFORM_4_X, DEFAULT_PLATFORM_5_X, -}; -static const rtapi_real init_platformy[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_Y, DEFAULT_PLATFORM_1_Y, DEFAULT_PLATFORM_2_Y, - DEFAULT_PLATFORM_3_Y, DEFAULT_PLATFORM_4_Y, DEFAULT_PLATFORM_5_Y, -}; -static const rtapi_real init_platformz[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_Z, DEFAULT_PLATFORM_1_Z, DEFAULT_PLATFORM_2_Z, - DEFAULT_PLATFORM_3_Z, DEFAULT_PLATFORM_4_Z, DEFAULT_PLATFORM_5_Z, -}; -static const rtapi_real init_basenx[NUM_STRUTS] = { - DEFAULT_BASE_0_NX, DEFAULT_BASE_1_NX, DEFAULT_BASE_2_NX, - DEFAULT_BASE_3_NX, DEFAULT_BASE_4_NX, DEFAULT_BASE_5_NX, -}; -static const rtapi_real init_baseny[NUM_STRUTS] = { - DEFAULT_BASE_0_NY, DEFAULT_BASE_1_NY, DEFAULT_BASE_2_NY, - DEFAULT_BASE_3_NY, DEFAULT_BASE_4_NY, DEFAULT_BASE_5_NY, -}; -static const rtapi_real init_basenz[NUM_STRUTS] = { - DEFAULT_BASE_0_NZ, DEFAULT_BASE_1_NZ, DEFAULT_BASE_2_NZ, - DEFAULT_BASE_3_NZ, DEFAULT_BASE_4_NZ, DEFAULT_BASE_5_NZ, -}; -static const rtapi_real init_platformnx[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_NX, DEFAULT_PLATFORM_1_NX, DEFAULT_PLATFORM_2_NX, - DEFAULT_PLATFORM_3_NX, DEFAULT_PLATFORM_4_NX, DEFAULT_PLATFORM_5_NX, -}; -static const rtapi_real init_platformny[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_NY, DEFAULT_PLATFORM_1_NY, DEFAULT_PLATFORM_2_NY, - DEFAULT_PLATFORM_3_NY, DEFAULT_PLATFORM_4_NY, DEFAULT_PLATFORM_5_NY, -}; -static const rtapi_real init_platformnz[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_NZ, DEFAULT_PLATFORM_1_NZ, DEFAULT_PLATFORM_2_NZ, - DEFAULT_PLATFORM_3_NZ, DEFAULT_PLATFORM_4_NZ, DEFAULT_PLATFORM_5_NZ, +} //genhex_inverse() + +/************************ genhex_jacobian() ***********************/ +/* A strut length changes by the component of its platform end's motion + along the strut. That end moves with the platform, dP + w x (R a), so + the row for strut i is [u_i, (R a_i x u_i) . E] with u_i the unit strut + vector and E the matrix taking the rates of the roll, pitch and yaw + words to the angular velocity w for R = Rz(c) Ry(b) Rx(a). The forward + kinematics builds the same rows for its Newton step, in radians. */ + +// the inverse alone, for differencing where the closed form does not apply +static const kins_ops genhex_diff_ops = { + .forward = genhex_forward, + .inverse = genhex_inverse, }; -static -int genhexKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) +static int genhex_jacobian(const kins_params *p, const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { - (void)coordinates; - int i,res=0; - - if (kp->max_joints < 0 || kp->max_joints > NUM_STRUTS) { - rtapi_print_msg(RTAPI_MSG_ERR, "genhexKinematicsSetup: max_joints %d less than 0 or larger NUM_STRUTS %d\n", - kp->max_joints, NUM_STRUTS); - return -1; - } + genhex_geometry g; + PmCartesian aw, RMatrix_a, strut, u, moment; + PmRotationMatrix RMatrix; + PmRpy rpy; + PmCartesian E[3]; + double sb, cb, sc, cc; + int i, j, col, m; + + geometry_of(p, &g); + + /* the screw lead correction is a function of the pose too, and this + does not differentiate it; difference the inverse instead */ + if (g.screw_lead != 0.0) { + kins_scratch scratch; + kinsScratchInit(&scratch); + return kinsOpsJacobian(&genhex_diff_ops, p, &scratch, joints, pos, jac, iflags); + } - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) { - rtapi_print_msg(RTAPI_MSG_ERR,"genhexKinematicsSetup: hal_malloc fail\n"); - return -1; - } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (col = 0; col < EMCMOT_MAX_AXIS; col++) { jac[j][col] = 0; } + } - for (i = 0; i < kp->max_joints; i++) { - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->basex[i]), - init_basex[i], "%s.base.%d.x", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->basey[i], - init_basey[i], "%s.base.%d.y", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->basez[i], - init_basez[i], "%s.base.%d.z", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformx[i], - init_platformx[i], "%s.platform.%d.x", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformy[i], - init_platformy[i], "%s.platform.%d.y", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformz[i], - init_platformz[i], "%s.platform.%d.z", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->basenx[i], - init_basenx[i], "%s.base-n.%d.x", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->baseny[i], - init_baseny[i], "%s.base-n.%d.y", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->basenz[i], - init_basenz[i], "%s.base-n.%d.z", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformnx[i], - init_platformnx[i], "%s.platform-n.%d.x", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformny[i], - init_platformny[i], "%s.platform-n.%d.y", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformnz[i], - init_platformnz[i], "%s.platform-n.%d.z", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_OUT, &haldata->correction[i], - 0.0, "%s.correction.%d", kp->halprefix, i); - if (res) {goto error;} - } + rpy.r = pos->a * PM_PI / 180.0; + rpy.p = pos->b * PM_PI / 180.0; + rpy.y = pos->c * PM_PI / 180.0; + pmRpyMatConvert(&rpy, &RMatrix); - res += hal_pin_new_ui32(comp_id, HAL_OUT, &haldata->last_iter, - 0, "genhexkins.last-iterations"); - res += hal_pin_new_ui32(comp_id, HAL_OUT, &haldata->max_iter, - 0, "genhexkins.max-iterations"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->max_error, - 500.0, "genhexkins.max-error"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->conv_criterion, - 1e-9, "genhexkins.convergence-criterion"); - res += hal_pin_new_ui32(comp_id, HAL_IN, &haldata->iter_limit, - 120, "genhexkins.limit-iterations"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset, - 0.0, "genhexkins.tool-offset"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->spindle_offset, - 0.0, "genhexkins.spindle-offset"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->screw_lead, - DEFAULT_SCREW_LEAD, "genhexkins.screw-lead"); - - if (res) {goto error;} - - //note: switchkins does not uses these as it provides gui.x, gui.y, etc. - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_x, 0.0, "genhexkins.x"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_y, 0.0, "genhexkins.y"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_z, 0.0, "genhexkins.z"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_a, 0.0, "genhexkins.a"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_b, 0.0, "genhexkins.b"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_c, 0.0, "genhexkins.c"); - - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->fwd_kins_fail, - 0, "genhexkins.fwd-kins-fail"); - - if (res) goto error; - return 0; + /* w = E [da db dc]: the roll axis carried by pitch and yaw, the pitch + axis carried by yaw, and the yaw axis fixed */ + sb = sin(rpy.p); cb = cos(rpy.p); + sc = sin(rpy.y); cc = cos(rpy.y); + E[0].x = cb*cc; E[0].y = cb*sc; E[0].z = -sb; + E[1].x = -sc; E[1].y = cc; E[1].z = 0; + E[2].x = 0; E[2].y = 0; E[2].z = 1; -error: - return res; -} // genhexKinematicsSetup() + for (i = 0; i < NUM_STRUTS; i++) { + double len; + + pmMatCartMult(&RMatrix, &g.a[i], &RMatrix_a); + pmCartCartAdd(&pos->tran, &RMatrix_a, &aw); + pmCartCartSub(&aw, &g.b[i], &strut); + pmCartMag(&strut, &len); + if (len <= 0) { return -1; } + pmCartScalMult(&strut, 1.0/len, &u); + pmCartCartCross(&RMatrix_a, &u, &moment); + + jac[i][0] = u.x; + jac[i][1] = u.y; + jac[i][2] = u.z; + for (m = 0; m < 3; m++) { + double dot; + pmCartCartDot(&moment, &E[m], &dot); + jac[i][3+m] = dot * PM_PI / 180.0; + } + } + return 0; +} // genhex_jacobian() + +// the forward iterates from the pose it is handed, so it is seeded with +// the last answer after a switch +static const kins_ops genhex_ops = { + .forward = genhex_forward, + .inverse = genhex_inverse, + .jacobian = genhex_jacobian, + .fwd_iterates = 1, + .primary = 1, +}; int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -688,44 +704,32 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "genhexkins"; // !!! must agree with filename kp->halprefix = "genhexkins"; // hal pin names kp->required_coordinates = "xyzabc"; kp->max_joints = strlen(kp->required_coordinates); kp->allow_duplicates = 0; + kp->params = genhex_params; + kp->nparams = P_COUNT; + if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); kp->fwd_iterates_mask = 0x2; //genhexkins switchkins_type==1 kp->gui_kinstype = 1; //vismach gui for switchkins_type==1 - - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = genhexKinematicsSetup; - *kfwd1 = genhexKinematicsForward; - *kinv1 = genhexKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &genhex_ops); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); kp->fwd_iterates_mask = 0x1; //genhexkins switchkins_type==0 kp->gui_kinstype = 0; //vismach gui for switchkins_type==0 - - *kset0 = genhexKinematicsSetup; - *kfwd0 = genhexKinematicsForward; - *kinv0 = genhexKinematicsInverse; - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &genhex_ops); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } //switchkinsSetup() diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index 5600ab2be1f..17889e63160 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -28,6 +28,11 @@ Currently the type of the joints is hardcoded to ANGULAR, although the kins support both ANGULAR and LINEAR axes. + The maths is written as pure functions of the parameter block (see + kinematics.h): the pins are the table below, read into the block + before every call, and the link description is built from the block + on each call. + TODO: * make number of joints a loadtime parameter * add HAL pins for all settable parameters, including joint type: ANGULAR / LINEAR @@ -48,44 +53,53 @@ #if __GNUC__ && !defined(__clang__) // The matrix and vector storage is just big. // genser_kin_jac_inv() is 2112 -// genserKinematicsInverse() is 2640 - #pragma GCC diagnostic warning "-Wframe-larger-than=2648" +// genser_inverse() is 2640 plus the link description it builds + #pragma GCC diagnostic warning "-Wframe-larger-than=3400" #endif -static struct haldata { - hal_uint_t max_iterations; - hal_uint_t last_iterations; - hal_real_t a[GENSER_MAX_JOINTS]; - hal_real_t alpha[GENSER_MAX_JOINTS]; - hal_real_t d[GENSER_MAX_JOINTS]; - hal_sint_t unrotate[GENSER_MAX_JOINTS]; - genser_struct *kins; - go_pose *pos; // used in various functions, we malloc it - // only once in genserKinematicsSetup() -} *haldata = NULL; - -static int total_joints; -double j[GENSER_MAX_JOINTS]; +// the table: four entries per joint, then the iteration count in and out +#define P_A(i) (4*(i) + 0) +#define P_ALPHA(i) (4*(i) + 1) +#define P_D(i) (4*(i) + 2) +#define P_UNROT(i) (4*(i) + 3) +enum { + P_LAST_ITER = 4*GENSER_MAX_JOINTS, + P_MAX_ITER, + P_COUNT +}; -#define KINS_PTR (haldata->kins) +#define JOINT_ROWS(i, a, alpha, d) \ + { "A-" #i, KINS_PARAM_FLOAT, KINS_IN, 0, a }, \ + { "ALPHA-" #i, KINS_PARAM_FLOAT, KINS_IN, 0, alpha }, \ + { "D-" #i, KINS_PARAM_FLOAT, KINS_IN, 0, d }, \ + { "unrotate-" #i, KINS_PARAM_S32, KINS_IN, 0, 0 } + +const kins_param_desc GENSER_PARAMS[P_COUNT] = { + JOINT_ROWS(0, DEFAULT_A1, DEFAULT_ALPHA1, DEFAULT_D1), + JOINT_ROWS(1, DEFAULT_A2, DEFAULT_ALPHA2, DEFAULT_D2), + JOINT_ROWS(2, DEFAULT_A3, DEFAULT_ALPHA3, DEFAULT_D3), + JOINT_ROWS(3, DEFAULT_A4, DEFAULT_ALPHA4, DEFAULT_D4), + JOINT_ROWS(4, DEFAULT_A5, DEFAULT_ALPHA5, DEFAULT_D5), + JOINT_ROWS(5, DEFAULT_A6, DEFAULT_ALPHA6, DEFAULT_D6), + [P_LAST_ITER] = { "last-iterations", KINS_PARAM_U32, KINS_OUT, 0, 0 }, + [P_MAX_ITER] = { "max-iterations", KINS_PARAM_U32, KINS_IN, 0, GENSER_DEFAULT_MAX_ITERATIONS }, +}; +const int GENSER_NPARAMS = P_COUNT; #if GENSER_MAX_JOINTS < 6 #error GENSER_MAX_JOINTS must be at least 6; fix genserkins.h #endif -static int genser_hal_inited = 0; - -int genser_kin_init(void) { - genser_struct *genser = KINS_PTR; +void genser_links_of(const kins_params *p, genser_struct *genser) { int t; static volatile double tst=0;tst=sqrt(tst); // ensure -lm used /* init them all and make them revolute joints */ /* FIXME: should allow LINEAR joints based on HAL param too */ for (t = 0; t < GENSER_MAX_JOINTS; t++) { - genser->links[t].u.dh.a = hal_get_real(haldata->a[t]); - genser->links[t].u.dh.alpha = hal_get_real(haldata->alpha[t]); - genser->links[t].u.dh.d = hal_get_real(haldata->d[t]); + genser->links[t].u.dh.a = p->geometry[P_A(t)]; + genser->links[t].u.dh.alpha = p->geometry[P_ALPHA(t)]; + genser->links[t].u.dh.d = p->geometry[P_D(t)]; genser->links[t].u.dh.theta = 0; genser->links[t].type = GO_LINK_DH; genser->links[t].quantity = GO_QUANTITY_ANGLE; @@ -94,8 +108,13 @@ int genser_kin_init(void) { /* set a select few to make it PUMA-like */ // FIXME-AJ: make a hal pin, also set number of joints based on it genser->link_num = 6; + genser->iterations = 0; +} // genser_links_of() - return GO_RESULT_OK; +/* the unrotate coupling of one joint, from the block */ +static rtapi_s32 unrotate_of(const kins_params *p, int link) +{ + return (rtapi_s32)p->geometry[P_UNROT(link)]; } /* compute the forward jacobian function: @@ -313,79 +332,168 @@ int genser_kin_jac_fwd(void *kins, return GO_RESULT_OK; } +/* The Jacobian in the terms of kinematics.h: joints in degrees per pose + word in EmcPose units, the derivative of genser_inverse(). + + compute_jinv() gives the geometric inverse Jacobian, radians of joint per + unit of base-frame twist. A pose word rate is not a twist: the roll, + pitch and yaw rates reach the angular velocity through E, the matrix of + the axes each one turns about, for the RPY convention of go_rpy_mat_convert, + R = Rz(yaw) Ry(pitch) Rx(roll). So + + dq/dp = unrotate . deg . Jinv . blockdiag(I, E . rad) + + with the unit conversions and the unrotate coupling applied in the order + the inverse applies them. */ +static int genser_jacobian(const kins_params *p, const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)iflags; + genser_struct genser_stg; + genser_struct *genser = &genser_stg; + GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS); + GO_MATRIX_DECLARE(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6); + go_pose T_L_0; + go_link linkout[GENSER_MAX_JOINTS] = {}; + go_real jest[GENSER_MAX_JOINTS]; + double E[3][3]; + double sb, cb, sc, cc; + int link, i, j, a, m, retval; + + genser_links_of(p, genser); + + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + + // the kinematic joint angles, in radians and with the unrotate + // coupling removed, exactly as the forward prepares them + for (link = 0; link < genser->link_num; link++) { + rtapi_s32 unrotate = unrotate_of(p, link); + jest[link] = joint[link] * (PM_PI / 180); + if (link && unrotate) + jest[link] -= unrotate * jest[link-1]; + } + + go_matrix_init(Jfwd, Jfwd_stg, 6, genser->link_num); + go_matrix_init(Jinv, Jinv_stg, genser->link_num, 6); + + for (link = 0; link < genser->link_num; link++) { + retval = go_link_joint_set(&genser->links[link], jest[link], &linkout[link]); + if (GO_RESULT_OK != retval) + return -1; + } + retval = compute_jfwd(linkout, genser->link_num, &Jfwd, &T_L_0); + if (GO_RESULT_OK != retval) + return -1; + retval = compute_jinv(&Jfwd, &Jinv); + if (GO_RESULT_OK != retval) + return -1; // singular: no finite joint rate follows the pose + + // E columns: the roll axis carried by pitch and yaw, the pitch axis + // carried by yaw, and the yaw axis fixed + sb = sin(world->b * PM_PI / 180); cb = cos(world->b * PM_PI / 180); + sc = sin(world->c * PM_PI / 180); cc = cos(world->c * PM_PI / 180); + E[0][0] = cb*cc; E[1][0] = cb*sc; E[2][0] = -sb; + E[0][1] = -sc; E[1][1] = cc; E[2][1] = 0; + E[0][2] = 0; E[1][2] = 0; E[2][2] = 1; + + for (i = 0; i < genser->link_num; i++) { + // linear pose words: the twist column is the pose column, and the + // joint comes out in radians + for (a = 0; a < 3; a++) { + jac[i][a] = Jinv.el[i][a] * (180 / PM_PI); + } + // angular pose words: through E, radians of pose word per degree + // of pose word and degrees of joint per radian of joint cancel + for (m = 0; m < 3; m++) { + double s = 0; + for (a = 0; a < 3; a++) { s += Jinv.el[i][3+a] * E[a][m]; } + jac[i][3+m] = s; + } + } + + // the unrotate coupling, in link order as the inverse applies it + for (link = 1; link < genser->link_num; link++) { + rtapi_s32 unrotate = unrotate_of(p, link); + if (unrotate) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + jac[link][a] += unrotate * jac[link-1][a]; + } + } + } + + // uvw pass through as joints 6, 7, 8 + if (p->max_joints > 6) jac[6][6] = 1; + if (p->max_joints > 7) jac[7][7] = 1; + if (p->max_joints > 8) jac[8][8] = 1; + + return 0; +} // genser_jacobian() + /* main function called by emc2 for forward Kins */ -int genserKinematicsForward(const double *joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) { +static int genser_forward(const kins_params *p, kins_scratch *s, + const double *joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - go_pose *pos; + genser_struct genser; + go_pose pos; go_rpy rpy; go_real jcopy[GENSER_MAX_JOINTS]; // will hold the radian conversion of joints int ret = 0; - int i, changed=0; - if (!genser_hal_inited) { - rtapi_print_msg(RTAPI_MSG_ERR, - "genserKinematicsForward: not initialized\n"); - return -1; - } + int i; + + genser_links_of(p, &genser); for (i=0; i< 6; i++) { - // FIXME - debug hack - if (!GO_ROT_CLOSE(j[i],joint[i])) changed = 1; // convert to radians to pass to genser_kin_fwd jcopy[i] = joint[i] * PM_PI / 180; - rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[i]); + rtapi_s32 unrotate = unrotate_of(p, i); if ((i) && unrotate) jcopy[i] -= unrotate * jcopy[i-1]; } - if (changed) { - for (i=0; i< 6; i++) - j[i] = joint[i]; - // rtapi_print("genserKinematicsForward(joints: %f %f %f %f %f %f)\n", - //joint[0],joint[1],joint[2],joint[3],joint[4],joint[5]); - } // AJ: convert from emc2 coords (XYZABC - which are actually rpy euler // angles) // to go angles (quaternions) - pos = haldata->pos; rpy.y = world->c * PM_PI / 180; rpy.p = world->b * PM_PI / 180; rpy.r = world->a * PM_PI / 180; - go_rpy_quat_convert(&rpy, &pos->rot); - pos->tran.x = world->tran.x; - pos->tran.y = world->tran.y; - pos->tran.z = world->tran.z; + go_rpy_quat_convert(&rpy, &pos.rot); + pos.tran.x = world->tran.x; + pos.tran.y = world->tran.y; + pos.tran.z = world->tran.z; //pass through unused 678 as uvw - if (total_joints > 6) world->u = joint[6]; - if (total_joints > 7) world->v = joint[7]; - if (total_joints > 8) world->w = joint[8]; + if (p->max_joints > 6) world->u = joint[6]; + if (p->max_joints > 7) world->v = joint[7]; + if (p->max_joints > 8) world->w = joint[8]; // pos will be the world location // jcopy: joitn position in radians - ret = genser_kin_fwd(KINS_PTR, jcopy, pos); + ret = genser_kin_fwd(&genser, jcopy, &pos); if (ret < 0) return ret; // AJ: convert back to emc2 coords - ret = go_quat_rpy_convert(&pos->rot, &rpy); + ret = go_quat_rpy_convert(&pos.rot, &rpy); if (ret < 0) return ret; - world->tran.x = pos->tran.x; - world->tran.y = pos->tran.y; - world->tran.z = pos->tran.z; + world->tran.x = pos.tran.x; + world->tran.y = pos.tran.y; + world->tran.z = pos.tran.z; world->a = rpy.r * 180 / PM_PI; world->b = rpy.p * 180 / PM_PI; world->c = rpy.y * 180 / PM_PI; - if (changed) { -// rtapi_print("genserKinematicsForward(world: %f %f %f %f %f %f)\n", world->tran.x, world->tran.y, world->tran.z, world->a, world->b, world->c); - } return 0; } @@ -397,8 +505,6 @@ int genser_kin_fwd(void *kins, const go_real * joints, go_pose * pos) int link; int retval; - genser_kin_init(); - for (link = 0; link < genser->link_num; link++) { retval = go_link_joint_set(&genser->links[link], joints[link], &linkout[link]); if (GO_RESULT_OK != retval) @@ -412,22 +518,25 @@ int genser_kin_fwd(void *kins, const go_real * joints, go_pose * pos) return GO_RESULT_OK; } -int genserKinematicsInverse(const EmcPose * world, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int genser_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * world, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { (void)iflags; (void)fflags; - genser_struct *genser = KINS_PTR; + genser_struct genser_stg; + genser_struct *genser = &genser_stg; GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS); GO_MATRIX_DECLARE(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6); go_pose T_L_0; go_real dvw[6]; go_real jest[GENSER_MAX_JOINTS]; go_real dj[GENSER_MAX_JOINTS]; - go_pose pest, pestinv, Tdelta; // pos = converted pose from EmcPose + go_pose pos; // converted pose from EmcPose + go_pose pest, pestinv, Tdelta; go_rpy rpy; go_rvec rvec; go_cart cart; @@ -435,30 +544,19 @@ int genserKinematicsInverse(const EmcPose * world, int link; int smalls; int retval; + const unsigned max_iterations = (unsigned)p->geometry[P_MAX_ITER]; - // rtapi_print("kineInverse(joints: %f %f %f %f %f %f)\n", - // joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]); - // rtapi_print("kineInverse(world: %f %f %f %f %f %f)\n", - // world->tran.x, world->tran.y, world->tran.z, world->a, world->b, world->c); - -#ifndef ULAPI - genser_kin_init(); - if (!genser_hal_inited) { - rtapi_print_msg(RTAPI_MSG_ERR, - "genserKinematicsInverse: not initialized\n"); - return -1; - } -#endif + genser_links_of(p, genser); // FIXME-AJ: rpy or zyx ? rpy.y = world->c * PM_PI / 180; rpy.p = world->b * PM_PI / 180; rpy.r = world->a * PM_PI / 180; - go_rpy_quat_convert(&rpy, &haldata->pos->rot); - haldata->pos->tran.x = world->tran.x; - haldata->pos->tran.y = world->tran.y; - haldata->pos->tran.z = world->tran.z; + go_rpy_quat_convert(&rpy, &pos.rot); + pos.tran.x = world->tran.x; + pos.tran.y = world->tran.y; + pos.tran.z = world->tran.z; go_matrix_init(Jfwd, Jfwd_stg, 6, genser->link_num); go_matrix_init(Jinv, Jinv_stg, genser->link_num, 6); @@ -470,9 +568,10 @@ int genserKinematicsInverse(const EmcPose * world, } for (genser->iterations = 0; - genser->iterations < hal_get_ui32(haldata->max_iterations); + genser->iterations < max_iterations; genser->iterations++) { - hal_set_ui32(haldata->last_iterations, genser->iterations); + s->iterations = genser->iterations; + s->out[P_LAST_ITER] = genser->iterations; /* update the Jacobians */ for (link = 0; link < genser->link_num; link++) { go_link_joint_set(&genser->links[link], jest[link], &linkout[link]); @@ -491,8 +590,7 @@ int genserKinematicsInverse(const EmcPose * world, } /* pest is the resulting pose estimate given joint estimate */ - genser_kin_fwd(KINS_PTR, jest, &pest); - //printf("jest: %f %f %f %f %f %f\n",jest[0],jest[1],jest[2],jest[3],jest[4],jest[5]); + genser_kin_fwd(genser, jest, &pest); /* pestinv is its inverse */ go_pose_inv(&pest, &pestinv); /* @@ -506,7 +604,7 @@ int genserKinematicsInverse(const EmcPose * world, .Tdelta = pestinv * pos L 0 L */ - go_pose_pose_mult(&pestinv, haldata->pos, &Tdelta); + go_pose_pose_mult(&pestinv, &pos, &Tdelta); /* We need Tdelta in 0 frame, not pest frame, so rotate it @@ -534,10 +632,31 @@ int genserKinematicsInverse(const EmcPose * world, /* push the Cartesian velocity vector through the inverse Jacobian */ go_matrix_vector_mult(&Jinv, dvw, dj); + /* The Jacobian holds only near the estimate, and a far pose asks + for a step of many radians: the arm lands somewhere unrelated + and converges by way of whole turns its limits refuse. Cap the + step, keeping its direction, and let the iteration walk there. */ + { + double worst = 0.0; + + for (link = 0; link < genser->link_num; link++) { + if (GO_QUANTITY_ANGLE == linkout[link].quantity + && fabs(dj[link]) > worst) { + worst = fabs(dj[link]); + } + } + if (worst > GENSER_MAX_ANGLE_STEP) { + double scale = GENSER_MAX_ANGLE_STEP / worst; + for (link = 0; link < genser->link_num; link++) { + dj[link] *= scale; + } + } + } + //pass through 678 as uvw - if (total_joints > 6) joints[6] = world->u; - if (total_joints > 7) joints[7] = world->v; - if (total_joints > 8) joints[8] = world->w; + if (p->max_joints > 6) joints[6] = world->u; + if (p->max_joints > 7) joints[7] = world->v; + if (p->max_joints > 8) joints[8] = world->w; /* check for small joint increments, if so we're done */ for (link = 0, smalls = 0; link < genser->link_num; link++) { @@ -554,14 +673,10 @@ int genserKinematicsInverse(const EmcPose * world, for (link = 0; link < genser->link_num; link++) { // convert from radians back to angles joints[link] = jest[link] * 180 / PM_PI; - rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[link]); + rtapi_s32 unrotate = unrotate_of(p, link); if ((link) && unrotate) joints[link] += unrotate * joints[link-1]; } - //rtapi_print("DONEkineInverse(joints: %f %f %f %f %f %f), (iterations=%d)\n", - // joints[0],joints[1],joints[2],joints[3],joints[4],joints[5], genser->iterations); - //rtapi_print("OKkineInverse: %.2f %.2f %.2f %.2f %.2f %.2f)\n", - // world->tran.x, world->tran.y, world->tran.z, world->a, world->b, world->c); return GO_RESULT_OK; } /* else keep iterating */ @@ -575,6 +690,13 @@ int genserKinematicsInverse(const EmcPose * world, return GO_RESULT_ERROR; } +const kins_ops GENSER_OPS = { + .forward = genser_forward, + .inverse = genser_inverse, + .jacobian = genser_jacobian, + .primary = 1, +}; + /* Extras, not callable using go_kin_ wrapper but if you know you have linked in these kinematics, go ahead and call these for your ad hoc @@ -585,68 +707,3 @@ int genser_kin_inv_iterations(genser_struct * genser) { return genser->iterations; } - -int genser_kin_inv_set_max_iterations(int i) -{ - if (i <= 0) return GO_RESULT_ERROR; - hal_set_ui32(haldata->max_iterations, i); - return GO_RESULT_OK; -} - -int genser_kin_inv_get_max_iterations() -{ - return hal_get_ui32(haldata->max_iterations); -} - -static const rtapi_real init_a[GENSER_MAX_JOINTS] = { - DEFAULT_A1, DEFAULT_A2, DEFAULT_A3, DEFAULT_A4, DEFAULT_A5, DEFAULT_A6 -}; -static const rtapi_real init_alpha[GENSER_MAX_JOINTS] = { - DEFAULT_ALPHA1, DEFAULT_ALPHA2, DEFAULT_ALPHA3, DEFAULT_ALPHA4, DEFAULT_ALPHA5, DEFAULT_ALPHA6 -}; -static const rtapi_real init_d[GENSER_MAX_JOINTS] = { - DEFAULT_D1, DEFAULT_D2, DEFAULT_D3, DEFAULT_D4, DEFAULT_D5, DEFAULT_D6 -}; - - -int genserKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - (void)coordinates; - int i,res=0; - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) {goto error;} - - // allow for pass through joints 6,7,8 u,v,w - total_joints = kp->max_joints; - - // only the first 6 joints have A,ALPHA,D,unrotate pins - for (i = 0; i < GENSER_MAX_JOINTS; i++) { - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a[i]), - init_a[i], "%s.A-%d", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->alpha[i]), - init_alpha[i], "%s.ALPHA-%d", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d[i]), - init_d[i], "%s.D-%d", kp->halprefix, i); - res += hal_pin_new_si32(comp_id, HAL_IN, &(haldata->unrotate[i]), - 0, "%s.unrotate-%d", kp->halprefix, i); - } - res += hal_pin_new_ui32(comp_id, HAL_OUT, &(haldata->last_iterations), - 0, "%s.last-iterations",kp->halprefix); - - KINS_PTR = hal_malloc(sizeof(genser_struct)); - haldata->pos = (go_pose *) hal_malloc(sizeof(go_pose)); - if (KINS_PTR == NULL) {goto error;} - if (haldata->pos == NULL) {goto error;} - res += hal_pin_new_ui32(comp_id, HAL_IN, &haldata->max_iterations, - GENSER_DEFAULT_MAX_ITERATIONS, "%s.max-iterations",kp->halprefix); - - if (res) {goto error;} - - genser_hal_inited = 1; - return 0; - -error: - return -1; -} // genserKinematicsSetup() diff --git a/src/emc/kinematics/genserkins.c b/src/emc/kinematics/genserkins.c index fa8d30598dc..09c72a9fcbe 100644 --- a/src/emc/kinematics/genserkins.c +++ b/src/emc/kinematics/genserkins.c @@ -4,7 +4,8 @@ * * NOTEs: * 1) specify all kparms items -* 2) specify 3 KS,KF,KI functions (setup,forward,inverse) +* 2) the maths and the geometry table are in genserfuncs.c, written as +* pure functions of the parameter block (see kinematics.h) */ /******************************************************************** @@ -42,7 +43,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 @@ -57,39 +58,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "genserkins"; // !!! must agree with filename kp->halprefix = "genserkins"; // hal pin names kp->required_coordinates = "xyzabcuvw"; // u,v,w are joints 6,7,8 kp->max_joints = strlen(kp->required_coordinates); kp->allow_duplicates = 0; + kp->params = GENSER_PARAMS; + kp->nparams = GENSER_NPARAMS; if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = genserKinematicsSetup; - *kfwd1 = genserKinematicsForward; - *kinv1 = genserKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &GENSER_OPS); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = genserKinematicsSetup; - *kfwd0 = genserKinematicsForward; - *kinv0 = genserKinematicsInverse; - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &GENSER_OPS); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } diff --git a/src/emc/kinematics/genserkins.h b/src/emc/kinematics/genserkins.h index 3aa0756fc5a..cb92fe605b2 100644 --- a/src/emc/kinematics/genserkins.h +++ b/src/emc/kinematics/genserkins.h @@ -45,6 +45,8 @@ #define GENSER_MAX_JOINTS 6 #define GENSER_DEFAULT_MAX_ITERATIONS 100 +/* the most a rotary joint moves in one pass of the inverse, in radians */ +#define GENSER_MAX_ANGLE_STEP 0.2 #define PI_2 GO_PI_2 @@ -81,8 +83,6 @@ typedef struct { extern int genser_kin_size(void); -extern int genser_kin_init(void); - extern const char * genser_kin_get_name(void); extern int genser_kin_num_joints(void * kins); @@ -125,15 +125,6 @@ extern int genser_kin_fwd_interations(genser_struct * genser); inverse kinematics functions */ extern int genser_kin_inv_iterations(genser_struct * genser); -/*! Sets the maximum number of iterations to use in future calls to - the inverse kinematics functions, after which an error will be - reported */ -extern int genser_kin_inv_set_max_iterations(int i); - -/*! Returns the maximum number of iterations that will be used to - compute inverse kinematics functions */ -extern int genser_kin_inv_get_max_iterations(void); - extern int compute_jfwd(go_link * link_params, int link_number, go_matrix * Jfwd, @@ -142,18 +133,14 @@ extern int compute_jfwd(go_link * link_params, extern int compute_jinv(go_matrix * Jfwd, go_matrix * Jinv); -extern int genserKinematicsForward(const double *joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags); - -extern int genserKinematicsInverse(const EmcPose * world, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags); +/* The kinematics as functions of the parameter block (see kinematics.h): + the DH parameters and the unrotate couplings are the table, the maths + is the ops. genser_links_of() fills a link description from a block, + for a caller that wants the go_ routines directly. */ +extern const kins_param_desc GENSER_PARAMS[]; +extern const int GENSER_NPARAMS; +extern const kins_ops GENSER_OPS; -extern int genserKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* ksetup_parms); +extern void genser_links_of(const kins_params *p, genser_struct *genser); #endif diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index ba285cb8861..456257a78de 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -16,8 +16,13 @@ #define __LINUXCNC_KINEMATICS_H #include "emcpos.h" /* EmcPose */ +#include "emcmotcfg.h" /* EMCMOT_MAX_JOINTS, EMCMOT_MAX_AXIS */ #include "rtapi_bool.h" +#ifdef __cplusplus +extern "C" { +#endif + /* The type of kinematics used. @@ -102,6 +107,26 @@ extern int kinematicsHome(struct EmcPose * world, extern KINEMATICS_TYPE kinematicsType(void); +/* Switchable kinematics: a module provides several kinematics, numbered +** 0..SWITCHKINS_MAX_TYPES-1, and motion runs one of them at a time. +** The count is here, not in switchkins.h, because motion and the NML +** status channel need it; it aliases KINS_MAX_TYPES below. +*/ +#define SWITCHKINS_MAX_TYPES KINS_MAX_TYPES + +/* What a kinematics type IS, declared by the module with +** switchkinsDeclare() and read back with kinematicsTypeFlags(). +** G13.1 resolves "identity" from these flags instead of assuming a +** number; a module that declares nothing leaves its types numeric-only +** and G13.1 refuses to guess. +*/ +#define KINSTYPE_IDENTITY 0x1 /* no transform: the joints are the world */ +#define KINSTYPE_PRIMARY 0x2 /* the module's working transform */ + +/* flags of a kinematics type, or -1 for a type the module does not +** provide (and for every type on a machine with plain kinematics) */ +extern int kinematicsTypeFlags(int ktype); + /* These two give the orientation of the tool and of the workpiece for a set of joint values. Each returns a rotation whose columns are that frame's axes expressed in MACHINE coordinates, the frame fixed to the bed that @@ -156,26 +181,6 @@ extern int kinematicsWorkFrame(const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags); -/* Switchable kinematics: a module provides several kinematics, numbered -** 0..SWITCHKINS_MAX_TYPES-1, and motion runs one of them at a time. -** The count is here, not in switchkins.h, because motion and the NML -** status channel need it. -*/ -#define SWITCHKINS_MAX_TYPES 9 - -/* What a kinematics type IS, declared by the module with -** switchkinsDeclare() and read back with kinematicsTypeFlags(). -** G13.1 resolves "identity" from these flags instead of assuming a -** number; a module that declares nothing leaves its types numeric-only -** and G13.1 refuses to guess. -*/ -#define KINSTYPE_IDENTITY 0x1 /* no transform: the joints are the world */ -#define KINSTYPE_PRIMARY 0x2 /* the module's working transform */ - -/* flags of a kinematics type, or -1 for a type the module does not -** provide (and for every type on a machine with plain kinematics) */ -extern int kinematicsTypeFlags(int ktype); - /* parameters for use with switchkins.c */ typedef struct kinematics_parms { char* sparm; // module string parameter passed to kins @@ -192,6 +197,8 @@ typedef struct kinematics_parms { // bitmask: 0x4 bit2: switchkins_type==2 int gui_kinstype; // may be reqd for parallel kins with vismach // to select switchkins_type for gui pins + const struct kins_param_desc_tag *params; // geometry table, see below + int nparams; } kparms; /* map letters in a coordinates string to joint numbers @@ -348,6 +355,30 @@ typedef int (*kinsFrameFunc)(const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags); +/* Which joints turn the work: a bit per joint whose motion changes the + work frame at the seed. This is what a caller needs to hold the table + still while the head orients the tool (Heidenhain COORD ROT), or to let + it take part (TABLE ROT), without a config entry naming it. Returns 0 + or -1 if the frame cannot be evaluated. */ +extern int toolFrameWorkJoints(kinsFrameFunc work, int num_joints, + const double *seed, unsigned int *mask); + +/* The two rotaries that orient the tool, told apart. One has its axis + fixed in the machine frame, the primary, and the other has its axis + carried by the first, the secondary. The two poses that reach one tool + direction differ in the sign of the secondary, which is what a caller + needs to name a pose rather than count them, Heidenhain's SEQ+ and SEQ-. + + Both are found from the module's own tool frame, by turning each joint a + little and reading the axis of the rotation that results, so a module + declares nothing and a switchkins type that turns nothing answers -1. + Returns 0 with both joints set, or -1 where the machine has any number + of orienting rotaries but two, a robot wrist among them, or where the + frame cannot be evaluated. */ +extern int toolFrameOrientJoints(kinsFrameFunc tool, int num_joints, + const double *seed, + int *primary, int *secondary); + extern int toolFrameSolve(kinsFrameFunc work, kinsFrameFunc tool, int num_joints, @@ -360,22 +391,322 @@ extern int toolFrameSolve(kinsFrameFunc work, int *free_directions, double *tool_spin); +/* How each joint responds to a unit rate of each pose coordinate: + + jac[j][a] = d joint[j] / d pose[a] + + Rows are joints, columns are pose coordinates in EmcPose order, x y z a b + c u v w. This is the derivative of kinematicsInverse(): multiply it by a + pose velocity and the result is the joint velocity that motion will + command, which is what a feed limit checks against the joint limits. A + row that grows without bound is a pose approaching a singularity, where + the joints cannot keep up with any world speed at all. + + Each entry is in joint units per pose unit, whatever units the module's + own forward and inverse already use. Nothing is converted here: a caller + that feeds pose rates in EmcPose units gets joint rates in the units + motion already commands, and never has to know which unit a rotary joint + is in. On every module in the tree both are degrees, so a table rotary's + own row is a plain 1 in its own column. + + The columns are pose coordinates, so the answer lives in the work frame, + where kinematicsForward() reports positions. The a, b and c columns are + rates of the pose words, the wrapped linear axes the planner already + treats as coordinates, and not an angular velocity vector. That makes + this a different object from the frames above, which are orientations + and are given against the machine; see the Kinematics Conventions + chapter. + + joint and world are one pose in both descriptions: world is what + kinematicsForward() reports for joint under these flags. Both are given + because a closed form differentiates at the joints while the generic + default perturbs the pose, and iflags keeps every inverse the default + calls on the same solution branch. Rows past the module's joint count + are zero. + + Optional, like the frames. Modules built on switchkins.c export it + always and answer for every type, since it can always be obtained from + the inverse where a frame cannot; other modules need not export it, and + a caller that resolves it dynamically and finds nothing can call + kinsJacobianFromInverse() itself with the module's inverse. + + Returns 0, or -1 if the module cannot answer at this pose. */ +extern int kinematicsJacobian(const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + +typedef int (*kinsInverseFunc)(const EmcPose *world, + double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); + +/* The generic Jacobian, by central differences of an inverse about world: + two inverse calls per pose coordinate, eighteen in all, on the solution + branch iflags selects. The joint array handed to every call starts from + joint, so a module that reads its joint argument sees the machine where + it is. + + The answer is as good as the inverse: a closed form gives it to rounding, + an inverse that iterates to a tolerance gives it to that tolerance over + the step, and should supply its own. num_joints is the module's joint + count. Returns 0, or -1 if any inverse fails. */ +#define KINS_JACOBIAN_STEP 1e-3 /* pose units, either kind */ + +extern int kinsJacobianFromInverse(kinsInverseFunc inverse, + int num_joints, + const double *joint, + const EmcPose *world, + const KINEMATICS_INVERSE_FLAGS *iflags, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); + +/* For a module whose inverse computes a position P and then hands it to + position_to_mapped_joints(): given dP[axis][pose], how each coordinate of + P responds to each pose coordinate, fill in jac so that every joint gets + the row of the letter it is mapped to. Duplicate letters get duplicate + rows, which is the gantry case. */ +extern int kinsJacobianFromMappedAxes(int max_joints, + const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); + +/* joints are axes: a 1 per joint in the column of its letter */ +extern int identityKinematicsJacobian(const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + +/* ------------------------------------------------------------------------ + Kinematics as pure functions of what the caller passes in. + + Everything above reads its geometry from HAL pins the module created and + keeps its mode and scratch in statics, so it can only answer for the + machine as it is now, from inside the module. The forms below take the + same questions with the machine described by the caller: a parameter + block naming the kinematics type, the joint map, the tool and the + geometry, and a scratch block for what an iterative method carries + between calls. Nothing is read from HAL and nothing is kept, so one copy + of the maths serves motion, a planner evaluating poses the machine has + not reached, task checking a program at load, and a tool asking what if. + + A module declares its geometry as a table of named entries. In RT the + shared code makes one HAL pin per entry, with the names configs already + use, and copies the pins into the block before every call; outside RT the + caller fills the block from wherever it likes. The maths reads + p->geometry[i] where it read a pin. + + The existing entry points stay and are supplied once, by kins_single.c + for a module with one kinematics type and by switchkins.c for one with + several, so nothing that calls kinematicsForward() changes. A module + that does not provide these forms keeps working as it did; it just cannot + be evaluated outside RT. + ------------------------------------------------------------------------ */ + +#define KINS_MAX_PARAMS 96 /* genhexkins declares 84 */ +#define KINS_MAX_TYPES 9 /* kinematics types a module may provide */ + +typedef enum { + KINS_PARAM_FLOAT = 0, + KINS_PARAM_BIT, + KINS_PARAM_S32, + KINS_PARAM_U32 +} kins_param_type; + +typedef enum { + KINS_IN = 0, /* read into the block before a call */ + KINS_OUT, /* a result, written from kins_scratch.out[] after it */ + KINS_IO /* read like an input; the pin is HAL_IO so it can be poked */ +} kins_param_dir; + +/* One entry of a module's geometry table. name follows the module's HAL + prefix. An entry with tool set is the tool length along the tool axis: + the shared code puts its value in kins_params.tool.tran.z as well, which + is what the maths should read, so that a caller outside RT can supply + the tool from the tool table without there being a pin. */ +typedef struct kins_param_desc_tag { + const char *name; + kins_param_type type; + kins_param_dir dir; + int tool; + double dflt; +} kins_param_desc; + +/* The machine, as far as the kinematics is concerned. One copy may be + shared by any number of callers: nothing writes it during a call. */ +typedef struct kins_params { + int size; /* sizeof(kins_params) */ + int ktype; /* kinematics type, 0 if one */ + int max_joints; /* joints the map covers */ + int joint_of_axis[EMCMOT_MAX_AXIS]; /* principal joint per letter */ + int joints_of_axis[EMCMOT_MAX_AXIS]; /* bit per joint, duplicates */ + EmcPose tool; /* tool offset, tool.tran.z along the tool axis */ + double geometry[KINS_MAX_PARAMS]; /* the table, in its order */ +} kins_params; + +/* What one caller carries between its own calls: the last pose an + iterative forward found, which seeds the next, and what a module reports + about its last call. Never shared between callers. */ +typedef struct kins_scratch { + EmcPose pose_seed; /* start an iterative forward here */ + int have_pose_seed; + int pose_seed_ok; /* pose_seed came from a solve that succeeded */ + double joint_seed[EMCMOT_MAX_JOINTS]; /* start an iterative inverse here */ + int have_joint_seed; + int iterations; + int failed; + double aux[8]; /* whatever else a module carries between calls */ + double out[KINS_MAX_PARAMS]; /* the table's KINS_OUT entries */ +} kins_scratch; + +typedef int (*kins_forward_fn)(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags); + +typedef int (*kins_inverse_fn)(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); + +typedef int (*kins_frame_fn)(const kins_params *p, const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + +typedef int (*kins_jacobian_fn)(const kins_params *p, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + +/* The maths of one kinematics type. forward and inverse are required; the + frames, the native rotation and the Jacobian are optional as before, and + a missing Jacobian is differenced from the inverse. fwd_iterates says the + forward starts from the pose it is handed, so the shared code seeds it + with the last answer after a switch. identity says joints are axes, which + a consumer may use to skip the maths altogether. primary says this is + the module's working transform, the type G43.4 switches to. */ +typedef struct kins_ops { + kins_forward_fn forward; + kins_inverse_fn inverse; + kins_frame_fn work; + kins_frame_fn tool; + const PmRotationMatrix *native; /* NULL means TOOL_FRAME_SPINDLE */ + kins_jacobian_fn jacobian; + int fwd_iterates; + int identity; /* joints are axes */ + int primary; /* the working transform */ +} kins_ops; + +/* A module described for a caller outside RT: its table, its joint + conventions and the maths of each type. ops[t] is NULL for a type the + module still implements the old way. */ +typedef struct kins_module_info { + const char *name; + const char *halprefix; + const kins_param_desc *params; + int nparams; + const char *required_coordinates; + int max_joints; /* the most the module allows */ + int allow_duplicates; + int ntypes; + const kins_ops *ops[KINS_MAX_TYPES]; +} kins_module_info; + +/* Exported by every module that provides the forms above. coordinates and + sparm are the module parameters the RT instance was loaded with; a module + whose types depend on them replays that choice here. Meant for a copy of + the module loaded outside RT; the RT instance answers from its own state + without redoing its setup. Returns 0, or -1 with info untouched. */ +extern int kinsDescribe(const char *coordinates, const char *sparm, + kins_module_info *info); + +/* Fill a block for a module: size, the joint map from coordinates (checked + against required_coordinates, the joint limit and the duplicate rule), + ktype 0, no tool, and every geometry entry at its table default. A + caller then overwrites what it knows better. Returns 0 or -1. */ +extern int kinsParamsInit(kins_params *p, + const kins_module_info *info, + const char *coordinates); + +/* The joint map alone, into a block, with no other field touched. */ +extern int kinsParamsMapCoordinates(kins_params *p, + const char *coordinates, + int max_joints, + int allow_duplicates, + const char *required_coordinates); + +/* Reset a scratch to "no seed, nothing reported". */ +extern void kinsScratchInit(kins_scratch *s); + +/* The map helpers above, reading the map from the block instead of from + the statics that map_coordinates_to_jnumbers() fills. */ +extern int kinsMappedJointsToPose(const kins_params *p, + const double *joints, EmcPose *pos); +extern int kinsPoseToMappedJoints(const kins_params *p, + const EmcPose *pos, double *joints); +extern int kinsJacobianFromMappedAxesP(const kins_params *p, + const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); + +/* Identity as pure functions: joints are axes through the block's map. */ +extern int kinsIdentityForward(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags); +extern int kinsIdentityInverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsIdentityFrame(const kins_params *p, const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsIdentityJacobian(const kins_params *p, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); +extern const kins_ops KINS_IDENTITY_OPS; + +/* The five questions asked of an ops table, with the defaults applied: + identity for a missing frame, the native rotation applied to the tool + frame, and the Jacobian differenced from the inverse when there is no + closed form. These are what the RT wrappers and a caller outside RT + both go through, so both get the same answers. */ +extern int kinsOpsForward(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags); +extern int kinsOpsInverse(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsOpsWorkFrame(const kins_ops *ops, const kins_params *p, + const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsOpsToolFrame(const kins_ops *ops, const kins_params *p, + const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsOpsJacobian(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + extern int kinematicsSwitchable(void); extern int kinematicsSwitch(int switchkins_type); + +/* The tool offset motion applies, handed to the module. Motion calls this + whenever the offset changes (G43, G49) and references it weakly, so a + module that does not export it still loads and keeps reading whatever + tool pin it has. kins_single.c and switchkins.c export it for every + module written on the parameter block: the tool then comes from the tool + table through motion, and the module's tool pin, where it has one, is + read only until motion has spoken. */ +extern int kinematicsSetTool(const EmcPose *tool); //NOTE: switchable kinematics may require Interp::Synch // before/after invoking kinematicsSwitch() // A convenient command to synch is: M66 E0 L0 -#define KINS_NOT_SWITCHABLE \ -extern int kinematicsSwitchable() {return 0;} \ -extern int kinematicsSwitch(int switchkins_type) { (void)switchkins_type; return 0;} \ -extern int kinematicsTypeFlags(int ktype) { (void)ktype; return -1;} \ -EXPORT_SYMBOL(kinematicsSwitchable); \ -EXPORT_SYMBOL(kinematicsSwitch); \ -EXPORT_SYMBOL(kinematicsTypeFlags); - - // support for template for user-defined switchkins_type==2 +extern const kins_ops USERK_OPS; + extern int userkKinematicsSetup(const int comp_id, const char* coordinates, kparms* ksetup_parms); @@ -389,48 +720,16 @@ extern int userkKinematicsInverse(const struct EmcPose * world, double *joint, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags); -#endif //********************************************************************* -// xyzac,xyzbc; -extern int trtKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* ksetup_parms); - -extern int xyzacKinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags); - -extern int xyzacKinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags); - -extern int xyzacKinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -extern int xyzacKinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - - -extern int xyzbcKinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags); - -extern int xyzbcKinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags); - -extern int xyzbcKinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -extern int xyzbcKinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); +// xyzac,xyzbc (trtfuncs.c): one geometry table, the maths of each machine +extern const kins_param_desc TRT_PARAMS[]; +extern const int TRT_NPARAMS; +extern const kins_ops XYZAC_OPS; +extern const kins_ops XYZBC_OPS; //********************************************************************* +#ifdef __cplusplus +} +#endif + +#endif // __LINUXCNC_KINEMATICS_H diff --git a/src/emc/kinematics/kins_rt.h b/src/emc/kinematics/kins_rt.h new file mode 100644 index 00000000000..78d0a61ef7d --- /dev/null +++ b/src/emc/kinematics/kins_rt.h @@ -0,0 +1,77 @@ +/******************************************************************** +* Description: kins_rt.h +* The RT side of a kinematics module written as pure functions: the HAL +* pins made from its geometry table, and the wrapper that supplies the +* classic entry points for a module with one kinematics type. A module +* with several types gets the same from switchkins.c. +* +* Kept apart from kinematics.h because everything here needs HAL, and +* kinematics.h is read by callers outside RT that do not. +* +* License: GPL Version 2 +********************************************************************/ +#ifndef __LINUXCNC_KINS_RT_H +#define __LINUXCNC_KINS_RT_H + +#include +#include "kinematics.h" + +/* one HAL pin handle per table entry, of whichever type the entry has */ +typedef union { + hal_real_t r; + hal_bool_t b; + hal_sint_t s; + hal_uint_t u; +} kins_pin_ref; + +/* Make one pin per table entry, named ., inputs at their + defaults. *out receives the handles, from hal_malloc(), or NULL for an + empty table. Returns 0 or -1. */ +extern int kinsParamsPinsCreate(int comp_id, const char *prefix, + const kins_param_desc *params, int nparams, + kins_pin_ref **out); + +/* Copy every input pin into p->geometry[], and the tool entry into + p->tool.tran.z as well. */ +extern void kinsParamsPinsRead(const kins_pin_ref *pins, + const kins_param_desc *params, int nparams, + kins_params *p); + +/* Copy s->out[] to every output pin. */ +extern void kinsParamsPinsWrite(const kins_pin_ref *pins, + const kins_param_desc *params, int nparams, + const kins_scratch *s); + +/* Where the RT block's tool comes from. kinematicsSetTool() records what + motion sends in one of these; kinsToolSourceApply() writes it into a + block after the pins have been read, over the tool entry, once motion + has sent anything. Until then the tool entry's pin is all there is, as + under halrun with the module alone. A config that still nets the tool + to the module's pin loses nothing; one that sets that pin to something + else is told, once, after the two have disagreed for a thousand calls, + since the pin lags the send by a cycle. */ +typedef struct { + EmcPose tool; + int have; /* motion has sent a tool */ + int disagreeing; /* consecutive calls with the pin elsewhere */ + int warned; +} kins_tool_source; + +extern void kinsToolSourceSet(kins_tool_source *src, const EmcPose *tool); +extern void kinsToolSourceApply(kins_tool_source *src, const char *prefix, + const kins_param_desc *params, int nparams, + kins_params *p); + +/* A module with one kinematics type defines this, describing itself, and + links kins_single.c, which supplies kinematicsForward() and the rest + from it. ops[0] is the maths; the other entries are ignored. */ +extern const kins_module_info kins_module; + +/* Called once from the module's rtapi_app_main() or EXTRA_SETUP(), after + hal_init() and before hal_ready(): makes the pins, builds the block for + coordinates and records the KINEMATICS_TYPE that kinematicsType() will + report. Returns 0 or -1. */ +extern int kinsSingleInit(int comp_id, const char *coordinates, + KINEMATICS_TYPE reported); + +#endif diff --git a/src/emc/kinematics/kins_single.c b/src/emc/kinematics/kins_single.c new file mode 100644 index 00000000000..2ca2f057e2a --- /dev/null +++ b/src/emc/kinematics/kins_single.c @@ -0,0 +1,174 @@ +/******************************************************************** +* Description: kins_single.c +* The classic kinematics entry points for a module with one kinematics +* type written as pure functions. The module defines kins_module and +* calls kinsSingleInit(); this file keeps the one RT parameter block, +* fills it from the pins before every call, and hands the call to the +* module's ops. It is the counterpart of switchkins.c for a module that +* does not switch. +* +* License: GPL Version 2 +********************************************************************/ + +#include +#include +#include + +#include +#include + +static kins_params rt_params; +static kins_scratch rt_scratch; +static kins_pin_ref *pins; +static kins_tool_source tool_source; +static int inited; +static KINEMATICS_TYPE reported_type = KINEMATICS_BOTH; + +static const kins_ops *ops(void) +{ + return inited ? kins_module.ops[0] : NULL; +} + +// the block sees the pins as they are now, and the tool motion sent +static void read_pins(void) +{ + kinsParamsPinsRead(pins, kins_module.params, kins_module.nparams, + &rt_params); + kinsToolSourceApply(&tool_source, kins_module.halprefix, + kins_module.params, kins_module.nparams, &rt_params); +} + +static void write_pins(void) +{ + kinsParamsPinsWrite(pins, kins_module.params, kins_module.nparams, + &rt_scratch); +} + +int kinsSingleInit(int comp_id, const char *coordinates, + KINEMATICS_TYPE reported) +{ + if (!kins_module.ops[0] || !kins_module.ops[0]->forward + || !kins_module.ops[0]->inverse) { + rtapi_print_msg(RTAPI_MSG_ERR, + "kinsSingleInit: %s supplies no forward or inverse\n", + kins_module.name ? kins_module.name : "?"); + return -1; + } + if (kinsParamsInit(&rt_params, &kins_module, coordinates)) { return -1; } + kinsScratchInit(&rt_scratch); + if (kinsParamsPinsCreate(comp_id, kins_module.halprefix, + kins_module.params, kins_module.nparams, + &pins)) { + return -1; + } + reported_type = reported; + inited = 1; + return 0; +} // kinsSingleInit() + +int kinematicsForward(const double *joint, + EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + int r; + if (!inited) { return -1; } + read_pins(); + r = kinsOpsForward(ops(), &rt_params, &rt_scratch, joint, pos, fflags, iflags); + write_pins(); + return r; +} + +int kinematicsInverse(const EmcPose *pos, + double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + int r; + if (!inited) { return -1; } + read_pins(); + r = kinsOpsInverse(ops(), &rt_params, &rt_scratch, pos, joint, iflags, fflags); + write_pins(); + return r; +} + +int kinematicsWorkFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + if (!inited) { return -1; } + read_pins(); + return kinsOpsWorkFrame(ops(), &rt_params, joint, rot, fflags); +} + +int kinematicsToolFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + if (!inited) { return -1; } + read_pins(); + return kinsOpsToolFrame(ops(), &rt_params, joint, rot, fflags); +} + +int kinematicsJacobian(const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + if (!inited) { return -1; } + read_pins(); + return kinsOpsJacobian(ops(), &rt_params, &rt_scratch, joint, pos, jac, iflags); +} + +int kinematicsSetTool(const EmcPose *tool) +{ + if (!tool) { return -1; } + kinsToolSourceSet(&tool_source, tool); + return 0; +} + +KINEMATICS_TYPE kinematicsType(void) +{ + return reported_type; +} + +int kinematicsSwitchable(void) { return 0; } + +int kinematicsSwitch(int switchkins_type) +{ + (void)switchkins_type; + return 0; +} + +// one type and nothing declared about it: -1, no information +int kinematicsTypeFlags(int ktype) +{ + (void)ktype; + return -1; +} + +// The module's description, for a copy of it loaded outside RT. A module +// with one type does not depend on its parameters for its shape, so this +// is the table as declared. +int kinsDescribe(const char *coordinates, const char *sparm, + kins_module_info *info) +{ + (void)coordinates; + (void)sparm; + if (!info) { return -1; } + *info = kins_module; + info->ntypes = 1; + return 0; +} + +EXPORT_SYMBOL(kinematicsType); +EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsWorkFrame); +EXPORT_SYMBOL(kinematicsToolFrame); +EXPORT_SYMBOL(kinematicsJacobian); +EXPORT_SYMBOL(kinematicsSetTool); +EXPORT_SYMBOL(kinematicsSwitchable); +EXPORT_SYMBOL(kinematicsSwitch); +EXPORT_SYMBOL(kinematicsTypeFlags); +EXPORT_SYMBOL(kinsDescribe); diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index fa20010d1d2..d97f228cf20 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -48,7 +48,9 @@ #include #include #include +#include #include +#include // principal joint numbers based on module 'coordinates' parameter static int JX = -1; @@ -76,38 +78,23 @@ 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 +// Letters to joint numbers, in order, with the checks every caller wants: +// a valid letter set, at most max_joints of them, duplicates only where +// allowed. Fills axis_idx_for_jno (-1 past the last letter) and touches +// nothing else, so the block form and the static form share it. +static int kins_scan_coordinates(const char *coordinates, + int max_joints, + int allow_duplicates, + int axis_idx_for_jno[], + const char *errtag) { - char* errtag="map_coordinates_to_jnumbers: ERROR:\n "; - int jno=0; - bool found=0; + 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) ) { @@ -168,6 +155,40 @@ int map_coordinates_to_jnumbers(const char *coordinates, } } } + return 0; +} // kins_scan_coordinates() + +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; + + 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; + } + } + if (kins_scan_coordinates(coordinates, max_joints, allow_duplicates, + axis_idx_for_jno, errtag)) { + return -1; + } for (jno=0; jno < max_joints; jno++) { int bitnumber = 1< Axis %c\n", jno,*(p+axis_idx_for_jno[jno])); } +#ifndef ULAPI + // the module's own report of its type; this file is also built + // outside RT, where there is no module around it if (kinematicsType() != KINEMATICS_BOTH) { rtapi_print("identityKinematicsSetup: Recommend: kinstype=both\n"); } +#endif rtapi_print("\n"); } @@ -537,6 +562,9 @@ int identityKinematicsToolFrame(const double *joints, #define TFS_ITERS 60 #define TFS_FD_STEP 1e-6 // internal radians #define TFS_MOVED_TOL 1e-9 // frame difference that counts as movement +#define TFS_PROBE_STEP 0.05 // joint units, to read a joint's own axis +#define TFS_CARRY_STEP 40.0 // joint units, far enough to swing a carried axis +#define TFS_CARRY_TOL 1e-6 // axes closer than this counted as the same #define TFS_RANK_TOL 1e-4 // a direction worth less than this is free #define TFS_SOLVED 1e-18 // sum of squared residuals #define TFS_STEP_LIMIT 0.4 // internal radians per iteration @@ -945,6 +973,117 @@ static int tfs_spin(tfs_ctx *c, const double *joint, return 0; } +int toolFrameWorkJoints(kinsFrameFunc work, int num_joints, + const double *seed, unsigned int *mask) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + PmRotationMatrix base, moved; + double joint[EMCMOT_MAX_JOINTS]; + int i, j; + + if (!work || !seed || !mask || num_joints <= 0 || num_joints > EMCMOT_MAX_JOINTS) { + return -1; + } + *mask = 0; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joint[i] = (i < num_joints) ? seed[i] : 0; } + if (work(joint, &base, &fflags)) { return -1; } + + // a step of one joint unit: a degree on every module in the tree, and + // a linear joint never turns a frame whatever its unit + for (j = 0; j < num_joints; j++) { + double diff = 0; + const double *a = &base.x.x, *b = &moved.x.x; + + joint[j] = seed[j] + 1.0; + if (work(joint, &moved, &fflags)) { return -1; } + joint[j] = seed[j]; + for (i = 0; i < 9; i++) { diff += fabs(a[i] - b[i]); } + if (diff > TFS_MOVED_TOL) { *mask |= 1u << j; } + } + return 0; +} + +// The axis a joint turns the tool frame about, in machine coordinates: move +// the joint a little and read the rotation that took the frame there. +// Returns 0 and a unit axis where the joint turns the tool, 1 where it does +// not, which is every linear joint and every joint the module ignores. +static int tfs_joint_axis(kinsFrameFunc tool, const double *joint, + int j, double step, double axis[3]) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + PmRotationMatrix r1, r2; + double moved[EMCMOT_MAX_JOINTS]; + const double *a, *b; + double len; + int i; + + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { moved[i] = joint[i]; } + moved[j] += step; + if (tool(joint, &r1, &fflags)) { return -1; } + if (tool(moved, &r2, &fflags)) { return -1; } + + // The rotation from one frame to the other is r2 * transpose(r1), and + // the axis of a small rotation is the skew part of it. Both matrices + // are columns of axes, so element (row, col) is (&r.x.x)[3*col + row]. + a = &r1.x.x; + b = &r2.x.x; + axis[0] = axis[1] = axis[2] = 0; + for (i = 0; i < 3; i++) { + // m[2][1] - m[1][2], m[0][2] - m[2][0], m[1][0] - m[0][1] + axis[0] += b[3*i + 2] * a[3*i + 1] - b[3*i + 1] * a[3*i + 2]; + axis[1] += b[3*i + 0] * a[3*i + 2] - b[3*i + 2] * a[3*i + 0]; + axis[2] += b[3*i + 1] * a[3*i + 0] - b[3*i + 0] * a[3*i + 1]; + } + len = sqrt(axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]); + if (len < 1e-9) { return 1; } + for (i = 0; i < 3; i++) { axis[i] /= len; } + return 0; +} + +int toolFrameOrientJoints(kinsFrameFunc tool, int num_joints, + const double *seed, int *primary, int *secondary) +{ + double joint[EMCMOT_MAX_JOINTS], elsewhere[EMCMOT_MAX_JOINTS]; + double axis[3], turned[3]; + int turns[2], count = 0, carried = -1; + int i, j, k, r; + + if (!tool || !seed || !primary || !secondary + || num_joints <= 0 || num_joints > EMCMOT_MAX_JOINTS) { + return -1; + } + *primary = *secondary = -1; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joint[i] = (i < num_joints) ? seed[i] : 0; } + + for (j = 0; j < num_joints; j++) { + r = tfs_joint_axis(tool, joint, j, TFS_PROBE_STEP, axis); + if (r < 0) { return -1; } + if (r > 0) { continue; } + if (count >= 2) { return -1; } // a wrist, not a head + turns[count++] = j; + } + if (count != 2) { return -1; } + + // whichever axis swings when the other joint moves is the carried one + for (i = 0; i < 2; i++) { + j = turns[i]; + k = turns[1 - i]; + if (tfs_joint_axis(tool, joint, j, TFS_PROBE_STEP, axis) != 0) { return -1; } + for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { elsewhere[r] = joint[r]; } + elsewhere[k] += TFS_CARRY_STEP; + if (tfs_joint_axis(tool, elsewhere, j, TFS_PROBE_STEP, turned) != 0) { return -1; } + if (fabs(axis[0]*turned[0] + axis[1]*turned[1] + axis[2]*turned[2] - 1.0) + > TFS_CARRY_TOL) { + if (carried >= 0) { return -1; } // both carried: not a head + carried = j; + } + } + if (carried < 0) { return -1; } + *secondary = carried; + *primary = (turns[0] == carried) ? turns[1] : turns[0]; + return 0; +} + int toolFrameSolve(kinsFrameFunc work, kinsFrameFunc tool, int num_joints, @@ -1040,3 +1179,568 @@ int toolFrameSolve(kinsFrameFunc work, } return found; } + +//---------------------------------------------------------------------- +// The Jacobian. See kinematics.h for what it is and which way it points. +//---------------------------------------------------------------------- + +static void kj_zero(double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) +{ + int j, a; + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } +} + +// pose coordinate a of p, in EmcPose order +static double *kj_coord(EmcPose *p, int a) +{ + switch (a) { + 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; + } +} + +int kinsJacobianFromInverse(kinsInverseFunc inverse, + int num_joints, + const double *joint, + const EmcPose *world, + const KINEMATICS_INVERSE_FLAGS *iflags, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) +{ + double qp[EMCMOT_MAX_JOINTS], qm[EMCMOT_MAX_JOINTS]; + KINEMATICS_INVERSE_FLAGS ifl = iflags ? *iflags : 0; + KINEMATICS_FORWARD_FLAGS ffl = 0; + EmcPose p; + int j, a; + + if (!inverse || !joint || !world || !jac + || num_joints <= 0 || num_joints > EMCMOT_MAX_JOINTS) { + return -1; + } + + kj_zero(jac); + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + p = *world; + // the joint array every call sees starts at the machine's own + // position, for a module that reads it before writing it + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { qp[j] = qm[j] = joint[j]; } + + *kj_coord(&p, a) += KINS_JACOBIAN_STEP; + if (inverse(&p, qp, &ifl, &ffl)) { return -1; } + + *kj_coord(&p, a) -= 2 * KINS_JACOBIAN_STEP; + if (inverse(&p, qm, &ifl, &ffl)) { return -1; } + + for (j = 0; j < num_joints; j++) { + jac[j][a] = (qp[j] - qm[j]) / (2 * KINS_JACOBIAN_STEP); + } + } + return 0; +} // kinsJacobianFromInverse() + +int kinsJacobianFromMappedAxes(int max_joints, + const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) +{ + int jno, a; + + if (!map_initialized) { + rtapi_print_msg(RTAPI_MSG_ERR, + "kinsJacobianFromMappedAxes before map_initialized\n"); + return -1; + } + if (max_joints <= 0 || max_joints > EMCMOT_MAX_JOINTS) { return -1; } + + kj_zero(jac); + + for (jno = 0; jno < max_joints; jno++) { + int bit = 1<joint_of_axis[a] = -1; + p->joints_of_axis[a] = 0; + } + p->max_joints = 0; + for (jno = 0; jno < EMCMOT_MAX_JOINTS; jno++) { + a = axis_idx_for_jno[jno]; + if (a < 0) { break; } + if (p->joint_of_axis[a] < 0) { p->joint_of_axis[a] = jno; } + p->joints_of_axis[a] |= 1 << jno; + p->max_joints = jno + 1; + } + return 0; +} // kinsParamsMapCoordinates() + +int kinsParamsInit(kins_params *p, + const kins_module_info *info, + const char *coordinates) +{ + int i; + + if (!p || !info) { return -1; } + if (info->nparams < 0 || info->nparams > KINS_MAX_PARAMS) { + rtapi_print_msg(RTAPI_MSG_ERR, + "kinsParamsInit: %s declares %d parameters, at most %d allowed\n", + info->name ? info->name : "?", info->nparams, KINS_MAX_PARAMS); + return -1; + } + + memset(p, 0, sizeof(*p)); + p->size = sizeof(*p); + p->ktype = 0; + if (!coordinates) { coordinates = info->required_coordinates; } + if (kinsParamsMapCoordinates(p, coordinates, info->max_joints, + info->allow_duplicates, + info->required_coordinates)) { + return -1; + } + for (i = 0; i < info->nparams; i++) { + p->geometry[i] = info->params[i].dflt; + if (info->params[i].tool) { p->tool.tran.z = info->params[i].dflt; } + } + return 0; +} // kinsParamsInit() + +void kinsScratchInit(kins_scratch *s) +{ + if (s) { memset(s, 0, sizeof(*s)); } +} + +int kinsMappedJointsToPose(const kins_params *p, + const double *joints, EmcPose *pos) +{ + int a; + if (!p || !joints || !pos) { return -1; } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + int j = p->joint_of_axis[a]; + if (j < 0) { continue; } + switch (a) { + case 0: pos->tran.x = joints[j]; break; + case 1: pos->tran.y = joints[j]; break; + case 2: pos->tran.z = joints[j]; break; + case 3: pos->a = joints[j]; break; + case 4: pos->b = joints[j]; break; + case 5: pos->c = joints[j]; break; + case 6: pos->u = joints[j]; break; + case 7: pos->v = joints[j]; break; + default: pos->w = joints[j]; break; + } + } + return 0; +} // kinsMappedJointsToPose() + +static double kins_pose_coord(const EmcPose *pos, int a) +{ + switch (a) { + case 0: return pos->tran.x; + case 1: return pos->tran.y; + case 2: return pos->tran.z; + case 3: return pos->a; + case 4: return pos->b; + case 5: return pos->c; + case 6: return pos->u; + case 7: return pos->v; + default: return pos->w; + } +} + +int kinsPoseToMappedJoints(const kins_params *p, + const EmcPose *pos, double *joints) +{ + int a, jno; + if (!p || !pos || !joints) { return -1; } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + int bits = p->joints_of_axis[a]; + if (!bits) { continue; } + for (jno = 0; jno < p->max_joints; jno++) { + if (bits & (1 << jno)) { joints[jno] = kins_pose_coord(pos, a); } + } + } + return 0; +} // kinsPoseToMappedJoints() + +int kinsJacobianFromMappedAxesP(const kins_params *p, + const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) +{ + int a, jno, col; + if (!p || !dP || !jac) { return -1; } + kj_zero(jac); + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + int bits = p->joints_of_axis[a]; + if (!bits) { continue; } + for (jno = 0; jno < p->max_joints; jno++) { + if (!(bits & (1 << jno))) { continue; } + for (col = 0; col < EMCMOT_MAX_AXIS; col++) { jac[jno][col] = dP[a][col]; } + } + } + return 0; +} // kinsJacobianFromMappedAxesP() + +//---------------------------------------------------------------------- +// identity through the block +//---------------------------------------------------------------------- + +int kinsIdentityForward(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)s; (void)fflags; (void)iflags; + return kinsMappedJointsToPose(p, joint, pos); +} + +int kinsIdentityInverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)s; (void)iflags; (void)fflags; + return kinsPoseToMappedJoints(p, pos, joint); +} + +int kinsIdentityFrame(const kins_params *p, const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)p; (void)joint; (void)fflags; + *rot = TOOL_FRAME_SPINDLE; + return 0; +} + +int kinsIdentityJacobian(const kins_params *p, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; + int a, b; + (void)joint; (void)pos; (void)iflags; + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + for (b = 0; b < EMCMOT_MAX_AXIS; b++) { dP[a][b] = (a == b) ? 1.0 : 0.0; } + } + return kinsJacobianFromMappedAxesP(p, (const double (*)[EMCMOT_MAX_AXIS])dP, jac); +} + +const kins_ops KINS_IDENTITY_OPS = { + .forward = kinsIdentityForward, + .inverse = kinsIdentityInverse, + .work = kinsIdentityFrame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = kinsIdentityJacobian, + .fwd_iterates = 0, + .identity = 1, +}; + +//---------------------------------------------------------------------- +// asking an ops table, defaults applied +//---------------------------------------------------------------------- + +int kinsOpsForward(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + int r; + if (!ops || !ops->forward || !p || !s) { return -1; } + if (ops->fwd_iterates && s->have_pose_seed) { + /* no pose of our own yet: start from the caller's estimate, + which stays in *pos, rather than from a never-solved seed */ + if (s->pose_seed_ok) { *pos = s->pose_seed; } + s->have_pose_seed = 0; + } + r = ops->forward(p, s, joint, pos, fflags, iflags); + if (ops->fwd_iterates && r == 0) { + /* keep the result only when the solve succeeds */ + s->pose_seed = *pos; + s->pose_seed_ok = 1; + } + return r; +} + +int kinsOpsInverse(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + if (!ops || !ops->inverse || !p || !s) { return -1; } + return ops->inverse(p, s, pos, joint, iflags, fflags); +} + +int kinsOpsWorkFrame(const kins_ops *ops, const kins_params *p, + const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + if (!ops || !p || !rot) { return -1; } + if (!ops->work) { return -1; } // not supplied; not an error + return ops->work(p, joint, rot, fflags); +} + +int kinsOpsToolFrame(const kins_ops *ops, const kins_params *p, + const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + int r; + if (!ops || !p || !rot) { return -1; } + if (!ops->tool) { return -1; } // not supplied; not an error + r = ops->tool(p, joint, rot, fflags); + if (r) { return r; } + return toolFrameApplyNative(rot, ops->native ? ops->native + : &TOOL_FRAME_SPINDLE); +} + +int kinsOpsJacobian(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + double qp[EMCMOT_MAX_JOINTS], qm[EMCMOT_MAX_JOINTS]; + KINEMATICS_INVERSE_FLAGS ifl = iflags ? *iflags : 0; + KINEMATICS_FORWARD_FLAGS ffl = 0; + EmcPose q; + int j, a; + + if (!ops || !p || !s || !joint || !pos || !jac) { return -1; } + if (ops->jacobian) { return ops->jacobian(p, joint, pos, jac, iflags); } + if (!ops->inverse) { return -1; } + + // the same differences as kinsJacobianFromInverse(), on the block form + kj_zero(jac); + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + q = *pos; + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { qp[j] = qm[j] = joint[j]; } + + *kj_coord(&q, a) += KINS_JACOBIAN_STEP; + if (ops->inverse(p, s, &q, qp, &ifl, &ffl)) { return -1; } + + *kj_coord(&q, a) -= 2 * KINS_JACOBIAN_STEP; + if (ops->inverse(p, s, &q, qm, &ifl, &ffl)) { return -1; } + + for (j = 0; j < p->max_joints && j < EMCMOT_MAX_JOINTS; j++) { + jac[j][a] = (qp[j] - qm[j]) / (2 * KINS_JACOBIAN_STEP); + } + } + return 0; +} // kinsOpsJacobian() + +//---------------------------------------------------------------------- +// the RT side of the table: one HAL pin per entry, copied into the block +// before a call and out of the scratch after it +//---------------------------------------------------------------------- + +int kinsParamsPinsCreate(int comp_id, const char *prefix, + const kins_param_desc *params, int nparams, + kins_pin_ref **out) +{ + kins_pin_ref *pins; + int i, res = 0; + + if (!out) { return -1; } + *out = NULL; + if (nparams < 0 || nparams > KINS_MAX_PARAMS) { return -1; } + if (nparams == 0) { return 0; } + if (!params || !prefix) { return -1; } + + pins = hal_malloc(nparams * sizeof(*pins)); + if (!pins) { + rtapi_print_msg(RTAPI_MSG_ERR, "kinsParamsPinsCreate: hal_malloc failed\n"); + return -1; + } + for (i = 0; i < nparams; i++) { + const kins_param_desc *d = ¶ms[i]; + hal_pdir_t dir = d->dir == KINS_OUT ? HAL_OUT : d->dir == KINS_IO ? HAL_IO : HAL_IN; + switch (d->type) { + case KINS_PARAM_FLOAT: + res += hal_pin_new_real(comp_id, dir, &pins[i].r, d->dflt, "%s.%s", prefix, d->name); + break; + case KINS_PARAM_BIT: + res += hal_pin_new_bool(comp_id, dir, &pins[i].b, d->dflt != 0, "%s.%s", prefix, d->name); + break; + case KINS_PARAM_S32: + res += hal_pin_new_si32(comp_id, dir, &pins[i].s, (rtapi_s32)d->dflt, "%s.%s", prefix, d->name); + break; + case KINS_PARAM_U32: + res += hal_pin_new_ui32(comp_id, dir, &pins[i].u, (rtapi_u32)d->dflt, "%s.%s", prefix, d->name); + break; + default: + res = -1; + } + } + if (res) { + rtapi_print_msg(RTAPI_MSG_ERR, "kinsParamsPinsCreate: pin create failed for %s\n", prefix); + return -1; + } + *out = pins; + return 0; +} // kinsParamsPinsCreate() + +void kinsParamsPinsRead(const kins_pin_ref *pins, + const kins_param_desc *params, int nparams, + kins_params *p) +{ + int i; + if (!pins || !params || !p) { return; } + for (i = 0; i < nparams && i < KINS_MAX_PARAMS; i++) { + const kins_param_desc *d = ¶ms[i]; + double v; + if (d->dir == KINS_OUT) { continue; } + switch (d->type) { + case KINS_PARAM_FLOAT: v = hal_get_real(pins[i].r); break; + case KINS_PARAM_BIT: v = hal_get_bool(pins[i].b) ? 1.0 : 0.0; break; + case KINS_PARAM_S32: v = hal_get_si32(pins[i].s); break; + case KINS_PARAM_U32: v = hal_get_ui32(pins[i].u); break; + default: v = 0; + } + p->geometry[i] = v; + if (d->tool) { p->tool.tran.z = v; } + } +} // kinsParamsPinsRead() + +void kinsParamsPinsWrite(const kins_pin_ref *pins, + const kins_param_desc *params, int nparams, + const kins_scratch *s) +{ + int i; + if (!pins || !params || !s) { return; } + for (i = 0; i < nparams && i < KINS_MAX_PARAMS; i++) { + const kins_param_desc *d = ¶ms[i]; + if (d->dir != KINS_OUT) { continue; } + switch (d->type) { + case KINS_PARAM_FLOAT: hal_set_real(pins[i].r, s->out[i]); break; + case KINS_PARAM_BIT: hal_set_bool(pins[i].b, s->out[i] != 0); break; + case KINS_PARAM_S32: hal_set_si32(pins[i].s, (rtapi_s32)s->out[i]); break; + case KINS_PARAM_U32: hal_set_ui32(pins[i].u, (rtapi_u32)s->out[i]); break; + default: break; + } + } +} // kinsParamsPinsWrite() + +void kinsToolSourceSet(kins_tool_source *src, const EmcPose *tool) +{ + if (!src || !tool) { return; } + src->tool = *tool; + src->have = 1; +} // kinsToolSourceSet() + +void kinsToolSourceApply(kins_tool_source *src, const char *prefix, + const kins_param_desc *params, int nparams, + kins_params *p) +{ + int i; + if (!src || !p || !src->have) { return; } + for (i = 0; i < nparams && i < KINS_MAX_PARAMS; i++) { + const kins_param_desc *d = ¶ms[i]; + double diff; + if (!d->tool || d->dir == KINS_OUT) { continue; } + /* a pin nobody set reads zero, which is not a disagreement */ + diff = p->geometry[i] - src->tool.tran.z; + if (p->geometry[i] != 0.0 && (diff > 1e-9 || diff < -1e-9)) { + if (src->disagreeing < 1000) { + src->disagreeing++; + } else if (!src->warned) { + rtapi_print_msg(RTAPI_MSG_ERR, + "%s.%s disagrees with the tool offset motion applies;" + " motion's is used, the pin is not needed\n", + prefix ? prefix : "kins", d->name); + src->warned = 1; + } + } else { + src->disagreeing = 0; + } + p->geometry[i] = src->tool.tran.z; + } + p->tool = src->tool; +} // kinsToolSourceApply() diff --git a/src/emc/kinematics/lineardeltakins-common.h b/src/emc/kinematics/lineardeltakins-common.h index 6e0b0037375..203343a20e1 100644 --- a/src/emc/kinematics/lineardeltakins-common.h +++ b/src/emc/kinematics/lineardeltakins-common.h @@ -32,10 +32,16 @@ // common routines used by the userspace kinematics and the realtime kinematics // user must include a math.h-type header first // Inspired by Marlin delta firmware and https://gist.github.com/kastner/5279172 +// +// The geometry is a value the caller holds and passes in, so the same +// routines serve the realtime module through its parameter block and the +// python module through its own copy. #include -static double L, R; -static double Ax, Ay, Bx, By, Cx, Cy, L2; +typedef struct { + double L, R; + double Ax, Ay, Bx, By, Cx, Cy, L2; +} lineardelta_geometry; #define SQ3 (sqrt(3)) @@ -44,31 +50,30 @@ static double Ax, Ay, Bx, By, Cx, Cy, L2; static double sq(double x) { return x*x; } -static void set_geometry(double r_, double l_) +static void lineardelta_set_geometry(lineardelta_geometry *g, double r_, double l_) { - if(L == l_ && R == r_) return; - - L = l_; - R = r_; + g->L = l_; + g->R = r_; - L2 = sq(L); + g->L2 = sq(g->L); - Ax = 0.0; - Ay = R; + g->Ax = 0.0; + g->Ay = g->R; - Bx = -SIN_60 * R; - By = -COS_60 * R; + g->Bx = -SIN_60 * g->R; + g->By = -COS_60 * g->R; - Cx = SIN_60 * R; - Cy = -COS_60 * R; + g->Cx = SIN_60 * g->R; + g->Cy = -COS_60 * g->R; } -static int kinematics_inverse(const EmcPose *pos, double *joints) +static int lineardelta_inverse(const lineardelta_geometry *g, + const EmcPose *pos, double *joints) { double x = pos->tran.x, y = pos->tran.y, z = pos->tran.z; - joints[0] = z + sqrt(L2 - sq(Ax-x) - sq(Ay-y)); - joints[1] = z + sqrt(L2 - sq(Bx-x) - sq(By-y)); - joints[2] = z + sqrt(L2 - sq(Cx-x) - sq(Cy-y)); + joints[0] = z + sqrt(g->L2 - sq(g->Ax-x) - sq(g->Ay-y)); + joints[1] = z + sqrt(g->L2 - sq(g->Bx-x) - sq(g->By-y)); + joints[2] = z + sqrt(g->L2 - sq(g->Cx-x) - sq(g->Cy-y)); joints[3] = pos->a; joints[4] = pos->b; joints[5] = pos->c; @@ -80,11 +85,14 @@ static int kinematics_inverse(const EmcPose *pos, double *joints) ? -1 : 0; } -static int kinematics_forward(const double *joints, EmcPose *pos) +static int lineardelta_forward(const lineardelta_geometry *g, + const double *joints, EmcPose *pos) { double q1 = joints[0]; double q2 = joints[1]; double q3 = joints[2]; + const double Ay = g->Ay, Bx = g->Bx, By = g->By, Cx = g->Cx, Cy = g->Cy; + const double L = g->L; double den = (By-Ay)*Cx-(Cy-Ay)*Bx; diff --git a/src/emc/kinematics/lineardeltakins.c b/src/emc/kinematics/lineardeltakins.c index 353e9234562..8aa454258d8 100644 --- a/src/emc/kinematics/lineardeltakins.c +++ b/src/emc/kinematics/lineardeltakins.c @@ -18,62 +18,109 @@ #include #include #include +#include #include "lineardeltakins-common.h" -static struct haldata -{ - hal_real_t r; - hal_real_t l; -} *haldata; +// the two lengths, one pin each +static const kins_param_desc ld_params[] = { + { "R", KINS_PARAM_FLOAT, KINS_IN, 0, DELTA_RADIUS }, + { "L", KINS_PARAM_FLOAT, KINS_IN, 0, DELTA_DIAGONAL_ROD }, +}; +enum { P_R, P_L }; static int comp_id; -int kinematicsForward(const double * joints, +// the tower positions follow from the block's two lengths +static void geometry_of(const kins_params *p, lineardelta_geometry *g) +{ + lineardelta_set_geometry(g, p->geometry[P_R], p->geometry[P_L]); +} + +static int ld_forward(const kins_params *p, kins_scratch *s, + const double * joints, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + lineardelta_geometry g; + (void)s; (void)fflags; (void)iflags; - set_geometry(hal_get_real(haldata->r), hal_get_real(haldata->l)); - return kinematics_forward(joints, pos); + geometry_of(p, &g); + return lineardelta_forward(&g, joints, pos); } -int kinematicsInverse(const EmcPose *pos, double *joints, - const KINEMATICS_INVERSE_FLAGS *iflags, - KINEMATICS_FORWARD_FLAGS *fflags) { +static int ld_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) { + lineardelta_geometry g; + (void)s; (void)iflags; (void)fflags; - set_geometry(hal_get_real(haldata->r), hal_get_real(haldata->l)); - return kinematics_inverse(pos, joints); + geometry_of(p, &g); + return lineardelta_inverse(&g, pos, joints); } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; +static int ld_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { + lineardelta_geometry g; + double x = pos->tran.x, y = pos->tran.y, z = pos->tran.z; + int i, j, a; + (void)iflags; + geometry_of(p, &g); + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + // each carriage is the platform height plus the rise of its rod, and + // the rise changes with the horizontal offset from the tower + for (i = 0; i < 3; i++) { + double tx = (i == 0) ? g.Ax : (i == 1) ? g.Bx : g.Cx; + double ty = (i == 0) ? g.Ay : (i == 1) ? g.By : g.Cy; + double rise = joints[i] - z; + if (rise <= 0) { return -1; } + jac[i][0] = (tx - x)/rise; + jac[i][1] = (ty - y)/rise; + jac[i][2] = 1; + } + for (j = 3; j < 9; j++) { jac[j][j] = 1; } + return 0; } +static const kins_ops ld_ops = { + .forward = ld_forward, + .inverse = ld_inverse, + .jacobian = ld_jacobian, +}; + +// three towers for the three linear coordinates, the rest passed +// through; the entry points come from kins_single.c +const kins_module_info kins_module = { + .name = "lineardeltakins", + .halprefix = "lineardeltakins", + .params = ld_params, + .nparams = sizeof(ld_params)/sizeof(ld_params[0]), + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &ld_ops }, +}; + int rtapi_app_main(void) { - int retval; - comp_id = hal_init("lineardeltakins"); if(comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(*haldata)); - if(!haldata) { retval = -ENOMEM; goto error; } - - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->r, DELTA_RADIUS, "lineardeltakins.R")) < 0) - goto error; - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->l, DELTA_DIAGONAL_ROD, "lineardeltakins.L")) < 0) - goto error; + if (kinsSingleInit(comp_id, "XYZABCUVW", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return retval; } void rtapi_app_exit(void) @@ -81,8 +128,4 @@ void rtapi_app_exit(void) hal_exit(comp_id); } -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); MODULE_LICENSE("GPL"); diff --git a/src/emc/kinematics/lineardeltakins.cc b/src/emc/kinematics/lineardeltakins.cc index 351746081c7..780542ba380 100644 --- a/src/emc/kinematics/lineardeltakins.cc +++ b/src/emc/kinematics/lineardeltakins.cc @@ -21,11 +21,19 @@ using namespace boost::python; #define isnan(x) std::isnan(x) #include "lineardeltakins-common.h" +// the python module keeps one geometry, set from python +static lineardelta_geometry geometry; + +static void set_geometry(double r, double l) +{ + lineardelta_set_geometry(&geometry, r, l); +} + static object forward(double j0, double j1, double j2) { double joints[9] = {j0, j1, j2}; EmcPose pos; - int result = kinematics_forward(joints, &pos); + int result = lineardelta_forward(&geometry, joints, &pos); if(result == 0) return make_tuple(pos.tran.x, pos.tran.y, pos.tran.z); return object(); @@ -35,7 +43,7 @@ static object inverse(double x, double y, double z) { double joints[9]; EmcPose pos = {{x,y,z}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - int result = kinematics_inverse(&pos, joints); + int result = lineardelta_inverse(&geometry, &pos, joints); if(result == 0) return make_tuple(joints[0], joints[1], joints[2]); return object(); @@ -43,7 +51,7 @@ static object inverse(double x, double y, double z) static object get_geometry() { - return make_tuple(R, L); + return make_tuple(geometry.R, geometry.L); } #pragma GCC diagnostic push diff --git a/src/emc/kinematics/maxkins.c b/src/emc/kinematics/maxkins.c index 773525b3566..1d72d46b54b 100644 --- a/src/emc/kinematics/maxkins.c +++ b/src/emc/kinematics/maxkins.c @@ -6,13 +6,13 @@ * * Author: Chris Radek * License: GPL Version 2 -* +* * Copyright (c) 2007 Chris Radek ********************************************************************/ /******************************************************************** -* Note: The direction of the B axis is the opposite of the -* conventional axis direction. See +* Note: The direction of the B axis is the opposite of the +* conventional axis direction. See * https://linuxcnc.org/docs/html/gcode/machining-center.html ********************************************************************/ @@ -21,6 +21,7 @@ #include #include #include /* these decls */ +#include #define d2r(d) ((d)*PM_PI/180.0) #define r2d(r) ((r)*180.0/PM_PI) @@ -29,31 +30,33 @@ #define hypot(a,b) (sqrt((a)*(a)+(b)*(b))) #endif -static struct haldata { - hal_real_t pivot_length; - hal_real_t tool_length; - hal_bool_t conventional_directions; //default is false -} *haldata; - -int kinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +// the geometry, one pin each; the maths reads it from the block +static const kins_param_desc max_params[] = { + { "pivot-length", KINS_PARAM_FLOAT, KINS_IO, 0, 0.666 }, + { "conventional-directions", KINS_PARAM_BIT, KINS_IN, 0, 0 }, // default is unconventional + { "tool-length", KINS_PARAM_FLOAT, KINS_IN, 1, 0 }, +}; +enum { P_PIVOT_LENGTH, P_CON, P_TOOL_LENGTH }; + +#define CON(p) ((p)->geometry[P_CON] != 0 ? 1.0 : -1.0) + +static int max_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); - rtapi_real tool_length = hal_get_real(haldata->tool_length); + const double con = CON(p); + const double pivot_length = p->geometry[P_PIVOT_LENGTH]; + const double tool_length = p->geometry[P_TOOL_LENGTH]; // B correction - const double zb = (pivot_length + joints[8] + tool_length) * cos(d2r(joints[4])); - const double xb = (pivot_length + joints[8] + tool_length) * sin(d2r(joints[4])); - - // C correction - const double xyr = hypot(joints[0], joints[1]); - const double xytheta = atan2(joints[1], joints[0]) + d2r(joints[5]); + const double zb = (pivot_length + tool_length + joints[8]) * cos(d2r(joints[4])); + const double xb = (pivot_length + tool_length + joints[8]) * sin(d2r(joints[4])); // U correction const double zv = joints[6] * sin(d2r(joints[4])); @@ -61,8 +64,18 @@ int kinematicsForward(const double *joints, // V correction is always in joint 1 only - pos->tran.x = xyr * cos(xytheta) - (con * xb) - xv; - pos->tran.y = xyr * sin(xytheta) - joints[7]; + // B, U and V are all machine frame: the head hangs off the Z slide and + // does not turn with the C table, so they apply before the rotation into + // the workpiece frame rather than after it. + const double mx = joints[0] - (con * xb) - xv; + const double my = joints[1] - joints[7]; + + // C correction + const double xyr = hypot(mx, my); + const double xytheta = atan2(my, mx) + d2r(joints[5]); + + pos->tran.x = xyr * cos(xytheta); + pos->tran.y = xyr * sin(xytheta); pos->tran.z = joints[2] - zb - (con * zv) + pivot_length + tool_length; pos->a = joints[3]; @@ -75,22 +88,24 @@ int kinematicsForward(const double *joints, return 0; } -int kinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int max_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); - rtapi_real tool_length = hal_get_real(haldata->tool_length); + const double con = CON(p); + const double pivot_length = p->geometry[P_PIVOT_LENGTH]; + const double tool_length = p->geometry[P_TOOL_LENGTH]; // B correction - const double zb = (pivot_length + pos->w + tool_length) * cos(d2r(pos->b)); - const double xb = (pivot_length + pos->w + tool_length) * sin(d2r(pos->b)); - + const double zb = (pivot_length + tool_length + pos->w) * cos(d2r(pos->b)); + const double xb = (pivot_length + tool_length + pos->w) * sin(d2r(pos->b)); + // C correction const double xyr = hypot(pos->tran.x, pos->tran.y); const double xytheta = atan2(pos->tran.y, pos->tran.x) - d2r(pos->c); @@ -103,7 +118,7 @@ int kinematicsInverse(const EmcPose * pos, joints[0] = xyr * cos(xytheta) + (con * xb) + xv; joints[1] = xyr * sin(xytheta) + pos->v; - joints[2] = pos->tran.z + zb - (con * zv) - pivot_length - tool_length; + joints[2] = pos->tran.z + zb + (con * zv) - pivot_length - tool_length; joints[3] = pos->a; joints[4] = pos->b; @@ -115,39 +130,84 @@ int kinematicsInverse(const EmcPose * pos, return 0; } -KINEMATICS_TYPE kinematicsType() +static int max_jacobian(const kins_params *p, const double *joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { - return KINEMATICS_BOTH; + const double con = CON(p); + const double pivot_length = p->geometry[P_PIVOT_LENGTH]; + const double k = M_PI/180; + const double sb = sin(d2r(pos->b)), cb = cos(d2r(pos->b)); + const double sc = sin(d2r(pos->c)), cc = cos(d2r(pos->c)); + const double x = pos->tran.x, y = pos->tran.y; + const double R = pivot_length + p->geometry[P_TOOL_LENGTH] + pos->w; + int j, a; + + (void)joints; + (void)iflags; + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + + // max_inverse() with the polar form expanded: rotating (x, y) by -c + // is x*cos(c) + y*sin(c) and y*cos(c) - x*sin(c), and the B and U + // corrections are what they are written as + jac[0][0] = cc; + jac[0][1] = sc; + jac[0][4] = (con * R * cb - pos->u * sb) * k; + jac[0][5] = (-x * sc + y * cc) * k; + jac[0][6] = cb; + jac[0][8] = con * sb; + + jac[1][0] = -sc; + jac[1][1] = cc; + jac[1][5] = (-x * cc - y * sc) * k; + jac[1][7] = 1; + + jac[2][2] = 1; + jac[2][4] = (-R * sb + con * pos->u * cb) * k; + jac[2][6] = con * sb; + jac[2][8] = cb; + + for (j = 3; j < 9; j++) { jac[j][j] = 1; } + return 0; } -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); +static const kins_ops max_ops = { + .forward = max_forward, + .inverse = max_inverse, + .jacobian = max_jacobian, +}; + +// joints 0..8 are X..W in order, always; the entry points come from +// kins_single.c +const kins_module_info kins_module = { + .name = "maxkins", + .halprefix = "maxkins", + .params = max_params, + .nparams = sizeof(max_params)/sizeof(max_params[0]), + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &max_ops }, +}; + MODULE_LICENSE("GPL"); static int comp_id; int rtapi_app_main(void) { - int result; comp_id = hal_init("maxkins"); if(comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(*haldata)); - if(!haldata) { result = -ENOMEM; goto error; } - - result = hal_pin_new_real(comp_id, HAL_IO, &(haldata->pivot_length), 0.666, "maxkins.pivot-length"); - result += hal_pin_new_real(comp_id, HAL_IN, &(haldata->tool_length), 0.0, "maxkins.tool-length"); - // default is unconventional - result += hal_pin_new_bool(comp_id, HAL_IN, &(haldata->conventional_directions), 0, "maxkins.conventional-directions"); - - if(result < 0) goto error; + if (kinsSingleInit(comp_id, "XYZABCUVW", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return result; } void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/src/emc/kinematics/pentakins.c b/src/emc/kinematics/pentakins.c index 18487be3134..42047ae8ac1 100644 --- a/src/emc/kinematics/pentakins.c +++ b/src/emc/kinematics/pentakins.c @@ -17,7 +17,7 @@ The default values for base and effector joints positions are defined in the header file pentakins.h. The actual values for a particular - machine can be adjusted by hal parameters: + machine can be adjusted by hal pins: pentakins.base.N.x pentakins.base.N.y @@ -45,6 +45,10 @@ pentakins.tool-offset - tool length from the origin along z axis, changes the effector pivot point. + The maths is written as pure functions of the parameter block (see + kinematics.h): the pins above are the table below, read into the block + before every call, and the entry points come from kins_single.c. + ----------------------------------------------------------------------------*/ #include @@ -52,23 +56,51 @@ #include #include #include /* these decls, KINEMATICS_FORWARD_FLAGS */ +#include #include "pentakins.h" -struct haldata { - hal_real_t basex[NUM_STRUTS]; - hal_real_t basey[NUM_STRUTS]; - hal_real_t basez[NUM_STRUTS]; - hal_real_t effectorr[NUM_STRUTS]; - hal_real_t effectorz[NUM_STRUTS]; - hal_uint_t last_iter; - hal_uint_t max_iter; - hal_uint_t iter_limit; - hal_real_t max_error; - hal_real_t conv_criterion; - hal_real_t tool_offset; -} *haldata; +// the table: five struts' worth of geometry, then the iteration controls +// and reports. P_BASE_X(i) and the rest index it. +#define P_BASE_X(i) (5*(i) + 0) +#define P_BASE_Y(i) (5*(i) + 1) +#define P_BASE_Z(i) (5*(i) + 2) +#define P_EFF_R(i) (5*(i) + 3) +#define P_EFF_Z(i) (5*(i) + 4) +enum { + P_LAST_ITER = 5*NUM_STRUTS, + P_MAX_ITER, + P_MAX_ERROR, + P_CONV_CRITERION, + P_ITER_LIMIT, + P_TOOL_OFFSET, + P_COUNT +}; + +#define STRUT_ROWS(i, bx, by, bz, er, ez) \ + { "base." #i ".x", KINS_PARAM_FLOAT, KINS_IN, 0, bx }, \ + { "base." #i ".y", KINS_PARAM_FLOAT, KINS_IN, 0, by }, \ + { "base." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, bz }, \ + { "effector." #i ".r", KINS_PARAM_FLOAT, KINS_IN, 0, er }, \ + { "effector." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, ez } + +static const kins_param_desc penta_params[P_COUNT] = { + STRUT_ROWS(0, DEFAULT_BASE_0_X, DEFAULT_BASE_0_Y, DEFAULT_BASE_0_Z, DEFAULT_EFFECTOR_0_R, DEFAULT_EFFECTOR_0_Z), + STRUT_ROWS(1, DEFAULT_BASE_1_X, DEFAULT_BASE_1_Y, DEFAULT_BASE_1_Z, DEFAULT_EFFECTOR_1_R, DEFAULT_EFFECTOR_1_Z), + STRUT_ROWS(2, DEFAULT_BASE_2_X, DEFAULT_BASE_2_Y, DEFAULT_BASE_2_Z, DEFAULT_EFFECTOR_2_R, DEFAULT_EFFECTOR_2_Z), + STRUT_ROWS(3, DEFAULT_BASE_3_X, DEFAULT_BASE_3_Y, DEFAULT_BASE_3_Z, DEFAULT_EFFECTOR_3_R, DEFAULT_EFFECTOR_3_Z), + STRUT_ROWS(4, DEFAULT_BASE_4_X, DEFAULT_BASE_4_Y, DEFAULT_BASE_4_Z, DEFAULT_EFFECTOR_4_R, DEFAULT_EFFECTOR_4_Z), + [P_LAST_ITER] = { "last-iterations", KINS_PARAM_U32, KINS_OUT, 0, 0 }, + [P_MAX_ITER] = { "max-iterations", KINS_PARAM_U32, KINS_OUT, 0, 0 }, + [P_MAX_ERROR] = { "max-error", KINS_PARAM_FLOAT, KINS_IO, 0, 100.0 }, + [P_CONV_CRITERION] = { "convergence-criterion", KINS_PARAM_FLOAT, KINS_IO, 0, 1e-9 }, + [P_ITER_LIMIT] = { "limit-iterations", KINS_PARAM_U32, KINS_IO, 0, 120 }, + [P_TOOL_OFFSET] = { "tool-offset", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, +}; +// the most iterations a converged solution has taken this session, kept +// in the caller's scratch so each caller reports its own +#define MAX_ITER_SEEN(s) ((s)->aux[0]) /******************************* MatInvert5() ***************************/ @@ -179,31 +211,29 @@ static double sqr(double x) return (x)*(x); } -/* declare arrays for base and effector coordinates */ -static PmCartesian b[NUM_STRUTS]; -static double za[NUM_STRUTS], ra[NUM_STRUTS]; +/* the base and effector geometry of one call, taken from the block */ +typedef struct { + PmCartesian b[NUM_STRUTS]; + double za[NUM_STRUTS], ra[NUM_STRUTS]; +} penta_geometry; -/************************pentakins_read_hal_pins**************************/ - -int pentakins_read_hal_pins(void) { +static void geometry_of(const kins_params *p, penta_geometry *g) { int t; - - /* set the base and effector coordinates from hal pin values */ - rtapi_real tool_offset = hal_get_real(haldata->tool_offset); + const double tool_offset = p->tool.tran.z; for (t = 0; t < NUM_STRUTS; t++) { - b[t].x = hal_get_real(haldata->basex[t]); - b[t].y = hal_get_real(haldata->basey[t]); - b[t].z = hal_get_real(haldata->basez[t]) + tool_offset; - ra[t] = hal_get_real(haldata->effectorr[t]); - za[t] = hal_get_real(haldata->effectorz[t]) + tool_offset; + g->b[t].x = p->geometry[P_BASE_X(t)]; + g->b[t].y = p->geometry[P_BASE_Y(t)]; + g->b[t].z = p->geometry[P_BASE_Z(t)] + tool_offset; + g->ra[t] = p->geometry[P_EFF_R(t)]; + g->za[t] = p->geometry[P_EFF_Z(t)] + tool_offset; } - return 0; } /************************ InvKins() ********************************/ -int InvKins(const double * coord, - double * struts) +static int InvKins(const penta_geometry *g, + const double * coord, + double * struts) { PmCartesian xyz, pmcoord, temp; @@ -211,8 +241,6 @@ int InvKins(const double * coord, PmRpy rpy; int i; -// pentakins_read_hal_pins(); - /* define Rotation Matrix */ pmcoord.x = coord[0]; pmcoord.y = coord[1]; @@ -226,32 +254,30 @@ int InvKins(const double * coord, for (i = 0; i < NUM_STRUTS; i++) { /* convert location of effector strut end from effector to world coordinates */ - pmCartCartSub(&b[i], &pmcoord, &temp); + pmCartCartSub(&g->b[i], &pmcoord, &temp); pmMatInv(&RMatrix, &InvRMatrix); pmMatCartMult(&InvRMatrix, &temp, &xyz); /* define strut lengths */ - struts[i] = sqrt( sqr(xyz.z - za[i]) + sqr( sqrt(sqr(xyz.x) + sqr(xyz.y)) - ra[i]) ); + struts[i] = sqrt( sqr(xyz.z - g->za[i]) + sqr( sqrt(sqr(xyz.x) + sqr(xyz.y)) - g->ra[i]) ); } return 0; } -/**************************** kinematicsForward() ***************************/ +/**************************** penta_forward() ***************************/ -int kinematicsForward(const double * joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int penta_forward(const kins_params *p, kins_scratch *s, + const double * joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; (void)iflags; -// PmCartesian aw; -// PmCartesian InvKinStrutVect,InvKinStrutVectUnit; -// PmCartesian q_trans, RMatrix_a, RMatrix_a_cross_Strut; - + penta_geometry g; double Jacobian[NUM_STRUTS][NUM_STRUTS]; double InverseJacobian[NUM_STRUTS][NUM_STRUTS]; double InvKinStrutLength[NUM_STRUTS], StrutLengthDiff[NUM_STRUTS]; @@ -260,14 +286,11 @@ int kinematicsForward(const double * joints, double coord[NUM_STRUTS]; double conv_err = 1.0; -// PmRotationMatrix RMatrix; -// PmRpy q_RPY; - int iterate = 1; int i, j; unsigned iteration = 0; - pentakins_read_hal_pins(); + geometry_of(p, &g); /* abort on obvious problems, like joints <= 0 */ if (joints[0] <= 0.0 || @@ -286,12 +309,15 @@ int kinematicsForward(const double * joints, coord[4] = pos->b * PM_PI / 180.0; /* Enter Newton-Raphson iterative method */ - rtapi_real max_error = hal_get_real(haldata->max_error); + const double max_error = p->geometry[P_MAX_ERROR]; + const unsigned iter_limit = (unsigned)p->geometry[P_ITER_LIMIT]; + const double conv_criterion = p->geometry[P_CONV_CRITERION]; while (iterate) { /* check for large error and return error flag if no convergence */ if ((conv_err > +(max_error)) || (conv_err < -(max_error))) { /* we can't converge */ + s->failed = 1; return -2; }; @@ -299,22 +325,23 @@ int kinematicsForward(const double * joints, /* check iteration to see if the kinematics can reach the convergence criterion and return error flag if it can't */ - if (iteration > hal_get_ui32(haldata->iter_limit)) { + if (iteration > iter_limit) { /* we can't converge */ + s->failed = 1; return -5; } /* compute StrutLengthDiff[] by running inverse kins on Cartesian estimate to get joint estimate, subtract joints to get joint deltas, and compute inv J while we're at it */ - InvKins(coord, InvKinStrutLength); + InvKins(&g, coord, InvKinStrutLength); for (i = 0; i < NUM_STRUTS; i++) { StrutLengthDiff[i] = InvKinStrutLength[i] - joints[i]; /* Build Inverse Jacobian Matrix */ coord[i] += 1e-4; - InvKins(coord, jointdelta); + InvKins(&g, coord, jointdelta); coord[i] -= 1e-4; for (j = 0; j < NUM_STRUTS; j++) { InverseJacobian[j][i] = (jointdelta[j] - InvKinStrutLength[j]) * 1e4; @@ -342,7 +369,6 @@ int kinematicsForward(const double * joints, /* enter loop to determine if a strut needs another iteration */ iterate = 0; /*assume iteration is done */ - rtapi_real conv_criterion = hal_get_real(haldata->conv_criterion); for (i = 0; i < NUM_STRUTS; i++) { if (fabs(StrutLengthDiff[i]) > conv_criterion) { iterate = 1; @@ -357,34 +383,37 @@ int kinematicsForward(const double * joints, pos->a = coord[3] * 180.0 / PM_PI; pos->b = coord[4] * 180.0 / PM_PI; - hal_set_ui32(haldata->last_iter, iteration); - - if (iteration > hal_get_ui32(haldata->max_iter)){ - hal_set_ui32(haldata->max_iter, iteration); + s->iterations = iteration; + s->failed = 0; + s->out[P_LAST_ITER] = iteration; + if (iteration > MAX_ITER_SEEN(s)) { + MAX_ITER_SEEN(s) = iteration; } + s->out[P_MAX_ITER] = MAX_ITER_SEEN(s); return 0; } -/************************ kinematicsInverse() ********************************/ +/************************ penta_inverse() ********************************/ /* the inverse kinematics take world coordinates and determine joint values, given the inverse kinematics flags to resolve any ambiguities. The forward flags are set to indicate their value appropriate to the world coordinates passed in. */ -/************************ kinematicsInverse() ********************************/ - -int kinematicsInverse(const EmcPose * pos, - double * joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int penta_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double * joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; + penta_geometry g; double coord[NUM_STRUTS]; - pentakins_read_hal_pins(); + geometry_of(p, &g); coord[0] = pos->tran.x; coord[1] = pos->tran.y; @@ -392,109 +421,125 @@ int kinematicsInverse(const EmcPose * pos, coord[3] = pos->a * PM_PI / 180.0; coord[4] = pos->b * PM_PI / 180.0; - if (0 != InvKins(coord,joints)) { + if (0 != InvKins(&g, coord, joints)) { return -1; } return 0; } -KINEMATICS_TYPE kinematicsType() +static int penta_jacobian(const kins_params *p, const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { - return KINEMATICS_BOTH; -} + penta_geometry g; + PmRotationMatrix R; + PmRpy rpy; + PmCartesian P, d, xyz, wa, wb, dxyz[5]; + int i, j, a, col; -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); + (void)joints; + (void)iflags; + geometry_of(p, &g); + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } -MODULE_LICENSE("GPL"); + /* InvKins() differentiated. The effector end of each strut is found in + effector coordinates as xyz = R^T (b - P) with R = Ry(b) Rx(a), so a + pose translation moves it by -R^T and a pose rotation about w moves + it by -R^T (w x (b - P)); the strut length is then the distance from + that point to the strut's pivot circle of radius ra at height za. */ + P = pos->tran; + rpy.r = pos->a * PM_PI / 180.0; + rpy.p = pos->b * PM_PI / 180.0; + rpy.y = 0; + pmRpyMatConvert(&rpy, &R); -int comp_id; + /* rotation axes for a and b, in world coordinates */ + wa.x = cos(rpy.p); wa.y = 0; wa.z = -sin(rpy.p); + wb.x = 0; wb.y = 1; wb.z = 0; -static const rtapi_real init_basex[NUM_STRUTS] = { - DEFAULT_BASE_0_X, DEFAULT_BASE_1_X, DEFAULT_BASE_2_X, DEFAULT_BASE_3_X, DEFAULT_BASE_4_X -}; -static const rtapi_real init_basey[NUM_STRUTS] = { - DEFAULT_BASE_0_Y, DEFAULT_BASE_1_Y, DEFAULT_BASE_2_Y, DEFAULT_BASE_3_Y, DEFAULT_BASE_4_Y -}; -static const rtapi_real init_basez[NUM_STRUTS] = { - DEFAULT_BASE_0_Z, DEFAULT_BASE_1_Z, DEFAULT_BASE_2_Z, DEFAULT_BASE_3_Z, DEFAULT_BASE_4_Z -}; -static const rtapi_real init_effectorr[NUM_STRUTS] = { - DEFAULT_EFFECTOR_0_R, DEFAULT_EFFECTOR_1_R, DEFAULT_EFFECTOR_2_R, DEFAULT_EFFECTOR_3_R, DEFAULT_EFFECTOR_4_R + for (i = 0; i < NUM_STRUTS; i++) { + double rho, A, B, len; + + pmCartCartSub(&g.b[i], &P, &d); + /* R^T d, written out since pmMatCartMult applies R */ + xyz.x = R.x.x*d.x + R.x.y*d.y + R.x.z*d.z; + xyz.y = R.y.x*d.x + R.y.y*d.y + R.y.z*d.z; + xyz.z = R.z.x*d.x + R.z.y*d.y + R.z.z*d.z; + + /* d xyz / d pose, one PmCartesian per pose column x y z a b */ + for (col = 0; col < 3; col++) { + /* -R^T e_col, which is minus row col of R^T, i.e. minus column + col of R read as a row of R^T */ + PmCartesian e = {0, 0, 0}, w; + if (col == 0) e.x = 1; else if (col == 1) e.y = 1; else e.z = 1; + w.x = -(R.x.x*e.x + R.x.y*e.y + R.x.z*e.z); + w.y = -(R.y.x*e.x + R.y.y*e.y + R.y.z*e.z); + w.z = -(R.z.x*e.x + R.z.y*e.y + R.z.z*e.z); + dxyz[col] = w; + } + for (col = 3; col < 5; col++) { + PmCartesian cr, w; + pmCartCartCross(col == 3 ? &wa : &wb, &d, &cr); + w.x = -(R.x.x*cr.x + R.x.y*cr.y + R.x.z*cr.z) * (PM_PI/180.0); + w.y = -(R.y.x*cr.x + R.y.y*cr.y + R.y.z*cr.z) * (PM_PI/180.0); + w.z = -(R.z.x*cr.x + R.z.y*cr.y + R.z.z*cr.z) * (PM_PI/180.0); + dxyz[col] = w; + } + + rho = sqrt(sqr(xyz.x) + sqr(xyz.y)); + A = xyz.z - g.za[i]; + B = rho - g.ra[i]; + len = sqrt(sqr(A) + sqr(B)); + if (len <= 0 || rho <= 0) { return -1; } + for (col = 0; col < 5; col++) { + jac[i][col] = (A*dxyz[col].z + + B*(xyz.x*dxyz[col].x + xyz.y*dxyz[col].y)/rho) / len; + } + } + return 0; +} + +// the forward iterates from the pose it is handed +static const kins_ops penta_ops = { + .forward = penta_forward, + .inverse = penta_inverse, + .jacobian = penta_jacobian, + .fwd_iterates = 1, }; -static const rtapi_real init_effectorz[NUM_STRUTS] = { - DEFAULT_EFFECTOR_0_Z, DEFAULT_EFFECTOR_1_Z, DEFAULT_EFFECTOR_2_Z, DEFAULT_EFFECTOR_3_Z, DEFAULT_EFFECTOR_4_Z + +const kins_module_info kins_module = { + .name = "pentakins", + .halprefix = "pentakins", + .params = penta_params, + .nparams = P_COUNT, + .required_coordinates = "XYZAB", + .max_joints = NUM_STRUTS, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &penta_ops }, }; +MODULE_LICENSE("GPL"); + +int comp_id; + int rtapi_app_main(void) { - int res = 0, i; - comp_id = hal_init("pentakins"); if (comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) - goto error; - - - for (i = 0; i < NUM_STRUTS; i++) { - - if ((res = hal_param_new_real(comp_id, HAL_RW, &(haldata->basex[i]), - init_basex[i], "pentakins.base.%d.x", i)) < 0) - goto error; - - if ((res = hal_param_new_real(comp_id, HAL_RW, &haldata->basey[i], - init_basey[i], "pentakins.base.%d.y", i)) < 0) - goto error; - - if ((res = hal_param_new_real(comp_id, HAL_RW, &haldata->basez[i], - init_basez[i], "pentakins.base.%d.z", i)) < 0) - goto error; - - if ((res = hal_param_new_real(comp_id, HAL_RW, &haldata->effectorr[i], - init_effectorr[i], "pentakins.effector.%d.r", i)) < 0) - goto error; - - if ((res = hal_param_new_real(comp_id, HAL_RW, &haldata->effectorz[i], - init_effectorz[i], "pentakins.effector.%d.z", i)) < 0) - goto error; + if (kinsSingleInit(comp_id, "XYZAB", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; } - if ((res = hal_pin_new_ui32(comp_id, HAL_OUT, &haldata->last_iter, - 0, "pentakins.last-iterations")) < 0) - goto error; - - if ((res = hal_pin_new_ui32(comp_id, HAL_OUT, &haldata->max_iter, - 0, "pentakins.max-iterations")) < 0) - goto error; - - if ((res = hal_pin_new_real(comp_id, HAL_IO, &haldata->max_error, - 100.0, "pentakins.max-error")) < 0) - goto error; - - if ((res = hal_pin_new_real(comp_id, HAL_IO, &haldata->conv_criterion, - 1e-9, "pentakins.convergence-criterion")) < 0) - goto error; - - if ((res = hal_pin_new_ui32(comp_id, HAL_IO, &haldata->iter_limit, - 120, "pentakins.limit-iterations")) < 0) - goto error; - - if ((res = hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset, - 0.0, "pentakins.tool-offset")) < 0) - goto error; - hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return res; } diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index 98dc83aa4c5..8985ec4b5ac 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -20,14 +20,29 @@ #include #include #include -#include #include "pumakins.h" -#include "switchkins.h" - -struct haldata { - hal_real_t a2, a3, d3, d4, d6; -} *haldata = NULL; +#include + +// the five dimensions, one pin each; the maths reads them from the block +static const kins_param_desc puma_params[] = { + { "A2", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PUMA560_A2 }, + { "A3", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PUMA560_A3 }, + { "D3", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PUMA560_D3 }, + { "D4", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PUMA560_D4 }, + { "D6", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PUMA560_D6 }, +}; +enum { P_A2, P_A3, P_D3, P_D4, P_D6 }; + +/* the difference of two angles, brought into (-pi, pi] so that a joint a + whole turn from the formula still matches it */ +static double angleDiff(double a, double b) +{ + double d = a - b; + while (d > PM_PI) { d -= 2*PM_PI; } + while (d <= -PM_PI) { d += 2*PM_PI; } + return d; +} /* The flange orientation for a joint set: the ISO 9787 mechanical interface frame, whose z points out of the interface towards the work. Shared by the @@ -98,11 +113,13 @@ static void pumaFlangeRotation(const double * joint, PmRotationMatrix * rot) *rot = hom.rot; } // pumaFlangeRotation() -static int pumaKinematicsForward(const double * joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int puma_forward(const kins_params *p, kins_scratch *s, + const double * joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; double s1, s2, s3; double c1, c2, c3; @@ -126,10 +143,10 @@ static int pumaKinematicsForward(const double * joint, s23 = c2 * s3 + s2 * c3; c23 = c2 * c3 - s2 * s3; - rtapi_real PUMA_A2 = hal_get_real(haldata->a2); - rtapi_real PUMA_A3 = hal_get_real(haldata->a3); - rtapi_real PUMA_D3 = hal_get_real(haldata->d3); - rtapi_real PUMA_D4 = hal_get_real(haldata->d4); + const double PUMA_A2 = p->geometry[P_A2]; + const double PUMA_A3 = p->geometry[P_A3]; + const double PUMA_D3 = p->geometry[P_D3]; + const double PUMA_D4 = p->geometry[P_D4]; /* Calculate term to be used in definition of... */ /* position vector. */ @@ -152,16 +169,16 @@ static int pumaKinematicsForward(const double * joint, *iflags = 0; /* Set shoulder-up flag if necessary */ - if (fabs(joint[0]*PM_PI/180 - atan2(hom.tran.y, hom.tran.x) + - atan2(PUMA_D3, -sqrt(sumSq))) < FLAG_FUZZ) + if (fabs(angleDiff(joint[0]*PM_PI/180, atan2(hom.tran.y, hom.tran.x) - + atan2(PUMA_D3, -sqrt(sumSq)))) < FLAG_FUZZ) { *iflags |= PUMA_SHOULDER_RIGHT; } /* Set elbow down flag if necessary */ - if (fabs(joint[2]*PM_PI/180 - atan2(PUMA_A3, PUMA_D4) + + if (fabs(angleDiff(joint[2]*PM_PI/180, atan2(PUMA_A3, PUMA_D4) - atan2(k, -sqrt(PUMA_A3 * PUMA_A3 + - PUMA_D4 * PUMA_D4 - k * k))) < FLAG_FUZZ) + PUMA_D4 * PUMA_D4 - k * k)))) < FLAG_FUZZ) { *iflags |= PUMA_ELBOW_DOWN; } @@ -177,12 +194,12 @@ static int pumaKinematicsForward(const double * joint, /* if not singular set wrist flip flag if necessary */ else{ - if (! (fabs(joint[3]*PM_PI/180 - atan2(t1, t2)) < FLAG_FUZZ)) + if (! (fabs(angleDiff(joint[3]*PM_PI/180, atan2(t1, t2))) < FLAG_FUZZ)) { *iflags |= PUMA_WRIST_FLIP; } } - rtapi_real PUMA_D6 = hal_get_real(haldata->d6); + const double PUMA_D6 = p->geometry[P_D6]; /* add effect of d6 parameter */ hom.tran.x = hom.tran.x + hom.rot.z.x*PUMA_D6; hom.tran.y = hom.tran.y + hom.rot.z.y*PUMA_D6; @@ -201,32 +218,25 @@ static int pumaKinematicsForward(const double * joint, return 0; } -static int pumaKinematicsToolFrame(const double * joint, - PmRotationMatrix * rot, - const KINEMATICS_FORWARD_FLAGS * fflags) +static int puma_tool_frame(const kins_params *p, const double * joint, + PmRotationMatrix * rot, + const KINEMATICS_FORWARD_FLAGS * fflags) { + (void)p; (void)fflags; - // answers in the flange frame; switchkins applies the declared half turn + // answers in the flange frame; the declared half turn is applied by + // the shared code pumaFlangeRotation(joint, rot); return 0; -} // pumaKinematicsToolFrame() - -static int pumaKinematicsWorkFrame(const double * joint, - PmRotationMatrix * rot, - const KINEMATICS_FORWARD_FLAGS * fflags) -{ - (void)joint; - (void)fflags; - // the arm carries the tool and nothing carries the work - *rot = TOOL_FRAME_SPINDLE; - return 0; -} // pumaKinematicsWorkFrame() +} // puma_tool_frame() -static int pumaKinematicsInverse(const EmcPose * world, - double * joint, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int puma_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * world, + double * joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; PmHomogeneous hom; PmPose worldPose; PmRpy rpy; @@ -262,11 +272,11 @@ static int pumaKinematicsInverse(const EmcPose * world, pmRpyQuatConvert(&rpy,&worldPose.rot); pmPoseHomConvert(&worldPose, &hom); - rtapi_real PUMA_A2 = hal_get_real(haldata->a2); - rtapi_real PUMA_A3 = hal_get_real(haldata->a3); - rtapi_real PUMA_D3 = hal_get_real(haldata->d3); - rtapi_real PUMA_D4 = hal_get_real(haldata->d4); - rtapi_real PUMA_D6 = hal_get_real(haldata->d6); + const double PUMA_A2 = p->geometry[P_A2]; + const double PUMA_A3 = p->geometry[P_A3]; + const double PUMA_D3 = p->geometry[P_D3]; + const double PUMA_D4 = p->geometry[P_D4]; + const double PUMA_D6 = p->geometry[P_D6]; /* remove effect of d6 parameter */ px = hom.tran.x - PUMA_D6*hom.rot.z.x; @@ -379,29 +389,19 @@ static int pumaKinematicsInverse(const EmcPose * world, return 0; } -int pumaKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - (void)coordinates; - int res=0; - - haldata = hal_malloc(sizeof(*haldata)); - if (!haldata) goto error; - - - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a2), DEFAULT_PUMA560_A2, "%s.A2", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a3), DEFAULT_PUMA560_A3, "%s.A3", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d3), DEFAULT_PUMA560_D3, "%s.D3", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d4), DEFAULT_PUMA560_D4, "%s.D4", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d6), DEFAULT_PUMA560_D6, "%s.D6", kp->halprefix); - if (res) { goto error; } - - return 0; - -error: - return -1; -} // pumaKinematicsSetup() +// the arm carries the tool and nothing carries the work, so the work frame +// is the shared identity one. The maths is the ISO 9787 flange frame, so +// the tool axis it produces runs holder towards tip, the opposite of the +// convention; the declared half turn puts it right. No closed form +// Jacobian: the shared code differences the inverse. +static const kins_ops puma_ops = { + .forward = puma_forward, + .inverse = puma_inverse, + .work = kinsIdentityFrame, + .tool = puma_tool_frame, + .native = &TOOL_FRAME_FLANGE, + .primary = 1, +}; int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -409,49 +409,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "pumakins"; // !!! must agree with filename kp->halprefix = "pumakins"; // hal pin names kp->required_coordinates = "xyzabc"; kp->allow_duplicates = 0; kp->max_joints = strlen(kp->required_coordinates); + kp->params = puma_params; + kp->nparams = sizeof(puma_params)/sizeof(puma_params[0]); if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = pumaKinematicsSetup; - *kfwd1 = pumaKinematicsForward; - *kinv1 = pumaKinematicsInverse; - // the maths is the ISO 9787 flange frame, so the tool axis it produces - // runs holder towards tip, the opposite of the convention - switchkinsRegisterFrames(1, pumaKinematicsWorkFrame, - pumaKinematicsToolFrame, - &TOOL_FRAME_FLANGE); - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &puma_ops); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = pumaKinematicsSetup; - *kfwd0 = pumaKinematicsForward; - *kinv0 = pumaKinematicsInverse; - // the maths is the ISO 9787 flange frame, so the tool axis it produces - // runs holder towards tip, the opposite of the convention - switchkinsRegisterFrames(0, pumaKinematicsWorkFrame, - pumaKinematicsToolFrame, - &TOOL_FRAME_FLANGE); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &puma_ops); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } // switchkinsSetup() diff --git a/src/emc/kinematics/rosekins.c b/src/emc/kinematics/rosekins.c index adfe763a33a..1622542154f 100644 --- a/src/emc/kinematics/rosekins.c +++ b/src/emc/kinematics/rosekins.c @@ -21,28 +21,36 @@ #include #include #include +#include -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); MODULE_LICENSE("GPL"); #ifndef hypot #define hypot(a,b) (sqrt((a)*(a)+(b)*(b))) #endif -static struct haldata { - hal_real_t revolutions; - hal_real_t theta_degrees; - hal_real_t bigtheta_degrees; -} *haldata; - -int kinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +// the inverse reports the turn count it keeps and the angles it saw +static const kins_param_desc rose_params[] = { + { "revolutions", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + { "theta_degrees", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + { "bigtheta_degrees", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, +}; +enum { O_REVOLUTIONS, O_THETA, O_BIGTHETA }; + +// what the inverse carries from one call to the next: the quadrant it +// last saw and the turns it has counted. In the scratch, so that each +// caller counts its own. +#define OLDQUAD(s) ((s)->aux[0]) +#define REVOLUTIONS(s) ((s)->aux[1]) + +static int rose_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)p; + (void)s; (void)fflags; (void)iflags; double radius,z,theta; @@ -64,18 +72,20 @@ int kinematicsForward(const double *joints, return 0; } -int kinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int rose_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)p; (void)iflags; (void)fflags; // There is a potential problem when accumulating bigtheta -- loss of // precision based on size of mantissa -- but in practice, it is probably ok - static int oldquad; - static int revolutions; + int oldquad = (int)OLDQUAD(s); + int revolutions = (int)REVOLUTIONS(s); double theta,bigtheta; int nowquad = 0; @@ -94,9 +104,9 @@ int kinematicsInverse(const EmcPose * pos, theta = atan2(y,x); bigtheta = theta + PM_2_PI * revolutions; - hal_set_real(haldata->revolutions, revolutions); - hal_set_real(haldata->theta_degrees, theta * TO_DEG); - hal_set_real(haldata->bigtheta_degrees, bigtheta * TO_DEG); + s->out[O_REVOLUTIONS] = revolutions; + s->out[O_THETA] = theta * TO_DEG; + s->out[O_BIGTHETA] = bigtheta * TO_DEG; joints[0] = hypot(x,y); joints[1] = z; @@ -108,38 +118,68 @@ int kinematicsInverse(const EmcPose * pos, joints[7] = 0; joints[8] = 0; - oldquad = nowquad; + OLDQUAD(s) = nowquad; + REVOLUTIONS(s) = revolutions; return 0; } -KINEMATICS_TYPE kinematicsType() +static int rose_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { - return KINEMATICS_BOTH; + double x = pos->tran.x, y = pos->tran.y; + double r2 = x*x + y*y; + double r = sqrt(r2); + int j, a; + (void)p; + (void)joints; + (void)iflags; + // on the axis the angle is undefined and its rate unbounded + if (r2 <= 0) { return -1; } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + jac[0][0] = x/r; jac[0][1] = y/r; + jac[1][2] = 1; + jac[2][0] = -y/r2 * TO_DEG; + jac[2][1] = x/r2 * TO_DEG; + return 0; } +static const kins_ops rose_ops = { + .forward = rose_forward, + .inverse = rose_inverse, + .jacobian = rose_jacobian, +}; + +// joints 0..2 are radius, z and the unwrapped angle; the entry points +// come from kins_single.c +const kins_module_info kins_module = { + .name = "rosekins", + .halprefix = "rosekins", + .params = rose_params, + .nparams = sizeof(rose_params)/sizeof(rose_params[0]), + .required_coordinates = "XYZ", + .max_joints = 3, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &rose_ops }, +}; + static int comp_id; void rtapi_app_exit(void) { hal_exit(comp_id); } int rtapi_app_main(void) { - int ans; comp_id = hal_init("rosekins"); if(comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(*haldata)); - if(!haldata) { ans = -ENOMEM; goto error; } - - if((ans = hal_pin_new_real(comp_id, HAL_OUT, &(haldata->revolutions), 0.0, "rosekins.revolutions")) < 0) - goto error; - if((ans = hal_pin_new_real(comp_id, HAL_OUT, &(haldata->theta_degrees), 0.0, "rosekins.theta_degrees")) < 0) - goto error; - if((ans = hal_pin_new_real(comp_id, HAL_OUT, &(haldata->bigtheta_degrees), 0.0, "rosekins.bigtheta_degrees")) < 0) - goto error; + if (kinsSingleInit(comp_id, "XYZ", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return ans; } diff --git a/src/emc/kinematics/rotarydeltakins-common.h b/src/emc/kinematics/rotarydeltakins-common.h index 59cc872200c..95c6ce95c9e 100644 --- a/src/emc/kinematics/rotarydeltakins-common.h +++ b/src/emc/kinematics/rotarydeltakins-common.h @@ -40,6 +40,10 @@ positive, the Z coordinate will get more negative. Joint zero is the one whose thigh swings in the YZ plane. + + The geometry is a value the caller holds and passes in, so the same + routines serve the realtime module through its parameter block and the + python module through its own copy. */ #ifndef LINUXCNCROTARYDELTAKINS_COMMON_H @@ -47,17 +51,19 @@ #include -// distance from origin to a hip joint -static double platformradius; +typedef struct { + // distance from origin to a hip joint + double platformradius; -// thigh connects the hip to the knee -static double thighlength; + // thigh connects the hip to the knee + double thighlength; -// shin (the parallelogram) connects the knee to the foot -static double shinlength; + // shin (the parallelogram) connects the knee to the foot + double shinlength; -// distance from center of foot (controlled point) to an ankle joint -static double footradius; + // distance from center of foot (controlled point) to an ankle joint + double footradius; +} rotarydelta_geometry; #ifndef sq #define sq(a) ((a)*(a)) @@ -66,15 +72,21 @@ static double footradius; #define D2R(d) ((d)*M_PI/180.) #endif -static void set_geometry(double pfr, double tl, double sl, double fr) { - platformradius = pfr; - thighlength = tl; - shinlength = sl; - footradius = fr; +static void rotarydelta_set_geometry(rotarydelta_geometry *g, + double pfr, double tl, double sl, double fr) { + g->platformradius = pfr; + g->thighlength = tl; + g->shinlength = sl; + g->footradius = fr; } // Given three hip joint angles, find the controlled point -static int kinematics_forward(const double *joints, EmcPose *pos) { +static int rotarydelta_forward(const rotarydelta_geometry *g, + const double *joints, EmcPose *pos) { + const double platformradius = g->platformradius; + const double thighlength = g->thighlength; + const double shinlength = g->shinlength; + const double footradius = g->footradius; double j0 = joints[0], j1 = joints[1], @@ -139,7 +151,12 @@ static int kinematics_forward(const double *joints, EmcPose *pos) { // Given controlled point, find joint zero's angle // (J0 is the easy one in the ZY plane) -static int inverse_j0(double x, double y, double z, double *theta) { +static int rotarydelta_inverse_j0(const rotarydelta_geometry *g, + double x, double y, double z, double *theta) { + const double platformradius = g->platformradius; + const double thighlength = g->thighlength; + const double shinlength = g->shinlength; + const double footradius = g->footradius; double a, b, d, knee_y, knee_z; a = 0.5 * (sq(x) + sq(y - footradius) + sq(z) + sq(thighlength) - @@ -157,25 +174,26 @@ static int inverse_j0(double x, double y, double z, double *theta) { return 0; } -static void rotate(double *x, double *y, double theta) { +static void rotarydelta_rotate(double *x, double *y, double theta) { double xx, yy; xx = *x, yy = *y; *x = xx * cos(theta) - yy * sin(theta); *y = xx * sin(theta) + yy * cos(theta); } -static int kinematics_inverse(const EmcPose *pos, double *joints) { +static int rotarydelta_inverse(const rotarydelta_geometry *g, + const EmcPose *pos, double *joints) { double xr, yr; - if(inverse_j0(pos->tran.x, pos->tran.y, pos->tran.z, &joints[0])) return -1; + if(rotarydelta_inverse_j0(g, pos->tran.x, pos->tran.y, pos->tran.z, &joints[0])) return -1; // now use symmetry property to get the other two just as easily... xr = pos->tran.x; yr = pos->tran.y; - rotate(&xr, &yr, -2*M_PI/3); - if(inverse_j0(xr, yr, pos->tran.z, &joints[1])) return -1; + rotarydelta_rotate(&xr, &yr, -2*M_PI/3); + if(rotarydelta_inverse_j0(g, xr, yr, pos->tran.z, &joints[1])) return -1; xr = pos->tran.x; yr = pos->tran.y; - rotate(&xr, &yr, 2*M_PI/3); - if(inverse_j0(xr, yr, pos->tran.z, &joints[2])) return -1; + rotarydelta_rotate(&xr, &yr, 2*M_PI/3); + if(rotarydelta_inverse_j0(g, xr, yr, pos->tran.z, &joints[2])) return -1; joints[3] = pos->a; joints[4] = pos->b; diff --git a/src/emc/kinematics/rotarydeltakins.c b/src/emc/kinematics/rotarydeltakins.c index 8c83ebdec4f..4cee9c38173 100644 --- a/src/emc/kinematics/rotarydeltakins.c +++ b/src/emc/kinematics/rotarydeltakins.c @@ -19,68 +19,137 @@ #include #include #include +#include #include "rotarydeltakins-common.h" -static struct haldata -{ - hal_real_t pfr; - hal_real_t tl; - hal_real_t sl; - hal_real_t fr; -} *haldata; +// the four lengths, one pin each +static const kins_param_desc rd_params[] = { + { "platformradius", KINS_PARAM_FLOAT, KINS_IN, 0, RDELTA_PFR }, + { "thighlength", KINS_PARAM_FLOAT, KINS_IN, 0, RDELTA_TL }, + { "shinlength", KINS_PARAM_FLOAT, KINS_IN, 0, RDELTA_SL }, + { "footradius", KINS_PARAM_FLOAT, KINS_IN, 0, RDELTA_FR }, +}; +enum { P_PFR, P_TL, P_SL, P_FR }; static int comp_id; -int kinematicsForward(const double * joints, +static void geometry_of(const kins_params *p, rotarydelta_geometry *g) +{ + rotarydelta_set_geometry(g, p->geometry[P_PFR], p->geometry[P_TL], + p->geometry[P_SL], p->geometry[P_FR]); +} + +static int rd_forward(const kins_params *p, kins_scratch *s, + const double * joints, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + rotarydelta_geometry g; + (void)s; (void)fflags; (void)iflags; - set_geometry(hal_get_real(haldata->pfr), hal_get_real(haldata->tl), hal_get_real(haldata->sl), hal_get_real(haldata->fr)); - return kinematics_forward(joints, pos); + geometry_of(p, &g); + return rotarydelta_forward(&g, joints, pos); } -int kinematicsInverse(const EmcPose *pos, double *joints, - const KINEMATICS_INVERSE_FLAGS *iflags, - KINEMATICS_FORWARD_FLAGS *fflags) { +static int rd_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) { + rotarydelta_geometry g; + (void)s; (void)iflags; (void)fflags; - set_geometry(hal_get_real(haldata->pfr), hal_get_real(haldata->tl), hal_get_real(haldata->sl), hal_get_real(haldata->fr)); - return kinematics_inverse(pos, joints); + geometry_of(p, &g); + return rotarydelta_inverse(&g, pos, joints); } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; +static int rd_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { + rotarydelta_geometry g; + int i, j, a; + (void)iflags; + geometry_of(p, &g); + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + // The foot stays a shin length from each knee, so along a leg the + // motion of the foot and the motion of the knee agree: + // (P - K) . dP = (P - K) . dK/dq dq + // K is the knee less the foot offset, written as rotarydelta_forward() + // writes it, and q the hip angle that swings it. + for (i = 0; i < 3; i++) { + double q = D2R(joints[i]); + double reach = g.platformradius - g.footradius + g.thighlength * cos(q); + double kx, ky, kz, dkx, dky, dkz, px, py, pz, denom; + switch (i) { + case 0: + kx = 0; ky = -reach; + dkx = 0; dky = g.thighlength * sin(q); + break; + case 1: + kx = reach * 0.5 * sqrt(3); ky = reach * 0.5; + dkx = -g.thighlength * sin(q) * 0.5 * sqrt(3); + dky = -g.thighlength * sin(q) * 0.5; + break; + default: + kx = -reach * 0.5 * sqrt(3); ky = reach * 0.5; + dkx = g.thighlength * sin(q) * 0.5 * sqrt(3); + dky = -g.thighlength * sin(q) * 0.5; + break; + } + kz = -g.thighlength * sin(q); + dkz = -g.thighlength * cos(q); + px = pos->tran.x - kx; + py = pos->tran.y - ky; + pz = pos->tran.z - kz; + denom = (px*dkx + py*dky + pz*dkz) * (M_PI/180.); + // the shin at right angles to the thigh's swing: the knee cannot + // move the foot, so no finite hip rate follows the foot + if (fabs(denom) < 1e-12) { return -1; } + jac[i][0] = px/denom; + jac[i][1] = py/denom; + jac[i][2] = pz/denom; + } + for (j = 3; j < 9; j++) { jac[j][j] = 1; } + return 0; } +static const kins_ops rd_ops = { + .forward = rd_forward, + .inverse = rd_inverse, + .jacobian = rd_jacobian, +}; + +// three hips for the three linear coordinates, the rest passed through; +// the entry points come from kins_single.c +const kins_module_info kins_module = { + .name = "rotarydeltakins", + .halprefix = "rotarydeltakins", + .params = rd_params, + .nparams = sizeof(rd_params)/sizeof(rd_params[0]), + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &rd_ops }, +}; + int rtapi_app_main(void) { - int retval; - comp_id = hal_init("rotarydeltakins"); if(comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(*haldata)); - if(!haldata) { retval = -ENOMEM; goto error; } - - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->pfr, RDELTA_PFR, "rotarydeltakins.platformradius")) < 0) - goto error; - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->tl, RDELTA_TL, "rotarydeltakins.thighlength")) < 0) - goto error; - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->sl, RDELTA_SL, "rotarydeltakins.shinlength")) < 0) - goto error; - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->fr, RDELTA_FR, "rotarydeltakins.footradius")) < 0) - goto error; + if (kinsSingleInit(comp_id, "XYZABCUVW", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return retval; } void rtapi_app_exit(void) @@ -88,8 +157,4 @@ void rtapi_app_exit(void) hal_exit(comp_id); } -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); MODULE_LICENSE("GPL"); diff --git a/src/emc/kinematics/rotarydeltakins.cc b/src/emc/kinematics/rotarydeltakins.cc index 49a26b3153e..a8227573ab9 100644 --- a/src/emc/kinematics/rotarydeltakins.cc +++ b/src/emc/kinematics/rotarydeltakins.cc @@ -20,11 +20,19 @@ #include using namespace boost::python; +// the python module keeps one geometry, set from python +static rotarydelta_geometry geometry; + +static void set_geometry(double pfr, double tl, double sl, double fr) +{ + rotarydelta_set_geometry(&geometry, pfr, tl, sl, fr); +} + static object forward(double j0, double j1, double j2) { double joints[9] = {j0, j1, j2}; EmcPose pos; - int result = kinematics_forward(joints, &pos); + int result = rotarydelta_forward(&geometry, joints, &pos); if(result == 0) return make_tuple(pos.tran.x, pos.tran.y, pos.tran.z); return object(); @@ -34,7 +42,7 @@ static object inverse(double x, double y, double z) { double joints[9]; EmcPose pos = {{x,y,z}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - int result = kinematics_inverse(&pos, joints); + int result = rotarydelta_inverse(&geometry, &pos, joints); if(result == 0) return make_tuple(joints[0], joints[1], joints[2]); return object(); @@ -42,7 +50,8 @@ static object inverse(double x, double y, double z) static object get_geometry() { - return make_tuple(platformradius, thighlength, shinlength, footradius); + return make_tuple(geometry.platformradius, geometry.thighlength, + geometry.shinlength, geometry.footradius); } #pragma GCC diagnostic push diff --git a/src/emc/kinematics/rotatekins.c b/src/emc/kinematics/rotatekins.c index 838c9178154..6fe38d8c11a 100644 --- a/src/emc/kinematics/rotatekins.c +++ b/src/emc/kinematics/rotatekins.c @@ -7,7 +7,7 @@ * Author: Chris Radek * License: GPL Version 2 * System: Linux -* +* * Copyright (c) 2006 All rights reserved. * ********************************************************************/ @@ -17,12 +17,16 @@ #include #include #include /* these decls */ +#include -int kinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int rotate_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)p; + (void)s; (void)fflags; (void)iflags; double c_rad = -joints[5]*M_PI/180; @@ -39,11 +43,14 @@ int kinematicsForward(const double *joints, return 0; } -int kinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int rotate_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)p; + (void)s; (void)iflags; (void)fflags; double c_rad = pos->c*M_PI/180; @@ -60,37 +67,64 @@ int kinematicsInverse(const EmcPose * pos, return 0; } -/* implemented for these kinematics as giving joints preference */ -int kinematicsHome(EmcPose * world, - double *joint, - KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int rotate_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { - *fflags = 0; - *iflags = 0; - - return kinematicsForward(joint, world, fflags, iflags); + double c_rad = pos->c*M_PI/180; + double cc = cos(c_rad), sc = sin(c_rad); + int j, a; + (void)p; + (void)joints; + (void)iflags; + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + // the inverse above, differentiated: the rotation itself for x and y, + // and the rotated point turned a quarter turn for c + jac[0][0] = cc; jac[0][1] = -sc; + jac[0][5] = (-pos->tran.x*sc - pos->tran.y*cc) * (M_PI/180); + jac[1][0] = sc; jac[1][1] = cc; + jac[1][5] = ( pos->tran.x*cc - pos->tran.y*sc) * (M_PI/180); + for (j = 2; j < 9; j++) { jac[j][j] = 1; } + return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +static const kins_ops rotate_ops = { + .forward = rotate_forward, + .inverse = rotate_inverse, + .jacobian = rotate_jacobian, +}; + +// no geometry; joints 0..8 are the nine letters in order, and the entry +// points come from kins_single.c +const kins_module_info kins_module = { + .name = "rotatekins", + .halprefix = "rotatekins", + .params = NULL, + .nparams = 0, + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &rotate_ops }, +}; -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); MODULE_LICENSE("GPL"); int comp_id; int rtapi_app_main(void) { comp_id = hal_init("rotatekins"); - if(comp_id > 0) { - hal_ready(comp_id); - return 0; + if(comp_id < 0) return comp_id; + + if (kinsSingleInit(comp_id, "XYZABCUVW", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; } - return comp_id; + + hal_ready(comp_id); + return 0; } void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/src/emc/kinematics/scarakins.c b/src/emc/kinematics/scarakins.c index 8db57eb7b0c..3092f33e189 100644 --- a/src/emc/kinematics/scarakins.c +++ b/src/emc/kinematics/scarakins.c @@ -19,13 +19,8 @@ #include #include #include -#include -#include "switchkins.h" - -static struct scara_data { - hal_real_t d1, d2, d3, d4, d5, d6; -} *haldata = NULL; +#include /* key dimensions @@ -63,13 +58,32 @@ static struct scara_data { on the value of joint[3]. */ +#define DEFAULT_D1 490 +#define DEFAULT_D2 340 +#define DEFAULT_D3 50 +#define DEFAULT_D4 250 +#define DEFAULT_D5 50 +#define DEFAULT_D6 50 + +// the six dimensions, one pin each; the maths reads them from the block +static const kins_param_desc scara_params[] = { + { "D1", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D1 }, + { "D2", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D2 }, + { "D3", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D3 }, + { "D4", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D4 }, + { "D5", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D5 }, + { "D6", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D6 }, +}; +enum { P_D1, P_D2, P_D3, P_D4, P_D5, P_D6 }; + /* joint[0], joint[1] and joint[3] are in degrees and joint[2] is in length units */ -static -int scaraKinematicsForward(const double * joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int scara_forward(const kins_params *p, kins_scratch *s, + const double * joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; double a0, a1, a3; double x, y, z, c; @@ -84,20 +98,21 @@ int scaraKinematicsForward(const double * joint, a1 = a1 + a0; a3 = a3 + a1; - rtapi_real D1 = hal_get_real(haldata->d1); - rtapi_real D2 = hal_get_real(haldata->d2); - rtapi_real D3 = hal_get_real(haldata->d3); - rtapi_real D4 = hal_get_real(haldata->d4); - rtapi_real D5 = hal_get_real(haldata->d5); - rtapi_real D6 = hal_get_real(haldata->d6); + const double D1 = p->geometry[P_D1]; + const double D2 = p->geometry[P_D2]; + const double D3 = p->geometry[P_D3]; + const double D4 = p->geometry[P_D4]; + const double D5 = p->geometry[P_D5]; + const double D6 = p->geometry[P_D6]; x = D2*cos(a0) + D4*cos(a1) + D6*cos(a3); y = D2*sin(a0) + D4*sin(a1) + D6*sin(a3); z = D1 + D3 - joint[2] - D5; c = a3; + // the elbow flag: which sign the inverse gives the acos of joint 1 *iflags = 0; - if (joint[1] < 90) + if (joint[1] < 0) *iflags = 1; world->tran.x = x; @@ -109,13 +124,15 @@ int scaraKinematicsForward(const double * joint, world->b = joint[5]; return (0); -} //scaraKinematicsForward() +} // scara_forward() -static int scaraKinematicsInverse(const EmcPose * world, - double * joint, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int scara_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * world, + double * joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; double a3; double q0, q1; double xt, yt, rsq, cc; @@ -129,12 +146,12 @@ static int scaraKinematicsInverse(const EmcPose * world, /* convert degrees to radians */ a3 = c * ( PM_PI / 180 ); - rtapi_real D1 = hal_get_real(haldata->d1); - rtapi_real D2 = hal_get_real(haldata->d2); - rtapi_real D3 = hal_get_real(haldata->d3); - rtapi_real D4 = hal_get_real(haldata->d4); - rtapi_real D5 = hal_get_real(haldata->d5); - rtapi_real D6 = hal_get_real(haldata->d6); + const double D1 = p->geometry[P_D1]; + const double D2 = p->geometry[P_D2]; + const double D3 = p->geometry[P_D3]; + const double D4 = p->geometry[P_D4]; + const double D5 = p->geometry[P_D5]; + const double D6 = p->geometry[P_D6]; /* center of end effector (correct for D6) */ xt = x - D6*cos(a3); @@ -176,38 +193,66 @@ static int scaraKinematicsInverse(const EmcPose * world, *fflags = 0; return (0); -} // scaraKinematicsInverse() +} // scara_inverse() -#define DEFAULT_D1 490 -#define DEFAULT_D2 340 -#define DEFAULT_D3 50 -#define DEFAULT_D4 250 -#define DEFAULT_D5 50 -#define DEFAULT_D6 50 - -static int scaraKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) +static int scara_jacobian(const kins_params *p, const double * joint, + const EmcPose * world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { - (void)coordinates; - int res=0; - - haldata = hal_malloc(sizeof(*haldata)); - if (!haldata) goto error; + (void)iflags; + const double D2 = p->geometry[P_D2]; + const double D4 = p->geometry[P_D4]; + const double D6 = p->geometry[P_D6]; + const double a3 = world->c * (PM_PI / 180); + const double q1 = joint[1] * (PM_PI / 180); + const double xt = world->tran.x - D6*cos(a3); + const double yt = world->tran.y - D6*sin(a3); + const double rsq = xt*xt + yt*yt; + /* gradients over (x, y, c) of the quantities the inverse builds */ + double d_xt[3] = { 1, 0, D6*sin(a3) * (PM_PI/180) }; + double d_yt[3] = { 0, 1, -D6*cos(a3) * (PM_PI/180) }; + double d_q1[3], d_q0[3], dphi_dq1; + int i, j, a; + + if (rsq <= 0 || fabs(sin(q1)) < 1e-12) { + /* the arm folded or straight out: the elbow rate is unbounded */ + return -1; + } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d1), DEFAULT_D1, "%s.D1", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d2), DEFAULT_D2, "%s.D2", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d3), DEFAULT_D3, "%s.D3", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d4), DEFAULT_D4, "%s.D4", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d5), DEFAULT_D5, "%s.D5", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d6), DEFAULT_D6, "%s.D6", kp->halprefix); - if (res) { goto error; } + /* rsq = D2^2 + D4^2 + 2 D2 D4 cos(q1), so q1 follows rsq; q0 is the + bearing of the end effector less the angle the outer arm subtends, + whose rate over q1 is (D2 D4 cos(q1) + D4^2) / rsq */ + dphi_dq1 = (D2*D4*cos(q1) + D4*D4) / rsq; + for (i = 0; i < 3; i++) { + double d_rsq = 2*xt*d_xt[i] + 2*yt*d_yt[i]; + d_q1[i] = -d_rsq / (2*D2*D4*sin(q1)); + d_q0[i] = (xt*d_yt[i] - yt*d_xt[i]) / rsq - dphi_dq1 * d_q1[i]; + } + /* columns x, y, c; the rest of the pose does not reach these joints */ + for (i = 0; i < 3; i++) { + int col = (i == 2) ? 5 : i; + jac[0][col] = d_q0[i] * (180 / PM_PI); + jac[1][col] = d_q1[i] * (180 / PM_PI); + jac[3][col] = -(jac[0][col] + jac[1][col]); + } + jac[3][5] += 1; + jac[2][2] = -1; + jac[4][3] = 1; + jac[5][4] = 1; return 0; +} // scara_jacobian() -error: - return -1; -} // scaraKinematicsSetup() +static const kins_ops scara_ops = { + .forward = scara_forward, + .inverse = scara_inverse, + .jacobian = scara_jacobian, + .primary = 1, +}; int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -215,39 +260,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "scarakins"; // !!! must agree with filename kp->halprefix = "scarakins"; // hal pin names kp->required_coordinates = "xyzabc"; // ab are scaragui table tilts kp->allow_duplicates = 0; kp->max_joints = strlen(kp->required_coordinates); + kp->params = scara_params; + kp->nparams = sizeof(scara_params)/sizeof(scara_params[0]); if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = scaraKinematicsSetup; - *kfwd1 = scaraKinematicsForward; - *kinv1 = scaraKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &scara_ops); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = scaraKinematicsSetup; - *kfwd0 = scaraKinematicsForward; - *kinv0 = scaraKinematicsInverse; - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &scara_ops); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } // switchkinsSetup() diff --git a/src/emc/kinematics/scorbot-kins.c b/src/emc/kinematics/scorbot-kins.c index bd8868a063d..43e96638389 100644 --- a/src/emc/kinematics/scorbot-kins.c +++ b/src/emc/kinematics/scorbot-kins.c @@ -43,6 +43,7 @@ #include #include #include +#include // @@ -75,12 +76,15 @@ static void compute_j1_cartesian_location(double j0, EmcPose *j1_cart) { // Forward kinematics takes the joint positions and computes the cartesian // coordinates of the controlled point. -int kinematicsForward( +static int scorbot_forward( + const kins_params *p, kins_scratch *s, const double *joints, EmcPose *pose, const KINEMATICS_FORWARD_FLAGS *fflags, KINEMATICS_INVERSE_FLAGS *iflags ) { + (void)p; + (void)s; (void)fflags; (void)iflags; EmcPose j1_vector; // the vector from j0 ("base") to joint 1 ("shoulder", end of link 0) @@ -89,16 +93,13 @@ int kinematicsForward( double r; - // rtapi_print("fwd: j0=%f, j1=%f, j2=%f\n", joints[0], joints[1], joints[2]); compute_j1_cartesian_location(joints[0], &j1_vector); - // rtapi_print("fwd: j1=(%f, %f, %f)\n", j1_vector.tran.x, j1_vector.tran.y, j1_vector.tran.z); // Link 1 connects j1 (shoulder) to j2 (elbow). r = L1_LENGTH * cos(TO_RAD * joints[1]); j2_vector.tran.x = r * cos(TO_RAD * joints[0]); j2_vector.tran.y = r * sin(TO_RAD * joints[0]); j2_vector.tran.z = L1_LENGTH * sin(TO_RAD * joints[1]); - // rtapi_print("fwd: j2=(%f, %f, %f)\n", j2_vector.tran.x, j2_vector.tran.y, j2_vector.tran.z); // Link 2 connects j2 (elbow) to j3 (wrist). // J3 is the controlled point. @@ -106,13 +107,11 @@ int kinematicsForward( j3_vector.tran.x = r * cos(TO_RAD * joints[0]); j3_vector.tran.y = r * sin(TO_RAD * joints[0]); j3_vector.tran.z = L2_LENGTH * sin(TO_RAD * joints[2]); - // rtapi_print("fwd: j3=(%f, %f, %f)\n", j3_vector.tran.x, j3_vector.tran.y, j3_vector.tran.z); // The end-effector location is the sum of the linkage vectors. pose->tran.x = j1_vector.tran.x + j2_vector.tran.x + j3_vector.tran.x; pose->tran.y = j1_vector.tran.y + j2_vector.tran.y + j3_vector.tran.y; pose->tran.z = j1_vector.tran.z + j2_vector.tran.z + j3_vector.tran.z; - // rtapi_print("fwd: pose=(%f, %f, %f)\n", pose->tran.x, pose->tran.y, pose->tran.z); // A and B are wrist roll and pitch, handled in hal by external kinematics pose->a = joints[3]; @@ -134,15 +133,17 @@ int kinematicsForward( // is the horizontal distance (ie, in the XY plane) of the controlled // point from J0. // -int kinematicsInverse( +static int scorbot_inverse( + const kins_params *p, kins_scratch *s, const EmcPose *pose, double *joints, const KINEMATICS_INVERSE_FLAGS *iflags, KINEMATICS_FORWARD_FLAGS *fflags ) { + (void)p; + (void)s; (void)iflags; (void)fflags; - // EmcPose j1_cart; double distance_to_cp, distance_to_center; double r_j1, z_j1; // (r_j1, z_j1) is the location of J1 in the RZ plane double r_cp, z_cp; // (r_cp, z_cp) is the location of the controlled point in the RZ plane @@ -152,16 +153,10 @@ int kinematicsInverse( // the location of J2, this is what we're trying to find double z_j2; - // rtapi_print("inv: x=%f, y=%f, z=%f\n", pose->tran.x, pose->tran.y, pose->tran.z); - // J0 is easy. Project the (X, Y, Z) of the pose onto the Z=0 plane. // J0 points at the projected (X, Y) point. tan(J0) = Y/X // J0 then defines the plane that the rest of the arm operates in. joints[0] = TO_DEG * atan2(pose->tran.y, pose->tran.x); - // rtapi_print("inv: j0=%f\n", joints[0]); - - // compute_j1_cartesian_location(joints[0], &j1_cart); - // rtapi_print("inv: j1=(X=%f, Y=%f, Z=%f)\n", j1_cart.tran.x, j1_cart.tran.y, j1_cart.tran.z); // FIXME: Until i figure the wrist differential out, the controlled // point will be the location of the wrist joint, J3/J4. @@ -175,19 +170,16 @@ int kinematicsInverse( // of J0. This is just a known, static vector. r_j1 = L0_HORIZONTAL_DISTANCE; z_j1 = L0_VERTICAL_DISTANCE; - // rtapi_print("inv: r_j1=%f, z_j1=%f\n", r_j1, z_j1); // (r_cp, z_cp) is the location of J3 (the controlled point), again in // the plane defined by the angle of J0, with the origin of the // machine. r_cp = sqrt(pow(pose->tran.x, 2) + pow(pose->tran.y, 2)); z_cp = pose->tran.z; - // rtapi_print("inv: r_cp=%f, z_cp=%f (controlled point)\n", r_cp, z_cp); // translate so (r_j1, z_j1) is the origin of the coordinate system r_cp -= r_j1; z_cp -= z_j1; - // rtapi_print("inv: r_cp=%f, z_cp=%f (translated controlled point)\n", r_cp, z_cp); // // Now the origin (aka J1), J2, and CP define a triangle in the RZ plane. @@ -206,102 +198,123 @@ int kinematicsInverse( distance_to_cp = sqrt(pow(r_cp, 2) + pow(z_cp, 2)); distance_to_center = distance_to_cp / 2; - // rtapi_print("inv: distance to cp: %f\n", distance_to_cp); // find the angle of the vector from the origin to the CP angle_to_cp = TO_DEG * acos(r_cp / distance_to_cp); if (z_cp < 0) { angle_to_cp *= -1; } - // rtapi_print("inv: angle to cp: %f\n", angle_to_cp); // find the angle (Center, J1, J2) j1_angle = TO_DEG * acos(distance_to_center / L1_LENGTH); - // rtapi_print("inv: j1 angle: %f\n", j1_angle); joints[1] = angle_to_cp + j1_angle; - // rtapi_print("inv: j1: %f\n", joints[1]); // now we can compute the location of J2 z_j2 = L1_LENGTH * sin(TO_RAD * joints[1]); - // rtapi_print("inv: r_j2=%f, z_j2=%f (translated j2)\n", r_j2, z_j2); joints[2] = -1.0 * TO_DEG * asin((z_j2 - z_cp) / L2_LENGTH); + // A and B are wrist roll and pitch, handled in hal by external kinematics + joints[3] = pose->a; + joints[4] = pose->b; -#if 0 - // Distance between controlled point and the location of j1. These two - // points are separated by link 1, joint 1, and link 2. - distance_between_centers = sqrt(pow((r2 - r1), 2) + pow((z2 - z1), 2)); + return 0; +} - if (distance_between_centers > (L1_LENGTH + L2_LENGTH)) { - // trying to reach too far - return GO_RESULT_RANGE_ERROR; - } - if (distance_between_centers < fabs(L1_LENGTH - L2_LENGTH)) { - // trying to reach too far into armpit - return GO_RESULT_RANGE_ERROR; +static int scorbot_jacobian( + const kins_params *p, + const double *joints, + const EmcPose *pose, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags +) { + // scorbot_inverse() above, differentiated step by step in the same + // order, each quantity carried as its gradient over (x, y, z) + const double x = pose->tran.x, y = pose->tran.y; + const double rho2 = x*x + y*y; + const double rho = sqrt(rho2); + double r_cp, z_cp, dist, angle_to_cp, j1_angle, j1, z_j2, u; + double d_r_cp[3], d_z_cp[3], d_dist[3], d_angle[3], d_j1a[3], d_j1[3], d_j2[3]; + double q; + int i, j, a; + + (void)p; + (void)joints; + (void)iflags; + if (rho2 <= 0) { return -1; } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } } - delta = (1.0 / 4.0) * sqrt((distance_between_centers + L1_LENGTH + L2_LENGTH) * (distance_between_centers + L1_LENGTH - L2_LENGTH) * (distance_between_centers - L1_LENGTH + L2_LENGTH) * (L1_LENGTH + L2_LENGTH - distance_between_centers)); + // j0 = atan2(y, x) + jac[0][0] = -y/rho2 * TO_DEG; + jac[0][1] = x/rho2 * TO_DEG; - ir1 = ((r1 + r2) / 2) + (((r2 - r1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) + ((2 * (z1 - z2) * delta) / pow(distance_between_centers, 2)); - ir2 = ((r1 + r2) / 2) + (((r2 - r1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) - ((2 * (z1 - z2) * delta) / pow(distance_between_centers, 2)); + r_cp = rho - L0_HORIZONTAL_DISTANCE; + z_cp = pose->tran.z - L0_VERTICAL_DISTANCE; + d_r_cp[0] = x/rho; d_r_cp[1] = y/rho; d_r_cp[2] = 0; + d_z_cp[0] = 0; d_z_cp[1] = 0; d_z_cp[2] = 1; - iz1 = ((z1 + z2) / 2) + (((z2 - z1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) - ((2 * (r1 - r2) * delta) / pow(distance_between_centers, 2)); - iz2 = ((z1 + z2) / 2) + (((z2 - z1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) + ((2 * (r1 - r2) * delta) / pow(distance_between_centers, 2)); - - - // (ir1, iz1) is one intersection point, (ir2, iz2) is the other. - // These are the possible locations of the J2 joint. - // FIXME: For now we arbitrarily pick the one with the bigger Z. + dist = sqrt(r_cp*r_cp + z_cp*z_cp); + if (dist <= 0 || dist >= 2*L1_LENGTH) { return -1; } + for (i = 0; i < 3; i++) { + d_dist[i] = (r_cp*d_r_cp[i] + z_cp*d_z_cp[i]) / dist; + } - if (iz1 > iz2) { - j2_r = ir1; - j2_z = iz1; - } else { - j2_r = ir2; - j2_z = iz2; + // the signed acos in the inverse is atan2(z_cp, r_cp) + angle_to_cp = TO_DEG * atan2(z_cp, r_cp); + for (i = 0; i < 3; i++) { + d_angle[i] = TO_DEG * (r_cp*d_z_cp[i] - z_cp*d_r_cp[i]) / (dist*dist); } - // rtapi_print("inv: j2_r=%f, j2_z=%f (J2, intersection point)\n", j2_r, j2_z); - // Make J1 point at J2 (j2_r, j2_z). - { - double l1_r = j2_r - r1; - joints[1] = TO_DEG * acos(l1_r / L1_LENGTH); - // rtapi_print("inv: l1_r=%f, j1=%f\n", l1_r, joints[1]); + q = dist / (2*L1_LENGTH); + j1_angle = TO_DEG * acos(q); + for (i = 0; i < 3; i++) { + d_j1a[i] = -TO_DEG / sqrt(1 - q*q) * d_dist[i] / (2*L1_LENGTH); } - // Make J2 point at the controlled point. - { - double l2_r = r2 - j2_r; - double j2; - j2 = TO_DEG * acos(l2_r / L2_LENGTH); - if (j2_z > pose->tran.z) { - j2 *= -1; - } - joints[2] = j2; - // rtapi_print("inv: l2_r=%f, j2=%f\n", l2_r, joints[2]); + j1 = angle_to_cp + j1_angle; + for (i = 0; i < 3; i++) { + d_j1[i] = d_angle[i] + d_j1a[i]; + jac[1][i] = d_j1[i]; } -#endif - // A and B are wrist roll and pitch, handled in hal by external kinematics - joints[3] = pose->a; - joints[4] = pose->b; + z_j2 = L1_LENGTH * sin(TO_RAD * j1); + u = (z_j2 - z_cp) / L2_LENGTH; + if (fabs(u) >= 1) { return -1; } + for (i = 0; i < 3; i++) { + double d_z_j2 = L1_LENGTH * cos(TO_RAD * j1) * TO_RAD * d_j1[i]; + d_j2[i] = -TO_DEG / sqrt(1 - u*u) * (d_z_j2 - d_z_cp[i]) / L2_LENGTH; + jac[2][i] = d_j2[i]; + } + jac[3][3] = 1; + jac[4][4] = 1; return 0; } +static const kins_ops scorbot_ops = { + .forward = scorbot_forward, + .inverse = scorbot_inverse, + .jacobian = scorbot_jacobian, +}; + +// the arm's dimensions are the constants above; no geometry pins. The +// entry points come from kins_single.c +const kins_module_info kins_module = { + .name = "scorbot-kins", + .halprefix = "scorbot-kins", + .params = NULL, + .nparams = 0, + .required_coordinates = "XYZAB", + .max_joints = 5, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &scorbot_ops }, +}; -KINEMATICS_TYPE kinematicsType(void) { - return KINEMATICS_BOTH; -} - -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); MODULE_LICENSE("GPL"); static int comp_id; @@ -311,6 +324,10 @@ int rtapi_app_main(void) { if (comp_id < 0) { return comp_id; } + if (kinsSingleInit(comp_id, "XYZAB", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; } @@ -318,4 +335,3 @@ int rtapi_app_main(void) { void rtapi_app_exit(void) { hal_exit(comp_id); } - diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index e4d35bb535e..4c32dfd2cb5 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -27,12 +27,12 @@ * Using modules must supply function: switchkinsSetup() */ #include -#include +#include #include #include -#include #include "switchkins.h" +#include //********************************************************************* // kinematic functions (default=0 for err detection): @@ -45,8 +45,19 @@ static KI kinvs[SWITCHKINS_MAX_TYPES] = {NULL}; static KT ktools[SWITCHKINS_MAX_TYPES] = {NULL}; static KT kworks[SWITCHKINS_MAX_TYPES] = {NULL}; static KTI ktinvs[SWITCHKINS_MAX_TYPES] = {NULL}; +static KJ kjacs[SWITCHKINS_MAX_TYPES] = {NULL}; static PmRotationMatrix knative[SWITCHKINS_MAX_TYPES]; +// types written as pure functions (see kinematics.h): the maths of each, +// the one RT parameter block they all read, a scratch per type, and the +// pins made from the module's table +static const kins_ops *kops[SWITCHKINS_MAX_TYPES] = {NULL}; +static kins_params rt_params; +static kins_scratch rt_scratch[SWITCHKINS_MAX_TYPES]; +static kins_pin_ref *pins; +static kins_tool_source tool_source; +static int inited; + // types provided, counted in rtapi_app_main() once they are all in static int kins_count; static int register_error; @@ -103,6 +114,48 @@ static void get_lastpose(int ktype, EmcPose* pos) pos->w = lastpose[ktype].w; } // get_lastpose() +// the block sees the pins as they are now, the tool motion sent, and +// the type asked for +static void read_block(int ktype) +{ + rt_params.ktype = ktype; + kinsParamsPinsRead(pins, kp.params, kp.nparams, &rt_params); + kinsToolSourceApply(&tool_source, kp.halprefix, kp.params, kp.nparams, + &rt_params); +} + +// the tool from motion, for the types written on the block; a type +// provided the older way reads its own pins and does not see it +int kinematicsSetTool(const EmcPose *tool) +{ + if (!tool) { return -1; } + kinsToolSourceSet(&tool_source, tool); + return 0; +} // kinematicsSetTool() + +static void write_block(int ktype) +{ + kinsParamsPinsWrite(pins, kp.params, kp.nparams, &rt_scratch[ktype]); +} + +// the forward of one type, whichever way it was provided, from the pose +// it is handed: no seeding, which is the caller's business +static int call_forward(int ktype, const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + int r; + if (kops[ktype]) { + read_block(ktype); + r = kops[ktype]->forward(&rt_params, &rt_scratch[ktype], + joint, pos, fflags, iflags); + write_block(ktype); + return r; + } + if (!kfwds[ktype]) { return -1; } + return kfwds[ktype](joint, pos, fflags, iflags); +} + static int gui_forward_kins(const double *joints, const EmcPose* estimate) { // the hexapod vismach gui uses these hal pins to @@ -114,7 +167,7 @@ static int gui_forward_kins(const double *joints, const EmcPose* estimate) KINEMATICS_INVERSE_FLAGS iflags; if ( kp.gui_kinstype < 0 || kp.gui_kinstype >= kins_count - || !kfwds[kp.gui_kinstype]) { + || (!kfwds[kp.gui_kinstype] && !kops[kp.gui_kinstype])) { rtapi_print_msg(RTAPI_MSG_ERR, "gui_forward_kins BAD gui_kinstype <%d>\n", kp.gui_kinstype); @@ -124,8 +177,8 @@ static int gui_forward_kins(const double *joints, const EmcPose* estimate) // no pose of our own yet, start from the caller's lastpose[kp.gui_kinstype] = *estimate; } - res = kfwds[kp.gui_kinstype](joints, &lastpose[kp.gui_kinstype], - &fflags, &iflags); + res = call_forward(kp.gui_kinstype, joints, &lastpose[kp.gui_kinstype], + &fflags, &iflags); lastpose_ok[kp.gui_kinstype] = (res == 0); hal_set_real(swdata->gui_x, lastpose[kp.gui_kinstype].tran.x); hal_set_real(swdata->gui_y, lastpose[kp.gui_kinstype].tran.y); @@ -164,6 +217,10 @@ int kinematicsSwitch(int new_switchkins_type) if (fwd_iterates[switchkins_type] && lastpose_ok[switchkins_type]) { use_lastpose[switchkins_type] = 1; // restarting a kins types } + // a pure type keeps the same restart pose in its own scratch + if (kops[switchkins_type] && kops[switchkins_type]->fwd_iterates) { + rt_scratch[switchkins_type].have_pose_seed = 1; + } return 0; // 0==> no error } // kinematicsSwitch() @@ -175,26 +232,37 @@ int kinematicsForward(const double *joint, int r; EmcPose estimate = *pos; // the caller's guess, the only one we get - if ( fwd_iterates[switchkins_type] - && use_lastpose[switchkins_type] - && lastpose_ok[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]) { + || (!kfwds[switchkins_type] && !kops[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); - lastpose_ok[switchkins_type] = (r == 0); + + if (kops[switchkins_type]) { + read_block(switchkins_type); + r = kinsOpsForward(kops[switchkins_type], &rt_params, + &rt_scratch[switchkins_type], + joint, pos, fflags, iflags); + write_block(switchkins_type); + // the gui forward below starts from here, as it did for the + // older form + if (kops[switchkins_type]->fwd_iterates) {save_lastpose(switchkins_type,pos);} + } else { + if ( fwd_iterates[switchkins_type] + && use_lastpose[switchkins_type] + && lastpose_ok[switchkins_type]) { + // initialize iterative forward kins (ok for identity too) + get_lastpose(switchkins_type,pos); + use_lastpose[switchkins_type] = 0; + } + r = kfwds[switchkins_type](joint, pos, fflags, iflags); + if (fwd_iterates[switchkins_type]) { + save_lastpose(switchkins_type,pos); + lastpose_ok[switchkins_type] = (r == 0); + } } if (r) return r; @@ -224,12 +292,20 @@ int kinematicsInverse(const EmcPose * pos, if ( switchkins_type < 0 || switchkins_type >= kins_count - || !kinvs[switchkins_type]) { + || (!kinvs[switchkins_type] && !kops[switchkins_type])) { rtapi_print_msg(RTAPI_MSG_ERR, "switchkins: Inverse BAD switchkins_type \n", switchkins_type); return -1; } + if (kops[switchkins_type]) { + read_block(switchkins_type); + r = kinsOpsInverse(kops[switchkins_type], &rt_params, + &rt_scratch[switchkins_type], + pos, joint, iflags, fflags); + write_block(switchkins_type); + return r; + } r = kinvs[switchkins_type](pos, joint, iflags, fflags); return r; } // kinematicsInverse() @@ -240,9 +316,13 @@ int kinematicsToolFrame(const double *joint, { int r; - if ( switchkins_type < 0 - || switchkins_type >= kins_count - || !ktools[switchkins_type]) { + if (switchkins_type < 0 || switchkins_type >= kins_count) { return -1; } + if (kops[switchkins_type]) { + read_block(switchkins_type); + return kinsOpsToolFrame(kops[switchkins_type], &rt_params, + joint, rot, fflags); + } + if (!ktools[switchkins_type]) { return -1; // this type does not supply one; not an error } r = ktools[switchkins_type](joint, rot, fflags); @@ -257,9 +337,13 @@ int kinematicsWorkFrame(const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags) { - if ( switchkins_type < 0 - || switchkins_type >= kins_count - || !kworks[switchkins_type]) { + if (switchkins_type < 0 || switchkins_type >= kins_count) { return -1; } + if (kops[switchkins_type]) { + read_block(switchkins_type); + return kinsOpsWorkFrame(kops[switchkins_type], &rt_params, + joint, rot, fflags); + } + if (!kworks[switchkins_type]) { return -1; // this type does not supply one; not an error } // no native rotation here: the work frame has no tool axis to point the @@ -276,10 +360,12 @@ int kinematicsToolFrameInverse(const PmCartesian *axis_in_work, int *free_directions, double *tool_spin) { - if ( switchkins_type < 0 - || switchkins_type >= kins_count - || !ktools[switchkins_type] - || !kworks[switchkins_type]) { + if (switchkins_type < 0 || switchkins_type >= kins_count) { return -1; } + if (kops[switchkins_type]) { + if (!kops[switchkins_type]->tool || !kops[switchkins_type]->work) { + return -1; // this type does not report its frames, so it cannot answer + } + } else if (!ktools[switchkins_type] || !kworks[switchkins_type]) { return -1; // this type does not report its frames, so it cannot answer } @@ -300,6 +386,31 @@ int kinematicsToolFrameInverse(const PmCartesian *axis_in_work, tool_spin); } // kinematicsToolFrameInverse() +int kinematicsJacobian(const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + if (switchkins_type < 0 || switchkins_type >= kins_count) { + return -1; + } + if (kops[switchkins_type]) { + read_block(switchkins_type); + return kinsOpsJacobian(kops[switchkins_type], &rt_params, + &rt_scratch[switchkins_type], + joint, world, jac, iflags); + } + // a closed form is exact and knows its own singular poses + if (kjacs[switchkins_type]) { + return kjacs[switchkins_type](joint, world, jac, iflags); + } + // otherwise the type's own inverse, differenced. The type function + // rather than the dispatch, so this cannot recurse through a switch. + if (!kinvs[switchkins_type]) { return -1; } + return kinsJacobianFromInverse(kinvs[switchkins_type], kp.max_joints, + joint, world, iflags, jac); +} // kinematicsJacobian() + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -315,7 +426,7 @@ int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv) register_error = 1; return -1; } - if (ksetups[ktype] || kfwds[ktype] || kinvs[ktype]) { + if (ksetups[ktype] || kfwds[ktype] || kinvs[ktype] || kops[ktype]) { rtapi_print_msg(RTAPI_MSG_ERR, "switchkinsRegister: switchkins-type %d" " already provided\n", ktype); @@ -328,6 +439,66 @@ int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv) return 0; } // switchkinsRegister() +int switchkinsDeclare(int ktype, int flags) +{ + if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsDeclare: BAD switchkins_type <%d>" + " (must be 0..%d)\n", + ktype, SWITCHKINS_MAX_TYPES - 1); + register_error = 1; + return -1; + } + ktype_flags[ktype] = flags; + return 0; +} // switchkinsDeclare() + +int kinematicsTypeFlags(int ktype) +{ + if ( ktype < 0 + || ktype >= kins_count + || (!kfwds[ktype] && !kops[ktype])) { return -1; } + return ktype_flags[ktype]; +} // kinematicsTypeFlags() + +int switchkinsRegisterOps(int ktype, const kins_ops *ops) +{ + if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterOps: 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] || kops[ktype]) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterOps: switchkins-type %d" + " already provided\n", ktype); + register_error = 1; + return -1; + } + if (!ops || !ops->forward || !ops->inverse) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterOps: switchkins-type %d" + " has no forward or inverse\n", ktype); + register_error = 1; + return -1; + } + if (ops->tool && ops->native && !toolFrameIsProper(ops->native)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterOps: switchkins-type %d" + " declared a rotation that is not orthonormal with" + " determinant +1\n", ktype); + register_error = 1; + return -1; + } + kops[ktype] = ops; + if (ops->identity) { ktype_flags[ktype] |= KINSTYPE_IDENTITY; } + if (ops->primary) { ktype_flags[ktype] |= KINSTYPE_PRIMARY; } + return 0; +} // switchkinsRegisterOps() + int switchkinsRegisterFrames(int ktype, KT kwork, KT ktool, const PmRotationMatrix *native) { @@ -354,45 +525,33 @@ int switchkinsRegisterFrames(int ktype, KT kwork, KT ktool, return 0; } // switchkinsRegisterFrames() -int switchkinsRegisterToolFrameInverse(int ktype, KTI kinv) +int switchkinsRegisterJacobian(int ktype, KJ kjac) { if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { rtapi_print_msg(RTAPI_MSG_ERR, - "switchkinsRegisterToolFrameInverse: BAD" - " switchkins_type <%d> (must be 0..%d)\n", + "switchkinsRegisterJacobian: BAD switchkins_type" + " <%d> (must be 0..%d)\n", ktype, SWITCHKINS_MAX_TYPES - 1); register_error = 1; return -1; } - ktinvs[ktype] = kinv; + kjacs[ktype] = kjac; return 0; -} // switchkinsRegisterToolFrameInverse() +} // switchkinsRegisterJacobian() -int switchkinsDeclare(int ktype, int flags) +int switchkinsRegisterToolFrameInverse(int ktype, KTI kinv) { if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { rtapi_print_msg(RTAPI_MSG_ERR, - "switchkinsDeclare: BAD switchkins_type <%d>" - " (must be 0..%d)\n", + "switchkinsRegisterToolFrameInverse: BAD" + " switchkins_type <%d> (must be 0..%d)\n", ktype, SWITCHKINS_MAX_TYPES - 1); register_error = 1; return -1; } - ktype_flags[ktype] = flags; + ktinvs[ktype] = kinv; return 0; -} // switchkinsDeclare() - -int kinematicsTypeFlags(int ktype) -{ - if (ktype < 0 || ktype >= kins_count || !kfwds[ktype]) { return -1; } - return ktype_flags[ktype]; -} // kinematicsTypeFlags() - -//********************************************************************* -static char *coordinates; -RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); -static char *sparm; -RTAPI_MP_STRING(sparm, "switchkins module-specific parameter"); +} // switchkinsRegisterToolFrameInverse() EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); @@ -402,38 +561,71 @@ EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsWorkFrame); EXPORT_SYMBOL(kinematicsToolFrameInverse); +EXPORT_SYMBOL(kinematicsJacobian); +EXPORT_SYMBOL(kinematicsSetTool); EXPORT_SYMBOL(switchkinsRegister); EXPORT_SYMBOL(switchkinsRegisterFrames); EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); EXPORT_SYMBOL(switchkinsDeclare); EXPORT_SYMBOL(kinematicsTypeFlags); -MODULE_LICENSE("GPL"); +EXPORT_SYMBOL(switchkinsRegisterJacobian); +EXPORT_SYMBOL(switchkinsRegisterOps); +EXPORT_SYMBOL(switchkinsInit); +EXPORT_SYMBOL(switchkinsDescribe); +EXPORT_SYMBOL(switchkinsDescribeSetup); -static int comp_id; //********************************************************************* -int rtapi_app_main(void) +// the module as registered so far, described for a caller outside RT +int switchkinsDescribeSetup(const kparms *k, kins_module_info *info) { - int i,res,identities; - 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;} + int i, n = 0; + + if (!k || !info) { return -1; } + if (k->nparams < 0 || k->nparams > KINS_MAX_PARAMS + || (k->nparams > 0 && !k->params)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkins: %s declares a bad parameter table\n", + k->kinsname ? k->kinsname : "?"); + return -1; + } + memset(info, 0, sizeof(*info)); + info->name = k->kinsname; + info->halprefix = k->halprefix ? k->halprefix : k->kinsname; + info->params = k->params; + info->nparams = k->nparams; + info->required_coordinates = k->required_coordinates; + info->max_joints = k->max_joints; + info->allow_duplicates = k->allow_duplicates; + for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { + info->ops[i] = kops[i]; + if (ksetups[i] || kfwds[i] || kinvs[i] || kops[i]) { n = i + 1; } + } + info->ntypes = n; + return 0; +} // switchkinsDescribeSetup() + +int switchkinsDescribe(kins_module_info *info) +{ + if (!inited) { return -1; } + return switchkinsDescribeSetup(&kp, info); +} // switchkinsDescribe() + +//********************************************************************* +// 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 identities, primaries; + int res = 0; + char* emsg = "other"; + + kp = *ksetup_parms; // kinematics parms are needed after this returns + + if (register_error) {emsg = "switchkinsRegister FAIL"; goto error;} // an identity type answers the tool frame the same way whichever module // asked for it, so supply it here rather than in every switchkinsSetup() @@ -443,17 +635,23 @@ int rtapi_app_main(void) ktools[i] = identityKinematicsToolFrame; knative[i] = TOOL_FRAME_SPINDLE; } + // and its Jacobian is exact, so do not difference for it + if (!kjacs[i] && kfwds[i] == identityKinematicsForward) { + kjacs[i] = identityKinematicsJacobian; + } } - // the highest type provided by either route sets the count + // 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 (ksetups[i] || kfwds[i] || kinvs[i] || kops[i]) { kins_count = i + 1; } } if (!kins_count) { emsg = "no switchkins-types provided"; goto error; } // declarations must name provided types, and identity is unique: - // G13.1 resolves it from the flags, so two answers is a load error + // G13.1 resolves it from the flags, so two answers is a load error. + // Primary is unique the same way: G43.4 switches to it. identities = 0; + primaries = 0; for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { if (!ktype_flags[i]) { continue; } if (i >= kins_count) { @@ -463,6 +661,7 @@ int rtapi_app_main(void) emsg = "declared switchkins-type not provided"; goto error; } if (ktype_flags[i] & KINSTYPE_IDENTITY) { identities++; } + if (ktype_flags[i] & KINSTYPE_PRIMARY) { primaries++; } rtapi_print("switchkins-type %d declared:%s%s\n", i, (ktype_flags[i] & KINSTYPE_IDENTITY) ? " identity" : "", (ktype_flags[i] & KINSTYPE_PRIMARY) ? " primary" : ""); @@ -470,6 +669,9 @@ int rtapi_app_main(void) if (identities > 1) { emsg = "more than one identity switchkins-type declared"; goto error; } + if (primaries > 1) { + emsg = "more than one primary switchkins-type declared"; goto error; + } for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { if (kp.fwd_iterates_mask & (1<kinstype_is[i]), @@ -522,26 +722,48 @@ 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); if (!coordinates) {coordinates = kp.required_coordinates;} + // the pure types share one block and one set of pins from the table + if (kp.params || kp.nparams) { + kins_module_info mi; + if (switchkinsDescribeSetup(&kp, &mi)) { emsg = "bad table"; goto error; } + if (kinsParamsInit(&rt_params, &mi, coordinates)) { + emsg = "coordinates"; goto error; + } + if (kinsParamsPinsCreate(comp_id, kp.halprefix, kp.params, kp.nparams, + &pins)) { + emsg = "table pin create fail"; goto error; + } + } else { + for (i=0; i < kins_count; i++) { + if (kops[i]) { + kins_module_info mi; + if (switchkinsDescribeSetup(&kp, &mi)) { emsg = "bad table"; goto error; } + if (kinsParamsInit(&rt_params, &mi, coordinates)) { + emsg = "coordinates"; goto error; + } + break; + } + } + } + for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { kinsScratchInit(&rt_scratch[i]); } + for (i=0; i < kins_count; i++) { - ksetups[i](comp_id,coordinates,&kp); + if (ksetups[i]) { ksetups[i](comp_id,coordinates,&kp); } } - hal_ready(comp_id); + inited = 1; 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 d325ce75e48..c5f87b35117 100644 --- a/src/emc/kinematics/switchkins.h +++ b/src/emc/kinematics/switchkins.h @@ -1,13 +1,16 @@ /* ** License GPL Version 2 */ -#ifndef SWITCHKINS_H // { -#define SWITCHKINS_H +#ifndef __LINUXCNC_SWITCHKINS_H +#define __LINUXCNC_SWITCHKINS_H -#include +#include "kinematics.h" //SWITCHKINS_MAX_TYPES (max number of types a module may provide) -//is in kinematics.h: motion and the NML status channel need it too +//is in kinematics.h as KINS_MAX_TYPES: motion and the NML +//status channel need it too +//max number of switchkins types a module may provide: +#define SWITCHKINS_MAX_TYPES KINS_MAX_TYPES // KinematicsFORWARD functions typedef int (*KF)(const double *joint, @@ -34,14 +37,14 @@ 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); // called from switchkinsSetup() for each type that reports its frames; a type @@ -73,4 +76,44 @@ extern int switchkinsRegisterToolFrameInverse(int ktype, KTI kinv); // never calls it leaves its types numeric-only: G12.1 P still works, // G13.1 refuses to guess which type is identity. extern int switchkinsDeclare(int ktype, int flags); -#endif // } + +// KinematicsJACOBIAN function (optional, see kinematics.h) +typedef int (*KJ)(const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + +// called from switchkinsSetup() only by a type with a closed form. A type +// that does not gets the exact answer if it is an identity type, and +// otherwise the generic differences of its own inverse. +extern int switchkinsRegisterJacobian(int ktype, KJ kjac); + +// provide one switchkins-type written as pure functions (see kinematics.h), +// before switchkinsInit(). Its pins come from the table in kparms, shared +// by every type of the module, so it has no setup function. A type may be +// provided this way or through switchkinsRegister(), not both. +extern int switchkinsRegisterOps(int ktype, const kins_ops *ops); + +// 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 + ); + +// Fill kp with the defaults, run the module's switchkinsSetup() and +// register the three types it may return, so that every type goes in by +// one route. In switchkins_setup.c, which a module links only if it +// defines switchkinsSetup(); a halcompile component that registers its +// types itself does not. Returns 0 or -1. +extern int switchkinsRunSetup(kparms* kp, const char* sparm); + +// The module as the core knows it after switchkinsInit(): its table and +// the ops of every type, NULL for one provided the old way. Behind +// kinsDescribe() for the RT instance; a copy outside RT that has not been +// initialised is described by switchkins_setup.c after a replay of setup. +// Returns 0, or -1 before switchkinsInit(). +extern int switchkinsDescribe(kins_module_info *info); +extern int switchkinsDescribeSetup(const kparms *kp, kins_module_info *info); + +#endif diff --git a/src/emc/kinematics/switchkins_main.c b/src/emc/kinematics/switchkins_main.c new file mode 100644 index 00000000000..8ab98b54223 --- /dev/null +++ b/src/emc/kinematics/switchkins_main.c @@ -0,0 +1,60 @@ +/* + 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 without this file and calls switchkinsInit() itself. +* +* Using modules must supply function: switchkinsSetup(), which +* switchkinsRunSetup() in switchkins_setup.c runs. +*/ +#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; + + if (switchkinsRunSetup(&kp, sparm)) { 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/switchkins_setup.c b/src/emc/kinematics/switchkins_setup.c new file mode 100644 index 00000000000..b44b7192412 --- /dev/null +++ b/src/emc/kinematics/switchkins_setup.c @@ -0,0 +1,85 @@ +/* + License GPL Version 2 +*/ + +/* switchkins_setup.c: the part of a switchkins module that depends on the +* module supplying switchkinsSetup(). Kept apart from switchkins.c so +* that a halcompile component, which registers its types itself and has +* no switchkinsSetup(), can link the core without it. +* +* switchkinsRunSetup() is what rtapi_app_main() and EXTRA_SETUP() call +* before switchkinsInit(). kinsDescribe() is the description a copy of +* the module loaded outside RT answers with: the RT instance describes +* itself from its own state, a fresh copy replays setup first, so the +* types come out the way the module parameters decide them. +*/ +#include +#include +#include + +#include + +int switchkinsRunSetup(kparms* kp, const char* sparm) +{ + KS ksetup[3] = {NULL}; + KF kfwd[3] = {NULL}; + KI kinv[3] = {NULL}; + int i; + + if (!kp) { return -1; } + memset(kp, 0, sizeof(*kp)); + + // 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 = (char*)sparm; // module parm passed to kins + + // switchkinsSetup() provides types 0,1,2 and may also call + // switchkinsRegister() or switchkinsRegisterOps() 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; + } + return 0; +} // switchkinsRunSetup() + +int kinsDescribe(const char *coordinates, const char *sparm, + kins_module_info *info) +{ + static kparms kp; + (void)coordinates; // the map is the caller's business, see kinsParamsInit() + + if (!info) { return -1; } + + // the RT instance knows itself already + if (switchkinsDescribe(info) == 0) { return 0; } + + // a copy outside RT: register the types the way the module would + if (switchkinsRunSetup(&kp, sparm)) { return -1; } + if (switchkinsDescribeSetup(&kp, info)) { return -1; } + return 0; +} // kinsDescribe() + +EXPORT_SYMBOL(switchkinsRunSetup); +EXPORT_SYMBOL(kinsDescribe); diff --git a/src/emc/kinematics/three21kins.c b/src/emc/kinematics/three21kins.c index a5a7b5dfa8f..ed6620da92d 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 @@ -28,24 +27,45 @@ /* flags for forward kinematics */ #define THREE21_REACH 0x01 -struct haldata { - hal_real_t a1, a2, a3, d1, d2, d3, d4, d6; -} *haldata = NULL; +// the eight dimensions, one pin each; the maths reads them from the block +static const kins_param_desc three21_params[] = { + { "A1", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_A1 }, + { "A2", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_A2 }, + { "A3", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_A3 }, + { "D1", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_D1 }, + { "D2", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_D2 }, + { "D3", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_D3 }, + { "D4", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_D4 }, + { "D6", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_D6 }, +}; +enum { P_A1, P_A2, P_A3, P_D1, P_D2, P_D3, P_D4, P_D6 }; + +/* the difference of two angles, brought into (-pi, pi] so that a joint a + whole turn from the formula still matches it */ +static double angleDiff(double a, double b) +{ + double d = a - b; + while (d > PM_PI) { d -= 2*PM_PI; } + while (d <= -PM_PI) { d += 2*PM_PI; } + return d; +} -static int three21KinematicsForward(const double * joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int three21_forward(const kins_params *p, kins_scratch *s, + const double * joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; - double a1 = hal_get_real(haldata->a1); - double a2 = hal_get_real(haldata->a2); - double a3 = hal_get_real(haldata->a3); - double d1 = hal_get_real(haldata->d1); - double d2 = hal_get_real(haldata->d2); - double d3 = hal_get_real(haldata->d3); - double d4 = hal_get_real(haldata->d4); - double d6 = hal_get_real(haldata->d6); + double a1 = p->geometry[P_A1]; + double a2 = p->geometry[P_A2]; + double a3 = p->geometry[P_A3]; + double d1 = p->geometry[P_D1]; + double d2 = p->geometry[P_D2]; + double d3 = p->geometry[P_D3]; + double d4 = p->geometry[P_D4]; + double d6 = p->geometry[P_D6]; double s1, s2, s3, s4, s5, s6; double c1, c2, c3, c4, c5, c6; @@ -132,8 +152,8 @@ static int three21KinematicsForward(const double * joint, *iflags = 0; /* set shoulder flag */ - if (fabs(joint[0]*PM_PI/180 - atan2(hom.tran.y, hom.tran.x) + - atan2(d23, -sqrt(sumSq))) < FLAG_FUZZ) + if (fabs(angleDiff(joint[0]*PM_PI/180, atan2(hom.tran.y, hom.tran.x) - + atan2(d23, -sqrt(sumSq)))) < FLAG_FUZZ) { *iflags |= THREE21_SHOULDER_RIGHT; } @@ -143,8 +163,8 @@ static int three21KinematicsForward(const double * joint, if (discr < 0.0) { discr = 0.0; } - if (fabs(joint[2]*PM_PI/180 - atan2(a3, d4) + - atan2(k, -sqrt(discr))) < FLAG_FUZZ) + if (fabs(angleDiff(joint[2]*PM_PI/180, atan2(a3, d4) - + atan2(k, -sqrt(discr)))) < FLAG_FUZZ) { *iflags |= THREE21_ELBOW_DOWN; } @@ -158,7 +178,7 @@ static int three21KinematicsForward(const double * joint, } else { - if (! (fabs(joint[3]*PM_PI/180 - atan2(t1, t2)) < FLAG_FUZZ)) + if (! (fabs(angleDiff(joint[3]*PM_PI/180, atan2(t1, t2))) < FLAG_FUZZ)) { *iflags |= THREE21_WRIST_FLIP; } @@ -180,23 +200,25 @@ static int three21KinematicsForward(const double * joint, return 0; } -static int three21KinematicsInverse(const EmcPose * world, - double * joint, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int three21_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * world, + double * joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; PmHomogeneous hom; PmPose worldPose; PmRpy rpy; - double a1 = hal_get_real(haldata->a1); - double a2 = hal_get_real(haldata->a2); - double a3 = hal_get_real(haldata->a3); - double d1 = hal_get_real(haldata->d1); - double d2 = hal_get_real(haldata->d2); - double d3 = hal_get_real(haldata->d3); - double d4 = hal_get_real(haldata->d4); - double d6 = hal_get_real(haldata->d6); + double a1 = p->geometry[P_A1]; + double a2 = p->geometry[P_A2]; + double a3 = p->geometry[P_A3]; + double d1 = p->geometry[P_D1]; + double d2 = p->geometry[P_D2]; + double d3 = p->geometry[P_D3]; + double d4 = p->geometry[P_D4]; + double d6 = p->geometry[P_D6]; double t1, t2, t3; double k; @@ -339,31 +361,13 @@ static int three21KinematicsInverse(const EmcPose * world, return 0; } -int three21KinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - (void)coordinates; - int res=0; - - haldata = hal_malloc(sizeof(*haldata)); - if (!haldata) goto error; - - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a1), DEFAULT_THREE21_A1, "%s.A1", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a2), DEFAULT_THREE21_A2, "%s.A2", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a3), DEFAULT_THREE21_A3, "%s.A3", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d1), DEFAULT_THREE21_D1, "%s.D1", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d2), DEFAULT_THREE21_D2, "%s.D2", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d3), DEFAULT_THREE21_D3, "%s.D3", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d4), DEFAULT_THREE21_D4, "%s.D4", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d6), DEFAULT_THREE21_D6, "%s.D6", kp->halprefix); - if (res) { goto error; } - - return 0; - -error: - return -1; -} +// no frames reported and no closed form Jacobian: the shared code +// differences the inverse +static const kins_ops three21_ops = { + .forward = three21_forward, + .inverse = three21_inverse, + .primary = 1, +}; int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -371,39 +375,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "three21kins"; kp->halprefix = "three21kins"; kp->required_coordinates = "xyzabc"; kp->allow_duplicates = 0; kp->max_joints = strlen(kp->required_coordinates); + kp->params = three21_params; + kp->nparams = sizeof(three21_params)/sizeof(three21_params[0]); if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = three21KinematicsSetup; - *kfwd1 = three21KinematicsForward; - *kinv1 = three21KinematicsInverse; - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &three21_ops); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = three21KinematicsSetup; - *kfwd0 = three21KinematicsForward; - *kinv0 = three21KinematicsInverse; - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &three21_ops); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } diff --git a/src/emc/kinematics/tripodkins.c b/src/emc/kinematics/tripodkins.c index 990b7997297..6f373cae2c8 100644 --- a/src/emc/kinematics/tripodkins.c +++ b/src/emc/kinematics/tripodkins.c @@ -4,10 +4,10 @@ * * Derived from a work by Fred Proctor * -* Author: +* Author: * License: GPL Version 2 * System: Linux -* +* * Copyright (c) 2004 All rights reserved. * * Last change: @@ -67,19 +67,15 @@ #include #include #include /* these decls */ +#include -/* ident tag */ -#ifndef __GNUC__ -#ifndef __attribute__ -#define __attribute__(x) -#endif -#endif - -static struct haldata { - hal_real_t bx; - hal_real_t cx; - hal_real_t cy; -} *haldata = NULL; +// the base geometry, one pin each, poked from HAL as before +static const kins_param_desc tripod_params[] = { + { "Bx", KINS_PARAM_FLOAT, KINS_IO, 0, 1.0 }, + { "Cx", KINS_PARAM_FLOAT, KINS_IO, 0, 1.0 }, + { "Cy", KINS_PARAM_FLOAT, KINS_IO, 0, 1.0 }, +}; +enum { P_BX, P_CX, P_CY }; #define sq(x) ((x)*(x)) @@ -123,11 +119,13 @@ static struct haldata { solutions. Positive means the tripod is above the xy plane, negative means below. */ -int kinematicsForward(const double * joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int tripod_forward(const kins_params *p, kins_scratch *s_, + const double * joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s_; (void)iflags; #define AD (joints[0]) #define BD (joints[1]) @@ -137,9 +135,9 @@ int kinematicsForward(const double * joints, #define Dz (pos->tran.z) double P, Q, R; double s, t, u; - rtapi_real Bx = hal_get_real(haldata->bx); - rtapi_real Cx = hal_get_real(haldata->cx); - rtapi_real Cy = hal_get_real(haldata->cy); + const double Bx = p->geometry[P_BX]; + const double Cx = p->geometry[P_CX]; + const double Cy = p->geometry[P_CY]; P = sq(AD); Q = sq(BD) - sq(Bx); @@ -183,11 +181,13 @@ int kinematicsForward(const double * joints, #undef Dz } -int kinematicsInverse(const EmcPose * pos, - double * joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int tripod_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double * joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; #define AD (joints[0]) #define BD (joints[1]) @@ -195,9 +195,9 @@ int kinematicsInverse(const EmcPose * pos, #define Dx (pos->tran.x) #define Dy (pos->tran.y) #define Dz (pos->tran.z) - rtapi_real Bx = hal_get_real(haldata->bx); - rtapi_real Cx = hal_get_real(haldata->cx); - rtapi_real Cy = hal_get_real(haldata->cy); + const double Bx = p->geometry[P_BX]; + const double Cx = p->geometry[P_CX]; + const double Cy = p->geometry[P_CY]; AD = sqrt(sq(Dx) + sq(Dy) + sq(Dz)); BD = sqrt(sq(Dx - Bx) + sq(Dy) + sq(Dz)); @@ -218,169 +218,71 @@ int kinematicsInverse(const EmcPose * pos, #undef Dz } -KINEMATICS_TYPE kinematicsType() +static int tripod_jacobian(const kins_params *p, const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { - return KINEMATICS_BOTH; -} - -#ifdef MAIN - -#include -#include - -/* - Interactive testing of kins. - - Syntax: a.out -*/ -int main(int argc, char *argv[]) -{ -#ifndef BUFFERLEN -#define BUFFERLEN 256 -#endif - char buffer[BUFFERLEN]; - char cmd[BUFFERLEN]; - EmcPose pos, vel; - double joints[3]={0.0,0.0,0.0}, jointvels[3]={0.0,0.0,0.0}; - char inverse; - char flags; - KINEMATICS_FORWARD_FLAGS fflags; - - inverse = 0; /* forwards, by default */ - flags = 0; /* didn't provide flags */ - fflags = 0; /* above xy plane, by default */ - if (argc != 4 || - 1 != sscanf(argv[1], "%lf", &Bx) || - 1 != sscanf(argv[2], "%lf", &Cx) || - 1 != sscanf(argv[3], "%lf", &Cy)) { - fprintf(stderr, "syntax: %s Bx Cx Cy\n", argv[0]); - return 1; + const double Bx = p->geometry[P_BX]; + const double Cx = p->geometry[P_CX]; + const double Cy = p->geometry[P_CY]; + /* the three strut base points, in the order of the joints */ + const double base[3][2] = { {0, 0}, {Bx, 0}, {Cx, Cy} }; + int i, j, a; + + (void)iflags; + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[j][a] = 0; } + } + /* a strut length changes by the component of the motion along the + strut, so each row is the unit vector from base to D */ + for (i = 0; i < 3; i++) { + double dx = pos->tran.x - base[i][0]; + double dy = pos->tran.y - base[i][1]; + double dz = pos->tran.z; + double len = joints[i]; + if (len <= 0) { return -1; } + jac[i][0] = dx/len; + jac[i][1] = dy/len; + jac[i][2] = dz/len; } - - while (! feof(stdin)) { - if (inverse) { - printf("inv> "); - } - else { - printf("fwd> "); - } - fflush(stdout); - - if (NULL == fgets(buffer, BUFFERLEN, stdin)) { - break; - } - if (1 != sscanf(buffer, "%255s", cmd)) { - continue; - } - - if (! strcmp(cmd, "quit")) { - break; - } - if (! strcmp(cmd, "i")) { - inverse = 1; - continue; - } - if (! strcmp(cmd, "f")) { - inverse = 0; - continue; - } - if (! strcmp(cmd, "ff")) { - if (1 != sscanf(buffer, "%*s %lu", &fflags)) { - printf("need forward flag\n"); - } - continue; - } - - if (inverse) { /* inverse kins */ - if (3 != sscanf(buffer, "%lf %lf %lf", - &pos.tran.x, - &pos.tran.y, - &pos.tran.z)) { - printf("need X Y Z\n"); - continue; - } - if (0 != kinematicsInverse(&pos, joints, NULL, &fflags)) { - printf("inverse kin error\n"); - } - else { - printf("%f\t%f\t%f\n", joints[0], joints[1], joints[2]); - if (0 != kinematicsForward(joints, &pos, &fflags, NULL)) { - printf("forward kin error\n"); - } - else { - printf("%f\t%f\t%f\n", pos.tran.x, pos.tran.y, pos.tran.z); - } - } - } - else { /* forward kins */ - if (flags) { - if (4 != sscanf(buffer, "%lf %lf %lf %lu", - &joints[0], - &joints[1], - &joints[2], - &fflags)) { - printf("need 3 strut values and flag\n"); - continue; - } - } - else { - if (3 != sscanf(buffer, "%lf %lf %lf", - &joints[0], - &joints[1], - &joints[2])) { - printf("need 3 strut values\n"); - continue; - } - } - if (0 != kinematicsForward(joints, &pos, &fflags, NULL)) { - printf("forward kin error\n"); - } - else { - printf("%f\t%f\t%f\n", pos.tran.x, pos.tran.y, pos.tran.z); - if (0 != kinematicsInverse(&pos, joints, NULL, &fflags)) { - printf("inverse kin error\n"); - } - else { - printf("%f\t%f\t%f\n", joints[0], joints[1], joints[2]); - } - } - } - } /* end while (! feof(stdin)) */ - return 0; } -#endif /* MAIN */ - -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); +static const kins_ops tripod_ops = { + .forward = tripod_forward, + .inverse = tripod_inverse, + .jacobian = tripod_jacobian, +}; + +// three struts for three coordinates; the entry points come from +// kins_single.c +const kins_module_info kins_module = { + .name = "tripodkins", + .halprefix = "tripodkins", + .params = tripod_params, + .nparams = sizeof(tripod_params)/sizeof(tripod_params[0]), + .required_coordinates = "XYZ", + .max_joints = 3, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &tripod_ops }, +}; MODULE_LICENSE("GPL"); - - static int comp_id; int rtapi_app_main(void) { - int res = 0; - comp_id = hal_init("tripodkins"); if(comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(struct haldata)); - if(!haldata) goto error; - - if((res = hal_pin_new_real(comp_id, HAL_IO, &(haldata->bx), 1.0, "tripodkins.Bx")) < 0) goto error; - if((res = hal_pin_new_real(comp_id, HAL_IO, &(haldata->cx), 1.0, "tripodkins.Cx")) < 0) goto error; - if((res = hal_pin_new_real(comp_id, HAL_IO, &(haldata->cy), 1.0, "tripodkins.Cy")) < 0) goto error; + if (kinsSingleInit(comp_id, "XYZ", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return res; } void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/src/emc/kinematics/trivkins.c b/src/emc/kinematics/trivkins.c index 4b3685dc6d6..2de2368614a 100644 --- a/src/emc/kinematics/trivkins.c +++ b/src/emc/kinematics/trivkins.c @@ -10,54 +10,27 @@ * ********************************************************************/ -#include #include /* RTAPI realtime OS API */ #include /* RTAPI realtime module decls */ -#include #include #include #include #include - - -#define SET(f) pos->f = joints[i] - -int kinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) -{ - return identityKinematicsForward(joints, pos, fflags, iflags); -} - -int kinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) -{ - return identityKinematicsInverse(pos, joints, iflags, fflags); -} - -int kinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) -{ - return identityKinematicsToolFrame(joints, rot, fflags); -} - -int kinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) -{ - return identityKinematicsWorkFrame(joints, rot, fflags); -} - -static KINEMATICS_TYPE ktype = -1; - -KINEMATICS_TYPE kinematicsType() -{ - return ktype; -} +#include + +// joints are axes, through whatever map coordinates= gives; the maths is +// the shared identity and the entry points come from kins_single.c +const kins_module_info kins_module = { + .name = "trivkins", + .halprefix = "trivkins", + .params = NULL, + .nparams = 0, + .required_coordinates = "", + .max_joints = EMCMOT_MAX_JOINTS, + .allow_duplicates = 1, + .ntypes = 1, + .ops = { &KINS_IDENTITY_OPS }, +}; #define TRIVKINS_DEFAULT_COORDINATES "XYZABCUVW" static char *coordinates = TRIVKINS_DEFAULT_COORDINATES; @@ -66,18 +39,40 @@ RTAPI_MP_STRING(coordinates, "Existing Axes"); static char *kinstype = "1"; // use KINEMATICS_IDENTITY RTAPI_MP_STRING(kinstype, "Kinematics Type (Identity,Both)"); -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsToolFrame); -EXPORT_SYMBOL(kinematicsWorkFrame); MODULE_LICENSE("GPL"); static int comp_id; +// say so when the joints are not in axis order, and which type suits that +static void show_map(KINEMATICS_TYPE ktype) +{ + kins_params p; + int a, unconventional = 0; + + if (kinsParamsInit(&p, &kins_module, coordinates)) { return; } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + if (p.joint_of_axis[a] >= 0 && p.joint_of_axis[a] != a) { unconventional = 1; } + if (p.joints_of_axis[a] & (p.joints_of_axis[a] - 1)) { unconventional = 1; } + } + if (!unconventional || !strcasecmp(coordinates, "xz")) { return; } + + rtapi_print("\ntrivkins: coordinates:%s\n", coordinates); + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + int j; + for (j = 0; j < p.max_joints; j++) { + if (p.joints_of_axis[a] & (1 << j)) { + rtapi_print(" Joint %d ==> Axis %c\n", j, "XYZABCUVW"[a]); + } + } + } + if (ktype != KINEMATICS_BOTH) { + rtapi_print("trivkins: Recommend: kinstype=both\n"); + } + rtapi_print("\n"); +} + int rtapi_app_main(void) { - kparms ksetup; + KINEMATICS_TYPE ktype; switch (*kinstype) { case 'b': case 'B': ktype = KINEMATICS_BOTH; break; @@ -89,12 +84,11 @@ int rtapi_app_main(void) { comp_id = hal_init("trivkins"); if(comp_id < 0) return comp_id; - // see typedef for KS KinematicsSETUP: - ksetup.max_joints = EMCMOT_MAX_JOINTS; - ksetup.allow_duplicates = 1; - if (identityKinematicsSetup(comp_id, coordinates, &ksetup)) { - return -1; //setup failed + if (kinsSingleInit(comp_id, coordinates, ktype)) { + hal_exit(comp_id); + return -1; } + show_map(ktype); hal_ready(comp_id); return 0; diff --git a/src/emc/kinematics/trtfuncs.c b/src/emc/kinematics/trtfuncs.c index 31c9ff1a6de..511bd4afae9 100644 --- a/src/emc/kinematics/trtfuncs.c +++ b/src/emc/kinematics/trtfuncs.c @@ -25,150 +25,67 @@ * This mill has a tilting table (B axis) and horizontal rotary * mounted to the table (C axis). * -* Note: The directions of the rotational axes are the opposite of the -* conventional axis directions. See +* Note: The directions of the rotational axes are the opposite of the +* conventional axis directions. See * https://linuxcnc.org/docs/html/gcode/machining-center.html - +* +* Written as pure functions of the parameter block (see kinematics.h): +* the geometry is the table below, the joint map comes from the block, +* and the tool length is p->tool.tran.z. ********************************************************************/ #include -#include -#include -#include #include #include -static int trtfuncs_max_joints; - -// joint number assignments (-1 ==> not assigned) -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; - -struct haldata { - hal_real_t x_rot_point; - hal_real_t y_rot_point; - hal_real_t z_rot_point; - hal_real_t x_offset; - hal_real_t y_offset; - hal_real_t z_offset; - hal_real_t tool_offset; - hal_bool_t conventional_directions; // default: false -} *haldata; - - -int trtKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - int i,jno,res=0; - int axis_idx_for_jno[EMCMOT_MAX_JOINTS]; - int rqdjoints = strlen(kp->required_coordinates); - - if (rqdjoints > kp->max_joints) { - rtapi_print_msg(RTAPI_MSG_ERR, - "ERROR %s: supports %d joints, <%s> requires %d\n", - kp->kinsname, - kp->max_joints, - coordinates, - rqdjoints); - goto error; - } - trtfuncs_max_joints = kp->max_joints; - - if (map_coordinates_to_jnumbers(coordinates, - kp->max_joints, - kp->allow_duplicates, - axis_idx_for_jno)) { - goto error; - } - // require all chars in reqd_coords (order doesn't matter) - for (i=0; i < rqdjoints; i++) { - char reqd_char; - reqd_char = *(kp->required_coordinates + i); - if ( !strchr(coordinates,toupper(reqd_char)) - && !strchr(coordinates,tolower(reqd_char)) ) { - rtapi_print_msg(RTAPI_MSG_ERR, - "ERROR %s:\nrequired coordinates:%s\n" - "specified coordinates:%s\n", - kp->kinsname, kp->required_coordinates, coordinates); - goto error; - } - } - - // assign principal joint numbers (first found in coordinates map) - // duplicates are handled by position_to_mapped_joints() - for (jno=0; jno < EMCMOT_MAX_JOINTS; jno++) { - if (axis_idx_for_jno[jno] == 0 && JX==-1) {JX = jno;} - if (axis_idx_for_jno[jno] == 1 && JY==-1) {JY = jno;} - if (axis_idx_for_jno[jno] == 2 && JZ==-1) {JZ = jno;} - if (axis_idx_for_jno[jno] == 3 && JA==-1) {JA = jno;} - if (axis_idx_for_jno[jno] == 4 && JB==-1) {JB = jno;} - if (axis_idx_for_jno[jno] == 5 && JC==-1) {JC = jno;} - if (axis_idx_for_jno[jno] == 6 && JU==-1) {JU = jno;} - if (axis_idx_for_jno[jno] == 7 && JV==-1) {JV = jno;} - if (axis_idx_for_jno[jno] == 8 && JW==-1) {JW = jno;} - } - - rtapi_print("%s coordinates=%s assigns:\n", kp->kinsname,coordinates); - for (jno=0; jno Axis %c\n", - jno,"XYZABCUVW"[axis_idx_for_jno[jno]]); - } - - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) {goto error;} - - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->x_rot_point), - 0.0, "%s.x-rot-point",kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->y_rot_point), - 0.0, "%s.y-rot-point",kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->z_rot_point), - 0.0, "%s.z-rot-point",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->z_offset), - 0.0, "%s.z-offset",kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->tool_offset), - 0.0, "%s.tool-offset",kp->halprefix); - res += hal_pin_new_bool(comp_id, HAL_IN, &(haldata->conventional_directions), - 0, "%s.conventional-directions", kp->halprefix); - if (res) {goto error;} - return 0; - -error: - rtapi_print_msg(RTAPI_MSG_ERR,"trtKinematicsSetup() FAIL\n"); - return -1; -} // trtKinematicsSetup() - -int xyzacKinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +// the geometry both machines share, one pin each +const kins_param_desc TRT_PARAMS[] = { + { "x-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "tool-offset", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + { "conventional-directions", KINS_PARAM_BIT, KINS_IN, 0, 0.0 }, // default: false +}; +const int TRT_NPARAMS = sizeof(TRT_PARAMS)/sizeof(TRT_PARAMS[0]); + +enum { TRT_XR, TRT_YR, TRT_ZR, TRT_XO, TRT_YO, TRT_ZO, TRT_TOOL, TRT_CON }; + +// joint number assignments from the block (-1 ==> not assigned) +#define JX (p->joint_of_axis[0]) +#define JY (p->joint_of_axis[1]) +#define JZ (p->joint_of_axis[2]) +#define JA (p->joint_of_axis[3]) +#define JB (p->joint_of_axis[4]) +#define JC (p->joint_of_axis[5]) +#define JU (p->joint_of_axis[6]) +#define JV (p->joint_of_axis[7]) +#define JW (p->joint_of_axis[8]) + +// the direction sign the conventional-directions pin selects +#define CON(p) ((p)->geometry[TRT_CON] != 0 ? 1.0 : -1.0) + +static int xyzac_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dt = hal_get_real(haldata->tool_offset); - const double dy = hal_get_real(haldata->y_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dt = p->tool.tran.z; + const double dy = p->geometry[TRT_YO]; + const double dz = p->geometry[TRT_ZO] + dt; const double a_rad = joints[JA]*TO_RAD; const double c_rad = joints[JC]*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); pos->tran.x = + cos(c_rad) * (joints[JX] - x_rot_point) - con * sin(c_rad) * cos(a_rad) * (joints[JY] - dy - y_rot_point) @@ -198,25 +115,27 @@ int xyzacKinematicsForward(const double *joints, pos->w = (JW != -1)? joints[JW] : 0; return 0; -} // xyzacKinematicsForward() +} // xyzac_forward() -int xyzacKinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int xyzac_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dy = hal_get_real(haldata->y_offset); - const double dt = hal_get_real(haldata->tool_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dy = p->geometry[TRT_YO]; + const double dt = p->tool.tran.z; + const double dz = p->geometry[TRT_ZO] + dt; const double a_rad = pos->a*TO_RAD; const double c_rad = pos->c*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); EmcPose P; // computed position @@ -253,16 +172,12 @@ int xyzacKinematicsInverse(const EmcPose * pos, // update joints with support for // multiple-joints per-coordinate letter: // based on computed position - position_to_mapped_joints(trtfuncs_max_joints, - &P, - joints); - - return 0; -} // xyzacKinematicsInverse() + return kinsPoseToMappedJoints(p, &P, joints); +} // xyzac_inverse() -int xyzacKinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int xyzac_work_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) { (void)fflags; // the forward transform's coefficients for a displacement of the X, Y and @@ -271,7 +186,7 @@ int xyzacKinematicsWorkFrame(const double *joints, const double a_rad = joints[JA]*TO_RAD; const double c_rad = joints[JC]*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); rot->x.x = cos(c_rad); rot->y.x = con * sin(c_rad); @@ -286,37 +201,92 @@ int xyzacKinematicsWorkFrame(const double *joints, rot->z.z = cos(a_rad); return 0; -} // xyzacKinematicsWorkFrame() +} // xyzac_work_frame() -int xyzacKinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int xyzac_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { (void)joints; - (void)fflags; - // both rotaries carry the work, so the tool never turns in the machine - *rot = TOOL_FRAME_SPINDLE; - return 0; -} // xyzacKinematicsToolFrame() + (void)iflags; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dy = p->geometry[TRT_YO]; + const double dt = p->tool.tran.z; + const double dz = p->geometry[TRT_ZO] + dt; + const double sa = sin(pos->a*TO_RAD), ca = cos(pos->a*TO_RAD); + const double sc = sin(pos->c*TO_RAD), cc = cos(pos->c*TO_RAD); + const double X = pos->tran.x - x_rot_point; + const double Y = pos->tran.y - y_rot_point; + const double Z = pos->tran.z - z_rot_point; + double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; + int a, b; + + const double con = CON(p); + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + for (b = 0; b < EMCMOT_MAX_AXIS; b++) { dP[a][b] = 0; } + } -int xyzbcKinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) + // the computed position P of xyzac_inverse(), differentiated: its + // coefficients for x, y and z, and the same expressions with the + // rotation taken a quarter turn on for a and for c + dP[0][0] = cc; + dP[0][1] = con * sc; + dP[0][5] = (-sc*X + con*cc*Y) * TO_RAD; + + dP[1][0] = - con * sc * ca; + dP[1][1] = cc * ca; + dP[1][2] = con * sa; + dP[1][3] = (con*sc*sa*X - cc*sa*Y + con*ca*Z + sa*dy - con*ca*dz) * TO_RAD; + dP[1][5] = (-con*cc*ca*X - sc*ca*Y) * TO_RAD; + + dP[2][0] = sc * sa; + dP[2][1] = - con * cc * sa; + dP[2][2] = ca; + dP[2][3] = (sc*ca*X - con*cc*ca*Y - sa*Z + con*ca*dy + sa*dz) * TO_RAD; + dP[2][5] = (cc*sa*X + con*sc*sa*Y) * TO_RAD; + + for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } + + return kinsJacobianFromMappedAxesP(p, (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // xyzac_jacobian() + +// both rotaries carry the work, so the tool never turns in the machine: +// the tool frame is the shared identity one +const kins_ops XYZAC_OPS = { + .forward = xyzac_forward, + .inverse = xyzac_inverse, + .work = xyzac_work_frame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = xyzac_jacobian, + .primary = 1, +}; + +static int xyzbc_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; // Note: 'principal' joints are used - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dx = hal_get_real(haldata->x_offset); - const double dt = hal_get_real(haldata->tool_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dx = p->geometry[TRT_XO]; + const double dt = p->tool.tran.z; + const double dz = p->geometry[TRT_ZO] + dt; const double b_rad = joints[JB]*TO_RAD; const double c_rad = joints[JC]*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); pos->tran.x = cos(c_rad) * cos(b_rad) * (joints[JX] - dx - x_rot_point) - con * sin(c_rad) * (joints[JY] - y_rot_point) @@ -345,27 +315,32 @@ int xyzbcKinematicsForward(const double *joints, pos->w = (JW != -1)? joints[JW] : 0; return 0; -} // xyzbcKinematicsForward() +} // xyzbc_forward() -int xyzbcKinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int xyzbc_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dx = hal_get_real(haldata->x_offset); - const double dt = hal_get_real(haldata->tool_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dx = p->geometry[TRT_XO]; + const double dt = p->tool.tran.z; + const double dz = p->geometry[TRT_ZO] + dt; const double b_rad = pos->b*TO_RAD; const double c_rad = pos->c*TO_RAD; - const double dpx = -cos(b_rad)*dx + sin(b_rad)*dz + dx; - const double dpz = -sin(b_rad)*dx - cos(b_rad)*dz + dz; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); + + // the offsets seen from the tilted table: the same rotation the + // forward applies to them, in the same sense + const double dpx = -cos(b_rad)*dx + con * sin(b_rad)*dz + dx; + const double dpz = -con * sin(b_rad)*dx - cos(b_rad)*dz + dz; EmcPose P; // computed position @@ -397,23 +372,19 @@ int xyzbcKinematicsInverse(const EmcPose * pos, // update joints with support for // multiple-joints per-coordinate letter: // based on computed position - position_to_mapped_joints(trtfuncs_max_joints, - &P, - joints); - - return 0; -} // xyzbcKinematicsInverse() + return kinsPoseToMappedJoints(p, &P, joints); +} // xyzbc_inverse() -int xyzbcKinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int xyzbc_work_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) { (void)fflags; - // see the comment in xyzacKinematicsWorkFrame() + // see the comment in xyzac_work_frame() const double b_rad = joints[JB]*TO_RAD; const double c_rad = joints[JC]*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); rot->x.x = cos(c_rad) * cos(b_rad); rot->y.x = con * sin(c_rad) * cos(b_rad); @@ -428,15 +399,65 @@ int xyzbcKinematicsWorkFrame(const double *joints, rot->z.z = cos(b_rad); return 0; -} // xyzbcKinematicsWorkFrame() +} // xyzbc_work_frame() -int xyzbcKinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int xyzbc_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { (void)joints; - (void)fflags; - // both rotaries carry the work, so the tool never turns in the machine - *rot = TOOL_FRAME_SPINDLE; - return 0; -} // xyzbcKinematicsToolFrame() + (void)iflags; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dx = p->geometry[TRT_XO]; + const double dt = p->tool.tran.z; + const double dz = p->geometry[TRT_ZO] + dt; + const double sb = sin(pos->b*TO_RAD), cb = cos(pos->b*TO_RAD); + const double sc = sin(pos->c*TO_RAD), cc = cos(pos->c*TO_RAD); + const double X = pos->tran.x - x_rot_point; + const double Y = pos->tran.y - y_rot_point; + const double Z = pos->tran.z - z_rot_point; + double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; + int a, b; + + const double con = CON(p); + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + for (b = 0; b < EMCMOT_MAX_AXIS; b++) { dP[a][b] = 0; } + } + + // see the comment in xyzac_jacobian(); dpx and dpz of the inverse + // depend on b as well + dP[0][0] = cc * cb; + dP[0][1] = con * sc * cb; + dP[0][2] = - con * sb; + dP[0][4] = (-cc*sb*X - con*sc*sb*Y - con*cb*Z + sb*dx + con*cb*dz) * TO_RAD; + dP[0][5] = (-sc*cb*X + con*cc*cb*Y) * TO_RAD; + + dP[1][0] = - con * sc; + dP[1][1] = cc; + dP[1][5] = (-con*cc*X - sc*Y) * TO_RAD; + + dP[2][0] = con * cc * sb; + dP[2][1] = sc * sb; + dP[2][2] = cb; + dP[2][4] = (con*cc*cb*X + sc*cb*Y - sb*Z - con*cb*dx + sb*dz) * TO_RAD; + dP[2][5] = (-con*sc*sb*X + cc*sb*Y) * TO_RAD; + + for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } + + return kinsJacobianFromMappedAxesP(p, (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // xyzbc_jacobian() + +const kins_ops XYZBC_OPS = { + .forward = xyzbc_forward, + .inverse = xyzbc_inverse, + .work = xyzbc_work_frame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = xyzbc_jacobian, + .primary = 1, +}; diff --git a/src/emc/kinematics/ugenserkins.c b/src/emc/kinematics/ugenserkins.c index 1d80e5ae70c..0d2c05b5a41 100644 --- a/src/emc/kinematics/ugenserkins.c +++ b/src/emc/kinematics/ugenserkins.c @@ -13,6 +13,7 @@ #include /* ulapi */ +#include #include /* struct timeval */ #include "genserkins.h" @@ -43,14 +44,25 @@ int main(int argc, char *argv[]) int retval = 0; double start, end; int comp_id; - kparms kp; - kp.max_joints = GENSER_MAX_JOINTS; - kp.allow_duplicates = 0; + kins_module_info info; + kins_params params; + kins_scratch scratch; - comp_id = hal_init("usergenserkins"); - if (genserKinematicsSetup(comp_id,"XYZABC",&kp)) printf("unexpected\n"); + /* the module described the way kinsDescribe() would, then a block at + the table defaults; setp has no say here */ + memset(&info, 0, sizeof(info)); + info.name = "genserkins"; + info.halprefix = "genserkins"; + info.params = GENSER_PARAMS; + info.nparams = GENSER_NPARAMS; + info.required_coordinates = "XYZABC"; + info.max_joints = GENSER_MAX_JOINTS; + info.ntypes = 1; + info.ops[0] = &GENSER_OPS; - genser_kin_init(); + comp_id = hal_init("usergenserkins"); + if (kinsParamsInit(¶ms, &info, "XYZABC")) printf("unexpected\n"); + kinsScratchInit(&scratch); /* syntax is a.out {i|f # # # # # #} */ if (argc == 8) { @@ -123,14 +135,14 @@ fprintf(stderr,"gki0:P %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f\n", pos.tran.x,pos.tran.y,pos.tran.z,pos.a,pos.b,pos.c); fprintf(stderr,"gki1:J %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f\n", joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]); - retval = genserKinematicsInverse(&pos, joints, &iflags, &fflags); + retval = GENSER_OPS.inverse(¶ms, &scratch, &pos, joints, &iflags, &fflags); fprintf(stderr,"gki2:J %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f\n", joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]); if (0 != retval) { printf("inv kins error %d <%s>\n", retval,go_result_to_string(retval)); } } else { - retval = genserKinematicsForward(joints, &pos, &fflags, &iflags); + retval = GENSER_OPS.forward(¶ms, &scratch, joints, &pos, &fflags, &iflags); if (0 != retval) { printf("fwd kins error %d\n", retval); } @@ -220,14 +232,14 @@ joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]); } else { fprintf(stderr,"gki1:\n"); retval = - genserKinematicsInverse(&pos, joints, &iflags, &fflags); + GENSER_OPS.inverse(¶ms, &scratch, &pos, joints, &iflags, &fflags); printf("%f %f %f %f %f %f\n", joints[0], joints[1], joints[2], joints[3], joints[4], joints[5]); if (0 != retval) { printf("inv kins error %d <%s>\n", retval,go_result_to_string(retval)); } else { retval = - genserKinematicsForward(joints, &pos, &fflags, &iflags); + GENSER_OPS.forward(¶ms, &scratch, joints, &pos, &fflags, &iflags); printf("%f %f %f %f %f %f\n", pos.tran.x, pos.tran.y, pos.tran.z, pos.a, pos.b, pos.c); if (0 != retval) { @@ -271,13 +283,13 @@ fprintf(stderr,"gki1:\n"); &joints[0], &joints[1], &joints[2], &joints[3], &joints[4], &joints[5])) { printf("?\n"); } else { - retval = genserKinematicsForward(joints, &pos, &fflags, &iflags); + retval = GENSER_OPS.forward(¶ms, &scratch, joints, &pos, &fflags, &iflags); printf("xyzabc: %f %f %f %f %f %f\n", pos.tran.x, pos.tran.y, pos.tran.z, pos.a, pos.b, pos.c); if (0 != retval) { printf("fwd kins error %d\n", retval); } else { - retval = genserKinematicsInverse(&pos, joints, &iflags, &fflags); + retval = GENSER_OPS.inverse(¶ms, &scratch, &pos, joints, &iflags, &fflags); printf("j0--j5: %f %f %f %f %f %f\n", joints[0], joints[1], joints[2], joints[3], joints[4], joints[5]); if (0 != retval) { diff --git a/src/emc/kinematics/userkfuncs.c b/src/emc/kinematics/userkfuncs.c index 81aa4c2d942..0e51ed651e5 100644 --- a/src/emc/kinematics/userkfuncs.c +++ b/src/emc/kinematics/userkfuncs.c @@ -2,6 +2,12 @@ ** switchable kinematics functions. ** License GPL Version 2 ** +** Two forms are here. USERK_OPS is the current one: identity through +** the parameter block, with no state of its own, registered by a module +** with switchkinsRegisterOps(2, &USERK_OPS). The functions below it are +** the older form, kept for the modules that still register their types +** through switchkinsSetup()'s out parameters. +** ** Example Usage (for customizing the genser-switchkins module): ** (works with rtpreempt only rtai --> Makefile needs work) ** @@ -24,6 +30,34 @@ // #include "genserkins.h" //includes gomath,hal //********************************************************************** +// the current form: pure functions of the block + +static int userk_forward(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *world, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + // replace with the machine's own forward; the block carries the + // geometry (p->geometry[]), the joint map and the tool + return kinsIdentityForward(p, s, joint, world, fflags, iflags); +} + +static int userk_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *world, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + return kinsIdentityInverse(p, s, world, joint, iflags, fflags); +} + +const kins_ops USERK_OPS = { + .forward = userk_forward, + .inverse = userk_inverse, + // .work, .tool, .native and .jacobian are optional, see kinematics.h +}; + +//********************************************************************** +// the older form // static local variables and functions go here static int userk_inited = 0; diff --git a/src/emc/kinematics/xyzac-trt-kins.c b/src/emc/kinematics/xyzac-trt-kins.c index 13fd6bd79ec..3fc14fc9de4 100644 --- a/src/emc/kinematics/xyzac-trt-kins.c +++ b/src/emc/kinematics/xyzac-trt-kins.c @@ -4,18 +4,19 @@ * * NOTEs: * 1) specify all kparms items -* 2) specify 3 KS,KF,KI functions for switchkins_type=0,1,2 -* 3) the 0th switchkins_type is the startup default -* 4) sparm is a module string parameter for configuration -* 5) The directions of the rotational axes are the opposite of the +* 2) the 0th switchkins_type is the startup default +* 3) sparm is a module string parameter for configuration +* 4) The directions of the rotational axes are the opposite of the * conventional axis directions. +* 5) the maths and the geometry table are in trtfuncs.c, written as +* pure functions of the parameter block (see kinematics.h) */ #include #include #include -#include "switchkins.h" +#include int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -23,45 +24,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "xyzac-trt-kins"; // !!! must agree with filename kp->halprefix = "xyzac-trt-kins"; // hal pin names kp->required_coordinates = "xyzac"; kp->allow_duplicates = 1; kp->max_joints = EMCMOT_MAX_JOINTS; + kp->params = TRT_PARAMS; + kp->nparams = TRT_NPARAMS; if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = trtKinematicsSetup; // trt: xyzac,xyzbc - *kfwd1 = xyzacKinematicsForward; - *kinv1 = xyzacKinematicsInverse; - switchkinsRegisterFrames(1, xyzacKinematicsWorkFrame, - xyzacKinematicsToolFrame, - &TOOL_FRAME_SPINDLE); - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &XYZAC_OPS); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = trtKinematicsSetup; // trt: xyzac,xyzbc - *kfwd0 = xyzacKinematicsForward; - *kinv0 = xyzacKinematicsInverse; - switchkinsRegisterFrames(0, xyzacKinematicsWorkFrame, - xyzacKinematicsToolFrame, - &TOOL_FRAME_SPINDLE); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &XYZAC_OPS); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } diff --git a/src/emc/kinematics/xyzbc-trt-kins.c b/src/emc/kinematics/xyzbc-trt-kins.c index 73ea69bf820..45c41b448dd 100644 --- a/src/emc/kinematics/xyzbc-trt-kins.c +++ b/src/emc/kinematics/xyzbc-trt-kins.c @@ -4,18 +4,19 @@ * * NOTEs: * 1) specify all kparms items -* 2) specify 3 KS,KF,KI functions for switchkins_type=0,1,2 -* 3) the 0th switchkins_type is the startup default -* 4) sparm is a module string parameter for configuration -* 5) The directions of the rotational axes are the opposite of the +* 2) the 0th switchkins_type is the startup default +* 3) sparm is a module string parameter for configuration +* 4) The directions of the rotational axes are the opposite of the * conventional axis directions. +* 5) the maths and the geometry table are in trtfuncs.c, written as +* pure functions of the parameter block (see kinematics.h) */ #include #include #include -#include "switchkins.h" +#include int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -23,45 +24,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "xyzbc-trt-kins"; // !!! must agree with filename kp->halprefix = "xyzbc-trt-kins"; // hal pin names kp->required_coordinates = "xyzbc"; kp->allow_duplicates = 1; kp->max_joints = EMCMOT_MAX_JOINTS; + kp->params = TRT_PARAMS; + kp->nparams = TRT_NPARAMS; if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = trtKinematicsSetup; // trt: xyzac,xyzbc - *kfwd1 = xyzbcKinematicsForward; - *kinv1 = xyzbcKinematicsInverse; - switchkinsRegisterFrames(1, xyzbcKinematicsWorkFrame, - xyzbcKinematicsToolFrame, - &TOOL_FRAME_SPINDLE); - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &XYZBC_OPS); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = trtKinematicsSetup; // trt: xyzac,xyzbc - *kfwd0 = xyzbcKinematicsForward; - *kinv0 = xyzbcKinematicsInverse; - switchkinsRegisterFrames(0, xyzbcKinematicsWorkFrame, - xyzbcKinematicsToolFrame, - &TOOL_FRAME_SPINDLE); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &XYZBC_OPS); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } 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..559bba46dc2 --- /dev/null +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -0,0 +1,661 @@ +/******************************************************************** + * Description: kinematics_user.c + * Non-RT loader for kinematics modules + * + * Loads a kinematics .so with dlopen, asks it to describe itself through + * kinsDescribe(), and evaluates its kinematics through the parameter + * block (see kinematics.h). The block is filled from HAL: the RT + * instance's own pins, read by name whenever the block is refreshed, so + * the values are the live ones and nothing is made in HAL to get at them. + * The tool is + * the caller's where it has given one, since a planner knows what a + * segment runs under better than the machine does; otherwise it comes + * from motion's own tooloffset pins where motion is loaded, so that the + * tool the module sees is the one motion has, whether or not the config + * netted it to the module's pin. + * + * A module exporting no kinsDescribe() is not an error; the context comes + * back flagged rt_only and answers nothing. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include "kinematics_user.h" +#include +#include +#include +#include +#include + +#include "config.h" /* EMC2_RTLIB_DIR, MODULE_EXT */ + +typedef int (*kins_describe_fn)(const char *coordinates, const char *sparm, + kins_module_info *info); + +#define MAX_PINS (KINS_MAX_PARAMS + AXIS_COUNT) + +struct KinematicsUserContext { + int initialized; + int rt_only; /* 1 if the module exports no kinsDescribe() */ + KINEMATICS_TYPE kins_type; + void *rt_handle; /* dlopen handle */ + kins_module_info info; + kins_params params; + kins_scratch scratch; + int ktype; /* kinematics type being evaluated */ + int num_joints; + char module_name[64]; + int comp_id; /* the caller's component, which has HAL mapped */ + char pin_name[MAX_PINS][HAL_NAME_LEN + 1]; /* the RT instance's pins read here */ + int num_pins; + int pin_of_param[KINS_MAX_PARAMS]; /* -1 if not read */ + int pin_of_tool[AXIS_COUNT]; /* motion.tooloffset.*, -1 if absent */ + int tool_param; /* the table's tool entry, -1 if none */ + int warned_tool; + EmcPose caller_tool; /* from kinematicsUserSetTool() */ + int have_caller_tool; + double last_joints[EMCMOT_MAX_JOINTS]; /* what the last inverse found */ +}; + +/* ======================================================================== + * Pin reading + * ======================================================================== */ + +/* The block is read from the RT instance's own pins by name at every + refresh, so a pin netted later reads its signal; nothing is made in HAL + to get at them. */ +static int pin_value(const char *pin_name, hal_type_t type, double *out) +{ + hal_query_t q; + + memset(&q, 0, sizeof(q)); + q.name = pin_name; + q.qtype = HAL_QTYPE_PIN; + q.pp.type = type; + if (hal_get_p(&q, NULL, NULL) != 0) return -1; + switch (type) { + case HAL_BIT: *out = q.pp.value.b ? 1.0 : 0.0; break; + case HAL_S32: *out = q.pp.value.s; break; + case HAL_U32: *out = q.pp.value.u; break; + default: *out = q.pp.value.r; break; + } + return 0; +} + +/* Does a pin of this name exist? Silent: absence is an answer, not an error. */ +static int pin_exists(const char *pin_name) +{ + hal_query_t q; + memset(&q, 0, sizeof(q)); + q.name = pin_name; + q.qtype = HAL_QTYPE_PIN; + return hal_getref_p(&q) == 0; +} + +/* Note pin_name for reading, once it has answered with the type; returns + its index, or -1. */ +static int note_pin(KinematicsUserContext *ctx, const char *pin_name, + hal_type_t type) +{ + double value; + + if (pin_value(pin_name, type, &value) != 0) { + fprintf(stderr, "kinematicsUserInit: no pin '%s' of the type expected\n", + pin_name); + return -1; + } + if (ctx->num_pins >= MAX_PINS) { + fprintf(stderr, "kinematicsUserInit: too many pins to read\n"); + return -1; + } + snprintf(ctx->pin_name[ctx->num_pins], sizeof(ctx->pin_name[0]), "%s", pin_name); + return ctx->num_pins++; +} + +static hal_type_t hal_type_of(kins_param_type t) +{ + switch (t) { + case KINS_PARAM_BIT: return HAL_BIT; + case KINS_PARAM_S32: return HAL_S32; + case KINS_PARAM_U32: return HAL_U32; + default: return HAL_FLOAT; + } +} + +/* Note every input of the table, and motion's tool where motion is there. */ +static int note_all(KinematicsUserContext *ctx) +{ + static const char letter[AXIS_COUNT] = { 'x','y','z','a','b','c','u','v','w' }; + char name[HAL_NAME_LEN + 1]; + int i; + + for (i = 0; i < KINS_MAX_PARAMS; i++) ctx->pin_of_param[i] = -1; + for (i = 0; i < AXIS_COUNT; i++) ctx->pin_of_tool[i] = -1; + ctx->tool_param = -1; + + for (i = 0; i < ctx->info.nparams; i++) { + const kins_param_desc *d = &ctx->info.params[i]; + if (d->dir == KINS_OUT) continue; + if (d->tool) ctx->tool_param = i; + snprintf(name, sizeof(name), "%s.%s", ctx->info.halprefix, d->name); + ctx->pin_of_param[i] = note_pin(ctx, name, hal_type_of(d->type)); + if (ctx->pin_of_param[i] < 0) return -1; + } + + /* motion publishes the tool it applies; take it from there when it is + loaded, so the module sees the tool whether or not the config netted + it through. Under halrun with the module alone there is no motion, + and the module's own tool entry is all there is. */ + for (i = 0; i < AXIS_COUNT; i++) { + snprintf(name, sizeof(name), "motion.tooloffset.%c", letter[i]); + if (!pin_exists(name)) continue; + ctx->pin_of_tool[i] = note_pin(ctx, name, HAL_FLOAT); + if (ctx->pin_of_tool[i] < 0) return -1; + } + return 0; +} + +/* The block sees the pins as they are now, and the tool of whoever + knows it best: the caller, then motion, then the module's own pin. */ +static void refresh(KinematicsUserContext *ctx) +{ + int i; + double tool[AXIS_COUNT]; + int have_motion_tool = 0; + + /* a pin that stops answering, its module unloaded, keeps its last value */ + for (i = 0; i < ctx->info.nparams; i++) { + int c = ctx->pin_of_param[i]; + double value; + if (c < 0) continue; + if (pin_value(ctx->pin_name[c], hal_type_of(ctx->info.params[i].type), &value) == 0) { + ctx->params.geometry[i] = value; + } + } + if (ctx->tool_param >= 0) { + ctx->params.tool.tran.z = ctx->params.geometry[ctx->tool_param]; + } + + if (ctx->have_caller_tool) { + ctx->params.tool = ctx->caller_tool; + if (ctx->tool_param >= 0) { + ctx->params.geometry[ctx->tool_param] = ctx->caller_tool.tran.z; + } + return; + } + + for (i = 0; i < AXIS_COUNT; i++) { + int c = ctx->pin_of_tool[i]; + tool[i] = 0.0; + if (c < 0) continue; + if (pin_value(ctx->pin_name[c], HAL_FLOAT, &tool[i]) != 0) continue; + have_motion_tool = 1; + } + if (!have_motion_tool) return; + + /* the module's pin and motion disagree: the config lost the tool + somewhere between them. Say so once; motion's value is the one + being cut with. A pin nobody set reads zero, which is not a + disagreement. */ + if (ctx->tool_param >= 0 && !ctx->warned_tool + && ctx->params.geometry[ctx->tool_param] != 0.0 + && fabs(tool[AXIS_Z] - ctx->params.geometry[ctx->tool_param]) > 1e-9) { + fprintf(stderr, + "kinematics_user: %s.%s is %.6g but motion.tooloffset.z is %.6g;" + " using motion's value\n", + ctx->info.halprefix, ctx->info.params[ctx->tool_param].name, + ctx->params.geometry[ctx->tool_param], tool[AXIS_Z]); + ctx->warned_tool = 1; + } + for (i = 0; i < AXIS_COUNT; i++) emcPoseSetAxis(&ctx->params.tool, i, tool[i]); + if (ctx->tool_param >= 0) { + ctx->params.geometry[ctx->tool_param] = tool[AXIS_Z]; + } +} + +/* ======================================================================== + * Module loading + * ======================================================================== */ + +static int load_module(KinematicsUserContext *ctx, + const char *module_name, + const char *coordinates, + const char *sparm) +{ + char module_path[512]; + void *handle, *hal_lib; + kins_describe_fn describe; + + snprintf(module_path, sizeof(module_path), + "%s/%s%s", EMC2_RTLIB_DIR, module_name, MODULE_EXT); + + /* A module calls rtapi_print() and the rest of the HAL library, and a + program holding that library under a shared object of its own (a GUI + holds it under the interpreter it loaded) keeps those symbols out of + the scope a module resolves against: the module loads and then dies + at the first call it cannot bind. Failing here is not itself an + error, since a program that links the library has them in reach. */ + hal_lib = dlopen("liblinuxcnchal.so.0", RTLD_LAZY | RTLD_GLOBAL); + if (!hal_lib) { + fprintf(stderr, "kinematicsUserInit: dlopen 'liblinuxcnchal.so.0':" + " %s\n", dlerror()); + } + + /* lazily: a halcompile component references hal_export_funct() and + the rest of what its rtapi_app_main() needs, which only the realtime + HAL library provides, and nothing here calls that main. What is + called, kinsDescribe() and the ops, resolves when it is called. */ + handle = dlopen(module_path, RTLD_LAZY | RTLD_LOCAL); + if (!handle) { + fprintf(stderr, "kinematicsUserInit: dlopen '%s': %s\n", + module_path, dlerror()); + return -1; + } + ctx->rt_handle = handle; + + describe = (kins_describe_fn)dlsym(handle, "kinsDescribe"); + if (!describe) { + fprintf(stderr, "kinematicsUserInit: '%s' exports no kinsDescribe;" + " it cannot be evaluated outside RT\n", module_name); + dlclose(handle); + ctx->rt_handle = NULL; + ctx->rt_only = 1; + return -1; + } + + if (describe(coordinates, sparm, &ctx->info) != 0) { + fprintf(stderr, "kinematicsUserInit: kinsDescribe failed for '%s'\n", + module_name); + dlclose(handle); + ctx->rt_handle = NULL; + ctx->rt_only = 1; + return -1; + } + + if (ctx->info.ntypes < 1 || !ctx->info.ops[0]) { + fprintf(stderr, "kinematicsUserInit: '%s' has no type 0 in the" + " parameter block form\n", module_name); + dlclose(handle); + ctx->rt_handle = NULL; + ctx->rt_only = 1; + return -1; + } + + if (kinsParamsInit(&ctx->params, &ctx->info, coordinates) != 0) { + fprintf(stderr, "kinematicsUserInit: '%s' refuses coordinates '%s'\n", + module_name, coordinates ? coordinates : "(default)"); + dlclose(handle); + ctx->rt_handle = NULL; + ctx->rt_only = 1; + return -1; + } + kinsScratchInit(&ctx->scratch); + + ctx->ktype = 0; + ctx->kins_type = ctx->info.ops[0]->identity ? KINEMATICS_IDENTITY + : KINEMATICS_BOTH; + return 0; +} + +/* ======================================================================== + * Public API + * ======================================================================== */ + +KinematicsUserContext* kinematicsUserInitSparm(const char* kins_type, + int num_joints, + const char* coordinates, + const char* sparm, + 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; + (void)prefix; + strncpy(ctx->module_name, kins_type, sizeof(ctx->module_name) - 1); + + if (load_module(ctx, kins_type, coordinates, sparm) == 0) { + if (note_all(ctx) != 0) { + fprintf(stderr, "kinematicsUserInit: cannot read the pins of '%s'\n", + kins_type); + ctx->rt_only = 1; + } + } + + ctx->initialized = 1; + return ctx; +} + +KinematicsUserContext* kinematicsUserInit(const char* kins_type, + int num_joints, + const char* coordinates, + int comp_id, + const char* prefix) +{ + return kinematicsUserInitSparm(kins_type, num_joints, coordinates, NULL, + comp_id, prefix); +} + +int kinematicsUserSetType(KinematicsUserContext* ctx, int ktype) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return -1; + if (ktype < 0 || ktype >= ctx->info.ntypes || !ctx->info.ops[ktype]) { + return -1; + } + ctx->ktype = ktype; + ctx->params.ktype = ktype; + kinsScratchInit(&ctx->scratch); + ctx->kins_type = ctx->info.ops[ktype]->identity ? KINEMATICS_IDENTITY + : KINEMATICS_BOTH; + return 0; +} + +int kinematicsUserGetNumTypes(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return 0; + return ctx->info.ntypes; +} + +int kinematicsUserSetTool(KinematicsUserContext* ctx, const EmcPose* tool) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return -1; + if (tool) { + ctx->caller_tool = *tool; + ctx->have_caller_tool = 1; + } else { + ctx->have_caller_tool = 0; + } + return 0; +} + +int kinematicsUserInverse(KinematicsUserContext* ctx, + const EmcPose* world, + double* joints) +{ + KINEMATICS_INVERSE_FLAGS iflags = 0; + KINEMATICS_FORWARD_FLAGS fflags = 0; + double j[EMCMOT_MAX_JOINTS]; + int i; + + if (!ctx || !ctx->initialized || !world || !joints) return -1; + if (ctx->rt_only) return -1; + + refresh(ctx); + /* the joints go in as well as out: motion hands a module where the + machine is, and some read that (a nutating head takes its rotary + angles from it), so the caller's array is the seed */ + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + j[i] = (i < ctx->num_joints) ? joints[i] : 0.0; + } + if (kinsOpsInverse(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, + world, j, &iflags, &fflags) != 0) { + return -1; + } + for (i = 0; i < ctx->num_joints; i++) joints[i] = j[i]; + memcpy(ctx->last_joints, j, sizeof(ctx->last_joints)); + return 0; +} + +int kinematicsUserForward(KinematicsUserContext* ctx, + const double* joints, + EmcPose* world) +{ + KINEMATICS_INVERSE_FLAGS iflags = 0; + KINEMATICS_FORWARD_FLAGS fflags = 0; + double j[EMCMOT_MAX_JOINTS]; + int i; + + if (!ctx || !ctx->initialized || !joints || !world) return -1; + if (ctx->rt_only) return -1; + + refresh(ctx); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + j[i] = (i < ctx->num_joints) ? joints[i] : 0.0; + } + /* a forward that iterates starts from the pose it is handed, so the + caller's world is the seed; any other gets a clean one */ + if (!ctx->info.ops[ctx->ktype]->fwd_iterates) { + memset(world, 0, sizeof(*world)); + } + return kinsOpsForward(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, + j, world, &fflags, &iflags); +} + +int kinematicsUserJacobian(KinematicsUserContext* ctx, + const EmcPose* world, + double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]) +{ + KINEMATICS_INVERSE_FLAGS iflags = 0; + KINEMATICS_FORWARD_FLAGS fflags = 0; + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + double j[EMCMOT_MAX_JOINTS]; + int r, a; + + if (!ctx || !ctx->initialized || !world || !J) return -1; + if (ctx->rt_only) return -1; + + refresh(ctx); + /* the joints at this pose, on the branch the last inverse was on */ + memcpy(j, ctx->last_joints, sizeof(j)); + if (kinsOpsInverse(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, + world, j, &iflags, &fflags) != 0) { + return -1; + } + if (kinsOpsJacobian(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, + j, world, jac, &iflags) != 0) { + return -1; + } + for (r = 0; r < KINEMATICS_USER_MAX_JOINTS; r++) { + for (a = 0; a < AXIS_COUNT; a++) J[r][a] = jac[r][a]; + } + return 0; +} + +int kinematicsUserIsIdentity(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return 0; + return ctx->info.ops[ctx->ktype]->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) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return -1; + refresh(ctx); + return 0; +} + +const kins_params* kinematicsUserParams(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return NULL; + refresh(ctx); + return &ctx->params; +} + +int kinematicsUserIsRtOnly(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return 1; + return ctx->rt_only; +} + +/* ======================================================================== + * Frames and the tool frame inverse + * + * toolFrameSolve() drives a pair of frame functions that take joints alone, + * the shape the RT modules export; the block form takes the parameters as + * well. The context the solver is running for is parked in a file static + * for the duration of the call, which is fine for the single threaded + * callers this has (the interpreter, a planner), and would not be for two + * threads solving at once. + * ======================================================================== */ + +static KinematicsUserContext *frame_ctx; + +static int frame_work(const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + KinematicsUserContext *ctx = frame_ctx; + return kinsOpsWorkFrame(ctx->info.ops[ctx->ktype], &ctx->params, joint, rot, fflags); +} + +static int frame_tool(const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + KinematicsUserContext *ctx = frame_ctx; + return kinsOpsToolFrame(ctx->info.ops[ctx->ktype], &ctx->params, joint, rot, fflags); +} + +static void pad_joints(KinematicsUserContext *ctx, const double *in, double *out) +{ + int i; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + out[i] = (i < ctx->num_joints) ? in[i] : 0.0; + } +} + +int kinematicsUserWorkFrame(KinematicsUserContext* ctx, const double* joints, + PmRotationMatrix* rot) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + double j[EMCMOT_MAX_JOINTS]; + + if (!ctx || !ctx->initialized || ctx->rt_only || !joints || !rot) return -1; + refresh(ctx); + pad_joints(ctx, joints, j); + return kinsOpsWorkFrame(ctx->info.ops[ctx->ktype], &ctx->params, j, rot, &fflags); +} + +int kinematicsUserToolFrame(KinematicsUserContext* ctx, const double* joints, + PmRotationMatrix* rot) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + double j[EMCMOT_MAX_JOINTS]; + + if (!ctx || !ctx->initialized || ctx->rt_only || !joints || !rot) return -1; + refresh(ctx); + pad_joints(ctx, joints, j); + return kinsOpsToolFrame(ctx->info.ops[ctx->ktype], &ctx->params, j, rot, &fflags); +} + +int kinematicsUserToolFrameInverse(KinematicsUserContext* ctx, + const PmCartesian* axis_in_work, + const PmCartesian* x_in_work, + const double* seed, + unsigned int held, + double* solutions, + int max_solutions, + int* free_directions, + double* tool_spin) +{ + double j[EMCMOT_MAX_JOINTS]; + int found; + + if (!ctx || !ctx->initialized || ctx->rt_only || !seed) return -1; + if (!ctx->info.ops[ctx->ktype]->work || !ctx->info.ops[ctx->ktype]->tool) return -1; + refresh(ctx); + pad_joints(ctx, seed, j); + frame_ctx = ctx; + found = toolFrameSolve(frame_work, frame_tool, ctx->num_joints, + axis_in_work, x_in_work, j, held, + solutions, max_solutions, free_directions, tool_spin); + frame_ctx = NULL; + return found; +} + +int kinematicsUserWorkJoints(KinematicsUserContext* ctx, const double* seed, + unsigned int* mask) +{ + double j[EMCMOT_MAX_JOINTS]; + int r; + + if (!ctx || !ctx->initialized || ctx->rt_only || !seed || !mask) return -1; + if (!ctx->info.ops[ctx->ktype]->work) return -1; + refresh(ctx); + pad_joints(ctx, seed, j); + frame_ctx = ctx; + r = toolFrameWorkJoints(frame_work, ctx->num_joints, j, mask); + frame_ctx = NULL; + return r; +} + +int kinematicsUserOrientJoints(KinematicsUserContext* ctx, const double* seed, + int* primary, int* secondary) +{ + double j[EMCMOT_MAX_JOINTS]; + int r; + + if (!ctx || !ctx->initialized || ctx->rt_only || !seed || !primary || !secondary) return -1; + if (!ctx->info.ops[ctx->ktype]->tool) return -1; + refresh(ctx); + pad_joints(ctx, seed, j); + frame_ctx = ctx; + r = toolFrameOrientJoints(frame_tool, ctx->num_joints, j, primary, secondary); + frame_ctx = NULL; + return r; +} + +KinematicsUserContext* kinematicsUserInitString(const char* kinematics, + int num_joints, + int comp_id, + const char* prefix) +{ + char buf[256], *tok, *save = NULL; + char module[64] = "", coords[64] = "", sparm[64] = ""; + + if (!kinematics) return NULL; + snprintf(buf, sizeof(buf), "%s", kinematics); + for (tok = strtok_r(buf, " \t", &save); tok; tok = strtok_r(NULL, " \t", &save)) { + if (!module[0]) { + snprintf(module, sizeof(module), "%s", tok); + } else if (!strncmp(tok, "coordinates=", 12)) { + snprintf(coords, sizeof(coords), "%s", tok + 12); + } else if (!strncmp(tok, "sparm=", 6)) { + snprintf(sparm, sizeof(sparm), "%s", tok + 6); + } + /* kinstype= and anything else is the RT loader's business */ + } + if (!module[0]) return NULL; + return kinematicsUserInitSparm(module, num_joints, + coords[0] ? coords : NULL, + sparm[0] ? sparm : NULL, + comp_id, prefix); +} + +void kinematicsUserFree(KinematicsUserContext* ctx) +{ + if (!ctx) return; + + 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..ff023113b95 --- /dev/null +++ b/src/emc/kinematics_userspace/kinematics_user.h @@ -0,0 +1,311 @@ +/******************************************************************** + * 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 evaluated through + * its parameter block form (see kinematics.h). The block is filled from + * input pins belonging to the caller's HAL component, connected to the + * same signals the running RT instance reads, so the maths runs on live + * values; the tool is the caller's where it gives one, and motion's + * otherwise, from motion's tool offset pins where motion is loaded. + * + * 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 module's pins are read through HAL, by name, so call this after + * hal_init(). Nothing is made in HAL: no pin, no signal. + * + * @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 + * @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); + +/** + * As kinematicsUserInit(), with the module's sparm= parameter as well, for + * a module whose kinematics types depend on it (5axiskins identityfirst). + */ +KinematicsUserContext* kinematicsUserInitSparm(const char* kins_type, + int num_joints, + const char* coordinates, + const char* sparm, + int comp_id, + const char* prefix); + +/** + * Select which kinematics type of a switchable module to evaluate. + * Type 0 is selected after init. + * + * @return 0, or -1 if the module has no such type in the block form + */ +int kinematicsUserSetType(KinematicsUserContext* ctx, int ktype); + +/** + * How many kinematics types the module has (1 for one that does not switch). + */ +int kinematicsUserGetNumTypes(KinematicsUserContext* ctx); + +/** + * The tool offset to evaluate with: what the caller knows the segment + * runs under, from canon or the tool table, rather than the offset the + * machine happens to have now. It stands until replaced, or until NULL + * puts the context back to taking the tool from motion. + * + * @return 0, or -1 for an RT-only context + */ +int kinematicsUserSetTool(KinematicsUserContext* ctx, const EmcPose* tool); + +/** + * Perform inverse kinematics (world coords -> joint positions) + * + * The joint array goes in as well as out: motion hands a module the + * joints the machine is at, and a module may read them (a nutating + * head takes its rotary angles from there, an iterating inverse starts + * there), so pass the current joints, not zeros. + * + * @param ctx Kinematics context from kinematicsUserInit + * @param world World coordinates (X, Y, Z, A, B, C, U, V, W) + * @param joints Joint positions in and out [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) + * + * A module whose forward iterates (the hexapod, the pentapod) starts + * from the pose in *world, so hand it one near the answer. + * + * @param ctx Kinematics context from kinematicsUserInit + * @param joints Array of joint positions [KINEMATICS_USER_MAX_JOINTS] + * @param world Output world coordinates, and the seed on input + * @return 0 on success, -1 on failure + */ +int kinematicsUserForward(KinematicsUserContext* ctx, + const double* joints, + EmcPose* world); + +/** + * The Jacobian at a pose, J[joint][axis] = d joint / d axis, from the + * module's closed form where it has one and by differencing its inverse + * where it does not. The inverse is run at the pose first, seeded with + * what the last kinematicsUserInverse() found, so the derivative is + * taken on the solution branch the caller is on. + * + * @return 0 on success, -1 on failure + */ +int kinematicsUserJacobian(KinematicsUserContext* ctx, + const EmcPose* world, + double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]); + +/** + * The parameter block as it stands, refreshed from HAL first. For + * reporting; the block belongs to the context. + */ +const kins_params* kinematicsUserParams(KinematicsUserContext* ctx); + +/** + * 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); + +/** + * Copy the bound pins into the block now. Every evaluation does this + * itself; call it only to observe the values. + * + * @param ctx Kinematics context + * @return 0, or -1 for an RT-only context + */ +int kinematicsUserRefreshParams(KinematicsUserContext* ctx); + +/** + * Check if this context is RT-only + * + * An RT-only module exports no kinsDescribe() 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); + +/** + * The frames, as the module reports them: the work frame and the tool + * frame at a joint set, each against the machine (see kinematics.h). + * + * @return 0, or -1 if the module supplies no frame for the selected type + */ +int kinematicsUserWorkFrame(KinematicsUserContext* ctx, const double* joints, + PmRotationMatrix* rot); +int kinematicsUserToolFrame(KinematicsUserContext* ctx, const double* joints, + PmRotationMatrix* rot); + +/** + * The tool frame inverse of kinematics.h, on the loaded module and the + * selected type: the joint sets that point the tool axis, and where given + * the tool x, along the directions asked for, in work coordinates. Same + * arguments and answers as kinematicsToolFrameInverse(). + */ +int kinematicsUserToolFrameInverse(KinematicsUserContext* ctx, + const PmCartesian* axis_in_work, + const PmCartesian* x_in_work, + const double* seed, + unsigned int held, + double* solutions, + int max_solutions, + int* free_directions, + double* tool_spin); + +/** + * Which joints turn the work at the seed, a bit per joint; what a caller + * passes as held to keep the table still. See toolFrameWorkJoints(). + */ +int kinematicsUserWorkJoints(KinematicsUserContext* ctx, const double* seed, + unsigned int* mask); + +/** + * The two rotaries that orient the tool, primary and secondary, told apart + * by which one carries the other's axis. The sign of the secondary names + * the pose a five axis machine reaches a tool direction in. Returns 0, or + * -1 where the machine has any number of orienting rotaries but two. + * See toolFrameOrientJoints(). + */ +int kinematicsUserOrientJoints(KinematicsUserContext* ctx, const double* seed, + int* primary, int* secondary); + +/** + * kinematicsUserInitSparm() from the value of [KINS] KINEMATICS as the + * HAL file hands it to loadrt: the module name first, then any of + * coordinates=, sparm= and kinstype=, in any order. + */ +KinematicsUserContext* kinematicsUserInitString(const char* kinematics, + int num_joints, + int comp_id, + const char* prefix); + +/** + * 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 a938fa5d613..316e3176e4f 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -70,6 +70,11 @@ #include "homing.h" #include "axis.h" +// the kinematics module takes the tool offset from here when it can; a +// module written before the call exports no such symbol, and the weak +// reference leaves it NULL rather than refusing to load motion +#pragma weak kinematicsSetTool + #define ABS(x) (((x) < 0) ? -(x) : (x)) @@ -115,6 +120,50 @@ void emcmotApplyPendingPlannerType(void) } /* ===== END PLANNER_SWITCH_DEFER ==================================================== */ +/* the inverse once, for an endpoint, run to a fixed point: some modules + read the joints they are handed (a nutating head takes its rotary angles + from them), so one pass from a stale seed answers for the wrong angles, + and running again from its own answer settles it */ +static int inverse_settled(EmcPose *pos, double *joints, + KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + int pass, j; + + for (pass = 0; pass < 8; pass++) { + double prev[EMCMOT_MAX_JOINTS], worst = 0.0; + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { prev[j] = joints[j]; } + if (kinematicsInverse(pos, joints, iflags, fflags) != 0) { return -1; } + for (j = 0; j < NO_OF_KINS_JOINTS; j++) { worst = fmax(worst, fabs(joints[j] - prev[j])); } + if (worst < 1e-9) { break; } + } + return 0; +} + +/* Where the queue leaves the joints, which is the seed an iterative + inverse wants: the reading runs ahead of the machine, so the joints to + hand it are not the ones the machine is standing in. A joint + interpolated segment knows its own end; the rest take the answer the + last endpoint checked came out with. Returns 0, and the joints the + machine stands in, when there is nothing queued to ask. */ +static double planned_joints[EMCMOT_MAX_JOINTS]; +static int planned_joints_ok = 0; + +static int queue_end_joints(double *joints_out) +{ + int j; + + if (tpGetQueueEndJoints(&emcmotInternal->coord_tp, joints_out)) { return 1; } + if (planned_joints_ok && tpQueueDepth(&emcmotInternal->coord_tp) > 0) { + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { joints_out[j] = planned_joints[j]; } + return 1; + } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + joints_out[j] = (j < ALL_JOINTS) ? joints[j].pos_cmd : 0.0; + } + return 0; +} + /* limits_ok() returns 1 if none of the hard limits are set, 0 if any are set. Called on a linear and circular move. */ STATIC int limits_ok(void) @@ -263,17 +312,15 @@ STATIC int inRange(EmcPose pos, int id, char *move_type) /* Now, check that the endpoint puts the joints within their limits too */ - /* fill in all joints with 0 */ - for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { - joint = &joints[joint_num]; - joint_pos[joint_num] = joint->pos_cmd; - } + /* start the inverse from where the queue leaves the joints */ + queue_end_joints(joint_pos); /* now fill in with real values, for joints that are used */ - if (kinematicsInverse(&pos, joint_pos, &iflags, &fflags) != 0) + if (inverse_settled(&pos, joint_pos, &iflags, &fflags) != 0) { reportError(_("%s move on line %d fails kinematicsInverse"), move_type, id); + planned_joints_ok = 0; return 0; } @@ -304,6 +351,15 @@ STATIC int inRange(EmcPose pos, int id, char *move_type) move_type, id, joint_num, joint->min_pos_limit); } } + + /* an endpoint on its way to the queue is where the next one starts + from; a refused one leaves the queue as it was */ + if (in_range) { + for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { + planned_joints[joint_num] = joint_pos[joint_num]; + } + planned_joints_ok = 1; + } return in_range; } @@ -1125,6 +1181,161 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) } break; + case EMCMOT_SET_JOINT_LINE: { + /* a move interpolated in joint space to a Cartesian endpoint: the + inverse runs once here, at the endpoint, and the planner takes + the joints from there; or the endpoint is given as joints and + the forward says where that is */ + double start[EMCMOT_MAX_JOINTS], target[EMCMOT_MAX_JOINTS]; + EmcPose end = emcmotCommand->pos; + double length = 0.0, vmax = 0.0, amax = 0.0, jmax = 0.0; + int moving = 0, jerk_limited = 0, bad = 0, axis_num; + + rtapi_print_msg(RTAPI_MSG_DBG, "SET_JOINT_LINE"); + if (!GET_MOTION_COORD_FLAG() || !GET_MOTION_ENABLE_FLAG()) { + reportError(_("need to be enabled, in coord mode for joint interpolated move")); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND; + SET_MOTION_ERROR_FLAG(1); + break; + } + if (!limits_ok()) { + reportError(_("can't do joint interpolated move with limits exceeded")); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + /* an external offset is applied to the world position on the way + to the inverse every cycle, which a joint interpolated segment + does not go through */ + for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { + if (axis_get_ext_offset_curr_pos(axis_num) != 0.0) { bad = 1; } + } + if (bad) { + reportError(_("can't do joint interpolated move on line %d with an external offset applied"), + emcmotCommand->id); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + + /* where the queue ends in joint space */ + if (!queue_end_joints(start)) { + EmcPose goal; + tpGetGoalPos(&emcmotInternal->coord_tp, &goal); + if (inverse_settled(&goal, start, &iflags, &fflags) != 0) { + reportError(_("joint interpolated move on line %d: the queue end fails kinematicsInverse"), + emcmotCommand->id); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + } + + for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { target[joint_num] = start[joint_num]; } + if (emcmotCommand->have_joint_target) { + for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS; joint_num++) { + target[joint_num] = emcmotCommand->joint_target[joint_num]; + } + if (kinematicsForward(target, &end, &fflags, &iflags) != 0) { + reportError(_("joint interpolated move on line %d fails kinematicsForward"), + emcmotCommand->id); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + } else { + if (!inRange(end, emcmotCommand->id, "Joint interpolated")) { + reportError(_("invalid params in joint interpolated move")); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + if (inverse_settled(&end, target, &iflags, &fflags) != 0) { + reportError(_("joint interpolated move on line %d fails kinematicsInverse"), + emcmotCommand->id); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + } + + /* the endpoint must be inside the joint limits, and every joint + that moves needs limits to move within; the segment length is + the joint space distance and each joint's limits are scaled + onto it so that the slowest joint sets the pace */ + for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS; joint_num++) { + double d = target[joint_num] - start[joint_num]; + joint = &joints[joint_num]; + if (!GET_JOINT_ACTIVE_FLAG(joint)) { continue; } + if (!isfinite(target[joint_num])) { + reportError(_("joint interpolated move on line %d gave non-finite joint location on joint %d"), + emcmotCommand->id, joint_num); + bad = 1; + } else if (target[joint_num] > joint->max_pos_limit || target[joint_num] < joint->min_pos_limit) { + reportError(_("joint interpolated move on line %d would exceed joint %d's limit"), + emcmotCommand->id, joint_num); + bad = 1; + } + length += d * d; + } + length = sqrt(length); + for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS && !bad; joint_num++) { + double d = fabs(target[joint_num] - start[joint_num]); + joint = &joints[joint_num]; + if (!GET_JOINT_ACTIVE_FLAG(joint) || d < TP_POS_EPSILON) { continue; } + if (joint->vel_limit <= 0.0 || joint->acc_limit <= 0.0) { + reportError(_("joint interpolated move on line %d: joint %d has no velocity or acceleration limit"), + emcmotCommand->id, joint_num); + bad = 1; + break; + } + if (!moving || joint->vel_limit * length / d < vmax) { vmax = joint->vel_limit * length / d; } + if (!moving || joint->acc_limit * length / d < amax) { amax = joint->acc_limit * length / d; } + if (joint->jerk_limit > 0.0) { + if (!jerk_limited || joint->jerk_limit * length / d < jmax) { jmax = joint->jerk_limit * length / d; } + jerk_limited = 1; + } + moving = 1; + } + if (bad) { + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } + + /* a feed asks for a time; the joint limits still cap it */ + double vreq = vmax; + if (emcmotCommand->joint_seconds > 0.0 && length / emcmotCommand->joint_seconds < vmax) { + vreq = length / emcmotCommand->joint_seconds; + } + tpSetId(&emcmotInternal->coord_tp, emcmotCommand->id); + int res_addjoint = tpAddJointLine(&emcmotInternal->coord_tp, + start, target, NO_OF_KINS_JOINTS, end, + emcmotCommand->motion_type, + vreq, vmax, amax, jmax, + emcmotStatus->enables_new, + emcmotCommand->tag); + if (res_addjoint < 0) { + reportError(_("can't add joint interpolated move at line %d, error code %d"), + emcmotCommand->id, res_addjoint); + emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } else if (res_addjoint == 0) { + SET_MOTION_ERROR_FLAG(0); + rehomeAll = 1; + } + break; + } + case EMCMOT_SET_CIRCLE: /* emcmotInternal->coord_tp up a circular move */ /* requires coordinated mode, enable on, not on limits */ @@ -2001,6 +2212,15 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) case EMCMOT_SET_OFFSET: rtapi_print_msg(RTAPI_MSG_DBG, "SET_OFFSET"); + if (kinematicsSetTool) { + /* the module applies the offset, so the point the joints + stand on changes with it */ + kinematicsSetTool(&emcmotCommand->tool_offset); + emcmotToolOffsetChanged(&emcmotStatus->tool_offset, + &emcmotCommand->tool_offset, + &emcmotCommand->pos, + emcmotCommand->have_point); + } emcmotStatus->tool_offset = emcmotCommand->tool_offset; break; diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 50ddadc6c07..05ce75a63d5 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -194,6 +194,7 @@ static void output_to_hal(void); static void update_status(void); static void handle_kinematicsSwitch(void); +static int reanchor_pose(const double *joint_pos, EmcPose *at); /*********************************************************************** * PUBLIC FUNCTION CODE * @@ -363,12 +364,6 @@ static void handle_kinematicsSwitch(void) { 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 double beforePose[EMCMOT_MAX_AXIS]; int anum; @@ -376,15 +371,22 @@ static void handle_kinematicsSwitch(void) { beforePose[anum] = *pcmd_p[anum]; } #endif - EmcPose poseKinsSwitch = emcmotStatus->carte_pos_cmd; - if (kinematicsForward(joint_posKinsSwitch, &poseKinsSwitch, - &tmpFFlags, &tmpIFlags)) { - reportError(_("kinematicsForward failed for kinematics type %d"), - switchkins_type); + /* the joints stay where they are, so a kinematics whose forward cannot + solve them is one the machine cannot run in from here: put the old + one back, or the inverse would run the joints to wherever the pose + we know lands in the new one */ + if (reanchor_pose(joint_posKinsSwitch, NULL) != 0) { + kinematicsSwitch(switchkins_type); + reportError(_("kinematicsForward failed for kinematics type %d," + " type %d is still in force"), + requested_type, switchkins_type); SET_MOTION_ERROR_FLAG(1); // abort - return; // keep the position we know rather than an unsolved one + return; // the kinematics in force and the position are unchanged } - emcmotStatus->carte_pos_cmd = poseKinsSwitch; + + switchkins_type = requested_type; + hal_set_real(emcmot_hal_data->kins_type, (double)switchkins_type); + emcmotStatus->switchkins_type = switchkins_type; #ifdef SWITCHKINS_DEBUG fprintf(stderr,"kswitch type=%d (%s:%d)\n",switchkins_type,__FUNCTION__,__LINE__); for (anum = 0; anum < EMCMOT_MAX_AXIS; anum++) { @@ -392,9 +394,29 @@ static void handle_kinematicsSwitch(void) { ,anum,beforePose[anum],*pcmd_p[anum],*pcmd_p[anum]-beforePose[anum]); } #endif +} //handle_kinematicsSwitch() + +/* The point re-read from the joints, for when what the joints mean has + changed without the joints moving: a kinematics switch, or a tool + offset the module applies. The pose they put the tool at is the new + commanded point, the external offsets taken off it, and the planner + is moved onto it. A forward that fails leaves the point alone. + The pose with the external offsets still on it is returned in at. */ +static int reanchor_pose(const double *joint_pos, EmcPose *at) +{ + KINEMATICS_FORWARD_FLAGS tmpFFlags = fflags; + KINEMATICS_INVERSE_FLAGS tmpIFlags = iflags; + EmcPose pose = emcmotStatus->carte_pos_cmd; + + if (kinematicsForward(joint_pos, &pose, &tmpFFlags, &tmpIFlags) != 0) { + return -1; + } + emcmotStatus->carte_pos_cmd = pose; + if (at) { *at = pose; } axis_apply_ext_offsets_to_carte_pos(-1, pcmd_p); tpSetPos(&emcmotInternal->coord_tp, &emcmotStatus->carte_pos_cmd); -} //handle_kinematicsSwitch() + return 0; +} static void process_inputs(void) { @@ -923,6 +945,82 @@ static void check_for_faults(void) } } +/* The joints a joint interpolated segment ended on, held while the + planner stays at that point: a module's inverse answers with its own + joint set, which a robot wrist reaches with the forearm turned half a + revolution from the one asked for. The hold ends when the point moves. */ +static int joint_hold_valid = 0; +static double joint_hold[EMCMOT_MAX_JOINTS]; +static EmcPose joint_hold_pose; + +/* whether two machine points are within tol of each other on every axis */ +static int carte_pos_within(const EmcPose *a, const EmcPose *b, double tol) +{ + return fabs(a->tran.x - b->tran.x) < tol && fabs(a->tran.y - b->tran.y) < tol + && fabs(a->tran.z - b->tran.z) < tol + && fabs(a->a - b->a) < tol && fabs(a->b - b->b) < tol && fabs(a->c - b->c) < tol + && fabs(a->u - b->u) < tol && fabs(a->v - b->v) < tol && fabs(a->w - b->w) < tol; +} + +/* whether two machine points are the same, to a hair either way */ +static int same_carte_pos(const EmcPose *a, const EmcPose *b) +{ + return carte_pos_within(a, b, 1e-9); +} + +/* A tool offset the module applies has changed under the point. The + machine stays where it is: the point is re-read from the joints under + the new offset, and the joints are held there, since the inverse of the + re-read point may answer with another joint set. The interpreter + works out the same point ahead of motion and sends it along when it + can evaluate the kinematics; where the two disagree, or where it could + not say and the point moved, the program is not let go on from a point + the interpreter does not have. Nothing to do outside coordinated mode: + the point follows the joints there anyway. */ +void emcmotToolOffsetChanged(const EmcPose *from, const EmcPose *to, + const EmcPose *expected, int have_expected) +{ + const double tol = 1e-4; + double joint_pos[EMCMOT_MAX_JOINTS] = {0,}; + EmcPose was = emcmotStatus->carte_pos_cmd; + EmcPose now; + int joint_num; + + if (same_carte_pos(from, to) || !GET_MOTION_COORD_FLAG()) { return; } + for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) { + joint_pos[joint_num] = joints[joint_num].coarse_pos; + } + if (reanchor_pose(joint_pos, &now) != 0) { + reportError(_("the kinematics cannot place the tool from the joints" + " after the tool offset change")); + SET_MOTION_ERROR_FLAG(1); + return; + } + for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { + joint_hold[joint_num] = joint_pos[joint_num]; + } + joint_hold_pose = now; + joint_hold_valid = 1; + + if (have_expected) { + if (!carte_pos_within(&emcmotStatus->carte_pos_cmd, expected, tol)) { + reportError(_("the tool offset change put the point at" + " %.4f %.4f %.4f, the interpreter expected" + " %.4f %.4f %.4f"), + emcmotStatus->carte_pos_cmd.tran.x, + emcmotStatus->carte_pos_cmd.tran.y, + emcmotStatus->carte_pos_cmd.tran.z, + expected->tran.x, expected->tran.y, expected->tran.z); + SET_MOTION_ERROR_FLAG(1); + } + } else if (!carte_pos_within(&was, &now, tol)) { + reportError(_("the tool offset change moved the point under a" + " kinematics the interpreter cannot evaluate;" + " change the tool offset with the machine untilted")); + SET_MOTION_ERROR_FLAG(1); + } +} + static void set_operating_mode(void) { int joint_num; @@ -1421,18 +1519,73 @@ static void get_pos_cmds(long period) emcmotStatus->syncOverrunSpindle = 0; SET_MOTION_ERROR_FLAG(1); } + + if (tpGetJointPos(&emcmotInternal->coord_tp, positions) > 0) { + /* a joint interpolated segment: the planner hands out the + joints and the forward kinematics says where the tool is, + for status and for the display; nothing is inverted, and + the planner's own position is the chord between the ends. + The joints are commanded either way; a forward that fails, + as an iterating one can at a singularity, leaves the last + solved position reported rather than an unsolved one */ + EmcPose pose = emcmotStatus->carte_pos_cmd; + if (kinematicsForward(positions, &pose, &fflags, &iflags) == 0) { + emcmotStatus->carte_pos_cmd = pose; + emcmotStatus->carte_pos_cmd_ok = 1; + } else { + emcmotStatus->carte_pos_cmd_ok = 0; + } + result = 0; + } else { + /* a joint interpolated segment that ended this cycle is gone + from the queue: its end joints seed the inverse, since the + modules that read their rotary angles from the seed would + otherwise get last cycle's */ + int joint_end_fresh = tpTakeJointEnd(&emcmotInternal->coord_tp, positions); /* get new commanded traj pos */ tpGetPos(&emcmotInternal->coord_tp, &emcmotStatus->carte_pos_cmd); - if (axis_update_coord_with_bound(pcmd_p, servo_period)) { + if (tpJointSegmentsQueued(&emcmotInternal->coord_tp)) { + /* an external offset cannot ride on a joint interpolated + segment: its joints were solved without one, and the + queue refused the segment while one was applied. A + request that arrives while one is queued waits here, + unplanned, and ramps in at its own limits once the last + joint segment is done, instead of landing as a step at + the segment's ends */ + } else if (axis_update_coord_with_bound(pcmd_p, servo_period)) { ext_offset_coord_limit = 1; } else { ext_offset_coord_limit = 0; } - /* OUTPUT KINEMATICS - convert to joints in local array */ - result = kinematicsInverse(&emcmotStatus->carte_pos_cmd, positions, - &iflags, &fflags); + /* OUTPUT KINEMATICS - convert to joints in local array, or + hold the joints a joint interpolated segment ended on while + the planner stays at the point they put the machine on */ + if (joint_end_fresh) { + EmcPose at = emcmotStatus->carte_pos_cmd; + joint_hold_valid = 0; + if (kinematicsForward(positions, &at, &fflags, &iflags) == 0 + && same_carte_pos(&at, &emcmotStatus->carte_pos_cmd)) { + for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { + joint_hold[joint_num] = positions[joint_num]; + } + joint_hold_pose = emcmotStatus->carte_pos_cmd; + joint_hold_valid = 1; + } + } + if (joint_hold_valid + && same_carte_pos(&joint_hold_pose, &emcmotStatus->carte_pos_cmd)) { + for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { + positions[joint_num] = joint_hold[joint_num]; + } + result = 0; + } else { + joint_hold_valid = 0; + result = kinematicsInverse(&emcmotStatus->carte_pos_cmd, positions, + &iflags, &fflags); + } + } if(result == 0) { /* copy to joint structures and spline them up */ diff --git a/src/emc/motion/mot_priv.h b/src/emc/motion/mot_priv.h index a996e183fa6..847fa57aa38 100644 --- a/src/emc/motion/mot_priv.h +++ b/src/emc/motion/mot_priv.h @@ -271,6 +271,8 @@ extern void refresh_jog_limits(emcmot_joint_t *joint,int joint_num); extern void clearHomes(int joint_num); extern void emcmot_config_change(void); +extern void emcmotToolOffsetChanged(const EmcPose *from, const EmcPose *to, + const EmcPose *expected, int have_expected); extern void reportError(const char *fmt, ...) __attribute__((format(printf,1,2))); /* Use the rtapi_print call */ diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 06ff25eb0ba..091e420e10b 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -116,6 +116,7 @@ extern "C" { EMCMOT_SET_LINE, /* queue up a linear move */ EMCMOT_SET_CIRCLE, /* queue up a circular move */ + EMCMOT_SET_JOINT_LINE, /* queue up a joint interpolated move */ EMCMOT_CLEAR_PROBE_FLAGS, /* clears probeTripped flag */ EMCMOT_PROBE, /* go to pos, stop if probe trips, record trip pos */ @@ -273,6 +274,17 @@ extern "C" { struct state_tag_t tag; int switchkins_type; /* switchkins type requested by G12.1 */ + + /* a joint interpolated move: either pos is the endpoint and the joints + come from the inverse, or these are the joints and pos comes from + the forward */ + double joint_target[EMCMOT_MAX_JOINTS]; + int have_joint_target; + double joint_seconds; /* 0 for a rapid, else the time the move is to take */ + + /* SET_OFFSET: pos is where the interpreter expects the point to be + once the offset is on, for motion to check its own answer against */ + int have_point; } emcmot_command_t; /*! \todo FIXME - these packed bits might be replaced with chars diff --git a/src/emc/motion_planning/Submakefile b/src/emc/motion_planning/Submakefile new file mode 100644 index 00000000000..3a8ccdf737a --- /dev/null +++ b/src/emc/motion_planning/Submakefile @@ -0,0 +1,48 @@ +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 \ + ) + +# kins_util.c is the shared kinematics code the modules link; the loader +# needs the same block helpers and ops dispatch on this side of dlopen. +LIBKINSLIMITS_CSRCS := $(addprefix emc/kinematics_userspace/, \ + kinematics_user.c \ + ) emc/kinematics/kins_util.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..ba8c69fdd42 --- /dev/null +++ b/src/emc/motion_planning/jacobian.cc @@ -0,0 +1,80 @@ +/******************************************************************** + * 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 + +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; +} + +bool JacobianCalculator::compute(const EmcPose& pose, double J[9][9]) { + if (!kins_ctx_) { + return false; + } + std::memset(J, 0, sizeof(double) * 9 * 9); + return kinematicsUserJacobian(kins_ctx_, &pose, J) == 0; +} + +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..2db3cf00d3e --- /dev/null +++ b/src/emc/motion_planning/jacobian.hh @@ -0,0 +1,83 @@ +/******************************************************************** + * Description: jacobian.hh + * Jacobian calculation for userspace kinematics trajectory planning + * + * Computes the Jacobian matrix relating world velocities to joint + * velocities, from the module's own closed form through the non-RT + * kinematics loader. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ +#ifndef JACOBIAN_HH +#define JACOBIAN_HH + +#include +#include + +namespace motion_planning { + +/** + * Jacobian calculator class + * + * Computes the Jacobian matrix J where: + * joint_velocities = J × world_velocities + * + * The module answers: a closed form where it has one, its inverse + * differenced where it does not. See kinematicsUserJacobian(). + */ +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: + KinematicsUserContext* kins_ctx_; + bool is_identity_; + int num_joints_; +}; + +} // 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..d8bcbd82b57 --- /dev/null +++ b/src/emc/motion_planning/joint_limits.hh @@ -0,0 +1,234 @@ +/******************************************************************** + * 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 + +#include +#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 51fce63005f..0e9afe6679e 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -155,6 +155,9 @@ typedef struct CanonConfig_t { rotary_unlock_for_traverse(-1), g5xOffset{}, g92Offset{}, + g68Offset{}, + g68Rotation{1, 0, 0, 0, 1, 0, 0, 0, 1}, + g68Active(0), endPoint{}, lengthUnits(CANON_UNITS_INCHES), activePlane(CANON_PLANE::XY), @@ -178,6 +181,11 @@ typedef struct CanonConfig_t { CANON_POSITION g5xOffset; CANON_POSITION g92Offset; +/* The tilted work plane (G68.2): a frame inside the G92 stage of the chain, + in mm. Program X Y Z go through R * xyz + O before anything else. */ + double g68Offset[3]; + double g68Rotation[9]; // row major + int g68Active; /* canonEndPoint is the last programmed end point, stored in case it's needed for subsequent calculations. It's in absolute frame, mm units. @@ -243,6 +251,12 @@ extern void SET_G92_OFFSET(double x, double y, double z, extern void SET_XY_ROTATION(double t); +/* The tilted work plane. Origin in program units and a row major rotation + matrix, both in the coordinate system active when the plane was defined; + active 0 cancels it. */ +extern void SET_G68_FRAME(double x, double y, double z, + const double rotation[9], int active); + /* Offset the origin to the point with absolute coordinates x, y, z, a, b, c, u, v, and w. Values of x, y, z, a, b, c, u, v, and w are real numbers. The units are whatever length units are being used at the time @@ -277,6 +291,24 @@ extern void STRAIGHT_TRAVERSE(int lineno, double x, double y, double z, double a, double b, double c, double u, double v, double w); + +/* A traverse interpolated in joint space. The endpoint x..w is in program + coordinates like STRAIGHT_TRAVERSE's; motion runs the inverse once there + and interpolates the joints to it. With have_joints the joints given + (machine units, one per joint) are the endpoint instead and x..w say + where the interpreter believes that is. Nothing blends into or out of + it. */ +extern void JOINT_TRAVERSE(int lineno, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w); +/* The same move at feed: it is to take 'seconds' seconds at the programmed + feed, the feed override applies, and the joint limits still cap it. */ +extern void JOINT_FEED(int lineno, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w, + double seconds); /* Move at traverse rate so that at any time during the move, all axes @@ -659,6 +691,11 @@ extern void USE_NO_SPINDLE_FORCE(); extern void SET_TOOL_TABLE_ENTRY(int pocket, int toolno, const EmcPose& offset, double diameter, double frontangle, double backangle, int orientation); extern void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset); +/* The same, with the point the interpreter expects the machine to stand + on once the offset is applied, in program coordinates: where a + kinematics applies the offset itself, motion keeps the joints and + re-reads the point from them, and compares it with this one. */ +extern void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset, const EmcPose& point); extern void CHANGE_TOOL(); @@ -913,6 +950,10 @@ extern int GET_EXTERNAL_KINS_TYPE(); kinematics.h); -1 where it says nothing: no such type, plain kinematics, or no motion controller attached (sai, preview) */ extern int GET_EXTERNAL_KINS_TYPE_FLAGS(int ktype); +/* whether the machine's kinematics is the identity, the joints being the + world, so that no tool offset is applied by the kinematics; true where + no motion controller is attached (sai, a preview without a machine) */ +extern bool GET_EXTERNAL_KINEMATICS_IDENTITY(); // Returns the current motion path-following tolerance extern double GET_EXTERNAL_MOTION_CONTROL_TOLERANCE(); @@ -979,6 +1020,12 @@ extern double GET_EXTERNAL_POSITION_V(); // returns the current w-axis position extern double GET_EXTERNAL_POSITION_W(); +// Copies up to max of the joint positions the machine stands in and +// returns how many were written. A point does not name one joint set, so +// an iterative inverse needs somewhere to start. Zero when the caller +// has no machine to ask. +extern int GET_EXTERNAL_JOINT_POSITIONS(double *joints, int max); + // Returns the position of the specified axis at the last probe trip, // in the current work coordinate system. diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index bf20de160e3..82059df40c6 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -308,6 +308,12 @@ int emcFormat(NMLTYPE type, void *buffer, CMS * cms) case EMC_TRAJ_SET_ROTATION_TYPE: ((EMC_TRAJ_SET_ROTATION *) buffer)->update(cms); break; + case EMC_TRAJ_SET_G68_TYPE: + ((EMC_TRAJ_SET_G68 *) buffer)->update(cms); + break; + case EMC_TRAJ_JOINT_MOVE_TYPE: + ((EMC_TRAJ_JOINT_MOVE *) buffer)->update(cms); + break; case EMC_TRAJ_SET_SCALE_TYPE: ((EMC_TRAJ_SET_SCALE *) buffer)->update(cms); break; @@ -531,6 +537,10 @@ const char *emc_symbol_lookup(uint32_t type) return "EMC_TRAJ_SET_G92"; case EMC_TRAJ_SET_ROTATION_TYPE: return "EMC_TRAJ_SET_ROTATION"; + case EMC_TRAJ_SET_G68_TYPE: + return "EMC_TRAJ_SET_G68"; + case EMC_TRAJ_JOINT_MOVE_TYPE: + return "EMC_TRAJ_JOINT_MOVE"; case EMC_TRAJ_SET_SCALE_TYPE: return "EMC_TRAJ_SET_SCALE"; case EMC_TRAJ_SET_RAPID_SCALE_TYPE: @@ -1399,6 +1409,9 @@ void EMC_TASK_STAT::update(CMS * cms) cms->update(g5x_index); EmcPose_update(cms, &g92_offset); cms->update(rotation_xy); + EmcPose_update(cms, &g68_offset); + cms->update(g68_rotation, 9); + cms->update(g68_active); EmcPose_update(cms, &toolOffset); cms->update(activeGCodes, ACTIVE_G_CODES); cms->update(activeMCodes, ACTIVE_M_CODES); @@ -1594,6 +1607,8 @@ void EMC_TRAJ_SET_OFFSET::update(CMS * cms) { EMC_TRAJ_CMD_MSG::update(cms); EmcPose_update(cms, &offset); + EmcPose_update(cms, &point); + cms->update(have_point); } // cppcheck-suppress duplInheritedMember @@ -1682,6 +1697,25 @@ void EMC_TRAJ_SET_ROTATION::update(CMS * cms) cms->update(rotation); } +// cppcheck-suppress duplInheritedMember +void EMC_TRAJ_SET_G68::update(CMS * cms) +{ + EMC_TRAJ_CMD_MSG::update(cms); + EmcPose_update(cms, &origin); + cms->update(rotation, 9); + cms->update(active); +} + +// cppcheck-suppress duplInheritedMember +void EMC_TRAJ_JOINT_MOVE::update(CMS * cms) +{ + EMC_TRAJ_CMD_MSG::update(cms); + EmcPose_update(cms, &end); + cms->update(joints, EMCMOT_MAX_JOINTS); + cms->update(have_joints); + cms->update(seconds); +} + /* * NML/CMS Update function for EMC_SPINDLE_BRAKE_ENGAGE * Automatically generated by NML CodeGen Java Applet. diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index bf4777478e7..9da8f5fdfe2 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -111,6 +111,8 @@ struct PM_CARTESIAN; #define EMC_TRAJ_SET_SO_ENABLE_TYPE ((NMLTYPE) 235) #define EMC_TRAJ_SET_FH_ENABLE_TYPE ((NMLTYPE) 236) #define EMC_TRAJ_RIGID_TAP_TYPE ((NMLTYPE) 237) +#define EMC_TRAJ_SET_G68_TYPE ((NMLTYPE) 239) +#define EMC_TRAJ_JOINT_MOVE_TYPE ((NMLTYPE) 240) #define EMC_TRAJ_SELECT_KINS_TYPE ((NMLTYPE) 289) #define EMC_TRAJ_STAT_TYPE ((NMLTYPE) 299) @@ -374,11 +376,12 @@ extern int emcTrajResume(); extern int emcTrajDelay(double delay); extern int emcTrajLinearMove(const EmcPose& end, int type, double vel, double ini_maxvel, double acc, double ini_maxjerk, int indexer_jnum); +extern int emcTrajJointMove(const EmcPose& end, const double *joints, int have_joints, double seconds); extern int emcTrajCircularMove(const EmcPose& end, const PM_CARTESIAN& center, const PM_CARTESIAN& normal, int turn, int type, double vel, double ini_maxvel, double acc, double ini_maxjerk); extern int emcTrajSetTermCond(int cond, double tolerance); extern int emcTrajSetSpindleSync(int spindle, double feed_per_revolution, bool wait_for_index); -extern int emcTrajSetOffset(const EmcPose& tool_offset); +extern int emcTrajSetOffset(const EmcPose& tool_offset, const EmcPose *point); extern int emcTrajSetHome(const EmcPose& home); extern int emcTrajClearProbeTrippedFlag(); extern int emcTrajProbe(const EmcPose& pos, int type, double vel, diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index 7690268f9ee..c7feda8a7e6 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -832,7 +832,7 @@ class EMC_TRAJ_SET_OFFSET:public EMC_TRAJ_CMD_MSG { public: EMC_TRAJ_SET_OFFSET() : EMC_TRAJ_CMD_MSG(EMC_TRAJ_SET_OFFSET_TYPE, sizeof(EMC_TRAJ_SET_OFFSET)), - offset{} + offset{}, point{}, have_point(0) {}; // Sub-class update() calls base-class update() @@ -841,6 +841,10 @@ class EMC_TRAJ_SET_OFFSET:public EMC_TRAJ_CMD_MSG { void update(CMS * cms); EmcPose offset; + // where the interpreter expects the machine to stand once the offset + // is on, for motion to check its own answer against; only when set + EmcPose point; + int have_point; }; class EMC_TRAJ_SET_G5X:public EMC_TRAJ_CMD_MSG { @@ -889,6 +893,26 @@ class EMC_TRAJ_SET_ROTATION:public EMC_TRAJ_CMD_MSG { double rotation; }; +// the tilted work plane frame (G68.2, G68.3, G68.4, G69): origin in user +// units and a rotation matrix, both in the coordinate system that was active +// when the plane was defined +class EMC_TRAJ_SET_G68:public EMC_TRAJ_CMD_MSG { + public: + EMC_TRAJ_SET_G68() + : EMC_TRAJ_CMD_MSG(EMC_TRAJ_SET_G68_TYPE, sizeof(EMC_TRAJ_SET_G68)), + origin{}, rotation{1, 0, 0, 0, 1, 0, 0, 0, 1}, active(0) + {}; + + // For internal NML/CMS use only. + // Sub-class update() calls base-class update() + // cppcheck-suppress duplInheritedMember + void update(CMS * cms); + + EmcPose origin; + double rotation[9]; // row major + int active; +}; + class EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG:public EMC_TRAJ_CMD_MSG { public: EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG() @@ -940,6 +964,27 @@ class EMC_TRAJ_PROBE:public EMC_TRAJ_CMD_MSG { unsigned char probe_type; }; +// a move interpolated in joint space: to the world endpoint, whose joints +// motion finds with the inverse; or to the joints given, whose world +// position motion finds with the forward +class EMC_TRAJ_JOINT_MOVE:public EMC_TRAJ_CMD_MSG { + public: + EMC_TRAJ_JOINT_MOVE() + : EMC_TRAJ_CMD_MSG(EMC_TRAJ_JOINT_MOVE_TYPE, sizeof(EMC_TRAJ_JOINT_MOVE)), + end{}, joints{}, have_joints(0), seconds(0.0) + {}; + + // For internal NML/CMS use only. + // Sub-class update() calls base-class update() + // cppcheck-suppress duplInheritedMember + void update(CMS * cms); + + EmcPose end; + double joints[EMCMOT_MAX_JOINTS]; + int have_joints; + double seconds; /* 0 for a rapid, else the time it is to take */ +}; + class EMC_TRAJ_RIGID_TAP:public EMC_TRAJ_CMD_MSG { public: EMC_TRAJ_RIGID_TAP() @@ -1493,6 +1538,9 @@ class EMC_TASK_STAT:public EMC_TASK_STAT_MSG { int g5x_index; // index of active g5x system EmcPose g92_offset; // in user units, currently active double rotation_xy; + EmcPose g68_offset; // tilted work plane origin, in user units + double g68_rotation[9]; // tilted work plane rotation, row major + int g68_active; // a tilted work plane is in effect EmcPose toolOffset; // tool offset, in general pose form int activeGCodes[ACTIVE_G_CODES]; int activeMCodes[ACTIVE_M_CODES]; diff --git a/src/emc/nml_intf/emcops.cc b/src/emc/nml_intf/emcops.cc index c6e77f40d9d..d38ebd8bced 100644 --- a/src/emc/nml_intf/emcops.cc +++ b/src/emc/nml_intf/emcops.cc @@ -141,6 +141,9 @@ EMC_TASK_STAT::EMC_TASK_STAT() g5x_index(0), g92_offset{}, rotation_xy(0.0), + g68_offset{}, + g68_rotation{1, 0, 0, 0, 1, 0, 0, 0, 1}, + g68_active(0), toolOffset{}, activeSettings{}, programUnits(CANON_UNITS_MM), diff --git a/src/emc/rs274ngc/Submakefile b/src/emc/rs274ngc/Submakefile index 28e974a36c5..895258a6e24 100644 --- a/src/emc/rs274ngc/Submakefile +++ b/src/emc/rs274ngc/Submakefile @@ -15,6 +15,7 @@ LIBRS274SRCS := $(addprefix emc/rs274ngc/, \ interp_inverse.cc \ interp_read.cc \ interp_write.cc \ + interp_workplane.cc \ interp_o_word.cc \ interp_g7x.cc \ nurbs_additional_functions.cc \ @@ -41,7 +42,7 @@ TARGETS += ../lib/librs274.so ../lib/librs274.so.0 ../lib/librs274.so.0: $(patsubst %.cc,objects/%.o,$(LIBRS274SRCS)) \ ../lib/liblinuxcncini.so ../lib/libpyplugin.so ../lib/liblinuxcnchal.so.0 \ - ../lib/libtooldata.so.0 + ../lib/libtooldata.so.0 ../lib/libkinslimits.so.0 ../lib/libposemath.so.0 $(ECHO) Linking $(notdir $@) @mkdir -p ../lib @rm -f $@ diff --git a/src/emc/rs274ngc/canonmodule.cc b/src/emc/rs274ngc/canonmodule.cc index 002be0c8de9..447001c9efc 100644 --- a/src/emc/rs274ngc/canonmodule.cc +++ b/src/emc/rs274ngc/canonmodule.cc @@ -247,7 +247,7 @@ BOOST_PYTHON_MODULE(emccanon) { def("USE_NO_SPINDLE_FORCE",&USE_NO_SPINDLE_FORCE); // def("USER_DEFINED_FUNCTION_ADD",&USER_DEFINED_FUNCTION_ADD); // def("USE_SPINDLE_FORCE",&USE_SPINDLE_FORCE); - def("USE_TOOL_LENGTH_OFFSET",&USE_TOOL_LENGTH_OFFSET); + def("USE_TOOL_LENGTH_OFFSET",static_cast(&USE_TOOL_LENGTH_OFFSET)); def("WAIT",&WAIT); // from interp_queue.cc diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index 58df5809b06..8b6d596b9be 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -49,6 +49,7 @@ #include "rs274ngc_interp.hh" #include "nml_intf/interp_return.hh" #include "nml_intf/canon.hh" +#include // KINEMATICS_IDENTITY int _task = 0; // control preview behaviour when remapping @@ -636,6 +637,28 @@ void STRAIGHT_FEED(int line_number, Py_XDECREF(result); } +// the preview draws a joint interpolated move as the traverse between its +// ends: the path between them depends on the kinematics, which the +// preview does not have +void JOINT_TRAVERSE(int line_number, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w) { + (void)joints; + (void)have_joints; + STRAIGHT_TRAVERSE(line_number, x, y, z, a, b, c, u, v, w); +} + +void JOINT_FEED(int line_number, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w, double seconds) { + (void)joints; + (void)have_joints; + (void)seconds; + STRAIGHT_FEED(line_number, x, y, z, a, b, c, u, v, w); +} + void STRAIGHT_TRAVERSE(int line_number, double x, double y, double z, double a, double b, double c, @@ -694,6 +717,21 @@ void SET_XY_ROTATION(double t) { Py_XDECREF(result); }; +void SET_G68_FRAME(double x, double y, double z, + const double rotation[9], int active) { + if(metric) { x /= 25.4; y /= 25.4; z /= 25.4; } + maybe_new_line(); + if(interp_error) return; + PyObject *result = + callmethod(callback, "set_g68_frame", "ffffffffffffi", + x, y, z, + rotation[0], rotation[1], rotation[2], + rotation[3], rotation[4], rotation[5], + rotation[6], rotation[7], rotation[8], active); + if(result == NULL) interp_error ++; + Py_XDECREF(result); +}; + void USE_LENGTH_UNITS(CANON_UNITS u) { metric = u == CANON_UNITS_MM; } void SELECT_PLANE(CANON_PLANE pl) { @@ -864,6 +902,10 @@ void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) { Py_XDECREF(result); } +void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset, const EmcPose& /*point*/) { + USE_TOOL_LENGTH_OFFSET(offset); +} + void SET_FEED_REFERENCE(double /*reference*/) { } void SET_CUTTER_RADIUS_COMPENSATION(double /*radius*/) {} void START_CUTTER_RADIUS_COMPENSATION(int /*direction*/) {} @@ -890,13 +932,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)switchkins_type; - printf("gcodemodule: SELECT_KINS_TYPE\n"); - - return; -} +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;} @@ -1012,6 +1048,31 @@ double GET_EXTERNAL_POSITION_C() { return _pos_c; } double GET_EXTERNAL_POSITION_U() { return _pos_u; } double GET_EXTERNAL_POSITION_V() { return _pos_v; } double GET_EXTERNAL_POSITION_W() { return _pos_w; } + +// Where the machine's joints stand. A point does not name one joint set, +// so an iterative inverse has to start somewhere: a canon that watches the +// status buffer says where. One that cannot answers nothing, and the +// interpreter works from the point alone. +int GET_EXTERNAL_JOINT_POSITIONS(double *joints, int max) { + PyObject *result, *seq; + Py_ssize_t n, i; + + if(interp_error) return 0; + if(!PyObject_HasAttrString(callback, "get_external_joint_positions")) return 0; + result = callmethod(callback, "get_external_joint_positions", ""); + if(result == NULL) { PyErr_Clear(); return 0; } + seq = PySequence_Fast(result, "joint positions"); + if(seq == NULL) { PyErr_Clear(); Py_DECREF(result); return 0; } + n = PySequence_Fast_GET_SIZE(seq); + if(n > max) n = max; + for(i = 0; i < n; i++) { + joints[i] = PyFloat_AsDouble(PySequence_Fast_GET_ITEM(seq, i)); + if(PyErr_Occurred()) { PyErr_Clear(); n = 0; break; } + } + Py_DECREF(seq); + Py_DECREF(result); + return (int)n; +} void INIT_CANON() {} void SET_PARAMETER_FILE_NAME(const char *name) @@ -1208,6 +1269,24 @@ void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode) { motion_mode = mode; } CANON_MOTION_MODE GET_EXTERNAL_MOTION_CONTROL_MODE() { return motion_mode; } int GET_EXTERNAL_KINS_TYPE() { return 0; } int GET_EXTERNAL_KINS_TYPE_FLAGS(int ktype) { (void)ktype; return -1; } + +// the kind of transform the machine runs, from a canon that watches the +// status buffer; one that cannot answer has no machine, and the joints +// are the world +bool GET_EXTERNAL_KINEMATICS_IDENTITY() { + PyObject *result; + bool identity = true; + + if(interp_error) return true; + if(!PyObject_HasAttrString(callback, "get_kinematics_type")) return true; + result = callmethod(callback, "get_kinematics_type", ""); + if(result == NULL) { PyErr_Clear(); return true; } + if(PyLong_Check(result)) { + identity = (PyLong_AsLong(result) == KINEMATICS_IDENTITY); + } + Py_DECREF(result); + return identity; +} void SET_NAIVECAM_TOLERANCE(double /*tolerance*/) { } #define RESULT_OK (result == INTERP_OK || result == INTERP_EXECUTE_FINISH) @@ -1457,7 +1536,8 @@ static PyObject *rs274_arc_to_segments(PyObject * /*self*/, PyObject *args) { PyObject *canon; double x1, y1, cx, cy, z1, a, b, c, u, v, w; double o[9], n[9], g5xoffset[9], g92offset[9]; - int rot, plane; + double g68o[3], g68r[9]; + int rot, plane, g68active; int X, Y, Z; double rotation_cos, rotation_sin; int max_segments = 128; @@ -1488,6 +1568,32 @@ static PyObject *rs274_arc_to_segments(PyObject * /*self*/, PyObject *args) { if(!get_attr(canon, "g92_offset_u", &g92offset[6])) return NULL; if(!get_attr(canon, "g92_offset_v", &g92offset[7])) return NULL; if(!get_attr(canon, "g92_offset_w", &g92offset[8])) return NULL; + if(!get_attr(canon, "g68_active", &g68active)) return NULL; + if(g68active) { + if(!get_attr(canon, "g68_offset", "ddd:arcs_to_segments g68_offset", + &g68o[0], &g68o[1], &g68o[2])) + return NULL; + if(!get_attr(canon, "g68_rotation", "ddddddddd:arcs_to_segments g68_rotation", + &g68r[0], &g68r[1], &g68r[2], &g68r[3], &g68r[4], + &g68r[5], &g68r[6], &g68r[7], &g68r[8])) + return NULL; + } + // the tilted work plane sits inside G92: take it off the last point on + // the way in and put it back on every point on the way out + auto g68_remove = [&](double *p) { + if(!g68active) return; + double x = p[0] - g68o[0], y = p[1] - g68o[1], z = p[2] - g68o[2]; + p[0] = g68r[0]*x + g68r[3]*y + g68r[6]*z; + p[1] = g68r[1]*x + g68r[4]*y + g68r[7]*z; + p[2] = g68r[2]*x + g68r[5]*y + g68r[8]*z; + }; + auto g68_apply = [&](double *p) { + if(!g68active) return; + double x = p[0], y = p[1], z = p[2]; + p[0] = g68r[0]*x + g68r[1]*y + g68r[2]*z + g68o[0]; + p[1] = g68r[3]*x + g68r[4]*y + g68r[5]*z + g68o[1]; + p[2] = g68r[6]*x + g68r[7]*y + g68r[8]*z + g68o[2]; + }; if(plane == 1) { X=0; Y=1; Z=2; @@ -1508,6 +1614,7 @@ static PyObject *rs274_arc_to_segments(PyObject * /*self*/, PyObject *args) { for(int ax=0; ax<9; ax++) o[ax] -= g5xoffset[ax]; unrotate(o[0], o[1], rotation_cos, rotation_sin); for(int ax=0; ax<9; ax++) o[ax] -= g92offset[ax]; + g68_remove(o); double theta1 = atan2(o[Y]-cy, o[X]-cx); double theta2 = atan2(n[Y]-cy, n[X]-cx); @@ -1550,12 +1657,14 @@ static PyObject *rs274_arc_to_segments(PyObject * /*self*/, PyObject *args) { p[6] = o[6] + d[6] * f; p[7] = o[7] + d[7] * f; p[8] = o[8] + d[8] * f; + g68_apply(p); for(int ax=0; ax<9; ax++) p[ax] += g92offset[ax]; rotate(p[0], p[1], rotation_cos, rotation_sin); for(int ax=0; ax<9; ax++) p[ax] += g5xoffset[ax]; PyList_SET_ITEM(segs, i, Py_BuildValue("ddddddddd", p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7], p[8])); } + g68_apply(n); for(int ax=0; ax<9; ax++) n[ax] += g92offset[ax]; rotate(n[0], n[1], rotation_cos, rotation_sin); for(int ax=0; ax<9; ax++) n[ax] += g5xoffset[ax]; diff --git a/src/emc/rs274ngc/interp_array.cc b/src/emc/rs274ngc/interp_array.cc index 63f6d20975a..456e68b338a 100644 --- a/src/emc/rs274ngc/interp_array.cc +++ b/src/emc/rs274ngc/interp_array.cc @@ -91,12 +91,12 @@ const int Interp::gees[] = { /* 360 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 380 */ -1,-1, 1, 1, 1, 1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 400 */ 7,-1,-1,-1,-1,-1,-1,-1,-1,-1, 7, 7,-1,-1,-1,-1,-1,-1,-1,-1, -/* 420 */ 7, 7,-1,-1,-1,-1,-1,-1,-1,-1, 8, 8, 8,-1,-1,-1,-1,-1,-1,-1, +/* 420 */ 7, 7,-1,-1,-1,-1,-1,-1,-1,-1, 8, 8, 8,-1, 8, 8,-1,-1,-1,-1, /* 440 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 460 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 480 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 8,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 500 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, -/* 520 */ 0,-1,-1,-1,-1,-1,-1,-1,-1,-1, 0,-1,-1,-1,-1,-1,-1,-1,-1,-1, +/* 520 */ 0,-1,-1,-1,-1,-1,-1,-1,-1,-1, 0, 0, 0, 0, 0, 0, 0, 0,-1,-1, /* 540 */ 12,-1,-1,-1,-1,-1,-1,-1,-1,-1,12,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 560 */ 12,-1,-1,-1,-1,-1,-1,-1,-1,-1,12,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 580 */ 12,-1,-1,-1,-1,-1,-1,-1,-1,-1,12,12,12,12,-1,-1,-1,-1,-1,-1, @@ -104,7 +104,7 @@ const int Interp::gees[] = { /* 620 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 640 */ 13,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 660 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, -/* 680 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, +/* 680 */ -1,-1, 9, 9, 9,-1,-1,-1,-1,-1, 9,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 700 */ 1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 1, 1, 1,-1,-1,-1,-1,-1,-1,-1, /* 720 */ 1, 1, 1,-1,-1,-1,-1,-1,-1,-1, 1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 740 */ 1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, @@ -228,6 +228,7 @@ const int Interp::required_parameters[] = { const int Interp::readonly_parameters[] = { 5021, 5022, 5023, 5024, 5025, 5026, 5027, 5028, 5029, // machine X Y ... W + 5071, 5072, 5073, 5074, 5075, 5076, 5077, 5078, 5079, 5080, // G53.2 pose X Y ... W, valid 5400, // tool toolno 5401, // tool x offset 5402, // tool y offset diff --git a/src/emc/rs274ngc/interp_base.hh b/src/emc/rs274ngc/interp_base.hh index 5ac25b3f446..7c930a1bf1e 100644 --- a/src/emc/rs274ngc/interp_base.hh +++ b/src/emc/rs274ngc/interp_base.hh @@ -63,6 +63,10 @@ public: virtual void print_state_tag(StateTag const &tag) = 0; virtual void set_loglevel(int level) = 0; virtual void set_loop_on_main_m99(bool state) = 0; + // true while the startup code runs at task init, when the motion + // queue cannot drain yet: a kinematics switch there queues without + // the drain-and-assert wait, which could never complete + virtual void set_in_startup_code(bool) {}; virtual FILE* get_stdout() { return stdout; }; }; diff --git a/src/emc/rs274ngc/interp_check.cc b/src/emc/rs274ngc/interp_check.cc index 8be61f9c609..3ab706efb5b 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -109,6 +109,30 @@ int Interp::check_g_codes(block_pointer block, //!< pointer to a block to be c (settings->distance_mode == DISTANCE_MODE::INCREMENTAL))), NCE_CANNOT_USE_G53_INCREMENTAL); } else if (mode0 == G_92) { + } else if (mode0 == G_53_1 || mode0 == G_53_2 || mode0 == G_53_6) { + CHKS((block->x_flag || block->y_flag || block->z_flag || block->a_flag || block->b_flag || + block->c_flag || block->u_flag || block->v_flag || block->w_flag), + _("Cannot use axis words with G53.1, G53.2 or G53.6")); + } else if (mode0 == G_53_3) { + CHKS((block->a_flag || block->b_flag || block->c_flag || block->u_flag || block->v_flag || block->w_flag), + _("Only X, Y and Z words can be used with G53.3")); + } else if (mode0 == G_53_4 || mode0 == G_53_5 || mode0 == G_53_7) { + CHKS(((block->motion_to_be != G_0) && (block->motion_to_be != G_1)), + _("G53.4, G53.5 and G53.7 need G0 or G1")); + if (mode0 == G_53_5 || mode0 == G_53_7) { + CHKS((block->radius_flag || block->theta_flag), + _("Cannot use polar coordinates with G53.5 or G53.7")); + CHKS(((block->g_modes[GM_DISTANCE_MODE] == G_91) || + ((block->g_modes[GM_DISTANCE_MODE] != G_90) && + (settings->distance_mode == DISTANCE_MODE::INCREMENTAL))), + _("Cannot use G53.5 or G53.7 in incremental distance mode")); + } + if (mode0 == G_53_7) { + CHKS((block->x_flag || block->y_flag || block->z_flag || block->a_flag || block->b_flag || + block->c_flag || block->u_flag || block->v_flag || block->w_flag), + _("G53.7 takes joint words, J=, not axis words; G53.5 takes the axis words")); + CHKS((block->j_flag), _("Cannot use a J word with G53.7; a joint is J=")); + } } else if (mode0 == G_12_1){ // kins-switch CHKS((!block->p_flag), NCE_P_WORD_MISSING_WITH_G121); @@ -285,29 +309,46 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block } if (block->h_flag) { - CHKS((block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43 && motion != G_76 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_2), - _("H word with no G43 or G76 to use it")); + CHKS((block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43 && motion != G_76 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_2 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_4 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_5), + _("H word with no G43, G43.4, G43.5 or G76 to use it")); } - if (block->i_flag) { /* could still be useless if yz_plane arc */ + // under G43.5 the I J K of a G0 or G1 line are the tool axis: on the + // line that puts G43.5 in effect, and on the lines after it that do + // not change the tool length mode + int tool_vector = (block->g_modes[GM_TOOL_LENGTH_OFFSET] == G_43_5 + || (_setup.tool_vector && block->g_modes[GM_TOOL_LENGTH_OFFSET] == -1)) + && (motion == G_0 || motion == G_1); + + if (block->i_flag && !tool_vector) { /* could still be useless if yz_plane arc */ CHKS(((motion != G_2) && (motion != G_3) && (motion != G_5) && (motion != G_5_1) && (motion != G_6) && (motion != G_6_1) && (motion != G_71) && (motion != G_71_1) && (motion != G_71_2) && (motion != G_72) && (motion != G_72_1) && (motion != G_72_2) && - (motion != G_76) && (motion != G_87) && (motion != G_33_1) && (block->g_modes[GM_MODAL_0] != G_10)), - _("I word with no G2, G3, G5, G5.1, G6, G6.1, G10, G33.1, G76, or G87 to use it")); + (motion != G_76) && (motion != G_87) && (motion != G_33_1) && (block->g_modes[GM_MODAL_0] != G_10) && + (block->g_modes[GM_WORK_PLANE] == -1)), + _("I word with no G2, G3, G5, G5.1, G6, G6.1, G10, G33.1, G68.2, G76, or G87 to use it")); } - if (block->j_flag) { /* could still be useless if xz_plane arc */ + if (block->j_flag && !tool_vector) { /* could still be useless if xz_plane arc */ CHKS(((motion != G_2) && (motion != G_3) && (motion != G_5) && (motion != G_5_1) && (motion != G_6) && (motion != G_6_1) && - (motion != G_76) && (motion != G_87) && (block->g_modes[GM_MODAL_0] != G_10)), - _("J word with no G2, G3, G5, G5.1, G6, G6.1, G10, G76 or G87 to use it")); + (motion != G_76) && (motion != G_87) && (block->g_modes[GM_MODAL_0] != G_10) && + (block->g_modes[GM_WORK_PLANE] == -1)), + _("J word with no G2, G3, G5, G5.1, G6, G6.1, G10, G68.2, G76 or G87 to use it")); + } + + for (int n = 0; n < EMCMOT_MAX_JOINTS; n++) { + if (block->joint_flag[n]) { + CHKS((block->g_modes[GM_MODAL_0] != G_53_7), _("J%d= word with no G53.7 to use it"), n); + break; + } } - if (block->k_flag) { /* could still be useless if xy_plane arc */ - CHKS(((motion != G_2) && (motion != G_3) && (motion != G_6_2) && (motion != G_33) && (motion != G_33_1) && (motion != G_76) && (motion != G_87)), - _("K word with no G2, G3, G6.2, G33, G33.1, G76, or G87 to use it")); + if (block->k_flag && !tool_vector) { /* could still be useless if xy_plane arc */ + CHKS(((motion != G_2) && (motion != G_3) && (motion != G_6_2) && (motion != G_33) && (motion != G_33_1) && (motion != G_76) && (motion != G_87) && + (block->g_modes[GM_WORK_PLANE] == -1)), + _("K word with no G2, G3, G6.2, G33, G33.1, G68.2, G76, or G87 to use it")); } if (block->l_number != -1) { @@ -325,6 +366,8 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block if (block->p_flag) { CHKS(((block->g_modes[GM_MODAL_0] != G_10) && (block->g_modes[GM_MODAL_0] != G_4) && (block->g_modes[GM_CONTROL_MODE] != G_64 && (block->g_modes[GM_MODAL_0] != G_12_1)) && + (block->g_modes[GM_WORK_PLANE] == -1) && + (block->g_modes[GM_MODAL_0] != G_53_1) && (block->g_modes[GM_MODAL_0] != G_53_2) && (block->g_modes[GM_MODAL_0] != G_53_3) && (block->g_modes[GM_MODAL_0] != G_53_6) && (motion != G_76) && (motion != G_82) && (motion != G_86) && (motion != G_88) && (motion != G_89) && (motion != G_5) && (motion != G_5_2) && (motion != G_70) && @@ -336,7 +379,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block (block->m_modes[5] != 64) && (block->m_modes[5] != 65) && (block->m_modes[5] != 66) && (block->m_modes[7] != 19) && (block->user_m != 1) && (block->o_type != M_98)), - _("P word with no G2 G3 G4 G10 G12.1 G64 G5 G5.2 G6, G6.2, G76 G82 G86 G88 G89" + _("P word with no G2 G3 G4 G10 G12.1 G53.1 G53.2 G53.3 G53.6 G64 G68.2 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); @@ -353,11 +396,13 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block CHKS((motion != G_83) && (motion != G_73) && (motion != G_5) && (motion != G_6) && (motion != G_6_2) && (block->user_m != 1) && (motion != G_76) && (block->m_modes[5] != 66) && (block->m_modes[5] != 67) && (block->m_modes[5] != 68) && (block->g_modes[GM_MODAL_0] != G_10) && (block->m_modes[6] != 61) && (block->g_modes[GM_CONTROL_MODE] != G_64) && + (block->g_modes[GM_WORK_PLANE] == -1) && + (block->g_modes[GM_MODAL_0] != G_53_1) && (block->g_modes[GM_MODAL_0] != G_53_2) && (block->g_modes[GM_MODAL_0] != G_53_3) && (block->g_modes[GM_MODAL_0] != G_53_6) && (motion != G_70) && (motion != G_71) && (motion != G_71_1) && (motion != G_71_2) && (motion != G_72) && (motion != G_72_1) && (motion != G_72_2) && (block->m_modes[7] != 19), - _("Q word with no G5, G6, G10, G64, G73, G76, G83, M19, M66, M67, M68 or user M code that uses it")); + _("Q word with no G5, G6, G10, G53.1, G53.2, G53.3, G53.6, G64, G68.2, G73, G76, G83, M19, M66, M67, M68 or user M code that uses it")); } if (block->r_flag) { @@ -368,6 +413,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block (motion != G_74) && (block->g_modes[GM_CUTTER_COMP] != G_41_1) && (block->g_modes[GM_CUTTER_COMP] != G_42_1) && (block->g_modes[GM_MODAL_0] != G_10) && (block->m_modes[7] != 19) && + (block->g_modes[GM_WORK_PLANE] == -1) && (block->g_modes[GM_CONTROL_MODE] != G_64) ), /* G64_R_PLANNER: R selects planner on G64 */ NCE_R_WORD_WITH_NO_G_CODE_THAT_USES_IT); /* G64_R_PLANNER: a block has one shared R word; with G64 it is the planner diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 4ed206d83e7..4937ffa96e8 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -34,7 +34,7 @@ #include "interp_internal.hh" #include "interp_queue.hh" #include "interp_parameter_def.hh" -#include "kinematics.h" // KINSTYPE_IDENTITY, SWITCHKINS_MAX_TYPES +#include // KINSTYPE_IDENTITY, SWITCHKINS_MAX_TYPES #include "units.h" #define TOOL_INSIDE_ARC(side, turn) (((side)==CUTTER_COMP::LEFT&&(turn)>0)||((side)==CUTTER_COMP::RIGHT&&(turn)<0)) @@ -1641,6 +1641,8 @@ int Interp::convert_axis_offsets(int g_code, //!< g_code being executed (mus CHKS((settings->cutter_comp_side != CUTTER_COMP::OFF), /* not "== true" */ NCE_CANNOT_CHANGE_AXIS_OFFSETS_WITH_CUTTER_RADIUS_COMP); + CHKS((settings->g68_active), + _("Cannot change G92 offsets while a tilted work plane (G68.2) is active")); CHKS((block->a_flag && settings->a_axis_wrapped && (block->a_number <= -360.0 || block->a_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), @@ -2364,6 +2366,12 @@ int Interp::convert_coordinate_system(int g_code, //!< g_code called (mus CHKS((settings->cutter_comp_side != CUTTER_COMP::OFF), (_("Cannot change coordinate systems with cutter radius compensation on"))); + { + // the plane sits on the active system; reselecting that one is harmless + int target = (g_code < G_59_1) ? (g_code - G_54) / 10 + 1 : g_code - G_59_1 + 7; + CHKS((settings->g68_active && target != settings->origin_index), + _("Cannot change coordinate systems while a tilted work plane (G68.2) is active")); + } parameters = settings->parameters; switch (g_code) { case G_54: @@ -2960,6 +2968,7 @@ int Interp::convert_g(block_pointer block, //!< pointer to a block of RS27 { int status; + CHP(work_plane_check_sequence(block, settings)); if ((block->g_modes[GM_MODAL_0] == G_4) && ONCE(STEP_DWELL)) { status = convert_dwell(settings, block->p_number); CHP(status); @@ -2988,6 +2997,10 @@ int Interp::convert_g(block_pointer block, //!< pointer to a block of RS27 status = convert_coordinate_system(block->g_modes[GM_COORD_SYSTEM], settings); CHP(status); } + if ((block->g_modes[GM_WORK_PLANE] != -1) && ONCE(STEP_WORK_PLANE)){ + status = convert_work_plane(block->g_modes[GM_WORK_PLANE], block, settings); + CHP(status); + } if ((block->g_modes[GM_CONTROL_MODE] != -1) && ONCE(STEP_CONTROL_MODE)) { status = convert_control_mode(block->g_modes[GM_CONTROL_MODE], block->p_number, block->q_number, @@ -3040,12 +3053,8 @@ offsetless machine coordinate. void Interp::get_abs_position(setup_pointer s, double abs_pos[9]) { - double x = s->current_x + s->axis_offset_x; - double y = s->current_y + s->axis_offset_y; - rotate(&x, &y, s->rotation_xy); - abs_pos[0] = x + s->origin_offset_x + s->tool_offset.tran.x; - abs_pos[1] = y + s->origin_offset_y + s->tool_offset.tran.y; - abs_pos[2] = s->current_z + s->axis_offset_z + s->origin_offset_z + s->tool_offset.tran.z; + program_to_world_xyz(s, s->current_x, s->current_y, s->current_z, + &abs_pos[0], &abs_pos[1], &abs_pos[2]); abs_pos[3] = s->AA_current + s->AA_axis_offset + s->AA_origin_offset + s->tool_offset.a; abs_pos[4] = s->BB_current + s->BB_axis_offset + s->BB_origin_offset + s->tool_offset.b; abs_pos[5] = s->CC_current + s->CC_axis_offset + s->CC_origin_offset + s->tool_offset.c; @@ -3077,12 +3086,11 @@ int Interp::convert_savehome(int code, block_pointer /*block*/, setup_pointer s) ERS(_("Cannot set reference point with cutter compensation in effect")); } - double x = s->current_x + s->axis_offset_x; - double y = s->current_y + s->axis_offset_y; - rotate(&x, &y, s->rotation_xy); - x = PROGRAM_TO_USER_LEN(x + s->tool_offset.tran.x + s->origin_offset_x); - y = PROGRAM_TO_USER_LEN(y + s->tool_offset.tran.y + s->origin_offset_y); - double z = PROGRAM_TO_USER_LEN(s->current_z + s->tool_offset.tran.z + s->origin_offset_z + s->axis_offset_z); + double x, y, z; + program_to_world_xyz(s, s->current_x, s->current_y, s->current_z, &x, &y, &z); + x = PROGRAM_TO_USER_LEN(x); + y = PROGRAM_TO_USER_LEN(y); + z = PROGRAM_TO_USER_LEN(z); double a = PROGRAM_TO_USER_ANG(s->AA_current + s->tool_offset.a + s->AA_origin_offset + s->AA_axis_offset); double b = PROGRAM_TO_USER_ANG(s->BB_current + s->tool_offset.b + s->BB_origin_offset + s->BB_axis_offset); double c = PROGRAM_TO_USER_ANG(s->CC_current + s->tool_offset.c + s->CC_origin_offset + s->CC_axis_offset); @@ -3368,6 +3376,9 @@ int Interp::convert_length_units(int g_code, //!< g_code being executed (mus settings->origin_offset_x = (settings->origin_offset_x * INCH_PER_MM); settings->origin_offset_y = (settings->origin_offset_y * INCH_PER_MM); settings->origin_offset_z = (settings->origin_offset_z * INCH_PER_MM); + settings->g68_offset[0] = (settings->g68_offset[0] * INCH_PER_MM); + settings->g68_offset[1] = (settings->g68_offset[1] * INCH_PER_MM); + settings->g68_offset[2] = (settings->g68_offset[2] * INCH_PER_MM); settings->u_current = (settings->u_current * INCH_PER_MM); settings->v_current = (settings->v_current * INCH_PER_MM); @@ -3411,6 +3422,9 @@ int Interp::convert_length_units(int g_code, //!< g_code being executed (mus settings->origin_offset_x = (settings->origin_offset_x * MM_PER_INCH); settings->origin_offset_y = (settings->origin_offset_y * MM_PER_INCH); settings->origin_offset_z = (settings->origin_offset_z * MM_PER_INCH); + settings->g68_offset[0] = (settings->g68_offset[0] * MM_PER_INCH); + settings->g68_offset[1] = (settings->g68_offset[1] * MM_PER_INCH); + settings->g68_offset[2] = (settings->g68_offset[2] * MM_PER_INCH); settings->u_current = (settings->u_current * MM_PER_INCH); settings->v_current = (settings->v_current * MM_PER_INCH); @@ -4049,12 +4063,14 @@ int Interp::convert_m(block_pointer block, //!< pointer to a block of RS27 if (FEATURE(RETAIN_G43)) { - if ((settings->active_g_codes[9] == G_43) && ONCE(STEP_RETAIN_G43)) { + if (((settings->active_g_codes[9] == G_43) || + (settings->active_g_codes[9] == G_43_4) || + (settings->active_g_codes[9] == G_43_5)) && ONCE(STEP_RETAIN_G43)) { if(settings->selected_pocket > 0) { struct block_struct g43; init_block(&g43); - block->g_modes[gees[G_43]] = G_43; - CHP(convert_tool_length_offset(G_43, &g43, settings)); + block->g_modes[gees[settings->active_g_codes[9]]] = settings->active_g_codes[9]; + CHP(convert_tool_length_offset(settings->active_g_codes[9], &g43, settings)); } else { struct block_struct g49; init_block(&g49); @@ -4354,7 +4370,11 @@ int Interp::convert_modal_0(int code, //!< G-code, must be from group 0 CHP(convert_axis_offsets(code, block, settings)); } else if ((code == G_5_3)||(code == G_6_3)) { // jjf CHP(convert_nurbs(code, block, settings)); - } else if ((code == G_4) || (code == G_53)); // handled elsewhere + } else if ((code == G_4) || (code == G_53) || (code == G_53_4) || (code == G_53_5) + || (code == G_53_7)); // handled elsewhere + else if ((code == G_53_1) || (code == G_53_2) || (code == G_53_3) || (code == G_53_6)) { + CHP(convert_orient_tool(code, block, settings)); + } else if ((code == G_12_1) || (code == G_13_1)) { // The flag makes the interpreter wait for motion to drain, so that no // motion is planned across a change of kinematics. Reading runs far @@ -4362,8 +4382,10 @@ int Interp::convert_modal_0(int code, //!< G-code, must be from group 0 // will be queued: ask every time. The exception is an // ON_ABORT_COMMAND routine, run by one execute() call that cannot // service INTERP_EXECUTE_FINISH and would drop the rest of the - // routine; the abort has just flushed the queue anyway. - if (!settings->in_abort_command) { + // routine; the abort has just flushed the queue anyway. The startup + // code is the other exception: it runs before the main loop can + // service the wait, and no motion exists yet to protect. + if (!settings->in_abort_command && !settings->in_startup_code) { settings->kinsSwitch_flag = true; } CHP(convert_kins_switch(code, block, settings)); @@ -4606,6 +4628,8 @@ int Interp::convert_setup_tool(block_pointer block, setup_pointer settings) { double tx, ty, tz, ta, tb, tc, tu, tv, tw; int direct = block->l_number == 1; + CHKS((settings->g68_active && !direct), + _("Cannot use G10 L%d while a tilted work plane (G68.2) is active"), block->l_number); is_near_int(&toolno, block->p_number); CHP((find_tool_index(settings, toolno, &idx))); @@ -4845,6 +4869,9 @@ int Interp::convert_setup(block_pointer block, //!< pointer to a block of RS27 double c; double u, v, w; double r; + + CHKS((settings->g68_active), + _("Cannot use G10 L%d while a tilted work plane (G68.2) is active"), block->l_number); double *parameters; int p_int; @@ -5303,6 +5330,8 @@ int Interp::convert_stop(block_pointer block, //!< pointer to a block of RS27 ) { /* reset stuff here */ /*1*/ + // a tilted work plane does not survive the end of the program + CHP(work_plane_cancel(settings)); if (!settings->disable_auto_g54) { rotate(&settings->current_x, &settings->current_y, settings->rotation_xy); @@ -5534,8 +5563,22 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 } settings->motion_mode = move; + // under G43.5 the I J K of a G0 or G1 line are the tool axis, which the + // rotaries are solved for once the line's other words are read + bool tool_vector = settings->tool_vector && (move == G_0 || move == G_1) + && (block->i_flag || block->j_flag || block->k_flag); + if (block->g_modes[GM_MODAL_0] == G_53_5 || block->g_modes[GM_MODAL_0] == G_53_7) { + // the words name joints, by letter or by number: nothing below applies + CHKS(tool_vector, _("G43.5: a tool vector cannot go with %s, whose words name the joints"), + (block->g_modes[GM_MODAL_0] == G_53_5) ? "G53.5" : "G53.7"); + CHP(convert_ptp_joints(block->g_modes[GM_MODAL_0], move, block, settings)); + return INTERP_OK; + } CHP(find_ends(block, settings, &end_x, &end_y, &end_z, &AA_end, &BB_end, &CC_end, &u_end, &v_end, &w_end)); + if (tool_vector) { + CHP(tool_vector_ends(block, settings, &AA_end, &BB_end, &CC_end)); + } if (move == G_1) { inverse_time_rate_straight(end_x, end_y, end_z, @@ -5547,7 +5590,28 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 // Create a state tag and dump it to canon write_canon_state_tag(block, settings); - if ((settings->cutter_comp_side != CUTTER_COMP::OFF) && /* ! "== true" */ + if (block->g_modes[GM_MODAL_0] == G_53_4) { + // point-to-point: the endpoint is this Cartesian point, the path to it + // is whatever the joints make of it + CHKS((settings->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot use G53.4 with cutter radius compensation on")); + tag_straight(block,end_x, end_y); + if (move == G_0) { + JOINT_TRAVERSE(block->line_number, NULL, 0, end_x, end_y, end_z, + AA_end, BB_end, CC_end, + u_end, v_end, w_end); + } else { + double seconds; + CHP(ptp_seconds(block, settings, end_x, end_y, end_z, + AA_end, BB_end, CC_end, u_end, v_end, w_end, &seconds)); + JOINT_FEED(block->line_number, NULL, 0, end_x, end_y, end_z, + AA_end, BB_end, CC_end, + u_end, v_end, w_end, seconds); + } + settings->current_x = end_x; + settings->current_y = end_y; + settings->current_z = end_z; + } else if ((settings->cutter_comp_side != CUTTER_COMP::OFF) && /* ! "== true" */ (settings->cutter_comp_radius > 0.0)) { /* radius always is >= 0 */ CHKS((block->g_modes[GM_MODAL_0] == G_53), @@ -6389,6 +6453,45 @@ int Interp::convert_tool_change(setup_pointer settings) //!< pointer to machine /****************************************************************************/ +// the kinematics module declares what each type is (KINSTYPE_* flags); +// where the flags say nothing at all there is no kinematics attached +// (sai, preview) and the codes fall back to type 0, as before +static int kins_type_info_available() +{ + int k; + + for (k = 0; k < SWITCHKINS_MAX_TYPES; k++) { + if (GET_EXTERNAL_KINS_TYPE_FLAGS(k) >= 0) return 1; + } + return 0; +} + +// the type carrying a KINSTYPE_ flag, or -1 when the module declares none; +// -1 for a type is "no information", and it matches every flag, so it must +// be excluded before the bit test +static int flagged_kins_type(int flag) +{ + int k, f; + + for (k = 0; k < SWITCHKINS_MAX_TYPES; k++) { + f = GET_EXTERNAL_KINS_TYPE_FLAGS(k); + if (f >= 0 && (f & flag)) { return k; } + } + return -1; +} + +// a kinematics switch as G12.1/G13.1 do, with the drain wait and its two +// exceptions; already on the type there is nothing to do +static void switch_kins_type(int kins_type, setup_pointer settings) +{ + if (settings->kins_type == kins_type) { return; } + if (!settings->in_abort_command && !settings->in_startup_code) { + settings->kinsSwitch_flag = true; + } + SELECT_KINS_TYPE(kins_type); + settings->kins_type = kins_type; +} + /*! convert_tool_length_offset Returned Value: int @@ -6424,14 +6527,41 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu { int idx; EmcPose tool_offset; + double standing[EMCMOT_MAX_JOINTS]; + bool have_standing = false; ZERO_EMC_POSE(tool_offset); settings->g43_with_zero_offset = 0; CHKS((settings->cutter_comp_side != CUTTER_COMP::OFF), (_("Cannot change tool offset with cutter radius compensation on"))); + if (g_code == G_43_4 || g_code == G_43_5) { + int primary = flagged_kins_type(KINSTYPE_PRIMARY); + // G43.4 is G43 on the module's working transform: switch first, then + // apply the offset, as if the switch line had run and drained. With + // no kinematics attached there is nothing to switch to. G43.5 is + // the same, and the lines after it may give the tool axis as I J K. + CHKS(primary < 0 && kins_type_info_available(), NCE_NO_PRIMARY_KINEMATICS_TYPE); + if (primary >= 0 && primary != settings->kins_type) { + // the switch keeps the joints and moves the point, so the point + // the offset is read from is not the one the program is at: take + // the joints, while the kinematics they are known in is in force + void *vctx; + CHP(kins_here(settings, &vctx)); + if (vctx) { + CHP(current_joints(settings, vctx, standing)); + have_standing = true; + } + switch_kins_type(primary, settings); + } + settings->kins_by_g43_4 = true; + } else if (g_code != G_49) { + // the offset in effect is no longer G43.4's, so G49 has no switch to undo + settings->kins_by_g43_4 = false; + } + settings->tool_vector = (g_code == G_43_5); if (g_code == G_49) { idx = 0; - } else if (g_code == G_43) { + } else if (g_code == G_43 || g_code == G_43_4 || g_code == G_43_5) { logDebug("convert_tool_length_offset h_flag=%d h_number=%d toolchange_flag=%d current_pocket=%d\n", block->h_flag,block->h_number,settings->toolchange_flag,settings->current_pocket); if(block->h_flag) { @@ -6511,28 +6641,67 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu if(block->w_flag) tool_offset.w += block->w_number; } } else { - ERS("BUG: Code not G43, G43.1, G43.2, or G49"); + ERS("BUG: Code not G43, G43.1, G43.2, G43.4, G43.5, or G49"); + } + // The machine does not move, so the program coordinates of the point change. + // A kinematics that applies the offset itself moves the point by more than + // the offset difference, along the tilted tool axis: evaluate it here as + // motion will, and send the point along for motion to check against. + EmcPose point; + bool point_known = false; + CHP(tool_offset_point(settings, &tool_offset, have_standing ? standing : NULL, + &point, &point_known)); + if (point_known) { + double prog[9]; + EmcPose in_program; + + settings->tool_offset = tool_offset; + machine_pose_to_program(settings, &point, prog); + settings->current_x = prog[0]; + settings->current_y = prog[1]; + settings->current_z = prog[2]; + settings->AA_current = prog[3]; + settings->BB_current = prog[4]; + settings->CC_current = prog[5]; + settings->u_current = prog[6]; + settings->v_current = prog[7]; + settings->w_current = prog[8]; + in_program.tran.x = USER_TO_PROGRAM_LEN(point.tran.x); + in_program.tran.y = USER_TO_PROGRAM_LEN(point.tran.y); + in_program.tran.z = USER_TO_PROGRAM_LEN(point.tran.z); + in_program.a = USER_TO_PROGRAM_ANG(point.a); + in_program.b = USER_TO_PROGRAM_ANG(point.b); + in_program.c = USER_TO_PROGRAM_ANG(point.c); + in_program.u = USER_TO_PROGRAM_LEN(point.u); + in_program.v = USER_TO_PROGRAM_LEN(point.v); + in_program.w = USER_TO_PROGRAM_LEN(point.w); + USE_TOOL_LENGTH_OFFSET(tool_offset, in_program); + } else { + double dx, dy, dz; + + USE_TOOL_LENGTH_OFFSET(tool_offset); + + // by the offset difference seen from the program: the XY rotation + // and the tilted work plane taken off it + dx = settings->tool_offset.tran.x - tool_offset.tran.x; + dy = settings->tool_offset.tran.y - tool_offset.tran.y; + dz = settings->tool_offset.tran.z - tool_offset.tran.z; + + rotate(&dx, &dy, -settings->rotation_xy); + g68_unrotate(settings, &dx, &dy, &dz); + + settings->current_x += dx; + settings->current_y += dy; + settings->current_z += dz; + settings->AA_current += settings->tool_offset.a - tool_offset.a; + settings->BB_current += settings->tool_offset.b - tool_offset.b; + settings->CC_current += settings->tool_offset.c - tool_offset.c; + settings->u_current += settings->tool_offset.u - tool_offset.u; + settings->v_current += settings->tool_offset.v - tool_offset.v; + settings->w_current += settings->tool_offset.w - tool_offset.w; + + settings->tool_offset = tool_offset; } - USE_TOOL_LENGTH_OFFSET(tool_offset); - - double dx, dy; - - dx = settings->tool_offset.tran.x - tool_offset.tran.x; - dy = settings->tool_offset.tran.y - tool_offset.tran.y; - - rotate(&dx, &dy, -settings->rotation_xy); - - settings->current_x += dx; - settings->current_y += dy; - settings->current_z += settings->tool_offset.tran.z - tool_offset.tran.z; - settings->AA_current += settings->tool_offset.a - tool_offset.a; - settings->BB_current += settings->tool_offset.b - tool_offset.b; - settings->CC_current += settings->tool_offset.c - tool_offset.c; - settings->u_current += settings->tool_offset.u - tool_offset.u; - settings->v_current += settings->tool_offset.v - tool_offset.v; - settings->w_current += settings->tool_offset.w - tool_offset.w; - - settings->tool_offset = tool_offset; // Update parameters #5081-#5089 to reflect the tool length offset // actually applied to motion (covers G43, G43Hn with n != loaded tool, @@ -6552,6 +6721,16 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu settings->parameters[5088] = PROGRAM_TO_USER_LEN(tool_offset.v); settings->parameters[5089] = PROGRAM_TO_USER_LEN(tool_offset.w); + if (g_code == G_49 && settings->kins_by_g43_4) { + // G49 undoes what G43.4 did: after the cancel it drops the machine + // to identity kinematics, as if G13.1 had run on the next line. A + // kinematics the program selected itself is left alone, and a + // module that declares no identity type keeps the plain cancel. + int identity = flagged_kins_type(KINSTYPE_IDENTITY); + if (identity >= 0) { switch_kins_type(identity, settings); } + settings->kins_by_g43_4 = false; + } + return INTERP_OK; } @@ -6609,19 +6788,6 @@ so no motion is ever planned across a change of kinematics. */ -// the kinematics module declares what each type is (KINSTYPE_* flags); -// where the flags say nothing at all there is no kinematics attached -// (sai, preview) and the codes fall back to type 0, as before -static int kins_type_info_available() -{ - int k; - - for (k = 0; k < SWITCHKINS_MAX_TYPES; k++) { - if (GET_EXTERNAL_KINS_TYPE_FLAGS(k) >= 0) return 1; - } - return 0; -} - 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 @@ -6629,16 +6795,9 @@ int Interp::convert_kins_switch(int code, //!< G_12_1 or G_13_1 int kins_type; if (code == G_13_1) { - int k; - // G13.1 cancels to identity kinematics; which type that is, the // module declares, the number is not the answer - for (k = 0, kins_type = -1; k < SWITCHKINS_MAX_TYPES; k++) { - if (GET_EXTERNAL_KINS_TYPE_FLAGS(k) & KINSTYPE_IDENTITY) { - kins_type = k; - break; - } - } + kins_type = flagged_kins_type(KINSTYPE_IDENTITY); if (kins_type < 0) { CHKS(kins_type_info_available(), NCE_NO_IDENTITY_KINEMATICS_TYPE); kins_type = 0; // no kinematics attached: standalone interpreter @@ -6655,6 +6814,8 @@ int Interp::convert_kins_switch(int code, //!< G_12_1 or G_13_1 SELECT_KINS_TYPE(kins_type); settings->kins_type = kins_type; + // the program has taken the kinematics over from G43.4 + settings->kins_by_g43_4 = false; return INTERP_OK; } diff --git a/src/emc/rs274ngc/interp_find.cc b/src/emc/rs274ngc/interp_find.cc index 7a12d49d878..64b81bb989a 100644 --- a/src/emc/rs274ngc/interp_find.cc +++ b/src/emc/rs274ngc/interp_find.cc @@ -172,31 +172,14 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 #endif CHKS((block->radius_flag || block->theta_flag), _("Cannot use polar coordinates with G53")); - double cx = s->current_x + s->axis_offset_x; - double cy = s->current_y + s->axis_offset_y; - rotate(&cx, &cy, s->rotation_xy); - - if(block->x_flag) { - *px = block->x_number - s->origin_offset_x - s->tool_offset.tran.x; - } else { - *px = cx; - } - - if(block->y_flag) { - *py = block->y_number - s->origin_offset_y - s->tool_offset.tran.y; - } else { - *py = cy; - } - - rotate(px, py, -s->rotation_xy); - *px -= s->axis_offset_x; - *py -= s->axis_offset_y; - - if(block->z_flag) { - *pz = block->z_number - s->origin_offset_z - s->axis_offset_z - s->tool_offset.tran.z; - } else { - *pz = s->current_z; - } + // the words are absolute; the current point supplies the rest, + // taken to the absolute frame and back with them + double wx, wy, wz; + program_to_world_xyz(s, s->current_x, s->current_y, s->current_z, &wx, &wy, &wz); + if(block->x_flag) { wx = block->x_number; } + if(block->y_flag) { wy = block->y_number; } + if(block->z_flag) { wz = block->z_number; } + world_to_program_xyz(s, wx, wy, wz, px, py, pz); if(block->a_flag) { if(s->a_axis_wrapped) { @@ -424,12 +407,7 @@ int Interp::find_relative(double x1, //!< absolute x position double *w_2, setup_pointer settings) //!< pointer to machine settings { - *x2 = x1 - settings->origin_offset_x - settings->tool_offset.tran.x; - *y2 = y1 - settings->origin_offset_y - settings->tool_offset.tran.y; - rotate(x2, y2, -settings->rotation_xy); - *x2 -= settings->axis_offset_x; - *y2 -= settings->axis_offset_y; - *z2 = z1 - settings->origin_offset_z - settings->axis_offset_z - settings->tool_offset.tran.z; + world_to_program_xyz(settings, x1, y1, z1, x2, y2, z2); if(settings->a_axis_wrapped) { CHP(unwrap_rotary(AA_2, AA_1, diff --git a/src/emc/rs274ngc/interp_internal.cc b/src/emc/rs274ngc/interp_internal.cc index ff4ba82cb86..061c221c3dc 100644 --- a/src/emc/rs274ngc/interp_internal.cc +++ b/src/emc/rs274ngc/interp_internal.cc @@ -174,7 +174,13 @@ int Interp::enhance_block(block_pointer block, //!< pointer to a block to be c mode1 = block->g_modes[GM_MOTION]; mode_zero_covets_axes = ((mode0 == G_10) || (mode0 == G_28) || (mode0 == G_30) - || (mode0 == G_52) || (mode0 == G_92)); + || (mode0 == G_52) || (mode0 == G_92) || (mode0 == G_53_3)); + // a tilted work plane definition takes the axis words the same way + if (block->g_modes[GM_WORK_PLANE] == G_68_2 || block->g_modes[GM_WORK_PLANE] == G_68_3 + || block->g_modes[GM_WORK_PLANE] == G_68_4) { + CHKS(polar_flag, _("Polar coordinates cannot define a tilted work plane")); + mode_zero_covets_axes = 1; + } if (mode1 != -1) { if (mode1 == G_80) { @@ -210,8 +216,11 @@ int Interp::enhance_block(block_pointer block, //!< pointer to a block to be c if (block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_1) { block->motion_to_be = settings->motion_mode; } - } else if (!axis_flag && !polar_flag && ijk_flag && (settings->motion_mode == G_2 || settings->motion_mode == G_3)) { - // this is a block like simply "i1" which should be accepted if we're in arc mode + } else if (!axis_flag && !polar_flag && ijk_flag && + (settings->motion_mode == G_2 || settings->motion_mode == G_3 || + (settings->tool_vector && (settings->motion_mode == G_0 || settings->motion_mode == G_1)))) { + // this is a block like simply "i1" which should be accepted if we're in arc mode, + // or a tool vector alone under G43.5, which turns the tool where it stands block->motion_to_be = settings->motion_mode; } CHKS((polar_flag && block->motion_to_be == -1), _("Polar coordinates can only be used for motion")); @@ -281,6 +290,10 @@ int Interp::init_block(block_pointer block) //!< pointer to a block to be i block->h_number = -1; block->i_flag = false; block->j_flag = false; + for (n = 0; n < EMCMOT_MAX_JOINTS; n++) { + block->joint_flag[n] = false; + block->joint_value[n] = 0.0; + } block->k_flag = false; block->l_number = -1; block->l_flag = false; diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index b58d40e525f..a0ec55d094d 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -247,11 +247,20 @@ enum GCodes G_43 = 430, G_43_1 = 431, G_43_2 = 432, + G_43_4 = 434, + G_43_5 = 435, G_49 = 490, G_50 = 500, G_51 = 510, G_52 = 520, G_53 = 530, + G_53_1 = 531, + G_53_2 = 532, + G_53_3 = 533, + G_53_4 = 534, + G_53_5 = 535, + G_53_6 = 536, + G_53_7 = 537, G_54 = 540, G_55 = 550, G_56 = 560, @@ -261,6 +270,10 @@ enum GCodes G_59_1 = 591, G_59_2 = 592, G_59_3 = 593, + G_68_2 = 682, + G_68_3 = 683, + G_68_4 = 684, + G_69 = 690, G_61 = 610, G_61_1 = 611, G_64 = 640, @@ -364,6 +377,7 @@ enum phases { STEP_CUTTER_COMP, STEP_TOOL_LENGTH_OFFSET, STEP_COORD_SYSTEM, + STEP_WORK_PLANE, STEP_CONTROL_MODE, STEP_DISTANCE_MODE, STEP_IJK_DISTANCE_MODE, @@ -378,7 +392,7 @@ enum phases { // Modal groups // also indices into g_modes -// unused: 9,11 +// unused: 11 enum ModalGroups { GM_MODAL_0 = 0, @@ -390,7 +404,7 @@ enum ModalGroups GM_LENGTH_UNITS = 6, GM_CUTTER_COMP = 7, GM_TOOL_LENGTH_OFFSET = 8, - // 9 unused + GM_WORK_PLANE = 9, GM_RETRACT_MODE = 10, // 11 unused GM_COORD_SYSTEM = 12, @@ -496,6 +510,9 @@ struct block_struct bool dollar_flag{}; + bool joint_flag[EMCMOT_MAX_JOINTS]{}; // J= words, for G53.5 + double joint_value[EMCMOT_MAX_JOINTS]{}; + double radius{}; double theta{}; int radius_flag{}; @@ -746,6 +763,27 @@ struct setup double origin_offset_y; // g5x offset y double origin_offset_z; // g5x offset z double rotation_xy; // rotation of coordinate system around Z, in degrees + // the tilted work plane (G68.2): a frame inside G92, program units, + // in the coordinate system that was active when it was defined + bool g68_active; + int g68_code; // the code that defined it, for the modal display + double g68_offset[3]; + double g68_rotation[3][3]; // row major, columns are the plane's axes + int g68_seq_code; // a three-point or two-vector definition in progress + int g68_seq_p; + unsigned g68_seq_have; // bit per Q received + double g68_seq_word[4][7]; // per Q: x y z i j k r + // the pose G53.2 last solved, in program words, for #<_orient_a> and kin + bool orient_valid; + double orient_pose[6]; // x y z a b c + // the kinematics, for G68.3 and the orientation moves: loaded on first + // use through the non-realtime loader, on a HAL component of our own + void *kins_ctx; // KinematicsUserContext + int kins_comp_id; + char kins_module[LINELEN]; // [KINS] KINEMATICS, as loadrt gets it + int kins_joints; // [KINS] JOINTS + int kins_angular_joints; // bit per joint, [JOINT_n] TYPE = ANGULAR + double kins_seed[EMCMOT_MAX_JOINTS]; // the last inverse, seeding the next double parameters[interp_param_global::RS274NGC_MAX_PARAMETERS]; // system parameters int parameter_occurrence; // parameter buffer index int parameter_numbers[MAX_NAMED_PARAMETERS]; // parameter number buffer @@ -759,6 +797,8 @@ struct setup 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 kins_by_g43_4; // G43.4 selected the kinematics, for G49 to undo + bool tool_vector; // G43.5: I J K on G0 and G1 give the tool axis bool toolchange_flag; // flag indicating we just had a tool change int input_index; // channel queried bool input_digital; // input queried was digital (false=analog) @@ -861,6 +901,7 @@ struct setup boost::python::object *pythis; // boost::cref to 'this' const char *on_abort_command; bool in_abort_command; // running the ON_ABORT_COMMAND routine + bool in_startup_code; // running the startup code at task init int_remap_map g_remapped,m_remapped; remap_map remaps; #define INIT_FUNC "__init__" diff --git a/src/emc/rs274ngc/interp_namedparams.cc b/src/emc/rs274ngc/interp_namedparams.cc index fb12f1e8d26..cb830d8542b 100644 --- a/src/emc/rs274ngc/interp_namedparams.cc +++ b/src/emc/rs274ngc/interp_namedparams.cc @@ -59,6 +59,13 @@ enum predefined_named_parameters { NP_LINE, NP_MOTION_MODE, NP_KINS_TYPE, + NP_ORIENT_VALID, + NP_ORIENT_X, + NP_ORIENT_Y, + NP_ORIENT_Z, + NP_ORIENT_A, + NP_ORIENT_B, + NP_ORIENT_C, NP_PLANE, NP_CCOMP, NP_METRIC, @@ -546,6 +553,22 @@ int Interp::lookup_named_param(const char *nameBuf, *value = _setup.kins_type; break; + case NP_ORIENT_VALID: // _orient_valid: G53.2 has solved a pose + *value = _setup.orient_valid; + break; + + case NP_ORIENT_X: // _orient_x and kin: the pose G53.2 last solved + case NP_ORIENT_Y: + case NP_ORIENT_Z: + case NP_ORIENT_A: + case NP_ORIENT_B: + case NP_ORIENT_C: + if (!_setup.orient_valid) { + ERS(_("no G53.2 has solved an orientation yet")); + } + *value = _setup.orient_pose[cmd - NP_ORIENT_X]; + break; + case NP_PLANE: // _plane switch(_setup.plane) { case CANON_PLANE::XY: @@ -759,26 +782,27 @@ int Interp::lookup_named_param(const char *nameBuf, case NP_ABS_X: // abs position { - double x = _setup.current_x + _setup.axis_offset_x; - double y = _setup.current_y + _setup.axis_offset_y; - rotate(&x, &y, _setup.rotation_xy); - *value = x + _setup.origin_offset_x + _setup.tool_offset.tran.x; + double abs_pos[9]; + get_abs_position(&_setup, abs_pos); + *value = abs_pos[0]; } break; case NP_ABS_Y: // abs position { - double x = _setup.current_x + _setup.axis_offset_x; - double y = _setup.current_y + _setup.axis_offset_y; - rotate(&x, &y, _setup.rotation_xy); - *value = y + _setup.origin_offset_y + _setup.tool_offset.tran.y; + double abs_pos[9]; + get_abs_position(&_setup, abs_pos); + *value = abs_pos[1]; } break; case NP_ABS_Z: // abs position - *value = _setup.current_z + _setup.axis_offset_z + - _setup.origin_offset_z + _setup.tool_offset.tran.z; + { + double abs_pos[9]; + get_abs_position(&_setup, abs_pos); + *value = abs_pos[2]; + } break; case NP_ABS_A: // abs position @@ -898,6 +922,15 @@ int Interp::init_named_parameters() // kinematics selected by G12.1 P- / G13.1, 0 when none has been selected init_readonly_param("_kins_type", NP_KINS_TYPE, PA_USE_LOOKUP); + // the pose G53.2 last solved: 1.0 once one has been, and its words + init_readonly_param("_orient_valid", NP_ORIENT_VALID, PA_USE_LOOKUP); + init_readonly_param("_orient_x", NP_ORIENT_X, PA_USE_LOOKUP); + init_readonly_param("_orient_y", NP_ORIENT_Y, PA_USE_LOOKUP); + init_readonly_param("_orient_z", NP_ORIENT_Z, PA_USE_LOOKUP); + init_readonly_param("_orient_a", NP_ORIENT_A, PA_USE_LOOKUP); + init_readonly_param("_orient_b", NP_ORIENT_B, PA_USE_LOOKUP); + init_readonly_param("_orient_c", NP_ORIENT_C, PA_USE_LOOKUP); + // G17/18/19/17.1/18.1/19.1 -> return 170/180/190/171/181/191 init_readonly_param("_plane", NP_PLANE, PA_USE_LOOKUP); diff --git a/src/emc/rs274ngc/interp_read.cc b/src/emc/rs274ngc/interp_read.cc index 6e58d1635dc..d0a34a21b8a 100644 --- a/src/emc/rs274ngc/interp_read.cc +++ b/src/emc/rs274ngc/interp_read.cc @@ -888,11 +888,15 @@ Returned Value: int NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED 2. A j_coordinate has already been inserted in the block. NCE_MULTIPLE_J_WORDS_ON_ONE_LINE + 3. The value is followed by '=' and is not a joint number, or that joint + already has a value in the block. Side effects: counter is reset. The j_flag in the block is turned on. A j_coordinate setting is inserted in the block. + For the J= form, joint_flag[n] is turned on and joint_value[n] + is set instead; j_flag is left alone. Called by: read_one_item @@ -918,8 +922,20 @@ int Interp::read_j(char *line, //!< string: line of RS274 code being processed CHKS((line[*counter] != 'j'), NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED); *counter = (*counter + 1); - CHKS((block->j_flag), NCE_MULTIPLE_J_WORDS_ON_ONE_LINE); CHP(read_real_value(line, counter, &value, parameters)); + if (line[*counter] == '=') { + // J=: a joint value for G53.5, n is the joint number + int n = (int)value; + CHKS((value != (double)n || n < 0 || n >= EMCMOT_MAX_JOINTS), + _("The joint number in a J= word must be a whole number from 0 to %d"), EMCMOT_MAX_JOINTS - 1); + CHKS((block->joint_flag[n]), _("Multiple J%d= words on one line"), n); + *counter = (*counter + 1); + CHP(read_real_value(line, counter, &value, parameters)); + block->joint_flag[n] = true; + block->joint_value[n] = value; + return INTERP_OK; + } + CHKS((block->j_flag), NCE_MULTIPLE_J_WORDS_ON_ONE_LINE); block->j_flag = true; block->j_number = value; return INTERP_OK; diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index 3b5445795c3..3606c54b9b9 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -104,6 +104,22 @@ setup::setup() : origin_offset_y (0.0), origin_offset_z (0.0), rotation_xy (0.0), + g68_active(false), + g68_code(0), + g68_offset{0.0, 0.0, 0.0}, + g68_rotation{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}, + g68_seq_code(0), + g68_seq_p(0), + g68_seq_have(0), + g68_seq_word{}, + orient_valid(false), + orient_pose{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + kins_ctx(nullptr), + kins_comp_id(0), + kins_module{}, + kins_joints(0), + kins_angular_joints(0), + kins_seed{}, parameters{0}, parameter_occurrence(0), @@ -118,6 +134,8 @@ setup::setup() : input_flag(0), kinsSwitch_flag(0), kins_type(0), + kins_by_g43_4(false), + tool_vector(false), toolchange_flag(0), input_index(0), input_digital(0), @@ -201,6 +219,7 @@ setup::setup() : pythis(), on_abort_command(NULL), in_abort_command(false), + in_startup_code(false), init_once(CANON_STOPPED) { std::fill(parameters, parameters + interp_param_global::RS274NGC_MAX_PARAMETERS, 0); diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc new file mode 100644 index 00000000000..aac82513bb6 --- /dev/null +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -0,0 +1,1141 @@ +/******************************************************************** +* Description: interp_workplane.cc +* +* The tilted work plane: G68.2, G68.4 and G69, and the frame they put +* inside the offset chain. +* +* The chain, as canon applies it: +* +* world = TLO + G5x + Rz(rotation_xy) * (G92 + O + R * program) +* +* O and R are the plane's origin and rotation, expressed in the +* coordinate system that was active when the plane was defined: G5x +* with G92 and the XY rotation in place, which is what the operator +* sees on the display and what G68.2 X Y Z means on every control. +* Rotary and UVW words do not pass through the plane: on a TCP +* kinematics the rotary world coordinates are the rotary joints, and a +* plane does not change what a joint is. +* +* The interpreter keeps its current position in program coordinates +* and only needs the chain where it reasons about absolute coordinates +* itself (G53, G28/G30, #5021, G28.1, a G43 change). Those places +* call program_to_world_xyz() and world_to_program_xyz() from here +* rather than repeating the stages. +* +* The plane is not persistent: Interp::init(), M2/M30 and G69 clear +* it. Nothing is written to the var file. +* +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2026 All rights reserved. +********************************************************************/ + +#include +#include +#include "rs274ngc.hh" +#include "rs274ngc_return.hh" +#include "interp_internal.hh" +#include "rs274ngc_interp.hh" + +//---------------------------------------------------------------------- +// small matrix helpers, row major double[3][3] +//---------------------------------------------------------------------- + +static void mat_identity(double m[3][3]) +{ + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { m[i][j] = (i == j) ? 1.0 : 0.0; } + } +} + +// rotation about axis 1, 2 or 3 (X, Y, Z) by an angle in degrees +static void mat_rotation(int axis, double deg, double m[3][3]) +{ + double c = cos(deg * M_PI / 180.0), s = sin(deg * M_PI / 180.0); + mat_identity(m); + switch (axis) { + case 1: m[1][1] = c; m[1][2] = -s; m[2][1] = s; m[2][2] = c; break; + case 2: m[0][0] = c; m[0][2] = s; m[2][0] = -s; m[2][2] = c; break; + default: m[0][0] = c; m[0][1] = -s; m[1][0] = s; m[1][1] = c; break; + } +} + +static void mat_mul(const double a[3][3], const double b[3][3], double out[3][3]) +{ + double r[3][3]; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + r[i][j] = a[i][0]*b[0][j] + a[i][1]*b[1][j] + a[i][2]*b[2][j]; + } + } + memcpy(out, r, sizeof(r)); +} + +static void mat_apply(const double m[3][3], double *x, double *y, double *z) +{ + double px = *x, py = *y, pz = *z; + *x = m[0][0]*px + m[0][1]*py + m[0][2]*pz; + *y = m[1][0]*px + m[1][1]*py + m[1][2]*pz; + *z = m[2][0]*px + m[2][1]*py + m[2][2]*pz; +} + +static void mat_apply_transposed(const double m[3][3], double *x, double *y, double *z) +{ + double px = *x, py = *y, pz = *z; + *x = m[0][0]*px + m[1][0]*py + m[2][0]*pz; + *y = m[0][1]*px + m[1][1]*py + m[2][1]*pz; + *z = m[0][2]*px + m[1][2]*py + m[2][2]*pz; +} + +static double vec_norm(const double v[3]) +{ + return sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]); +} + +static void vec_cross(const double a[3], const double b[3], double out[3]) +{ + out[0] = a[1]*b[2] - a[2]*b[1]; + out[1] = a[2]*b[0] - a[0]*b[2]; + out[2] = a[0]*b[1] - a[1]*b[0]; +} + +// a rotation whose columns are the three axes +static void mat_from_axes(const double x[3], const double y[3], const double z[3], double m[3][3]) +{ + for (int i = 0; i < 3; i++) { m[i][0] = x[i]; m[i][1] = y[i]; m[i][2] = z[i]; } +} + +//---------------------------------------------------------------------- +// the chain +//---------------------------------------------------------------------- + +// the plane stage alone: program coordinates to the system the plane was +// defined in, and back +void Interp::g68_apply(setup_pointer s, double *x, double *y, double *z) +{ + if (!s->g68_active) { return; } + mat_apply(s->g68_rotation, x, y, z); + *x += s->g68_offset[0]; + *y += s->g68_offset[1]; + *z += s->g68_offset[2]; +} + +void Interp::g68_remove(setup_pointer s, double *x, double *y, double *z) +{ + if (!s->g68_active) { return; } + *x -= s->g68_offset[0]; + *y -= s->g68_offset[1]; + *z -= s->g68_offset[2]; + mat_apply_transposed(s->g68_rotation, x, y, z); +} + +// a displacement in the system the plane was defined in, seen from the +// program: the rotation without the origin +void Interp::g68_unrotate(setup_pointer s, double *x, double *y, double *z) +{ + if (!s->g68_active) { return; } + mat_apply_transposed(s->g68_rotation, x, y, z); +} + +// The whole chain for X Y Z, program coordinates to the absolute (G53) +// frame: the plane, G92, the XY rotation, G5x and the tool offset. +void Interp::program_to_world_xyz(setup_pointer s, + double px, double py, double pz, + double *wx, double *wy, double *wz) +{ + double x = px, y = py, z = pz; + + g68_apply(s, &x, &y, &z); + x += s->axis_offset_x; + y += s->axis_offset_y; + z += s->axis_offset_z; + rotate(&x, &y, s->rotation_xy); + *wx = x + s->origin_offset_x + s->tool_offset.tran.x; + *wy = y + s->origin_offset_y + s->tool_offset.tran.y; + *wz = z + s->origin_offset_z + s->tool_offset.tran.z; +} + +void Interp::world_to_program_xyz(setup_pointer s, + double wx, double wy, double wz, + double *px, double *py, double *pz) +{ + double x = wx - s->origin_offset_x - s->tool_offset.tran.x; + double y = wy - s->origin_offset_y - s->tool_offset.tran.y; + double z = wz - s->origin_offset_z - s->tool_offset.tran.z; + + rotate(&x, &y, -s->rotation_xy); + x -= s->axis_offset_x; + y -= s->axis_offset_y; + z -= s->axis_offset_z; + g68_remove(s, &x, &y, &z); + *px = x; + *py = y; + *pz = z; +} + +//---------------------------------------------------------------------- +// setting and clearing the plane +//---------------------------------------------------------------------- + +// Install a plane. The tool does not move, so its program coordinates +// change: take the current point through the old chain to the absolute +// frame and back through the new one. +int Interp::work_plane_set(setup_pointer s, int code, + const double origin[3], const double rotation[3][3]) +{ + double wx, wy, wz, flat[9]; + + program_to_world_xyz(s, s->current_x, s->current_y, s->current_z, &wx, &wy, &wz); + + for (int i = 0; i < 3; i++) { + s->g68_offset[i] = origin[i]; + for (int j = 0; j < 3; j++) { + s->g68_rotation[i][j] = rotation[i][j]; + flat[3*i + j] = rotation[i][j]; + } + } + s->g68_active = true; + s->g68_code = code; + + world_to_program_xyz(s, wx, wy, wz, &s->current_x, &s->current_y, &s->current_z); + + SET_G68_FRAME(origin[0], origin[1], origin[2], flat, 1); + return INTERP_OK; +} + +// Cancel the plane if one is in effect. Canon is told only when there was +// something to cancel, unless tell_canon_anyway: an abort throws away the +// queued cancel the read ahead sent, so status and the interpreter can +// disagree and only canon can settle it. +int Interp::work_plane_cancel(setup_pointer s, bool tell_canon_anyway) +{ + double wx, wy, wz; + static const double identity[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + + s->g68_seq_code = 0; + if (!s->g68_active) { + if (tell_canon_anyway) { SET_G68_FRAME(0.0, 0.0, 0.0, identity, 0); } + return INTERP_OK; + } + + program_to_world_xyz(s, s->current_x, s->current_y, s->current_z, &wx, &wy, &wz); + s->g68_active = false; + s->g68_code = 0; + for (int i = 0; i < 3; i++) { s->g68_offset[i] = 0.0; } + mat_identity(s->g68_rotation); + world_to_program_xyz(s, wx, wy, wz, &s->current_x, &s->current_y, &s->current_z); + + SET_G68_FRAME(0.0, 0.0, 0.0, identity, 0); + return INTERP_OK; +} + +// A block that is not part of a pending three-point or two-vector +// sequence: the sequence was left incomplete. +int Interp::work_plane_check_sequence(block_pointer block, setup_pointer s) +{ + if (s->g68_seq_code == 0) { return INTERP_OK; } + if (block->g_modes[GM_WORK_PLANE] == s->g68_seq_code) { return INTERP_OK; } + s->g68_seq_code = 0; + ERS(_("G68.2 P%d sequence is incomplete: the next block must carry the next Q"), s->g68_seq_p); +} + +//---------------------------------------------------------------------- +// the definitions +//---------------------------------------------------------------------- + +// Q names the axes of a three-angle definition, three digits from 1 to 3, +// no two adjacent alike: 313 is Z X Z, 123 is X Y Z. +static int parse_axis_order(double q, int order[3]) +{ + int n = (int)round(q); + if (fabs(q - n) > 1e-9 || n < 111 || n > 333) { return -1; } + order[0] = n / 100; + order[1] = (n / 10) % 10; + order[2] = n % 10; + for (int i = 0; i < 3; i++) { + if (order[i] < 1 || order[i] > 3) { return -1; } + } + if (order[0] == order[1] || order[1] == order[2]) { return -1; } + return 0; +} + +// The rotation of a G68.2 or G68.4 block, and whether the block completes +// a definition. The three-point and two-vector forms arrive over several +// blocks with Q; the words are kept in the setup until the last one. +int Interp::work_plane_build(block_pointer block, setup_pointer s, + double origin[3], double rotation[3][3], int *complete) +{ + int p = block->p_flag ? (int)round(block->p_number) : 0; + double r = block->r_flag ? block->r_number : 0.0; + double rz[3][3]; + + *complete = 0; + CHKS((block->p_flag && (fabs(block->p_number - p) > 1e-9 || p < 0 || p > 3)), + _("P word with G68.2 must be 0, 1, 2 or 3")); + + if (p == 0 || p == 1) { + // three angles. P0: each about an axis of the frame as rotated so + // far (Euler, ZXZ by default). P1: each about a fixed axis of the + // system the plane is defined in, in the order Q gives (XYZ by + // default). + int order[3]; + double angle[3], m[3][3]; + + CHKS((s->g68_seq_code != 0), _("G68.2 P%d cannot interrupt a P%d sequence"), p, s->g68_seq_p); + CHKS((parse_axis_order(block->q_flag ? block->q_number : (p == 0 ? 313.0 : 123.0), order) != 0), + _("Q word with G68.2 P%d must be three axis digits 1 to 3 with no two adjacent alike"), p); + angle[0] = block->i_flag ? block->i_number : 0.0; + angle[1] = block->j_flag ? block->j_number : 0.0; + angle[2] = block->k_flag ? block->k_number : 0.0; + + mat_identity(rotation); + for (int i = 0; i < 3; i++) { + mat_rotation(order[i], angle[i], m); + if (p == 0) { + mat_mul(rotation, m, rotation); + } else { + mat_mul(m, rotation, rotation); + } + } + origin[0] = block->x_flag ? block->x_number : 0.0; + origin[1] = block->y_flag ? block->y_number : 0.0; + origin[2] = block->z_flag ? block->z_number : 0.0; + mat_rotation(3, r, rz); + mat_mul(rotation, rz, rotation); + *complete = 1; + return INTERP_OK; + } + + // the sequences + { + int q = block->q_flag ? (int)round(block->q_number) : -1; + int code = block->g_modes[GM_WORK_PLANE]; + int first = (p == 2) ? 0 : 1, last = (p == 2) ? 3 : 2; + int expect; + + CHKS((q < 0 || fabs(block->q_number - q) > 1e-9), _("Q word missing with G68.2 P%d"), p); + if (s->g68_seq_code == 0) { + // the first block of a sequence; a three-point definition may + // leave out Q0 and take the first point as origin + CHKS((q != first && !(p == 2 && q == 1)), + _("G68.2 P%d sequence must start with Q%d"), p, first); + s->g68_seq_code = code; + s->g68_seq_p = p; + s->g68_seq_have = 0; + } else { + CHKS((s->g68_seq_code != code || s->g68_seq_p != p), + _("G68.2 P%d cannot interrupt a P%d sequence"), p, s->g68_seq_p); + } + expect = -1; + for (int i = first; i <= last; i++) { + if (!(s->g68_seq_have & (1 << i))) { expect = i; break; } + } + if (!(q == expect || (p == 2 && expect == 0 && q == 1))) { + s->g68_seq_code = 0; + ERS(_("G68.2 P%d expects Q%d here"), p, expect); + } + s->g68_seq_have |= 1 << q; + s->g68_seq_word[q][0] = block->x_flag ? block->x_number : 0.0; + s->g68_seq_word[q][1] = block->y_flag ? block->y_number : 0.0; + s->g68_seq_word[q][2] = block->z_flag ? block->z_number : 0.0; + s->g68_seq_word[q][3] = block->i_flag ? block->i_number : 0.0; + s->g68_seq_word[q][4] = block->j_flag ? block->j_number : 0.0; + s->g68_seq_word[q][5] = block->k_flag ? block->k_number : 0.0; + s->g68_seq_word[q][6] = r; + if (q != last) { return INTERP_OK; } + } + + // the sequence is complete + s->g68_seq_code = 0; + if (p == 2) { + // three points: the first to the second is +X, the third lies on + // the +Y side; Q0 gives the origin and R, else the origin is the + // first point + const double *p1 = s->g68_seq_word[1], *p2 = s->g68_seq_word[2], *p3 = s->g68_seq_word[3]; + double x[3], v[3], y[3], z[3], len; + + for (int i = 0; i < 3; i++) { x[i] = p2[i] - p1[i]; v[i] = p3[i] - p1[i]; } + len = vec_norm(x); + CHKS((len < 1e-9), _("G68.2 P2: the first two points coincide")); + for (int i = 0; i < 3; i++) { x[i] /= len; } + vec_cross(x, v, z); + len = vec_norm(z); + CHKS((len < 1e-9 * fmax(1.0, vec_norm(v))), _("G68.2 P2: the three points are on one line")); + for (int i = 0; i < 3; i++) { z[i] /= len; } + vec_cross(z, x, y); + mat_from_axes(x, y, z, rotation); + if (s->g68_seq_have & 1) { + for (int i = 0; i < 3; i++) { origin[i] = s->g68_seq_word[0][i]; } + r = s->g68_seq_word[0][6]; + } else { + for (int i = 0; i < 3; i++) { origin[i] = p1[i]; } + r = 0.0; + } + } else { + // two vectors: the origin and +X on the first block, +Z on the + // second; X is projected onto the plane so that a request a few + // digits off square still names a frame + const double *q1 = s->g68_seq_word[1], *q2 = s->g68_seq_word[2]; + double x[3], y[3], z[3], len, along; + + for (int i = 0; i < 3; i++) { z[i] = q2[3 + i]; x[i] = q1[3 + i]; } + len = vec_norm(z); + CHKS((len < 1e-12), _("G68.2 P3: the Z direction is a zero vector")); + for (int i = 0; i < 3; i++) { z[i] /= len; } + len = vec_norm(x); + CHKS((len < 1e-12), _("G68.2 P3: the X direction is a zero vector")); + along = x[0]*z[0] + x[1]*z[1] + x[2]*z[2]; + for (int i = 0; i < 3; i++) { x[i] -= along * z[i]; } + CHKS((vec_norm(x) < 1e-6 * len), _("G68.2 P3: the X direction lies along the Z direction")); + len = vec_norm(x); + for (int i = 0; i < 3; i++) { x[i] /= len; } + vec_cross(z, x, y); + mat_from_axes(x, y, z, rotation); + for (int i = 0; i < 3; i++) { origin[i] = q1[i]; } + r = q1[6]; + } + mat_rotation(3, r, rz); + mat_mul(rotation, rz, rotation); + *complete = 1; + return INTERP_OK; +} + +// G68.2, G68.4 and G69 from convert_g +int Interp::convert_work_plane(int g_code, block_pointer block, setup_pointer s) +{ + double origin[3], rotation[3][3]; + int complete; + + if (g_code == G_69) { + CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot cancel a tilted work plane with cutter radius compensation on")); + return work_plane_cancel(s, true); + } + if (g_code == G_68_3) { + CHKS((s->g68_seq_code != 0), _("G68.3 cannot interrupt a G68.2 sequence")); + return convert_work_plane_from_tool(block, s); + } + + CHKS((g_code != G_68_2 && g_code != G_68_4), "BUG: code not G68.2, G68.4 or G69"); + CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot define a tilted work plane with cutter radius compensation on")); + CHKS((g_code == G_68_4 && !s->g68_active), + _("G68.4 needs an active tilted work plane to build on")); + + CHP(work_plane_build(block, s, origin, rotation, &complete)); + if (!complete) { return INTERP_OK; } + + if (g_code == G_68_4) { + // composed onto the active plane: the new origin is a point of the + // old plane and the new rotation follows the old one + double ox = origin[0], oy = origin[1], oz = origin[2]; + + g68_apply(s, &ox, &oy, &oz); + origin[0] = ox; + origin[1] = oy; + origin[2] = oz; + mat_mul(s->g68_rotation, rotation, rotation); + } + return work_plane_set(s, g_code, origin, rotation); +} + +//---------------------------------------------------------------------- +// The kinematics. G68.3, the orientation moves and a tool offset change +// need the module motion runs, evaluated here, ahead of motion, through +// the loader in kinematics_userspace/. The loader reads the module's +// pins through HAL, so the interpreter connects as a component, named by +// its process, the first time it is asked. +//---------------------------------------------------------------------- + +#include +#include +#include "units.h" +#include +#include + +#define KINS_CTX(s) ((KinematicsUserContext *)(s)->kins_ctx) + +// the module loaded, once +int Interp::kins_load(setup_pointer s) +{ + KinematicsUserContext *ctx; + + if (!s->kins_ctx) { + char name[HAL_NAME_LEN + 1]; + int comp; + + CHKS((!s->kins_module[0]), + _("the INI file names no [KINS] KINEMATICS, so the kinematics cannot be evaluated here")); + CHKS((s->kins_joints < 1), + _("the INI file gives no [KINS] JOINTS, so the kinematics cannot be evaluated here")); + snprintf(name, sizeof(name), "interp.%d", (int)getpid()); + comp = hal_init(name); + CHKS((comp < 0), + _("cannot connect to HAL to evaluate the kinematics (is realtime running?)")); + s->kins_comp_id = comp; + ctx = kinematicsUserInitString(s->kins_module, s->kins_joints, comp, name); + hal_ready(comp); + CHKS((!ctx), _("kinematics module %s cannot be loaded here"), s->kins_module); + s->kins_ctx = ctx; + for (int i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = 0.0; } + } + return INTERP_OK; +} + +// the loaded module, on the kinematics type the program is in +int Interp::kins_context(setup_pointer s, void **out) +{ + KinematicsUserContext *ctx; + + *out = NULL; + CHP(kins_load(s)); + ctx = KINS_CTX(s); + CHKS((kinematicsUserIsRtOnly(ctx)), + _("kinematics module %s cannot be evaluated outside realtime"), s->kins_module); + CHKS((kinematicsUserSetType(ctx, s->kins_type) != 0), + _("kinematics type %d is not available outside realtime"), s->kins_type); + // with the tool offset the program is under here, not the one motion + // has reached: the interpreter runs ahead of motion + kins_set_tool(ctx, &s->tool_offset); + *out = ctx; + return INTERP_OK; +} + +// the offset the module evaluates with, given in program units like the +// interpreter keeps it +void Interp::kins_set_tool(void *vctx, const EmcPose *offset) +{ + EmcPose tool; + + tool.tran.x = PROGRAM_TO_USER_LEN(offset->tran.x); + tool.tran.y = PROGRAM_TO_USER_LEN(offset->tran.y); + tool.tran.z = PROGRAM_TO_USER_LEN(offset->tran.z); + tool.a = PROGRAM_TO_USER_ANG(offset->a); + tool.b = PROGRAM_TO_USER_ANG(offset->b); + tool.c = PROGRAM_TO_USER_ANG(offset->c); + tool.u = PROGRAM_TO_USER_LEN(offset->u); + tool.v = PROGRAM_TO_USER_LEN(offset->v); + tool.w = PROGRAM_TO_USER_LEN(offset->w); + kinematicsUserSetTool((KinematicsUserContext *)vctx, &tool); +} + +void Interp::kins_release(setup_pointer s) +{ + if (s->kins_ctx) { + kinematicsUserFree(KINS_CTX(s)); + s->kins_ctx = NULL; + } + if (s->kins_comp_id > 0) { + hal_exit(s->kins_comp_id); + s->kins_comp_id = 0; + } +} + +// the current point as the machine sees it: the absolute frame, in the +// machine's units, which is what the kinematics works in +void Interp::current_machine_pose(setup_pointer s, EmcPose *pose) +{ + double abs_pos[9]; + + get_abs_position(s, abs_pos); + pose->tran.x = PROGRAM_TO_USER_LEN(abs_pos[0]); + pose->tran.y = PROGRAM_TO_USER_LEN(abs_pos[1]); + pose->tran.z = PROGRAM_TO_USER_LEN(abs_pos[2]); + pose->a = PROGRAM_TO_USER_ANG(abs_pos[3]); + pose->b = PROGRAM_TO_USER_ANG(abs_pos[4]); + pose->c = PROGRAM_TO_USER_ANG(abs_pos[5]); + pose->u = PROGRAM_TO_USER_LEN(abs_pos[6]); + pose->v = PROGRAM_TO_USER_LEN(abs_pos[7]); + pose->w = PROGRAM_TO_USER_LEN(abs_pos[8]); +} + +// and back: a machine pose as program coordinates, through the chain +void Interp::machine_pose_to_program(setup_pointer s, const EmcPose *pose, double prog[9]) +{ + world_to_program_xyz(s, + USER_TO_PROGRAM_LEN(pose->tran.x), + USER_TO_PROGRAM_LEN(pose->tran.y), + USER_TO_PROGRAM_LEN(pose->tran.z), + &prog[0], &prog[1], &prog[2]); + prog[3] = USER_TO_PROGRAM_ANG(pose->a) - s->tool_offset.a - s->AA_origin_offset - s->AA_axis_offset; + prog[4] = USER_TO_PROGRAM_ANG(pose->b) - s->tool_offset.b - s->BB_origin_offset - s->BB_axis_offset; + prog[5] = USER_TO_PROGRAM_ANG(pose->c) - s->tool_offset.c - s->CC_origin_offset - s->CC_axis_offset; + prog[6] = USER_TO_PROGRAM_LEN(pose->u) - s->tool_offset.u - s->u_origin_offset - s->u_axis_offset; + prog[7] = USER_TO_PROGRAM_LEN(pose->v) - s->tool_offset.v - s->v_origin_offset - s->v_axis_offset; + prog[8] = USER_TO_PROGRAM_LEN(pose->w) - s->tool_offset.w - s->w_origin_offset - s->w_axis_offset; +} + +// whether two machine points are the same, to a hair either way +static bool same_pose(const EmcPose *a, const EmcPose *b) +{ + const double tol = 1e-9; + + return fabs(a->tran.x - b->tran.x) < tol && fabs(a->tran.y - b->tran.y) < tol + && fabs(a->tran.z - b->tran.z) < tol + && fabs(a->a - b->a) < tol && fabs(a->b - b->b) < tol && fabs(a->c - b->c) < tol + && fabs(a->u - b->u) < tol && fabs(a->v - b->v) < tol && fabs(a->w - b->w) < tol; +} + +// the joints the machine is at, as far as the interpreter can know ahead of +// motion: the seed while it still explains the current point, since a point +// does not name one joint set, else the joints the machine stands in +int Interp::current_joints(setup_pointer s, void *vctx, double *joints) +{ + KinematicsUserContext *ctx = (KinematicsUserContext *)vctx; + double standing[EMCMOT_MAX_JOINTS]; + EmcPose pose, seeded; + int pass, i, n; + + current_machine_pose(s, &pose); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = s->kins_seed[i]; } + seeded = pose; + if (kinematicsUserForward(ctx, joints, &seeded) == 0 && same_pose(&pose, &seeded)) { + return INTERP_OK; + } + n = GET_EXTERNAL_JOINT_POSITIONS(standing, EMCMOT_MAX_JOINTS); + for (i = 0; i < n; i++) { joints[i] = standing[i]; } + if (n > 0) { + seeded = pose; + if (kinematicsUserForward(ctx, joints, &seeded) == 0 && same_pose(&pose, &seeded)) { + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = joints[i]; } + return INTERP_OK; + } + } + for (pass = 0; pass < 8; pass++) { + double prev[EMCMOT_MAX_JOINTS], worst = 0.0; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { prev[i] = joints[i]; } + CHKS((kinematicsUserInverse(ctx, &pose, joints) != 0), + _("the kinematics cannot invert the current position")); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { worst = fmax(worst, fabs(joints[i] - prev[i])); } + if (worst < 1e-9) { break; } + } + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = joints[i]; } + return INTERP_OK; +} + +// The kinematics as far as it can be evaluated here: the context on the +// type the program is in, or NULL where there is nothing to evaluate, +// with no machine attached (sai, a preview without one) or a module that +// runs only in realtime. +int Interp::kins_here(setup_pointer s, void **out) +{ + *out = NULL; + if (GET_EXTERNAL_KINEMATICS_IDENTITY()) { return INTERP_OK; } + CHP(kins_load(s)); + if (kinematicsUserIsRtOnly(KINS_CTX(s))) { return INTERP_OK; } + return kins_context(s, out); +} + +// Where the point goes when the tool offset changes: the joints stay, so it is +// read back from them under the new offset, as motion reads it. known stays +// false under the identity and where nothing can be evaluated; the caller +// then shifts by the offset difference. +int Interp::tool_offset_point(setup_pointer s, const EmcPose *offset, const double *standing, + EmcPose *point, bool *known) +{ + void *vctx; + KinematicsUserContext *ctx; + double joints[EMCMOT_MAX_JOINTS]; + int flags, i; + + *known = false; + if (same_pose(offset, &s->tool_offset)) { return INTERP_OK; } + flags = GET_EXTERNAL_KINS_TYPE_FLAGS(s->kins_type); + if (flags >= 0 && (flags & KINSTYPE_IDENTITY)) { return INTERP_OK; } + CHP(kins_here(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + if (!ctx || kinematicsUserIsIdentity(ctx)) { return INTERP_OK; } + if (standing) { + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = standing[i]; } + } else { + CHP(current_joints(s, ctx, joints)); + } + kins_set_tool(ctx, offset); + CHKS((kinematicsUserForward(ctx, joints, point) != 0), + _("the kinematics cannot place the tool from the joints after the tool offset change")); + // the machine stays on these joints + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = joints[i]; } + *known = true; + return INTERP_OK; +} + +// a direction the program gives, in world coordinates: through the +// tilted work plane's rotation where one is active, then the XY rotation +// of the coordinate system it sits on +static void direction_in_world(setup_pointer s, const double v[3], double rotation_xy, PmCartesian *out) +{ + double x = v[0], y = v[1], z = v[2]; + double t = rotation_xy * M_PI / 180.0; + + if (s->g68_active) { + x = s->g68_rotation[0][0] * v[0] + s->g68_rotation[0][1] * v[1] + s->g68_rotation[0][2] * v[2]; + y = s->g68_rotation[1][0] * v[0] + s->g68_rotation[1][1] * v[1] + s->g68_rotation[1][2] * v[2]; + z = s->g68_rotation[2][0] * v[0] + s->g68_rotation[2][1] * v[1] + s->g68_rotation[2][2] * v[2]; + } + out->x = x * cos(t) - y * sin(t); + out->y = x * sin(t) + y * cos(t); + out->z = z; +} + +// a direction of the plane in world coordinates +static void plane_axis_in_world(setup_pointer s, int column, double rotation_xy, PmCartesian *out) +{ + double v[3] = { column == 0 ? 1.0 : 0.0, column == 1 ? 1.0 : 0.0, column == 2 ? 1.0 : 0.0 }; + + direction_in_world(s, v, rotation_xy, out); +} + +static void rotate_about(const PmCartesian *axis, double rad, PmCartesian *v) +{ + // Rodrigues, for a unit axis + PmCartesian c; + double d = axis->x*v->x + axis->y*v->y + axis->z*v->z; + + pmCartCartCross(axis, v, &c); + v->x = v->x*cos(rad) + c.x*sin(rad) + axis->x*d*(1 - cos(rad)); + v->y = v->y*cos(rad) + c.y*sin(rad) + axis->y*d*(1 - cos(rad)); + v->z = v->z*cos(rad) + c.z*sin(rad) + axis->z*d*(1 - cos(rad)); +} + +// G68.3: the plane from the tool. Z is the tool axis as the joints have it, +// X the default tool X of the conventions chapter, R turns the plane from +// there; X Y Z are the origin, as G68.2's +int Interp::convert_work_plane_from_tool(block_pointer block, setup_pointer s) +{ + void *vctx; + KinematicsUserContext *ctx; + double joints[EMCMOT_MAX_JOINTS]; + PmRotationMatrix work, tool, in_work; + PmCartesian zt, xt, yt, zm, xm, x, y; + double origin[3], rotation[3][3], r, t, along; + + CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot define a tilted work plane with cutter radius compensation on")); + CHP(kins_context(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + CHKS((kinematicsUserIsIdentity(ctx)), + _("G68.3 needs a kinematics type that describes the machine; select it with G12.1 first")); + CHP(current_joints(s, ctx, joints)); + CHKS((kinematicsUserWorkFrame(ctx, joints, &work) != 0 + || kinematicsUserToolFrame(ctx, joints, &tool) != 0), + _("the kinematics reports no tool frame, so G68.3 cannot read the tool direction")); + toolFrameInWork(&work, &tool, &in_work); + zt = in_work.z; + xt = in_work.x; + yt = in_work.y; + // machine Z and X seen from the work: the rows of the work frame + zm.x = work.x.z; zm.y = work.y.z; zm.z = work.z.z; + xm.x = work.x.x; xm.y = work.y.x; xm.z = work.z.x; + + pmCartCartCross(&zt, &zm, &x); + if (sqrt(x.x*x.x + x.y*x.y + x.z*x.z) < 1e-9) { + // vertical: tool x is machine x, less whatever of it lies along + // the tool axis, which is rounding + along = xm.x*zt.x + xm.y*zt.y + xm.z*zt.z; + x.x = xm.x - along*zt.x; x.y = xm.y - along*zt.y; x.z = xm.z - along*zt.z; + pmCartUnitEq(&x); + } else { + // the turn about the tool axis that takes tool x into the + // machine XY plane: (cos t x + sin t y) . zm = 0, the root nearer + // to no turn at all + double xz = xt.x*zm.x + xt.y*zm.y + xt.z*zm.z; + double yz = yt.x*zm.x + yt.y*zm.y + yt.z*zm.z; + + t = atan2(-xz, yz); + if (t > M_PI / 2) { t -= M_PI; } + if (t < -M_PI / 2) { t += M_PI; } + x = xt; + rotate_about(&zt, t, &x); + } + r = block->r_flag ? block->r_number : 0.0; + rotate_about(&zt, r * M_PI / 180.0, &x); + pmCartCartCross(&zt, &x, &y); + + // from world directions to the system the plane is defined in: the + // XY rotation comes off + { + PmCartesian cols[3] = { x, y, zt }; + double c = cos(-s->rotation_xy * M_PI / 180.0), sn = sin(-s->rotation_xy * M_PI / 180.0); + + for (int j = 0; j < 3; j++) { + rotation[0][j] = cols[j].x * c - cols[j].y * sn; + rotation[1][j] = cols[j].x * sn + cols[j].y * c; + rotation[2][j] = cols[j].z; + } + } + origin[0] = block->x_flag ? block->x_number : 0.0; + origin[1] = block->y_flag ? block->y_number : 0.0; + origin[2] = block->z_flag ? block->z_number : 0.0; + return work_plane_set(s, G_68_3, origin, rotation); +} + +// G53.1, G53.2, G53.3 and G53.6: the rotaries to the plane's normal. G53.1 +// turns the rotaries alone, in joint space; G53.6 keeps the tool centre point, +// a Cartesian move; G53.3 goes to X Y Z in the plane; G53.2 only publishes the +// pose on #<_orient_x> and kin (Heidenhain STAY). P and Q are orient_solve()'s. +int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) +{ + void *vctx; + KinematicsUserContext *ctx; + double now[EMCMOT_MAX_JOINTS], sol[EMCMOT_MAX_JOINTS]; + PmCartesian axis, xdir; + EmcPose end_pose; + double end_prog[9]; + int p, q, i; + const char *name = (code == G_53_1) ? "G53.1" : (code == G_53_2) ? "G53.2" : (code == G_53_3) ? "G53.3" : "G53.6"; + + CHKS((!s->g68_active), _("%s needs a tilted work plane; define one with G68.2 first"), name); + CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot orient the tool with cutter radius compensation on")); + p = block->p_flag ? (int)round(block->p_number) : 0; + CHKS((block->p_flag && (fabs(block->p_number - p) > 1e-9 || p < 0 || p > 2)), + _("P word with %s must be 0, 1 or 2"), name); + q = block->q_flag ? (int)round(block->q_number) : 0; + CHKS((block->q_flag && (fabs(block->q_number - q) > 1e-9 || (q != 0 && q != 1))), + _("Q word with %s must be 0 or 1"), name); + + CHP(kins_context(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + CHKS((kinematicsUserIsIdentity(ctx)), + _("%s needs a kinematics type that describes the machine; select it with G12.1 first"), name); + CHP(current_joints(s, ctx, now)); + + plane_axis_in_world(s, 2, s->rotation_xy, &axis); + plane_axis_in_world(s, 0, s->rotation_xy, &xdir); + CHP(orient_solve(s, ctx, &axis, &xdir, p, q, now, sol, name)); + + // where that puts the machine, and what the program calls it + end_pose = (EmcPose){}; + current_machine_pose(s, &end_pose); + CHKS((kinematicsUserForward(ctx, sol, &end_pose) != 0), + _("%s: the kinematics cannot place the orientation it found"), name); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = sol[i]; } + machine_pose_to_program(s, &end_pose, end_prog); + + if (code == G_53_2) { + // STAY: solve only, nothing moves. The pose goes to the named + // parameters #<_orient_x> and kin and to #5071-#5080, for the + // program to use in a move of its own making, the way + // Heidenhain's STAY fills Q120-122. The machine state does not + // change. + for (i = 0; i < 6; i++) { s->orient_pose[i] = end_prog[i]; } + s->orient_valid = true; + for (i = 0; i < 9; i++) { s->parameters[5071 + i] = end_prog[i]; } + s->parameters[5080] = 1.0; + return INTERP_OK; + } + + write_canon_state_tag(block, s); + if (code == G_53_1) { + // the rotaries alone: the linear joints are where they are, since + // the solver left them at the seed, and the tool goes wherever + // that carries it + JOINT_TRAVERSE(block->line_number, sol, 1, + end_prog[0], end_prog[1], end_prog[2], + end_prog[3], end_prog[4], end_prog[5], + end_prog[6], end_prog[7], end_prog[8]); + s->current_x = end_prog[0]; + s->current_y = end_prog[1]; + s->current_z = end_prog[2]; + } else if (code == G_53_6) { + // the tool centre point stays: a Cartesian move of the rotaries + STRAIGHT_TRAVERSE(block->line_number, s->current_x, s->current_y, s->current_z, + end_prog[3], end_prog[4], end_prog[5], + s->u_current, s->v_current, s->w_current); + } else { + double x = block->x_flag ? block->x_number : s->current_x; + double y = block->y_flag ? block->y_number : s->current_y; + double z = block->z_flag ? block->z_number : s->current_z; + + JOINT_TRAVERSE(block->line_number, NULL, 0, x, y, z, + end_prog[3], end_prog[4], end_prog[5], + s->u_current, s->v_current, s->w_current); + s->current_x = x; + s->current_y = y; + s->current_z = z; + } + s->AA_current = end_prog[3]; + s->BB_current = end_prog[4]; + s->CC_current = end_prog[5]; + if (code == G_53_1) { + s->u_current = end_prog[6]; + s->v_current = end_prog[7]; + s->w_current = end_prog[8]; + } + return INTERP_OK; +} + +// The joints that point the tool along axis, and its x along xdir where given, +// from the joints the machine is at: every pose the module reports, unwrapped +// onto the nearest turn and ranked by rotary travel. P picks by rank or by +// the sign of the tilting joint; Q0 holds the joints that carry the work +// (Heidenhain COORD ROT), Q1 frees them (TABLE ROT). +int Interp::orient_solve(setup_pointer s, void *vctx, const PmCartesian *axis, const PmCartesian *xdir, + int p, int q, const double *now, double *joints, const char *name) +{ + KinematicsUserContext *ctx = (KinematicsUserContext *)vctx; + double solutions[TOOL_FRAME_MAX_SOLUTIONS * EMCMOT_MAX_JOINTS]; + double spin[TOOL_FRAME_MAX_SOLUTIONS]; + double distance[TOOL_FRAME_MAX_SOLUTIONS]; + int order[TOOL_FRAME_MAX_SOLUTIONS], free_dirs[TOOL_FRAME_MAX_SOLUTIONS]; + unsigned int held = 0; + int n, i, j, chosen, njoints; + + njoints = kinematicsUserGetNumJoints(ctx); + if (q == 0) { + if (kinematicsUserWorkJoints(ctx, now, &held) != 0) { held = 0; } + } + n = kinematicsUserToolFrameInverse(ctx, axis, xdir, now, held, + solutions, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); + if (n == 0 && held) { + // nothing reachable with the work held still: let it move + held = 0; + n = kinematicsUserToolFrameInverse(ctx, axis, xdir, now, held, + solutions, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); + } + CHKS((n < 0), _("%s: the kinematics cannot answer the orientation"), name); + CHKS((n == 0), _("%s: the direction asked for cannot be reached by the rotary joints"), name); + + // the solver reports each answer in (-180, 180], but the machine stands + // somewhere in turn space: unwrap every angular joint onto the turn + // nearest the present position, or the nearest pose is not the nearest + // move and a free rotary swings the long way round + for (i = 0; i < n; i++) { + for (j = 0; j < njoints; j++) { + double *v = &solutions[i*njoints + j]; + if (!(s->kins_angular_joints & (1 << j))) { continue; } + *v += 360.0 * floor((now[j] - *v) / 360.0 + 0.5); + } + } + + // nearest first, by rotary travel in joint units + for (i = 0; i < n; i++) { + distance[i] = 0.0; + for (j = 0; j < njoints; j++) { distance[i] += fabs(solutions[i*njoints + j] - now[j]); } + order[i] = i; + } + for (i = 1; i < n; i++) { + int k = order[i]; + for (j = i; j > 0 && distance[order[j-1]] > distance[k]; j--) { order[j] = order[j-1]; } + order[j] = k; + } + if (p == 0) { + chosen = order[0]; + } else { + // P names the pose rather than its rank, so that the same program + // reaches the same pose from wherever the machine is standing + int primary, secondary; + CHKS((kinematicsUserOrientJoints(ctx, now, &primary, &secondary) != 0), + _("%s P%d: the poses of this machine cannot be told apart by a tilting" + " joint, so leave P out and take the nearest"), name, p); + chosen = -1; + for (i = 0; i < n; i++) { + double value = solutions[order[i]*njoints + secondary]; + if ((p == 1 && value > 1e-9) || (p == 2 && value < -1e-9)) { + chosen = order[i]; + break; + } + } + CHKS((chosen < 0), _("%s P%d: no reachable pose has joint %d %s"), + name, p, secondary, (p == 1) ? "positive" : "negative"); + } + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + joints[i] = (i < njoints) ? solutions[chosen * njoints + i] : 0.0; + } + return INTERP_OK; +} + +// G43.5: I J K on a G0 or G1 line are the tool axis, tip towards holder, in +// the coordinate system the line's X Y Z are in. The rotaries come from the +// tool frame inverse, every orienting joint free, the nearest pose, as program +// rotary coordinates so a rotary offset is right by construction. +int Interp::tool_vector_ends(block_pointer block, setup_pointer s, double *a, double *b, double *c) +{ + void *vctx; + KinematicsUserContext *ctx; + double now[EMCMOT_MAX_JOINTS], sol[EMCMOT_MAX_JOINTS]; + double v[3], prog[9]; + PmCartesian axis; + EmcPose pose; + int i; + + CHKS((block->a_flag || block->b_flag || block->c_flag), + _("G43.5: a tool vector and rotary words on one line give the orientation twice")); + v[0] = block->i_flag ? block->i_number : 0.0; + v[1] = block->j_flag ? block->j_number : 0.0; + v[2] = block->k_flag ? block->k_number : 0.0; + CHKS((vec_norm(v) < 1e-9), _("G43.5: the tool vector I J K is zero")); + CHP(kins_context(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + CHKS((kinematicsUserIsIdentity(ctx)), + _("G43.5: a tool vector needs a kinematics type that describes the machine; select it with G12.1 first")); + CHP(current_joints(s, ctx, now)); + if (block->g_modes[GM_MODAL_0] == G_53) { + axis.x = v[0]; + axis.y = v[1]; + axis.z = v[2]; + } else { + direction_in_world(s, v, s->rotation_xy, &axis); + } + CHP(orient_solve(s, ctx, &axis, NULL, 0, 1, now, sol, "G43.5")); + + // where that puts the rotaries, and what the program calls it + pose = (EmcPose){}; + current_machine_pose(s, &pose); + CHKS((kinematicsUserForward(ctx, sol, &pose) != 0), + _("G43.5: the kinematics cannot place the orientation it found")); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = sol[i]; } + machine_pose_to_program(s, &pose, prog); + *a = prog[3]; + *b = prog[4]; + *c = prog[5]; + return INTERP_OK; +} + +// how long a point-to-point feed is to take: the time the same straight +// move would take at the programmed feed +int Interp::ptp_seconds(block_pointer block, setup_pointer s, + double x, double y, double z, double a, double b, double c, + double u, double v, double w, double *seconds) +{ + if (s->feed_mode == FEED_MODE::INVERSE_TIME) { + CHKS((block->f_number <= 0.0), _("F must be positive with G93")); + *seconds = 60.0 / block->f_number; + } else if (s->feed_mode == FEED_MODE::UNITS_PER_MINUTE) { + double length = find_straight_length(x, y, z, a, b, c, u, v, w, + s->current_x, s->current_y, s->current_z, + s->AA_current, s->BB_current, s->CC_current, + s->u_current, s->v_current, s->w_current); + CHKS((length <= 0.0), + _("a point-to-point feed with no displacement has nothing to apply F to in G94 mode; use G93 or G0")); + *seconds = 60.0 * length / s->feed_rate; + } else { + ERS(_("Cannot use feed per revolution with a point-to-point move")); + } + return INTERP_OK; +} + +// The two point-to-point codes that name joints: G53.5 by axis letter through +// the module's identity mapping, refused where a letter names joints of the +// other unit class; G53.7 by J= in the joint's own units. A joint +// left out keeps its position. +int Interp::convert_ptp_joints(int code, int move, block_pointer block, setup_pointer s) +{ + void *vctx; + KinematicsUserContext *ctx; + const kins_params *p; + double joints[EMCMOT_MAX_JOINTS]; + EmcPose pose; + double prog[9]; + const int flags[9] = { block->x_flag, block->y_flag, block->z_flag, + block->a_flag, block->b_flag, block->c_flag, + block->u_flag, block->v_flag, block->w_flag }; + const double words[9] = { block->x_number, block->y_number, block->z_number, + block->a_number, block->b_number, block->c_number, + block->u_number, block->v_number, block->w_number }; + static const char letters[9] = { 'X', 'Y', 'Z', 'A', 'B', 'C', 'U', 'V', 'W' }; + const char *name = (code == G_53_5) ? "G53.5" : "G53.7"; + int a, j, njoints, given = 0; + + CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot use %s with cutter radius compensation on"), name); + CHP(kins_context(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + njoints = kinematicsUserGetNumJoints(ctx); + p = kinematicsUserParams(ctx); + CHP(current_joints(s, ctx, joints)); + + if (code == G_53_7) { + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + if (!block->joint_flag[j]) { continue; } + CHKS((j >= njoints), _("G53.7: this kinematics has no joint %d"), j); + joints[j] = block->joint_value[j]; + given++; + } + CHKS((given == 0), _("G53.7 needs at least one J= joint word")); + } else { + CHKS((!p), _("G53.5: the kinematics module gives no joint mapping")); + + // A letter carries a unit class and a joint does not, so the letters + // are only a way to name joints where the mapping agrees with them. + // On a serial robot the first joint answers to X and turns in degrees, + // and the whole machine is refused rather than that one letter, since + // a mapping that lies about X is telling nothing useful about A. + for (a = 0; a < 9; a++) { + const int angular = (a >= 3 && a <= 5); + for (j = 0; j < njoints; j++) { + int turns; + if (!(p->joints_of_axis[a] & (1 << j))) { continue; } + turns = (s->kins_angular_joints & (1 << j)) ? 1 : 0; + CHKS((turns != angular), + _("G53.5: on this machine %c names joint %d, which the INI file" + " declares %s, so the axis letters do not name the joints they" + " look like; give joints by number with G53.7 J%d="), + letters[a], j, turns ? "angular" : "linear", j); + } + } + + for (a = 0; a < 9; a++) { + const int angular = (a >= 3 && a <= 5); + double value; + if (!flags[a]) { continue; } + CHKS((p->joints_of_axis[a] == 0), + _("G53.5: %c is not a joint of this kinematics"), letters[a]); + value = angular ? PROGRAM_TO_USER_ANG(words[a]) : PROGRAM_TO_USER_LEN(words[a]); + for (j = 0; j < njoints; j++) { + if (p->joints_of_axis[a] & (1 << j)) { joints[j] = value; } + } + given++; + } + CHKS((given == 0), _("G53.5 needs at least one axis word")); + } + + // the joints of a gantry pair move together: both given, one value + if (code == G_53_7) { + for (a = 0; p && a < EMCMOT_MAX_AXIS; a++) { + int bits = p->joints_of_axis[a]; + int first = -1; + if (!(bits & (bits - 1))) { continue; } + for (j = 0; j < njoints; j++) { + if (!(bits & (1 << j))) { continue; } + if (first < 0) { first = j; continue; } + CHKS((block->joint_flag[j] != block->joint_flag[first]), + _("G53.7: joints %d and %d are a pair on this kinematics, give both"), first, j); + CHKS((block->joint_flag[j] && block->joint_value[j] != block->joint_value[first]), + _("G53.7: joints %d and %d are a pair on this kinematics, give them one value"), first, j); + } + } + } + + // where that puts the tool, and what the program calls it + current_machine_pose(s, &pose); + CHKS((kinematicsUserForward(ctx, joints, &pose) != 0), + _("%s: the kinematics cannot place those joints"), name); + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { s->kins_seed[j] = joints[j]; } + machine_pose_to_program(s, &pose, prog); + + write_canon_state_tag(block, s); + if (move == G_0) { + JOINT_TRAVERSE(block->line_number, joints, 1, + prog[0], prog[1], prog[2], prog[3], prog[4], prog[5], + prog[6], prog[7], prog[8]); + } else { + double seconds; + CHP(ptp_seconds(block, s, prog[0], prog[1], prog[2], prog[3], prog[4], prog[5], + prog[6], prog[7], prog[8], &seconds)); + JOINT_FEED(block->line_number, joints, 1, + prog[0], prog[1], prog[2], prog[3], prog[4], prog[5], + prog[6], prog[7], prog[8], seconds); + } + s->current_x = prog[0]; + s->current_y = prog[1]; + s->current_z = prog[2]; + s->AA_current = prog[3]; + s->BB_current = prog[4]; + s->CC_current = prog[5]; + s->u_current = prog[6]; + s->v_current = prog[7]; + s->w_current = prog[8]; + return INTERP_OK; +} diff --git a/src/emc/rs274ngc/interp_write.cc b/src/emc/rs274ngc/interp_write.cc index b6b2e7d53cf..755c7c8fc0f 100644 --- a/src/emc/rs274ngc/interp_write.cc +++ b/src/emc/rs274ngc/interp_write.cc @@ -22,6 +22,7 @@ #include "nml_intf/interp_return.hh" #include "interp_internal.hh" #include "rs274ngc_interp.hh" +#include // KINSTYPE_PRIMARY /****************************************************************************/ /*! write_g_codes @@ -71,8 +72,10 @@ group 16 - array[15] g7,g8 - lathe diameter mode */ int Interp::write_g_codes(block_pointer block, //!< pointer to a block of RS274/NGC instructions - setup_pointer settings) //!< pointer to machine settings + setup_pointer settings) //!< pointer to machine settings { + int kf; + settings->active_g_codes[0] = settings->sequence_number; settings->active_g_codes[1] = settings->motion_mode; settings->active_g_codes[2] = ((block == NULL) ? -1 : block->g_modes[GM_MODAL_0]); @@ -107,17 +110,24 @@ int Interp::write_g_codes(block_pointer block, //!< pointer to a block of RS27 (settings->origin_index < 7) ? (530 + (10 * settings->origin_index)) : (584 + settings->origin_index); + // the kins type, not the label, is the authority: a G43 given on the + // module's primary type shows as G43.4, or G43.5 while I J K give the + // tool axis, and the label follows the type motion reports after a + // resync. -1 is "no information" and matches every flag, so it is + // excluded before the bit test. + kf = GET_EXTERNAL_KINS_TYPE_FLAGS(settings->kins_type); settings->active_g_codes[9] = (settings->g43_with_zero_offset || settings->tool_offset.tran.x || settings->tool_offset.tran.y || settings->tool_offset.tran.z || settings->tool_offset.a || settings->tool_offset.b || settings->tool_offset.c || - settings->tool_offset.u || settings->tool_offset.v || settings->tool_offset.w) ? G_43 : G_49; + settings->tool_offset.u || settings->tool_offset.v || settings->tool_offset.w) ? + ((kf >= 0 && (kf & KINSTYPE_PRIMARY)) ? (settings->tool_vector ? G_43_5 : G_43_4) : G_43) : G_49; settings->active_g_codes[10] = (settings->retract_mode == RETRACT_MODE::OLD_Z) ? G_98 : G_99; // Three modes: G_64, G_61, G_61_1 or CANON_CONTINUOUS/EXACT_PATH/EXACT_STOP settings->active_g_codes[11] = (settings->control_mode == CANON_CONTINUOUS) ? G_64 : (settings->control_mode == CANON_EXACT_PATH) ? G_61 : G_61_1; - settings->active_g_codes[12] = -1; + settings->active_g_codes[12] = settings->g68_active ? settings->g68_code : -1; settings->active_g_codes[13] = //I don't even know how to display the mode of an arbitrary number of spindles (andypugh 17/6/16) (settings->spindle_mode[0] == SPINDLE_MODE::CONSTANT_RPM) ? G_97 : G_96; settings->active_g_codes[14] = (settings->ijk_distance_mode == DISTANCE_MODE::ABSOLUTE) ? G_90_1 : G_91_1; diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index 46491d6a627..55c5d420dba 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -46,6 +46,7 @@ public: // get ready to run int init() override; void set_loop_on_main_m99(bool state) override; + void set_in_startup_code(bool state) override; // load a tool table int load_tool_table(); @@ -361,6 +362,39 @@ public: setup_pointer settings); int convert_tool_select(block_pointer block, setup_pointer settings); int convert_kins_switch(int code, block_pointer block, setup_pointer settings); + int convert_work_plane(int g_code, block_pointer block, setup_pointer settings); + int work_plane_build(block_pointer block, setup_pointer settings, + double origin[3], double rotation[3][3], int *complete); + int work_plane_set(setup_pointer settings, int code, + const double origin[3], const double rotation[3][3]); + int work_plane_cancel(setup_pointer settings, bool tell_canon_anyway = false); + int work_plane_check_sequence(block_pointer block, setup_pointer settings); + int convert_work_plane_from_tool(block_pointer block, setup_pointer settings); + int convert_orient_tool(int code, block_pointer block, setup_pointer settings); + int orient_solve(setup_pointer settings, void *ctx, const PmCartesian *axis, const PmCartesian *xdir, + int p, int q, const double *now, double *joints, const char *name); + int tool_vector_ends(block_pointer block, setup_pointer settings, double *a, double *b, double *c); + int convert_ptp_joints(int code, int move, block_pointer block, setup_pointer settings); + int ptp_seconds(block_pointer block, setup_pointer settings, + double x, double y, double z, double a, double b, double c, + double u, double v, double w, double *seconds); + int kins_load(setup_pointer settings); + int kins_context(setup_pointer settings, void **ctx); + int kins_here(setup_pointer settings, void **ctx); + int tool_offset_point(setup_pointer settings, const EmcPose *offset, const double *standing, + EmcPose *point, bool *known); + void kins_set_tool(void *ctx, const EmcPose *offset); + void kins_release(setup_pointer settings); + void current_machine_pose(setup_pointer settings, EmcPose *pose); + void machine_pose_to_program(setup_pointer settings, const EmcPose *pose, double prog[9]); + int current_joints(setup_pointer settings, void *ctx, double *joints); + void g68_apply(setup_pointer settings, double *x, double *y, double *z); + void g68_remove(setup_pointer settings, double *x, double *y, double *z); + void g68_unrotate(setup_pointer settings, double *x, double *y, double *z); + void program_to_world_xyz(setup_pointer settings, double px, double py, double pz, + double *wx, double *wy, double *wz); + void world_to_program_xyz(setup_pointer settings, double wx, double wy, double wz, + double *px, double *py, double *pz); int update_tag(StateTag &tag); int cycle_feed(block_pointer block, CANON_PLANE plane, double end1, double end2, double end3); diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index 4163dc99a7c..dcb221c0e39 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -178,6 +178,7 @@ InterpBase *makeInterp() } Interp::~Interp() { + kins_release(&_setup); if(log_file) { if(log_file != stderr) fclose(log_file); @@ -889,6 +890,22 @@ int Interp::init() _setup.random_toolchanger = inifile.findBoolV("RANDOM_TOOLCHANGER", "EMCIO", false); _setup.num_spindles = inifile.findIntV("SPINDLES", "TRAJ", 1); + // the kinematics, for the codes that ask it something + if (auto kins = inifile.findString("KINEMATICS", "KINS")) { + snprintf(_setup.kins_module, sizeof(_setup.kins_module), "%s", kins->c_str()); + } + _setup.kins_joints = inifile.findIntV("JOINTS", "KINS", 0); + // which joints turn rather than slide, so that an axis letter is + // refused where it would name a joint of the other kind + _setup.kins_angular_joints = 0; + for (int jno = 0; jno < _setup.kins_joints && jno < EMCMOT_MAX_JOINTS; jno++) { + char section[16]; + snprintf(section, sizeof(section), "JOINT_%d", jno); + if (auto type = inifile.findString("TYPE", section)) { + if (*type == "ANGULAR") { _setup.kins_angular_joints |= 1 << jno; } + } + } + _setup.tolerance_default = inifile.findRealV("G64_DEFAULT_TOLERANCE", "RS274NGC", 0.0); _setup.naivecam_tolerance_default = inifile.findRealV("G64_DEFAULT_NAIVETOLERANCE", "RS274NGC", 0.0); @@ -1195,6 +1212,9 @@ int Interp::init() _setup.toolchange_flag = false; _setup.input_flag = false; _setup.kinsSwitch_flag = false; + // the tilted work plane does not survive an abort or a program start; + // canon hears about it only if there was one + work_plane_cancel(&_setup); _setup.input_index = -1; _setup.input_digital = false; _setup.program_x = 0.; /* for cutter comp */ @@ -1306,6 +1326,11 @@ void Interp::set_loop_on_main_m99(bool state) { _setup.loop_on_main_m99 = state; } +void Interp::set_in_startup_code(bool state) { + // the startup code runs before the motion queue can drain + _setup.in_startup_code = state; +} + /***********************************************************************/ @@ -2680,6 +2705,12 @@ int Interp::on_abort(int reason, const char *message) reset(); _setup.mdi_interrupt = false; + // the tilted work plane goes before the abort routine runs, so that + // routine can change coordinate systems as it likes. Canon is told + // even when the read ahead had already cancelled it, since the message + // that would have said so died with the queue. + work_plane_cancel(&_setup, true); + /* A thread's queued override restore is lost when abort clears the interpreter list, so re-assert the modal state here. */ if (_setup.speed_override[_setup.active_spindle]) { diff --git a/src/emc/rs274ngc/rs274ngc_return.hh b/src/emc/rs274ngc/rs274ngc_return.hh index ca45ccedc07..f7f8dfcacfc 100644 --- a/src/emc/rs274ngc/rs274ngc_return.hh +++ b/src/emc/rs274ngc/rs274ngc_return.hh @@ -208,6 +208,7 @@ #define NCE_QUEUE_IS_NOT_EMPTY_AFTER_KINS_SWITCH _("Queue is not empty after Kinematics Switch") #define NCE_KINS_TYPE_NOT_PROVIDED _("G12.1 P word does not name a kinematics type this module provides") #define NCE_NO_IDENTITY_KINEMATICS_TYPE _("G13.1 needs the kinematics module to declare its identity type (see the switchkins documentation)") +#define NCE_NO_PRIMARY_KINEMATICS_TYPE _("G43.4 needs the kinematics module to declare its primary type (see the switchkins documentation)") #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 87c77b36fe7..ad6bb3d4e74 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -112,6 +112,16 @@ void SET_XY_ROTATION(double t) { ECHO_WITH_ARGS("%.4f", t); } +void SET_G68_FRAME(double x, double y, double z, + const double rotation[9], int active) { + ECHO_WITH_ARGS("%.4f, %.4f, %.4f, " + "[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f], %d", + x, y, z, + rotation[0], rotation[1], rotation[2], + rotation[3], rotation[4], rotation[5], + rotation[6], rotation[7], rotation[8], active); +} + void SET_G5X_OFFSET(int index, double x, double y, double z, double a, double b, double c, @@ -209,6 +219,49 @@ void SET_TRAVERSE_RATE(double rate) _sai._traverse_rate = rate; } +void JOINT_TRAVERSE(int /*line_number*/, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double /*u*/, double /*v*/, double /*w*/) +{ + if (have_joints && joints) { + ECHO_WITH_ARGS("[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f], " + "%.4f, %.4f, %.4f, %.4f, %.4f, %.4f", + joints[0], joints[1], joints[2], joints[3], joints[4], + joints[5], joints[6], joints[7], joints[8], x, y, z, a, b, c); + } else { + ECHO_WITH_ARGS("%.4f, %.4f, %.4f, %.4f, %.4f, %.4f", x, y, z, a, b, c); + } + _sai._program_position_x = x; + _sai._program_position_y = y; + _sai._program_position_z = z; + _sai._program_position_a = a; + _sai._program_position_b = b; + _sai._program_position_c = c; +} + +void JOINT_FEED(int /*line_number*/, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double /*u*/, double /*v*/, double /*w*/, + double seconds) +{ + if (have_joints && joints) { + ECHO_WITH_ARGS("[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f], " + "%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f", + joints[0], joints[1], joints[2], joints[3], joints[4], + joints[5], joints[6], joints[7], joints[8], x, y, z, a, b, c, seconds); + } else { + ECHO_WITH_ARGS("%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f", x, y, z, a, b, c, seconds); + } + _sai._program_position_x = x; + _sai._program_position_y = y; + _sai._program_position_z = z; + _sai._program_position_a = a; + _sai._program_position_b = b; + _sai._program_position_c = c; +} + void STRAIGHT_TRAVERSE( int /*line_number*/, double x, double y, double z , double a /*AA*/ @@ -549,6 +602,11 @@ void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) offset.tran.x, offset.tran.y, offset.tran.z, offset.a, offset.b, offset.c, offset.u, offset.v, offset.w); } +void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset, const EmcPose& /*point*/) +{ + USE_TOOL_LENGTH_OFFSET(offset); +} + void CHANGE_TOOL() { PRINT("CHANGE_TOOL()\n"); @@ -791,6 +849,11 @@ extern int GET_EXTERNAL_KINS_TYPE_FLAGS(int ktype) return -1; } +extern bool GET_EXTERNAL_KINEMATICS_IDENTITY() +{ + return true; +} + extern void SET_PARAMETER_FILE_NAME(const char *name) { strncpy(_parameter_file_name, name, PARAMETER_FILE_NAME_LENGTH - 1); @@ -870,6 +933,11 @@ double GET_EXTERNAL_POSITION_W() return 0.; } +int GET_EXTERNAL_JOINT_POSITIONS(double * /*joints*/, int /*max*/) +{ + return 0; +} + double GET_EXTERNAL_PROBE_POSITION_U() { return 0.; @@ -1219,8 +1287,5 @@ void UPDATE_TAG(const StateTag& /*tag*/){ void SELECT_KINS_TYPE(int switchkins_type) { - (void)switchkins_type; - printf("saicanon: SELECT_KINS_TYPE\n"); - - return; + ECHO_WITH_ARGS("%d", switchkins_type); } diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 2c7e8ea5fa0..39228bee0e0 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -181,13 +181,46 @@ static void rotate(double &x, double &y, double theta) { } +// The tilted work plane, the innermost stage of the chain: what a program +// calls X Y Z is R * xyz + O in the coordinate system that was active when +// the plane was defined. Rotary and UVW words do not pass through it. +static void g68_apply(double &x, double &y, double &z) { + if (!canon.g68Active) { return; } + const double *r = canon.g68Rotation; + double px = x, py = y, pz = z; + x = r[0]*px + r[1]*py + r[2]*pz + canon.g68Offset[0]; + y = r[3]*px + r[4]*py + r[5]*pz + canon.g68Offset[1]; + z = r[6]*px + r[7]*py + r[8]*pz + canon.g68Offset[2]; +} + +static void g68_remove(double &x, double &y, double &z) { + if (!canon.g68Active) { return; } + const double *r = canon.g68Rotation; + double px = x - canon.g68Offset[0]; + double py = y - canon.g68Offset[1]; + double pz = z - canon.g68Offset[2]; + x = r[0]*px + r[3]*py + r[6]*pz; + y = r[1]*px + r[4]*py + r[7]*pz; + z = r[2]*px + r[5]*py + r[8]*pz; +} + +// a direction: the rotation of the plane without its origin +static void g68_rotate(double &x, double &y, double &z) { + if (!canon.g68Active) { return; } + const double *r = canon.g68Rotation; + double px = x, py = y, pz = z; + x = r[0]*px + r[1]*py + r[2]*pz; + y = r[3]*px + r[4]*py + r[5]*pz; + z = r[6]*px + r[7]*py + r[8]*pz; +} + /** - * Implementation of planar rotation for a 3D vector. - * This is basically a shortcut for "rotate" when the values are stored in a - * cartesian vector. + * Rotation of a direction vector into the world frame: the tilted work + * plane first, then the planar rotation about Z. * The use of static "xy_rotation" is ugly here, but is at least consistent. */ static void to_rotated(PM_CARTESIAN &vec) { + g68_rotate(vec.x, vec.y, vec.z); rotate(vec.x,vec.y,canon.xy_rotation); } #if 0 @@ -197,6 +230,8 @@ static void from_rotated(PM_CARTESIAN &vec) { #endif static void rotate_and_offset(CANON_POSITION & pos) { + g68_apply(pos.x, pos.y, pos.z); + pos += canon.g92Offset; rotate(pos.x, pos.y, canon.xy_rotation); @@ -208,6 +243,8 @@ static void rotate_and_offset(CANON_POSITION & pos) { static void rotate_and_offset_xyz(PM_CARTESIAN & xyz) { + g68_apply(xyz.x, xyz.y, xyz.z); + xyz += canon.g92Offset.xyz(); rotate(xyz.x, xyz.y, canon.xy_rotation); @@ -232,10 +269,14 @@ static CANON_POSITION unoffset_and_unrotate_pos(const CANON_POSITION& pos) { res -= canon.g92Offset; + g68_remove(res.x, res.y, res.z); + return res; } static void rotate_and_offset_pos(double &x, double &y, double &z, double &a, double &b, double &c, double &u, double &v, double &w) { + g68_apply(x, y, z); + x += canon.g92Offset.x; y += canon.g92Offset.y; z += canon.g92Offset.z; @@ -478,6 +519,26 @@ void SET_XY_ROTATION(double t) { canon.xy_rotation = t; } +void SET_G68_FRAME(double x, double y, double z, + const double rotation[9], int active) +{ + flush_segments(); + + canon.g68Offset[0] = FROM_PROG_LEN(x); + canon.g68Offset[1] = FROM_PROG_LEN(y); + canon.g68Offset[2] = FROM_PROG_LEN(z); + for (int i = 0; i < 9; i++) { canon.g68Rotation[i] = rotation[i]; } + canon.g68Active = active; + + auto msg = std::make_unique(); + msg->origin.tran.x = TO_EXT_LEN(canon.g68Offset[0]); + msg->origin.tran.y = TO_EXT_LEN(canon.g68Offset[1]); + msg->origin.tran.z = TO_EXT_LEN(canon.g68Offset[2]); + for (int i = 0; i < 9; i++) { msg->rotation[i] = rotation[i]; } + msg->active = active; + interp_list.append(std::move(msg)); +} + void SET_G5X_OFFSET(int index, double x, double y, double z, double a, double b, double c, @@ -1258,6 +1319,47 @@ void generate_fast_move(double x, double y, double z, canonUpdateEndPoint(x, y, z, a, b, c, u, v, w); } +static void joint_move(int line_number, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w, + double seconds) +{ + auto msg = std::make_unique(); + + flush_segments(); + from_prog(x,y,z,a,b,c,u,v,w); + rotate_and_offset_pos(x,y,z,a,b,c,u,v,w); + + msg->end = to_ext_pose(x, y, z, a, b, c, u, v, w); + msg->have_joints = have_joints ? 1 : 0; + for (int i = 0; i < EMCMOT_MAX_JOINTS; i++) { + msg->joints[i] = (have_joints && joints) ? joints[i] : 0.0; + } + msg->seconds = seconds; + interp_list.set_line_number(line_number); + tag_and_send(std::move(msg), _tag); + + canonUpdateEndPoint(x, y, z, a, b, c, u, v, w); +} + +void JOINT_TRAVERSE(int line_number, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w) +{ + joint_move(line_number, joints, have_joints, x, y, z, a, b, c, u, v, w, 0.0); +} + +void JOINT_FEED(int line_number, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w, + double seconds) +{ + joint_move(line_number, joints, have_joints, x, y, z, a, b, c, u, v, w, seconds); +} + void generate_move(double vel,double x, double y, double z, double a, double b, double c, double u, double v, double w) @@ -2599,7 +2701,9 @@ void ARC_FEED(int line_number, canon_debug("line = %d\n", line_number); canon_debug("first_end = %f, second_end = %f\n", first_end,second_end); - if( canon.activePlane == CANON_PLANE::XY && canon.motionMode == CANON_CONTINUOUS) { + // the naive cam detector works on the world XY projection of the arc, + // which a tilted work plane takes out of the XY plane + if( canon.activePlane == CANON_PLANE::XY && canon.motionMode == CANON_CONTINUOUS && !canon.g68Active) { double mx, my; double lx, ly, lz; double unused = 0; @@ -2838,7 +2942,7 @@ void ARC_FEED(int line_number, double j2 = FROM_EXT_LEN(emcAxisGetMaxJerk(axis2)); double j_min = MIN(j1, j2); - if(canon.xy_rotation && canon.activePlane != CANON_PLANE::XY) { + if((canon.xy_rotation && canon.activePlane != CANON_PLANE::XY) || canon.g68Active) { // also consider the third plane's constraint, which may get // involved since we're rotated. @@ -3109,7 +3213,7 @@ void SET_TOOL_TABLE_ENTRY(int pocket, int toolno, const EmcPose& offset, double EMC has no tool length offset. To implement it, we save it here, and apply it when necessary */ -void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) +static void use_tool_length_offset(const EmcPose& offset, const EmcPose *point) { auto set_offset_msg = std::make_unique(); @@ -3138,6 +3242,13 @@ void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) set_offset_msg->offset.v = TO_EXT_LEN(canon.toolOffset.v); set_offset_msg->offset.w = TO_EXT_LEN(canon.toolOffset.w); + set_offset_msg->have_point = (point != nullptr); + if (point) { + CANON_POSITION at(*point); + from_prog(at); + set_offset_msg->point = to_ext_pose(at); + } + for (int s = 0; s < emcStatus->motion.traj.spindles; s++){ if(canon.spindle[s].css_maximum) { SET_SPINDLE_SPEED(s, canon.spindle[s].speed); @@ -3146,6 +3257,16 @@ void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) interp_list.append(std::move(set_offset_msg)); } +void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) +{ + use_tool_length_offset(offset, nullptr); +} + +void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset, const EmcPose& point) +{ + use_tool_length_offset(offset, &point); +} + /* CHANGE_TOOL results from M6 */ void CHANGE_TOOL() { @@ -3618,6 +3739,9 @@ void INIT_CANON() // initialize locals to original values canon.xy_rotation = 0.0; + canon.g68Offset[0] = canon.g68Offset[1] = canon.g68Offset[2] = 0.0; + for (int i = 0; i < 9; i++) { canon.g68Rotation[i] = (i % 4 == 0) ? 1.0 : 0.0; } + canon.g68Active = 0; canon.rotary_unlock_for_traverse = -1; canon.feed_mode = 0; canon.g5xOffset.x = 0.0; @@ -4020,6 +4144,17 @@ double GET_EXTERNAL_POSITION_W(void) return position.w; } +int GET_EXTERNAL_JOINT_POSITIONS(double *joints, int max) +{ + int n = emcStatus->motion.traj.joints; + + if (n > max) { n = max; } + for (int i = 0; i < n; i++) { + joints[i] = emcStatus->motion.joint[i].output; + } + return n; +} + double GET_EXTERNAL_PROBE_POSITION_X(void) { CANON_POSITION position; @@ -4105,6 +4240,11 @@ int GET_EXTERNAL_KINS_TYPE_FLAGS(int ktype) return emcStatus->motion.traj.switchkins_flags[ktype]; } +bool GET_EXTERNAL_KINEMATICS_IDENTITY() +{ + return emcStatus->motion.traj.kinematics_type == KINEMATICS_IDENTITY; +} + double GET_EXTERNAL_MOTION_CONTROL_TOLERANCE() { return TO_PROG_LEN(canon.motionTolerance); diff --git a/src/emc/task/emctask.cc b/src/emc/task/emctask.cc index 67465d507dd..1e93d3b3090 100644 --- a/src/emc/task/emctask.cc +++ b/src/emc/task/emctask.cc @@ -463,10 +463,15 @@ int emcTaskPlanInit() print_interp_error(retval); } else { if (0 != rs274ngc_startup_code[0]) { + // the startup code runs before the main loop can service a + // drain-and-assert wait, so a kinematics switch there must not + // ask for one + interp.set_in_startup_code(true); retval = interp.execute(rs274ngc_startup_code); while (retval == INTERP_EXECUTE_FINISH) { retval = interp.execute(NULL); } + interp.set_in_startup_code(false); if (retval > INTERP_MIN_ERROR) { print_interp_error(retval); } diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index 5e41127a5a2..28ca94c953e 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -475,6 +475,9 @@ static int checkInterpList(NML_INTERP_LIST * il, EMC_STAT * /*stat*/) case EMC_TRAJ_LINEAR_MOVE_TYPE: break; + case EMC_TRAJ_JOINT_MOVE_TYPE: + break; + case EMC_TRAJ_CIRCULAR_MOVE_TYPE: break; @@ -1521,6 +1524,7 @@ static EMC_TASK_EXEC emcTaskCheckPreconditions(NMLmsg * cmd) break; case EMC_TRAJ_LINEAR_MOVE_TYPE: + case EMC_TRAJ_JOINT_MOVE_TYPE: case EMC_TRAJ_CIRCULAR_MOVE_TYPE: case EMC_TRAJ_SET_VELOCITY_TYPE: case EMC_TRAJ_SET_ACCELERATION_TYPE: @@ -1538,6 +1542,7 @@ static EMC_TASK_EXEC emcTaskCheckPreconditions(NMLmsg * cmd) case EMC_TRAJ_SET_G5X_TYPE: case EMC_TRAJ_SET_G92_TYPE: case EMC_TRAJ_SET_ROTATION_TYPE: + case EMC_TRAJ_SET_G68_TYPE: // this applies the program origin after previous motions return EMC_TASK_EXEC::WAITING_FOR_MOTION; break; @@ -1833,6 +1838,13 @@ static int emcTaskIssueCommand(NMLmsg * cmd) emcTrajLinearMoveMsg->indexer_jnum); break; + case EMC_TRAJ_JOINT_MOVE_TYPE: { + EMC_TRAJ_JOINT_MOVE *jm = reinterpret_cast(cmd); + emcTrajUpdateTag(jm->tag); + retval = emcTrajJointMove(jm->end, jm->joints, jm->have_joints, jm->seconds); + break; + } + case EMC_TRAJ_CIRCULAR_MOVE_TYPE: emcTrajUpdateTag((reinterpret_cast(cmd))->tag); emcTrajCircularMoveMsg = reinterpret_cast(cmd); @@ -1896,17 +1908,29 @@ static int emcTaskIssueCommand(NMLmsg * cmd) retval = emcTrajSetSpindleSync(emcTrajSetSpindlesyncMsg->spindle, emcTrajSetSpindlesyncMsg->feed_per_revolution, emcTrajSetSpindlesyncMsg->velocity_mode); break; - case EMC_TRAJ_SET_OFFSET_TYPE: + case EMC_TRAJ_SET_OFFSET_TYPE: { // update tool offset - emcStatus->task.toolOffset = (reinterpret_cast(cmd))->offset; - retval = emcTrajSetOffset(emcStatus->task.toolOffset); + EMC_TRAJ_SET_OFFSET *msg = reinterpret_cast(cmd); + emcStatus->task.toolOffset = msg->offset; + retval = emcTrajSetOffset(emcStatus->task.toolOffset, + msg->have_point ? &msg->point : nullptr); break; + } case EMC_TRAJ_SET_ROTATION_TYPE: emcStatus->task.rotation_xy = (reinterpret_cast(cmd))->rotation; retval = 0; break; + case EMC_TRAJ_SET_G68_TYPE: { + EMC_TRAJ_SET_G68 *g68 = reinterpret_cast(cmd); + emcStatus->task.g68_offset = g68->origin; + for (int i = 0; i < 9; i++) { emcStatus->task.g68_rotation[i] = g68->rotation[i]; } + emcStatus->task.g68_active = g68->active; + retval = 0; + break; + } + case EMC_TRAJ_SET_G5X_TYPE: // struct-copy program origin emcStatus->task.g5x_offset = (reinterpret_cast(cmd))->origin; @@ -2487,6 +2511,7 @@ static EMC_TASK_EXEC emcTaskCheckPostconditions(NMLmsg * cmd) return EMC_TASK_EXEC::WAITING_FOR_SYSTEM_CMD; break; + case EMC_TRAJ_JOINT_MOVE_TYPE: case EMC_TRAJ_LINEAR_MOVE_TYPE: case EMC_TRAJ_CIRCULAR_MOVE_TYPE: case EMC_TRAJ_SET_VELOCITY_TYPE: @@ -2497,6 +2522,7 @@ static EMC_TASK_EXEC emcTaskCheckPostconditions(NMLmsg * cmd) case EMC_TRAJ_SET_G5X_TYPE: case EMC_TRAJ_SET_G92_TYPE: case EMC_TRAJ_SET_ROTATION_TYPE: + case EMC_TRAJ_SET_G68_TYPE: case EMC_TRAJ_PROBE_TYPE: case EMC_TRAJ_RIGID_TAP_TYPE: case EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG_TYPE: diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 585a22af698..4c34d3078e7 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -1498,10 +1498,31 @@ double emcTrajGetAngularUnits() return TrajConfig.AngularUnits; } -int emcTrajSetOffset(const EmcPose& tool_offset) +int emcTrajJointMove(const EmcPose& end, const double *joints, int have_joints, double seconds) +{ + int i; + + emcmotCommand.command = EMCMOT_SET_JOINT_LINE; + emcmotCommand.pos = end; + emcmotCommand.id = TrajConfig.MotionId; + emcmotCommand.tag = localEmcTrajTag; + emcmotCommand.motion_type = seconds > 0.0 ? EMC_MOTION_TYPE_FEED : EMC_MOTION_TYPE_TRAVERSE; + emcmotCommand.joint_seconds = seconds; + emcmotCommand.have_joint_target = have_joints; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + emcmotCommand.joint_target[i] = (have_joints && joints) ? joints[i] : 0.0; + } + return usrmotWriteEmcmotCommand(&emcmotCommand); +} + +int emcTrajSetOffset(const EmcPose& tool_offset, const EmcPose *point) { emcmotCommand.command = EMCMOT_SET_OFFSET; emcmotCommand.tool_offset = tool_offset; + emcmotCommand.have_point = (point != nullptr); + if (point) { + emcmotCommand.pos = *point; + } return usrmotWriteEmcmotCommand(&emcmotCommand); } diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index e1c9fe5ffe3..177961cf4b3 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -203,6 +203,7 @@ int tcGetStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const ou tcCircleStartAccelUnitVector(tc,out); break; case TC_SPHERICAL: + case TC_JOINT: return -1; default: return -1; @@ -210,6 +211,24 @@ int tcGetStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const ou return 0; } +/** + * The world direction of a joint interpolated segment, end minus start, + * for the status fields that want a direction. The path between them is + * not straight, so this is the chord, and there is none when only the + * rotaries move. + */ +static int tcJointChordUnitVector(TC_STRUCT const * const tc, PmCartesian * const out) +{ + PmCartesian d; + double mag; + + pmCartCartSub(&tc->coords.joint.world_end.tran, &tc->coords.joint.world_start.tran, &d); + pmCartMag(&d, &mag); + if (mag < TP_POS_EPSILON) { return -1; } + pmCartScalMult(&d, 1.0 / mag, out); + return 0; +} + /** * Get the acceleration direction unit vector for blend velocity calculations. * This calculates the direction of acceleration at the end of a segment. @@ -328,6 +347,8 @@ int tcGetStartTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const case TC_CIRCULAR: pmCircleTangentVector(&tc->coords.circle.xyz, 0.0, out); break; + case TC_JOINT: + return tcJointChordUnitVector(tc, out); default: rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d!\n",tc->motion_type); return -1; @@ -351,6 +372,8 @@ int tcGetEndTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const ou pmCircleTangentVector(&tc->coords.circle.xyz, tc->coords.circle.xyz.angle, out); break; + case TC_JOINT: + return tcJointChordUnitVector(tc, out); default: rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d!\n",tc->motion_type); return -1; @@ -406,6 +429,8 @@ int tcGetCurrentTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * cons arcTangent(arc, out, at_end); } break; + case TC_JOINT: + return tcJointChordUnitVector(tc, out); default: rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d in tcGetCurrentTangentUnitVector!\n", tc->motion_type); return -1; @@ -529,6 +554,30 @@ int tcGetPosReal(TC_STRUCT const * const tc, int of_point, EmcPose * const pos) abc = tc->coords.arc.abc; uvw = tc->coords.arc.uvw; break; + case TC_JOINT: { + // the ends are exact; between them this is the chord, a proxy + // for the status fields, and the servo thread reports the + // real position from the forward kinematics + const PmJointLine *jl = &tc->coords.joint; + double f = (tc->target > 0.0) ? progress / tc->target : 0.0; + EmcPose d; + + emcPoseSub(&jl->world_end, &jl->world_start, &d); + pos->tran.x = jl->world_start.tran.x + f * d.tran.x; + pos->tran.y = jl->world_start.tran.y + f * d.tran.y; + pos->tran.z = jl->world_start.tran.z + f * d.tran.z; + pos->a = jl->world_start.a + f * d.a; + pos->b = jl->world_start.b + f * d.b; + pos->c = jl->world_start.c + f * d.c; + pos->u = jl->world_start.u + f * d.u; + pos->v = jl->world_start.v + f * d.v; + pos->w = jl->world_start.w + f * d.w; + if (of_point == TC_GET_ENDPOINT) { *pos = jl->world_end; } + return TP_ERR_OK; + } + default: + rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d in tcGetPosReal!\n", tc->motion_type); + return TP_ERR_FAIL; } if (res_fit == TP_ERR_OK) { @@ -539,6 +588,26 @@ int tcGetPosReal(TC_STRUCT const * const tc, int of_point, EmcPose * const pos) } +/** + * The joints of a joint interpolated segment at its progress. + * Returns the joint count, or 0 for any other segment. + */ +int tcGetJointPos(TC_STRUCT const * const tc, double * const joints) +{ + const PmJointLine *jl; + double f; + int i; + + if (!tc || tc->motion_type != TC_JOINT) { return 0; } + jl = &tc->coords.joint; + f = (tc->target > 0.0) ? tc->progress / tc->target : 1.0; + if (f > 1.0) { f = 1.0; } + for (i = 0; i < jl->num_joints; i++) { + joints[i] = jl->start[i] + f * (jl->end[i] - jl->start[i]); + } + return jl->num_joints; +} + /** * Set the terminal condition of a segment. * This function will eventually handle state changes associated with altering a terminal condition. @@ -624,7 +693,7 @@ int tcIsBlending(TC_STRUCT * const tc) { //FIXME Disabling blends for rigid tap cycle until changes can be verified. int is_blending_next = (tc->term_cond == TC_TERM_COND_PARABOLIC ) && tc->on_final_decel && (tc->currentvel < tc->blend_vel) && - tc->motion_type != TC_RIGIDTAP; + tc->motion_type != TC_RIGIDTAP && tc->motion_type != TC_JOINT; //Latch up the blending_next status here, so that even if the prev conditions //aren't necessarily true we still blend to completion once the blend @@ -1071,6 +1140,9 @@ double pmRigidTapTarget(PmRigidTap * const tap, double uu_per_rev) /** Returns true if segment has ONLY rotary motion, false otherwise. */ int tcPureRotaryCheck(TC_STRUCT const * const tc) { + // a joint interpolated segment measures its velocity in joint units, + // so the cartesian limit does not apply to it either + if (tc->motion_type == TC_JOINT) { return 1; } return (tc->motion_type == TC_LINEAR) && (tc->coords.line.xyz.tmag_zero) && (tc->coords.line.uvw.tmag_zero); diff --git a/src/emc/tp/tc.h b/src/emc/tp/tc.h index 5558a55e280..dce3df6753c 100644 --- a/src/emc/tp/tc.h +++ b/src/emc/tp/tc.h @@ -37,6 +37,7 @@ int tcGetEndpoint(TC_STRUCT const * const tc, EmcPose * const out); int tcGetStartpoint(TC_STRUCT const * const tc, EmcPose * const out); int tcGetPos(TC_STRUCT const * const tc, EmcPose * const out); int tcGetPosReal(TC_STRUCT const * const tc, int of_endpoint, EmcPose * const out); +int tcGetJointPos(TC_STRUCT const * const tc, double * const joints); int tcGetEndAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); int tcGetStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); int tcGetEndTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); diff --git a/src/emc/tp/tc_types.h b/src/emc/tp/tc_types.h index 135b34586ae..95cdf540dde 100644 --- a/src/emc/tp/tc_types.h +++ b/src/emc/tp/tc_types.h @@ -33,7 +33,8 @@ typedef enum { TC_LINEAR = 1, TC_CIRCULAR = 2, TC_RIGIDTAP = 3, - TC_SPHERICAL = 4 + TC_SPHERICAL = 4, + TC_JOINT = 5 } tc_motion_type_t; typedef enum { @@ -117,6 +118,19 @@ typedef struct { RIGIDTAP_STATE state; } PmRigidTap; +/* A segment interpolated in joint space: every joint runs from start to + * end together, the longest one setting the pace. The world poses at the + * two ends are what the segments around it see; the position along the way + * is not a line in world space and the servo thread reports it from the + * forward kinematics. */ +typedef struct { + double start[EMCMOT_MAX_JOINTS]; + double end[EMCMOT_MAX_JOINTS]; + int num_joints; + EmcPose world_start; + EmcPose world_end; +} PmJointLine; + typedef struct { double cycle_time; //Position stuff @@ -160,11 +174,13 @@ typedef struct { PmCircle9 circle; PmRigidTap rigidtap; Arc9 arc; + PmJointLine joint; } coords; int motion_type; // TC_LINEAR (coords.line) or // TC_CIRCULAR (coords.circle) or - // TC_RIGIDTAP (coords.rigidtap) + // TC_RIGIDTAP (coords.rigidtap) or + // TC_JOINT (coords.joint) int active; // this motion is being executed int canon_motion_type; // this motion is due to which canon function? int term_cond; // gcode requests continuous feed at the end of diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 80e7570222b..c705ca5bff1 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -158,6 +158,8 @@ STATIC int tcRotaryMotionCheck(TC_STRUCT const * const tc) { } case TC_SPHERICAL: return true; + case TC_JOINT: + return true; default: tp_debug_print("Unknown motion type!\n"); return false; @@ -440,12 +442,23 @@ STATIC void tpReleaseQueuedPlanners(TP_STRUCT * const tp) * intended to put the motion queue in the state it would be if all queued * motions finished at the current position. */ +/* What the planner knows in joint space belongs to the queue: with the + queue reset, the queue end is no longer at those joints, no segment is + left to hand its end joints out, and no joint segment is waiting. */ +STATIC void tpForgetJoints(TP_STRUCT * const tp) +{ + tp->queue_end_joints_valid = 0; + tp->joint_end_valid = 0; + tp->joint_segments_queued = 0; +} + int tpClear(TP_STRUCT * const tp) { tpReleaseQueuedPlanners(tp); tcqInit(&tp->queue); tp->queueSize = 0; tp->goalPos = tp->currentPos; + tpForgetJoints(tp); // Clear out status ID's tp->nextId = 0; tp->execId = 0; @@ -1649,6 +1662,8 @@ int tpAddRigidTap(TP_STRUCT * const tp, acc, ini_maxjerk); + tp->queue_end_joints_valid = 0; + // Setup rigid tap geometry pmRigidTapInit(&tc.coords.rigidtap, &tp->goalPos, @@ -2071,6 +2086,11 @@ tc_blend_type_t tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc) { tp_debug_print(" queue empty\n"); return NO_BLEND; } + if (prev_tc->motion_type == TC_JOINT) { + // nothing blends with a joint interpolated segment + tcSetTermCond(prev_tc, tc, TC_TERM_COND_STOP); + return NO_BLEND; + } if (prev_tc->progress > prev_tc->target / 2.0) { tp_debug_print(" prev_tc progress (%f) is too large, aborting blend arc\n", prev_tc->progress); return NO_BLEND; @@ -2109,6 +2129,78 @@ tc_blend_type_t tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc) { return blend_used; } +/** + * Add a joint interpolated segment to the tc queue. + * + * The joints run from start to end together over a "length" that is the + * joint space distance between them. vel and acc are already the tightest + * per-joint limits scaled onto that length, so no joint exceeds its own. + * Nothing blends into or out of it: the segment before it is made to stop + * and so is this one, since the path between the two world poses is not a + * line and the next segment has to start from rest at world_end. + */ +int tpAddJointLine(TP_STRUCT * const tp, const double *start, const double *end, + int num_joints, EmcPose world_end, int canon_motion_type, + double vel, double ini_maxvel, double acc, double ini_maxjerk, + unsigned char enables, struct state_tag_t tag) +{ + TC_STRUCT tc = {0}; + PmJointLine *jl = &tc.coords.joint; + TC_STRUCT *prev_tc; + double length = 0.0; + int i; + + if (!tp || !start || !end || num_joints <= 0 || num_joints > EMCMOT_MAX_JOINTS) { + return TP_ERR_MISSING_INPUT; + } + if (tp->aborting) { + rtapi_print_msg(RTAPI_MSG_ERR, "TP is aborting\n"); + return TP_ERR_FAIL; + } + + tcInit(&tc, TC_JOINT, canon_motion_type, tp->cycleTime, enables, 0); + tc.tag = tag; + tpSetupSyncedIO(tp, &tc); + tcSetupState(&tc, tp); + // a joint move has no path to synchronise to a spindle along + tc.synchronized = TC_SYNC_NONE; + tc.uu_per_rev = 0.0; + tcSetupMotion(&tc, vel, ini_maxvel, acc, ini_maxjerk); + + jl->num_joints = num_joints; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + jl->start[i] = (i < num_joints) ? start[i] : 0.0; + jl->end[i] = (i < num_joints) ? end[i] : 0.0; + length += (jl->end[i] - jl->start[i]) * (jl->end[i] - jl->start[i]); + } + jl->world_start = tp->goalPos; + jl->world_end = world_end; + + tc.target = pmSqrt(length); + if (tc.target < TP_POS_EPSILON) { + return TP_ERR_ZERO_LENGTH; + } + tc.nominal_length = tc.target; + tcClampVelocityByLength(&tc); + tc.indexer_jnum = -1; + tcSetTermCond(&tc, NULL, TC_TERM_COND_STOP); + + prev_tc = tcqLast(&tp->queue); + if (prev_tc) { + tcSetTermCond(prev_tc, &tc, TC_TERM_COND_STOP); + tcFinalizeLength(prev_tc); + } + + int retval = tpAddSegmentToQueue(tp, &tc, true); + if (retval == TP_ERR_OK) { + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { tp->queue_end_joints[i] = jl->end[i]; } + tp->queue_end_joints_valid = 1; + tp->joint_segments_queued++; + } + tpRunOptimization(tp); + return retval; +} + //TODO final setup steps as separate functions // /** @@ -2149,6 +2241,7 @@ int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, acc, ini_maxjerk); // Setup line geometry + tp->queue_end_joints_valid = 0; pmLine9Init(&tc.coords.line, &tp->goalPos, &end); @@ -2220,6 +2313,7 @@ int tpAddCircle(TP_STRUCT * const tp, tp->cycleTime, enables, atspeed); + tp->queue_end_joints_valid = 0; tc.tag = tag; // Setup any synced IO for this move tpSetupSyncedIO(tp, &tc); @@ -3314,6 +3408,7 @@ STATIC void tpHandleEmptyQueue(TP_STRUCT * const tp) tpReleaseQueuedPlanners(tp); tcqInit(&tp->queue); + tpForgetJoints(tp); tp->goalPos = tp->currentPos; tp->done = 1; tp->depth = tp->activeDepth = 0; @@ -3369,6 +3464,16 @@ STATIC int tpCompleteSegment(TP_STRUCT * const tp, return TP_ERR_FAIL; } + // a joint interpolated segment leaves its end joints behind for the + // servo thread: it asks after the segment is gone, and would otherwise + // invert the end position with the previous cycle's joints as seed + if (tc->motion_type == TC_JOINT) { + int i; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { tp->joint_end[i] = tc->coords.joint.end[i]; } + tp->joint_end_valid = 1; + if (tp->joint_segments_queued > 0) { tp->joint_segments_queued--; } + } + //Clear status flags associated since segment is done //TODO stuff into helper function? tc->active = 0; @@ -3415,6 +3520,7 @@ STATIC tp_err_t tpHandleAbort(TP_STRUCT * const tp, TC_STRUCT * const tc, (tc->currentvel == 0.0 && (!nexttc || nexttc->currentvel == 0.0))) { tpReleaseQueuedPlanners(tp); tcqInit(&tp->queue); + tpForgetJoints(tp); tp->goalPos = tp->currentPos; tp->done = 1; tp->depth = tp->activeDepth = 0; @@ -3502,7 +3608,8 @@ STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { return TP_ERR_MISSING_INPUT; } - if (tp->reverse_run && (tc->motion_type == TC_RIGIDTAP || tc->synchronized != TC_SYNC_NONE)) { + if (tp->reverse_run && (tc->motion_type == TC_RIGIDTAP || tc->motion_type == TC_JOINT + || tc->synchronized != TC_SYNC_NONE)) { //Can't activate a segment with synced motion in reverse return TP_ERR_REVERSE_EMPTY; } @@ -4379,6 +4486,72 @@ int tpGetPos(TP_STRUCT const * const tp, EmcPose * const pos) return TP_ERR_OK; } +int tpGetGoalPos(TP_STRUCT const * const tp, EmcPose * const pos) +{ + if (0 == tp) { + ZERO_EMC_POSE((*pos)); + return TP_ERR_FAIL; + } + *pos = tp->goalPos; + return TP_ERR_OK; +} + +/** + * The joints the active segment commands, when it is a joint interpolated + * one: the servo thread takes these instead of inverting the position. + * Returns the joint count, or 0 when the active segment is any other kind. + */ +int tpGetJointPos(TP_STRUCT const * const tp, double * const joints) +{ + TC_STRUCT const *tc; + + if (!tp || !joints) { return 0; } + tc = tcqItem((TC_QUEUE_STRUCT *)&tp->queue, 0); + if (!tc || !tc->active) { return 0; } + return tcGetJointPos(tc, joints); +} + +/** + * The end joints of a joint interpolated segment that completed this + * cycle, once: the seed for the servo thread's inverse of the position the + * planner is now at, which is that segment's end and whatever a following + * segment added in the rest of the cycle. Returns 1 and fills the joints, + * or 0. + */ +int tpTakeJointEnd(TP_STRUCT * const tp, double * const joints) +{ + int i; + + if (!tp || !joints || !tp->joint_end_valid) { return 0; } + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = tp->joint_end[i]; } + tp->joint_end_valid = 0; + return 1; +} + +/** + * How many joint interpolated segments the queue holds, active one + * included. An external offset cannot ride on one, so its planning waits + * while any is queued. + */ +int tpJointSegmentsQueued(TP_STRUCT const * const tp) +{ + return tp ? tp->joint_segments_queued : 0; +} + +/** + * Where the queue ends in joint space, if the last segment queued was a + * joint interpolated one. Returns 1 and fills the joints, or 0 when the + * answer is the inverse of the goal position. + */ +int tpGetQueueEndJoints(TP_STRUCT const * const tp, double * const joints) +{ + int i; + + if (!tp || !joints || !tp->queue_end_joints_valid) { return 0; } + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = tp->queue_end_joints[i]; } + return 1; +} + int tpIsDone(TP_STRUCT * const tp) { if (0 == tp) { @@ -4471,6 +4644,12 @@ EXPORT_SYMBOL(tpAbort); EXPORT_SYMBOL(tpActiveDepth); EXPORT_SYMBOL(tpAddCircle); EXPORT_SYMBOL(tpAddLine); +EXPORT_SYMBOL(tpAddJointLine); +EXPORT_SYMBOL(tpGetGoalPos); +EXPORT_SYMBOL(tpGetJointPos); +EXPORT_SYMBOL(tpTakeJointEnd); +EXPORT_SYMBOL(tpJointSegmentsQueued); +EXPORT_SYMBOL(tpGetQueueEndJoints); EXPORT_SYMBOL(tpAddRigidTap); EXPORT_SYMBOL(tpClear); EXPORT_SYMBOL(tpCreate); diff --git a/src/emc/tp/tp.h b/src/emc/tp/tp.h index e00b457ad23..a1af63deb11 100644 --- a/src/emc/tp/tp.h +++ b/src/emc/tp/tp.h @@ -63,6 +63,15 @@ int tpAddCircle(TP_STRUCT * const tp, EmcPose end, PmCartesian center, double ini_maxvel, double acc, double ini_maxjerk, unsigned char enables, char atspeed, struct state_tag_t tag); int tpGetPos(TP_STRUCT const * const tp, EmcPose * const pos); +int tpGetGoalPos(TP_STRUCT const * const tp, EmcPose * const pos); +int tpAddJointLine(TP_STRUCT * const tp, const double *start, const double *end, + int num_joints, EmcPose world_end, int canon_motion_type, + double vel, double ini_maxvel, double acc, double ini_maxjerk, + unsigned char enables, struct state_tag_t tag); +int tpGetJointPos(TP_STRUCT const * const tp, double * const joints); +int tpTakeJointEnd(TP_STRUCT * const tp, double * const joints); +int tpJointSegmentsQueued(TP_STRUCT const * const tp); +int tpGetQueueEndJoints(TP_STRUCT const * const tp, double * const joints); int tpIsDone(TP_STRUCT * const tp); int tpQueueDepth(TP_STRUCT * const tp); int tpActiveDepth(TP_STRUCT * const tp); diff --git a/src/emc/tp/tp_types.h b/src/emc/tp/tp_types.h index 0e9ab844322..26ed50933bd 100644 --- a/src/emc/tp/tp_types.h +++ b/src/emc/tp/tp_types.h @@ -109,6 +109,17 @@ typedef struct { EmcPose currentPos; EmcPose goalPos; + /* where the queue ends in joint space, known when the last segment + queued was a joint interpolated one; a world segment after it makes + the answer the inverse of goalPos again */ + double queue_end_joints[EMCMOT_MAX_JOINTS]; + int queue_end_joints_valid; + /* the end joints of a joint interpolated segment that completed this + cycle, for the servo thread to seed its inverse with: the segment is + gone from the queue by the time it asks */ + double joint_end[EMCMOT_MAX_JOINTS]; + int joint_end_valid; + int joint_segments_queued; /* joint interpolated segments in the queue */ int queueSize; double cycleTime; diff --git a/src/emc/usr_intf/axis/extensions/emcmodule.cc b/src/emc/usr_intf/axis/extensions/emcmodule.cc index 0c5a5742986..0e24ef9df82 100644 --- a/src/emc/usr_intf/axis/extensions/emcmodule.cc +++ b/src/emc/usr_intf/axis/extensions/emcmodule.cc @@ -1151,6 +1151,7 @@ static PyMemberDef Stat_members[] = { { "task_paused", T_INT, O(task.task_paused), READONLY, NULL}, { "input_timeout", T_BOOL, O(task.input_timeout), READONLY, NULL}, { "rotation_xy", T_DOUBLE, O(task.rotation_xy), READONLY, NULL}, + { "g68_active", T_INT, O(task.g68_active), READONLY, "A tilted work plane (G68.2) is in effect."}, { "ini_filename", T_STRING_INPLACE, O(task.ini_filename), READONLY, NULL}, { "delay_left", T_DOUBLE, O(task.delayLeft), READONLY, NULL}, { "queued_mdi_commands", T_INT, O(task.queuedMDIcommands), READONLY, @@ -1275,6 +1276,18 @@ static PyObject *Stat_tool_offset(pyStatChannel *s, void *) { return pose(s->status.task.toolOffset); } +static PyObject *Stat_g68_offset(pyStatChannel *s, void *) { + return pose(s->status.task.g68_offset); +} + +static PyObject *Stat_g68_rotation(pyStatChannel *s, void *) { + PyObject *res = PyTuple_New(9); + for (int i = 0; i < 9; i++) { + PyTuple_SET_ITEM(res, i, PyFloat_FromDouble(s->status.task.g68_rotation[i])); + } + return res; +} + static PyObject *Stat_position(pyStatChannel *s, void *) { return pose(s->status.motion.traj.position); } @@ -1550,6 +1563,10 @@ static PyGetSetDef Stat_getsetlist[] = { {(char*)"g5x_offset", (getter)Stat_g5x_offset, NULL, NULL, NULL}, {(char*)"g5x_index", (getter)Stat_g5x_index, NULL, NULL, NULL}, {(char*)"g92_offset", (getter)Stat_g92_offset, NULL, NULL, NULL}, + {(char*)"g68_offset", (getter)Stat_g68_offset, NULL, + (char*)"Origin of the tilted work plane (G68.2), in the coordinate system it was defined in.", NULL}, + {(char*)"g68_rotation", (getter)Stat_g68_rotation, NULL, + (char*)"Rotation matrix of the tilted work plane (G68.2), nine values row by row.", NULL}, {(char*)"position", (getter)Stat_position, NULL, NULL, NULL}, {(char*)"dtg", (getter)Stat_dtg, NULL, NULL, NULL}, {(char*)"joint_position", (getter)Stat_joint_position, NULL, NULL, NULL}, diff --git a/src/emc/usr_intf/axis/scripts/axis.py b/src/emc/usr_intf/axis/scripts/axis.py index 2a1c90cb57b..b2c5e46744d 100755 --- a/src/emc/usr_intf/axis/scripts/axis.py +++ b/src/emc/usr_intf/axis/scripts/axis.py @@ -1603,6 +1603,7 @@ def next_line(*args): pass def set_g5x_offset(*args): pass def set_g92_offset(*args): pass def set_xy_rotation(*args): pass + def set_g68_frame(*args): pass def get_external_angular_units(self): return 1.0 def get_external_length_units(self): return 1.0 def set_plane(*args): pass diff --git a/src/emc/usr_intf/halui.cc b/src/emc/usr_intf/halui.cc index 6796e4cf48d..1d46b23a28c 100644 --- a/src/emc/usr_intf/halui.cc +++ b/src/emc/usr_intf/halui.cc @@ -2041,28 +2041,45 @@ static void modify_hal_pins() hal_set_bool(halui_data->joint_has_fault[joint], emcStatus->motion.joint[joint].fault); } + // the relative position: the offset chain taken off in reverse, the + // tool offset, G5x, the XY rotation, G92 and the tilted work plane + double rx = emcStatus->motion.traj.actualPosition.tran.x - emcStatus->task.g5x_offset.tran.x - emcStatus->task.toolOffset.tran.x; + double ry = emcStatus->motion.traj.actualPosition.tran.y - emcStatus->task.g5x_offset.tran.y - emcStatus->task.toolOffset.tran.y; + double rz = emcStatus->motion.traj.actualPosition.tran.z - emcStatus->task.g5x_offset.tran.z - emcStatus->task.toolOffset.tran.z; + { + double t = -emcStatus->task.rotation_xy * TO_RAD; + double x = rx * cos(t) - ry * sin(t); + double y = ry * cos(t) + rx * sin(t); + rx = x - emcStatus->task.g92_offset.tran.x; + ry = y - emcStatus->task.g92_offset.tran.y; + rz -= emcStatus->task.g92_offset.tran.z; + } + if (emcStatus->task.g68_active) { + const double *r = emcStatus->task.g68_rotation; + double x = rx - emcStatus->task.g68_offset.tran.x; + double y = ry - emcStatus->task.g68_offset.tran.y; + double z = rz - emcStatus->task.g68_offset.tran.z; + rx = r[0]*x + r[3]*y + r[6]*z; + ry = r[1]*x + r[4]*y + r[7]*z; + rz = r[2]*x + r[5]*y + r[8]*z; + } + if (axis_mask & 0x0001) { hal_set_real(halui_data->axis_pos_commanded[0], emcStatus->motion.traj.position.tran.x); hal_set_real(halui_data->axis_pos_feedback[0], emcStatus->motion.traj.actualPosition.tran.x); - double x = emcStatus->motion.traj.actualPosition.tran.x - emcStatus->task.g5x_offset.tran.x - emcStatus->task.toolOffset.tran.x; - double y = emcStatus->motion.traj.actualPosition.tran.y - emcStatus->task.g5x_offset.tran.y - emcStatus->task.toolOffset.tran.y; - x = x * cos(-emcStatus->task.rotation_xy * TO_RAD) - y * sin(-emcStatus->task.rotation_xy * TO_RAD); - hal_set_real(halui_data->axis_pos_relative[0], x - emcStatus->task.g92_offset.tran.x); + hal_set_real(halui_data->axis_pos_relative[0], rx); } if (axis_mask & 0x0002) { hal_set_real(halui_data->axis_pos_commanded[1], emcStatus->motion.traj.position.tran.y); hal_set_real(halui_data->axis_pos_feedback[1], emcStatus->motion.traj.actualPosition.tran.y); - double x = emcStatus->motion.traj.actualPosition.tran.x - emcStatus->task.g5x_offset.tran.x - emcStatus->task.toolOffset.tran.x; - double y = emcStatus->motion.traj.actualPosition.tran.y - emcStatus->task.g5x_offset.tran.y - emcStatus->task.toolOffset.tran.y; - y = y * cos(-emcStatus->task.rotation_xy * TO_RAD) + x * sin(-emcStatus->task.rotation_xy * TO_RAD); - hal_set_real(halui_data->axis_pos_relative[1], y - emcStatus->task.g92_offset.tran.y); + hal_set_real(halui_data->axis_pos_relative[1], ry); } if (axis_mask & 0x0004) { hal_set_real(halui_data->axis_pos_commanded[2], emcStatus->motion.traj.position.tran.z); hal_set_real(halui_data->axis_pos_feedback[2], emcStatus->motion.traj.actualPosition.tran.z); - hal_set_real(halui_data->axis_pos_relative[2], emcStatus->motion.traj.actualPosition.tran.z - emcStatus->task.g5x_offset.tran.z - emcStatus->task.g92_offset.tran.z - emcStatus->task.toolOffset.tran.z); + hal_set_real(halui_data->axis_pos_relative[2], rz); } if (axis_mask & 0x0008) { diff --git a/src/hal/components/Submakefile b/src/hal/components/Submakefile index 62c9940cfbb..d8975f3f8d1 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,21 @@ 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/switchkins_setup.o emc/kinematics/kins_util.o +matrixkins-extra-objs := emc/kinematics/kins_util.o emc/kinematics/kins_single.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/matrixkins.comp b/src/hal/components/matrixkins.comp index b12dedc2fcf..f3b1834bbb1 100644 --- a/src/hal/components/matrixkins.comp +++ b/src/hal/components/matrixkins.comp @@ -40,7 +40,7 @@ mechanical issues, including: 3. Parallelism between spindle rotational axis and Z movement. 4. Perpendicularity between spindle rotational axis and X/Y movement. -The matrix coefficients are set by parameters C_xx .. C_zz. +The matrix coefficients are set by the pins C_xx .. C_zz. For 3 axis machine, the equations become: .... @@ -152,7 +152,7 @@ Specify matrixkins in LinuxCNC INI file as: KINEMATICS=matrixkins ---- -In your HAL configuration file, set the parameters C_xx .. C_zz: +In your HAL configuration file, set the pins C_xx .. C_zz: [source,hal] ---- @@ -167,7 +167,7 @@ setp matrixkins.C_zy 0 # Skew Y axis towards Z axis setp matrixkins.C_zz 1 # Z axis scale ---- -The parameters can be modified during runtime using halcmd. +The pins can be modified during runtime using halcmd. To avoid sudden movements, it is better to turn off machine power before changes. If recalibration is performed with already existing calibration being in effect, @@ -176,75 +176,34 @@ the adjustment values should be added to the old values instead of replacing the """; see_also "kins(9)"; pin out bool dummy=1; // halcompile requires at least one pin +option extra_setup; license "GPL"; ;; -static struct haldata { - hal_real_t C_xx; - hal_real_t C_xy; - hal_real_t C_xz; - hal_real_t C_yx; - hal_real_t C_yy; - hal_real_t C_yz; - hal_real_t C_zx; - hal_real_t C_zy; - hal_real_t C_zz; -} *haldata; - -static int matrixkins_setup(void) { - int res=0; - - // inherit comp_id from rtapi_main() - if (comp_id < 0) goto error; - - res = hal_set_unready(comp_id); - if (res) goto error; - - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) goto error; - - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_xx, 1.0, "matrixkins.C_xx"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_xy, 0.0, "matrixkins.C_xy"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_xz, 0.0, "matrixkins.C_xz"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_yx, 0.0, "matrixkins.C_yx"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_yy, 1.0, "matrixkins.C_yy"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_yz, 0.0, "matrixkins.C_yz"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_zx, 0.0, "matrixkins.C_zx"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_zy, 0.0, "matrixkins.C_zy"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_zz, 1.0, "matrixkins.C_zz"); - - if (res) goto error; - - res = hal_ready(comp_id); - if (res) goto error; - - rtapi_print("*** %s setup ok\n",__FILE__); - return 0; -error: - rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res); - return -1; -} - #include -#include - -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); - -KINEMATICS_TYPE kinematicsType() -{ - static bool is_setup=0; - if (!is_setup) matrixkins_setup(); - return KINEMATICS_BOTH; -} - -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +#include + +// the calibration matrix, one pin each; the maths reads it from the block +static const kins_param_desc matrix_params[] = { + { "C_xx", KINS_PARAM_FLOAT, KINS_IN, 0, 1.0 }, + { "C_xy", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_xz", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_yx", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_yy", KINS_PARAM_FLOAT, KINS_IN, 0, 1.0 }, + { "C_yz", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_zx", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_zy", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_zz", KINS_PARAM_FLOAT, KINS_IN, 0, 1.0 }, +}; +enum { C_XX, C_XY, C_XZ, C_YX, C_YY, C_YZ, C_ZX, C_ZY, C_ZZ }; + +static int matrix_forward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; // For forward kinematics (joint to axis position) we @@ -254,20 +213,20 @@ int kinematicsForward(const double *j, // https://ardoris.wordpress.com/2008/07/18/general-formula-for-the-inverse-of-a-3x3-matrix/ // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3_%C3%97_3_matrices - rtapi_real a = hal_get_real(haldata->C_xx); - rtapi_real b = hal_get_real(haldata->C_xy); - rtapi_real c = hal_get_real(haldata->C_xz); - rtapi_real d = hal_get_real(haldata->C_yx); - rtapi_real e = hal_get_real(haldata->C_yy); - rtapi_real f = hal_get_real(haldata->C_yz); - rtapi_real g = hal_get_real(haldata->C_zx); - rtapi_real h = hal_get_real(haldata->C_zy); - rtapi_real i = hal_get_real(haldata->C_zz); - - rtapi_real det = a * (e * i - f * h) - - b * (d * i - f * g) - + c * (d * h - e * g); - rtapi_real invdet = 1.0 / det; + const double a = p->geometry[C_XX]; + const double b = p->geometry[C_XY]; + const double c = p->geometry[C_XZ]; + const double d = p->geometry[C_YX]; + const double e = p->geometry[C_YY]; + const double f = p->geometry[C_YZ]; + const double g = p->geometry[C_ZX]; + const double h = p->geometry[C_ZY]; + const double i = p->geometry[C_ZZ]; + + const double det = a * (e * i - f * h) + - b * (d * i - f * g) + + c * (d * h - e * g); + const double invdet = 1.0 / det; // Apply inverse matrix transform to the 3 cartesian coordinates pos->tran.x = invdet * ( (e * i - f * h) * j[0] @@ -293,22 +252,24 @@ int kinematicsForward(const double *j, return 0; } -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int matrix_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - rtapi_real a = hal_get_real(haldata->C_xx); - rtapi_real b = hal_get_real(haldata->C_xy); - rtapi_real c = hal_get_real(haldata->C_xz); - rtapi_real d = hal_get_real(haldata->C_yx); - rtapi_real e = hal_get_real(haldata->C_yy); - rtapi_real f = hal_get_real(haldata->C_yz); - rtapi_real g = hal_get_real(haldata->C_zx); - rtapi_real h = hal_get_real(haldata->C_zy); - rtapi_real i = hal_get_real(haldata->C_zz); + const double a = p->geometry[C_XX]; + const double b = p->geometry[C_XY]; + const double c = p->geometry[C_XZ]; + const double d = p->geometry[C_YX]; + const double e = p->geometry[C_YY]; + const double f = p->geometry[C_YZ]; + const double g = p->geometry[C_ZX]; + const double h = p->geometry[C_ZY]; + const double i = p->geometry[C_ZZ]; // Apply matrix transform to the 3 cartesian coordinates j[0] = pos->tran.x * a + pos->tran.y * b + pos->tran.z * c; @@ -325,3 +286,56 @@ int kinematicsInverse(const EmcPose * pos, return 0; } + +static int matrix_jacobian(const kins_params *p, const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + int r, c; + (void)j; + (void)pos; + (void)iflags; + for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { + for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } + } + // the inverse is the calibration matrix itself, so its derivative is + // that matrix, and the pass-through axes are ones + jac[0][0] = p->geometry[C_XX]; + jac[0][1] = p->geometry[C_XY]; + jac[0][2] = p->geometry[C_XZ]; + jac[1][0] = p->geometry[C_YX]; + jac[1][1] = p->geometry[C_YY]; + jac[1][2] = p->geometry[C_YZ]; + jac[2][0] = p->geometry[C_ZX]; + jac[2][1] = p->geometry[C_ZY]; + jac[2][2] = p->geometry[C_ZZ]; + for (r = 3; r < 9; r++) { jac[r][r] = 1; } + return 0; +} + +static const kins_ops matrix_ops = { + .forward = matrix_forward, + .inverse = matrix_inverse, + .jacobian = matrix_jacobian, +}; + +// the entry points come from kins_single.c, linked in +const kins_module_info kins_module = { + .name = "matrixkins", + .halprefix = "matrixkins", + .params = matrix_params, + .nparams = sizeof(matrix_params)/sizeof(matrix_params[0]), + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &matrix_ops }, +}; + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what kinsSingleInit() expects +EXTRA_SETUP() { + (void)__comp_inst; (void)prefix; (void)extra_arg; + return kinsSingleInit(comp_id, "XYZABCUVW", KINEMATICS_BOTH); +} diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index d1271de882e..bde6136efff 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 @@ -27,28 +26,18 @@ chapter (docs/src/motion/switchkins.txt) """; // The fpin pin is not accessible in kinematics functions. -// 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 +#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; - - //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,122 +48,24 @@ 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(kinematicsTypeFlags); -EXPORT_SYMBOL(kinematicsSwitch); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); - -static rtapi_u32 switchkins_type; - -int kinematicsSwitchable() {return 1;} - -int kinematicsTypeFlags(int ktype) -{ - switch (ktype) { - case 0: return KINSTYPE_IDENTITY; - case 1: return 0; // the turn mapping, no flag to declare - default: 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); - 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) 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) +// the turn kinematics: no geometry, written as pure functions of the +// parameter block (see kinematics.h) +static int turn_forward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)p; + (void)s; (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; @@ -182,46 +73,83 @@ 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() +} // turn_forward() -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int turn_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)p; + (void)s; (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; +} // turn_inverse() +static int turn_jacobian(const kins_params *p, const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + int R, C; + (void)p; + (void)j; + (void)pos; + (void)iflags; + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } + } + // the derivative of turn_inverse(): which joint follows which pose + // coordinate, and in which sense + jac[2][0] = 1; + jac[1][1] = -1; + jac[0][2] = 1; + jac[3][3] = 1; return 0; -} // kinematicsInverse() +} // turn_jacobian() + +static const kins_ops turn_ops = { + .forward = turn_forward, + .inverse = turn_inverse, + .jacobian = turn_jacobian, + .primary = 1, +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "millturn"; + kp->halprefix = "millturn"; + kp->required_coordinates = "xyza"; + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &turn_ops); + return 0; +} // switchkinsSetup() + +// 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; + + if (switchkinsRunSetup(&kp, NULL)) { 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..e5ca4034b9c --- /dev/null +++ b/src/hal/components/switchkinscomp.comp @@ -0,0 +1,179 @@ +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. + +The kinematics are written as functions of a parameter block, see +kinematics.h and the Kinematics Conventions chapter: the geometry is +declared once in a table, one HAL pin is made per entry, and the maths +reads the block where it would have read a pin. The same maths can +then be evaluated outside realtime. + +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. +// switchkins_setup.c runs the switchkinsSetup() below and provides +// kinsDescribe() for a copy of the module loaded outside realtime. +// kins_util.c provides the identity kinematics, the parameter block +// helpers and the coordinates letters-to-joints mapping. All are +// installed with the headers, so halcompile finds them with no path of +// your own. + +#include +#include +#include +//===================================================================== + +// module parameter naming the joint order for the identity type +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); + +//--------------------------------------------------------------------- +// The geometry: one HAL pin per entry, named ., read +// into the block before every call. Add whatever the real kinematics +// need; an entry flagged as the tool arrives in p->tool.tran.z as well. + +static const kins_param_desc my_params[] = { + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, +}; +enum { P_X_OFFSET }; + +//--------------------------------------------------------------------- +// Example switchkins-type: a forward and an inverse over the block. +// Replace the arithmetic with the real kinematics. The frames and the +// Jacobian are optional, see kinematics.h. + +static int myForward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)s; + (void)fflags; + (void)iflags; + + pos->tran.x = j[0] + p->geometry[P_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; +} // myForward() + +static int myInverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)s; + (void)iflags; + (void)fflags; + + j[0] = pos->tran.x - p->geometry[P_X_OFFSET]; + j[1] = pos->tran.y; + j[2] = pos->tran.z; + + return 0; +} // myInverse() + +static const kins_ops my_ops = { + .forward = myForward, + .inverse = myInverse, +}; + +//--------------------------------------------------------------------- +// The module's configuration and its switchkins-types. Type 0 is the +// startup default. Types run from 0 to SWITCHKINS_MAX_TYPES-1 with no +// gaps. + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + // the pointer arguments are the older way of providing types 0 to 2 + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + + 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->max_joints = strlen(kp->required_coordinates); + kp->params = my_params; + kp->nparams = sizeof(my_params)/sizeof(my_params[0]); + + if (switchkinsRegisterOps(0, &KINS_IDENTITY_OPS)) { return -1; } + if (switchkinsRegisterOps(1, &my_ops)) { return -1; } + return 0; +} // switchkinsSetup() + +//--------------------------------------------------------------------- +// 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 setup is run and the implementation +// started from here. + +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + if (switchkinsRunSetup(&kp, NULL)) { return -1; } + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() diff --git a/src/hal/components/userkins.comp b/src/hal/components/userkins.comp index a2a25d88c29..f382b0f0544 100644 --- a/src/hal/components/userkins.comp +++ b/src/hal/components/userkins.comp @@ -16,9 +16,8 @@ where '2.8' is the branch name (use 'master' for the master branch). For a RIP (run-in-place) build, the file is located in the git tree as: `src/hal/components/userkins.comp`. -Edit the functions kinematicsForward() and kinematicsInverse() as required. - -If required, add HAL pins following examples in the template code. +Edit the functions userkins_forward() and userkins_inverse() as required, +and list the geometry the maths needs in the *userkins_params* table. Build and install the component using halcompile: @@ -50,37 +49,43 @@ change all instances of `userkins` to `mykins`. === NOTES +* The kinematics are written as functions of a parameter block, see + kinematics.h: the geometry is declared once in the *userkins_params* + table, one HAL pin is made per entry, and the maths reads + *p->geometry[]* where it would have read a pin. The classic entry + points (kinematicsForward() and the rest) are supplied by kins_single.c, + included below, so nothing here touches HAL and the same maths can be + evaluated outside realtime. * The *fpin* pin is included to satisfy the requirements of the halcompile utility but it is not accessible to kinematics functions. -* HAL pins and parameters needed in kinematics functions (kinematicsForward(), - kinematicsInverse()) must be setup in a function (*userkins_setup()*) invoked - by the initial motion module call to kinematicsType(). """; // The fpin pin is not accessible in kinematics functions. -// 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 "Dewey Garrett"; ;; #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; -} *haldata; -// hal pin/param types: -// hal_bool_t boolean bit -// hal_uint_t unsigned integer -// hal_sint_t signed integer -// hal_real_t floating point (double precision) +#include + +// the shared code for a module with one kinematics type, compiled in so +// that halcompile builds this file on its own +#include +#include + +// The geometry, one HAL pin per entry, named userkins.. An entry +// is an input (read into p->geometry[] before every call), an output +// (written from s->out[] after it), or an input that can be poked +// (KINS_IO). The example pair below echoes 'in' to 'out'. +static const kins_param_desc userkins_params[] = { + { "in", KINS_PARAM_U32, KINS_IN, 0, 0 }, + { "out", KINS_PARAM_U32, KINS_OUT, 0, 0 }, +}; +enum { P_IN, P_OUT }; FUNCTION(fdemo) { // This function can be added to a thread (addf) for @@ -91,62 +96,16 @@ FUNCTION(fdemo) { fpin_set(fpin + 1); } -static int userkins_setup(void) { -#define HAL_PREFIX "userkins" - 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(struct 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); - - 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 -} - -KINS_NOT_SWITCHABLE -// see millturn.comp for example of switchable kinematics - -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); - -KINEMATICS_TYPE kinematicsType() -{ -static bool is_setup=0; - if (!is_setup) userkins_setup(); - return KINEMATICS_IDENTITY; // 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 userkins_forward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)p; + (void)s; (void)fflags; (void)iflags; - static bool gave_msg; // [KINS]JOINTS=3 pos->tran.x = j[0]; // X coordinate pos->tran.y = j[1]; // Y coordinate @@ -159,23 +118,17 @@ 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() +} // userkins_forward() -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int userkins_inverse(const kins_params *p, kins_scratch *s, + 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 (3 required for this template). @@ -188,9 +141,60 @@ int kinematicsInverse(const EmcPose * pos, j[1] = pos->tran.y; // joint 1 j[2] = pos->tran.z; // joint 2 - //example hal pin update (homing reqd before kinematicsInverse) - hal_set_ui32(haldata->out, hal_get_ui32(haldata->in)); //dereference - //read from param example: hal_set_ui32(haldata->out, hal_get_real(haldata->param_rw)); + // example output: echo the 'in' pin to the 'out' pin + s->out[P_OUT] = p->geometry[P_IN]; + + return 0; +} // userkins_inverse() +static int userkins_jacobian(const kins_params *p, const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + int r, c; + (void)p; + (void)j; + (void)pos; + (void)iflags; + // How each joint responds to each pose coordinate, the derivative of + // userkins_inverse(): for this template joint 0 follows x, joint 1 + // follows y and joint 2 follows z, each one for one. See kinematics.h. + // Leave .jacobian out of the ops below to have it differenced instead. + for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { + for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } + } + jac[0][0] = 1; + jac[1][1] = 1; + jac[2][2] = 1; return 0; -} // kinematicsInverse() +} // userkins_jacobian() + +static const kins_ops userkins_ops = { + .forward = userkins_forward, + .inverse = userkins_inverse, + .jacobian = userkins_jacobian, + // .work, .tool and .native report the frames, see kinematics.h +}; + +const kins_module_info kins_module = { + .name = "userkins", + .halprefix = "userkins", + .params = userkins_params, + .nparams = sizeof(userkins_params)/sizeof(userkins_params[0]), + .required_coordinates = "XYZ", + .max_joints = 3, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &userkins_ops }, +}; + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what kinsSingleInit() expects. KINEMATICS_IDENTITY is what +// kinematicsType() reports; use KINEMATICS_BOTH for a machine whose +// joints are not the axes, or to let a gui display joint values in the +// preview before homing. +EXTRA_SETUP() { + (void)__comp_inst; (void)prefix; (void)extra_arg; + return kinsSingleInit(comp_id, "XYZ", KINEMATICS_IDENTITY); +} diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index b059f32c103..a2d41593b4b 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,133 +31,46 @@ 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 { - - // Declare hal pin pointers used for xyzab_tdr kinematics: - 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; - - //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(kinematicsTypeFlags); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); - -static rtapi_u32 switchkins_type; - -int kinematicsSwitchable() {return 1;} - -int kinematicsTypeFlags(int ktype) -{ - switch (ktype) { - case 0: return KINSTYPE_IDENTITY; - case 1: return KINSTYPE_PRIMARY; - default: 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); - 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) +#include + +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); + +// the geometry, one pin each; the maths reads it from the block (see +// kinematics.h), and the tool length from p->tool.tran.z +static const kins_param_desc tdr_params[] = { + { "tool-offset-z", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "x-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, +}; +enum { P_TOOL, P_XO, P_ZO, P_XR, P_YR, P_ZR }; + +static int tdr_forward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (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 = p->geometry[P_XR]; + double y_rot_point = p->geometry[P_YR]; + double z_rot_point = p->geometry[P_ZR]; - double dz = hal_get_real(haldata->z_offset); - double dt = hal_get_real(haldata->tool_offset_z); + double dz = p->geometry[P_ZO]; + double dt = p->tool.tran.z; // substitutions as used in mathematical documentation // including degree -> radians angle conversion @@ -168,39 +80,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; @@ -208,22 +103,24 @@ int kinematicsForward(const double *j, pos->w = 0; return 0; -} // kinematicsForward() +} // tdr_forward() -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int tdr_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (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 = p->geometry[P_XR]; + double y_rot_point = p->geometry[P_YR]; + double z_rot_point = p->geometry[P_ZR]; - 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 = p->geometry[P_XO]; + double dz = p->geometry[P_ZO]; + double dt = p->tool.tran.z; // substitutions as used in mathematical documentation // including degree -> radians angle conversion @@ -233,36 +130,110 @@ 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; +} // tdr_inverse() + +static int tdr_jacobian(const kins_params *p, const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)j; + (void)iflags; + double x_rot_point = p->geometry[P_XR]; + double y_rot_point = p->geometry[P_YR]; + double z_rot_point = p->geometry[P_ZR]; + double dx = p->geometry[P_XO]; + double dz = p->geometry[P_ZO]; + double dt = p->tool.tran.z; + double sa = sin(pos->a*TO_RAD); + double ca = cos(pos->a*TO_RAD); + double sb = sin(pos->b*TO_RAD); + double cb = cos(pos->b*TO_RAD); + 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; + int R, C; + + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } } + // tdr_inverse() differentiated: its coefficients of qx, qy and qz for + // the linear columns, and the same terms with a or b advanced a + // quarter turn for the rotary columns + jac[0][0] = cb; + jac[0][1] = sa*sb; + jac[0][2] = -ca*sb; + jac[0][3] = ( ca*sb*qy + sa*sb*qz) * TO_RAD; + jac[0][4] = (-sb*qx + sa*cb*qy - ca*cb*qz - sb*dx - cb*dz) * TO_RAD; + + jac[1][1] = ca; + jac[1][2] = sa; + jac[1][3] = (-sa*qy + ca*qz) * TO_RAD; + + jac[2][0] = sb; + jac[2][1] = -sa*cb; + jac[2][2] = ca*cb; + jac[2][3] = (-ca*cb*qy - sa*cb*qz) * TO_RAD; + jac[2][4] = ( cb*qx + sa*sb*qy - ca*sb*qz + cb*dx - sb*dz) * TO_RAD; + + jac[3][3] = 1; + jac[4][4] = 1; + return 0; +} // tdr_jacobian() + +static const kins_ops tdr_ops = { + .forward = tdr_forward, + .inverse = tdr_inverse, + .jacobian = tdr_jacobian, + .primary = 1, +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "xyzab_tdr_kins"; + kp->halprefix = "xyzab_tdr_kins"; + kp->required_coordinates = "xyzab"; + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + kp->params = tdr_params; + kp->nparams = sizeof(tdr_params)/sizeof(tdr_params[0]); + + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &tdr_ops); return 0; -} // kinematicsInverse() +} // switchkinsSetup() + +// 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; + + if (switchkinsRunSetup(&kp, NULL)) { 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 321d45584e5..5c0f09565a6 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -4,6 +4,11 @@ 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; @@ -14,170 +19,52 @@ author "David Mueller"; ;; #include -#include - - -static struct haldata { - // these should be parameters really but we want to be able to - // change them for demonstration purposes - hal_real_t y_pivot; - hal_real_t z_pivot; - hal_real_t x_offset; - hal_real_t y_offset; - hal_real_t y_rot_axis; - hal_real_t z_rot_axis; - hal_real_t pre_rot; - hal_real_t nut_angle; - hal_real_t prim_angle; - hal_real_t sec_angle; - - // Parameters used for xyzacb_trsrn kinematics: - - // 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; - - -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; -#define HAL_PREFIX "xyzacb_trsrn_kins" - int res=0; - // inherit comp_id from rtapi_main() - if (comp_id < 0) goto error; - - 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; - 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(kinematicsTypeFlags); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsToolFrame); -EXPORT_SYMBOL(kinematicsWorkFrame); - -static rtapi_u32 switchkins_type; - -int kinematicsSwitchable() {return 1;} - -int kinematicsTypeFlags(int ktype) +#include + +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); + +// The geometry of the universal spindle head, one pin each, shared by the +// TCP and TOOL kinematics; the maths reads it from the block (see +// kinematics.h) and the tool length from p->tool.tran.z. The two angle +// pins are what the TOOL kinematics uses in place of the head joints: +// the remap writes them. +static const kins_param_desc trsrn_params[] = { + { "tool-offset-z", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + { "y-pivot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-pivot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-rot-axis", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-rot-axis", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "pre-rot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "nut-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "primary-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "secondary-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, +}; +enum { P_TOOL, P_PIVOT, P_ZPIVOT, P_XO, P_YO, P_ROT_AXIS, P_ZROT_AXIS, + P_PRE_ROT, P_NUT, P_PRIM, P_SEC }; + +// geometric offsets of the universal spindle head as defined in the ini file +#define GEOMETRY(p) \ + const double Ly = (p)->geometry[P_PIVOT]; \ + const double Lz = (p)->geometry[P_ZPIVOT]; \ + const double Dx = (p)->geometry[P_XO]; \ + const double Dy = (p)->geometry[P_YO]; \ + const double Dray = (p)->geometry[P_ROT_AXIS] - (Dy + Ly); \ + const double Draz = (p)->geometry[P_ZROT_AXIS] - Lz; \ + const double tc = (p)->geometry[P_PRE_ROT]; \ + const double nu = (p)->geometry[P_NUT]; /* degrees */ \ + const double theta_1 = (p)->geometry[P_PRIM]; /* degrees */ \ + const double theta_2 = (p)->geometry[P_SEC]; /* degrees */ \ + const double Dt = (p)->tool.tran.z /* tool-length offset if G43 is used */ + +// 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 kins_params *p, const double *j, EmcPose * pos, int tool_kins) { - switch (ktype) { - case 0: return KINSTYPE_IDENTITY; - case 1: return KINSTYPE_PRIMARY; - default: 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 -} - -KINEMATICS_TYPE kinematicsType() -{ - 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)fflags; - (void)iflags; - - // START of custom variable declaration for Forward kinematics - - // geometric offsets of the universal spindle head as defined in the ini file - double Ly = hal_get_real(haldata->y_pivot); - double Lz = hal_get_real(haldata->z_pivot); - double Dx = hal_get_real(haldata->x_offset); - double Dy = hal_get_real(haldata->y_offset); - double Dray = hal_get_real(haldata->y_rot_axis) - (Dy + Ly); - double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double tc = hal_get_real(haldata->pre_rot); - double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees - - // tool-length offset if G43 is used (offset as defined in the tool editor) - double Dt = hal_get_real(haldata->tool_offset_z); + GEOMETRY(p); // variables used in both, TCP and TOOL kinematics double Sw = sin(j[3]*TO_RAD); @@ -204,22 +91,7 @@ int kinematicsForward(const double *j, double Py = j[1]; double Pz = j[2]; - // 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); @@ -231,40 +103,38 @@ int kinematicsForward(const double *j, s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - pos->tran.x = - (Cp*SvSs - Sp*t)*(Dt + Lz) - - Cp*Dx - + (Cp*CvSs + Sp*r)*Ly - + Dy*Sp - + Dx + pos->tran.x = - (Cp*SvSs - Sp*t)*(Dt + Lz) + - Cp*Dx + + (Cp*CvSs + Sp*r)*Ly + + Dy*Sp + + Dx + Px; - pos->tran.y = - Cp*Cw*Dy - - Cw*Dx*Sp - - Cw*(Dray - Py) - - (Cw*Sp*SvSs + Cp*Cw*t - Sw*s)*(Dt + Lz) - + (CvSs*Cw*Sp - Cp*Cw*r + Sw*t)*Ly - + (Draz - Pz)*Sw - + Dray - + Dy + pos->tran.y = - Cp*Cw*Dy + - Cw*Dx*Sp + - Cw*(Dray - Py) + - (Cw*Sp*SvSs + Cp*Cw*t - Sw*s)*(Dt + Lz) + + (CvSs*Cw*Sp - Cp*Cw*r + Sw*t)*Ly + + (Draz - Pz)*Sw + + Dray + + Dy + Ly; - pos->tran.z = - Cp*Dy*Sw - - Dx*Sp*Sw - - Cw*(Draz - Pz) - - (Sp*SvSs*Sw + Cp*Sw*t + Cw*s)*(Dt + Lz) - + (CvSs*Sp*Sw - Cp*Sw*r - Cw*t)*Ly - - (Dray - Py)*Sw - + Draz - + Dt - + Lz; + pos->tran.z = - Cp*Dy*Sw + - Dx*Sp*Sw + - Cw*(Draz - Pz) + - (Sp*SvSs*Sw + Cp*Sw*t + Cw*s)*(Dt + Lz) + + (CvSs*Sp*Sw - Cp*Sw*r - Cw*t)*Ly + - (Dray - Py)*Sw + + Draz + + Dt + + Lz; pos->a = j[3]; 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); @@ -276,36 +146,32 @@ int kinematicsForward(const double *j, s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - pos->tran.x = ((Cs*Ctc - CvSs*Stc)*Cp - (Ctc*CvSs + Stc*r)*Sp)*(Dx + Px) - - (Cs*Ctc - CvSs*Stc)*Dx - + ((Ctc*CvSs + Stc*r)*Cp - + (Cs*Ctc - CvSs*Stc)*Sp)*(Dy + Ly + Py) - - (Ctc*CvSs + Stc*r)*Dy - - (Ctc*SvSs - Stc*t)*(Lz + Pz) + pos->tran.x = ((Cs*Ctc - CvSs*Stc)*Cp - (Ctc*CvSs + Stc*r)*Sp)*(Dx + Px) + - (Cs*Ctc - CvSs*Stc)*Dx + + ((Ctc*CvSs + Stc*r)*Cp + + (Cs*Ctc - CvSs*Stc)*Sp)*(Dy + Ly + Py) + - (Ctc*CvSs + Stc*r)*Dy + - (Ctc*SvSs - Stc*t)*(Lz + Pz) - Ly*Stc; - pos->tran.y = - ((Ctc*CvSs + Cs*Stc)*Cp - (CvSs*Stc - Ctc*r)*Sp)*(Dx + Px) - + (Ctc*CvSs + Cs*Stc)*Dx - - ((CvSs*Stc - Ctc*r)*Cp - + (Ctc*CvSs + Cs*Stc)*Sp)*(Dy + Ly + Py) - + (CvSs*Stc - Ctc*r)*Dy - - Ctc*Ly + pos->tran.y = - ((Ctc*CvSs + Cs*Stc)*Cp - (CvSs*Stc - Ctc*r)*Sp)*(Dx + Px) + + (Ctc*CvSs + Cs*Stc)*Dx + - ((CvSs*Stc - Ctc*r)*Cp + + (Ctc*CvSs + Cs*Stc)*Sp)*(Dy + Ly + Py) + + (CvSs*Stc - Ctc*r)*Dy + - Ctc*Ly + (Stc*SvSs + Ctc*t)*(Lz + Pz); - pos->tran.z = (Cp*SvSs - Sp*t)*(Dx + Px) - + (Sp*SvSs + Cp*t)*(Dy + Ly + Py) - - Dx*SvSs - + (Lz + Pz)*s - - Dy*t + pos->tran.z = (Cp*SvSs - Sp*t)*(Dx + Px) + + (Sp*SvSs + Cp*t)*(Dy + Ly + Py) + - Dx*SvSs + + (Lz + Pz)*s + - Dy*t - Lz; pos->a = j[3]; pos->b = j[4]; pos->c = j[5]; - - break; - - } // unused coordinates: pos->u = 0; @@ -313,23 +179,179 @@ int kinematicsForward(const double *j, pos->w = 0; return 0; -} // kinematicsForward() +} // trsrnForward() -// These modules do not link kins_util.c, so they cannot reach the shared -// TOOL_FRAME_SPINDLE: a kernel module has to resolve its own symbols. -static void frame_square_with_machine(PmRotationMatrix *rot) +static int tcpKinematicsForward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { - rot->x.x = 1; rot->y.x = 0; rot->z.x = 0; - rot->x.y = 0; rot->y.y = 1; rot->z.y = 0; - rot->x.z = 0; rot->y.z = 0; rot->z.z = 1; -} - -int kinematicsToolFrame(const double *j, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) + (void)s; + (void)fflags; + (void)iflags; + return trsrnForward(p, j, pos, 0); +} // tcpKinematicsForward() + +static int toolKinematicsForward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; - double nu = hal_get_real(haldata->nut_angle); // degrees + (void)iflags; + return trsrnForward(p, j, pos, 1); +} // toolKinematicsForward() + +// The inverses read the rotary angles from the joint argument, where the +// machine is, as they always have. +static int trsrnInverse(const kins_params *p, const EmcPose * pos, double *j, int tool_kins) +{ + GEOMETRY(p); + + // variables used in both, TCP and TOOL kinematics + double Sw = sin(j[3]*TO_RAD); + double Cw = cos(j[3]*TO_RAD); + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + double Stc = sin(tc); + double Ctc = cos(tc); + + // in TCP we use the current positions of the spindle joints + // in TOOL we will use the angle values calculated in remap.py + double Ss = 0; + double Cs = 0; + double Sp = 0; + double Cp = 0; + double CvSs = 0; + double SvSs = 0; + double r = 0; + double s = 0; + double t = 0; + + // onLy used to be consistent with math in documentation + double Qx = pos->tran.x; + double Qy = pos->tran.y; + double Qz = pos->tran.z; + + 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); + Sp = sin(j[5]*TO_RAD); + Cp = cos(j[5]*TO_RAD); + CvSs = Cv*Ss; + SvSs = Sv*Ss; + r = Cs + Sv*Sv*(1-Cs); + s = Cs + Cv*Cv*(1-Cs); + t = Sv*Cv*(1-Cs); + + j[0] = (Cp*SvSs - Sp*t)*(Dt + Lz) + + Cp*Dx + - (Cp*CvSs + Sp*r)*Ly + - Dy*Sp + - Dx + + Qx; + + j[1] = Cp*Dy + + Dx*Sp + - Cw*(Dray + Dy + Ly - Qy) + + (Sp*SvSs + Cp*t)*(Dt + Lz) + - (CvSs*Sp - Cp*r)*Ly + - (Draz + Dt + Lz - Qz)*Sw + + Dray; + + j[2] = (Dt + Lz)*s + + Ly*t + - Cw*(Draz + Dt + Lz - Qz) + + (Dray + Dy + Ly - Qy)*Sw + + Draz; + + j[3] = pos->a; + j[4] = pos->b; + j[5] = pos->c; + + } 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); + Sp = sin(theta_1*TO_RAD); + Cp = cos(theta_1*TO_RAD); + CvSs = Cv*Ss; + SvSs = Sv*Ss; + r = Cs + Sv*Sv*(1-Cs); + s = Cs + Cv*Cv*(1-Cs); + t = Sv*Cv*(1-Cs); + + j[0] = Cp*Dx + - (Cp*CvSs + Sp*r)*Ly + + (Cp*SvSs - Sp*t)*Lz + + ((Cp*Cs - CvSs*Sp)*Ctc + - (Cp*CvSs + Sp*r)*Stc)*Qx + - ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc)*Qy + + (Cp*SvSs - Sp*t)*Qz + - Dy*Sp + - Dx; + + j[1] = Cp*Dy + - (CvSs*Sp - Cp*r)*Ly + + (Sp*SvSs + Cp*t)*Lz + + ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc)*Qx + - ((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc)*Qy + + (Sp*SvSs + Cp*t)*Qz + + Dx*Sp + - Dy + - Ly; + + j[2] = - (Ctc*SvSs - Stc*t)*Qx + + (Stc*SvSs + Ctc*t)*Qy + + Lz*s + + Qz*s + + Ly*t + - Lz; + + j[3] = pos->a; + j[4] = pos->b; + j[5] = pos->c; + } + + return 0; +} // trsrnInverse() + +static int tcpKinematicsInverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)s; + (void)iflags; + (void)fflags; + return trsrnInverse(p, pos, j, 0); +} // tcpKinematicsInverse() + +static int toolKinematicsInverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)s; + (void)iflags; + (void)fflags; + return trsrnInverse(p, pos, j, 1); +} // toolKinematicsInverse() + +// The head answers in the convention already, so the native rotation +// declared with these frames is TOOL_FRAME_SPINDLE. +static int tcpKinematicsToolFrame(const kins_params *p, const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + double nu = p->geometry[P_NUT]; // degrees double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); double Ss = sin(j[4]*TO_RAD); @@ -341,13 +363,6 @@ int kinematicsToolFrame(const double *j, double t = Sv*Cv*(1-Cs); int a, b, k; - // identity kinematics, and tool kinematics where the world axes are the - // tool axes by construction, both leave the tool square with the machine - if (switchkins_type != 1) { - frame_square_with_machine(rot); - return 0; - } - // the primary joint turns the head about z const double Rp[3][3] = {{Cp, -Sp, 0}, {Sp, Cp, 0}, {0, 0, 1}}; @@ -369,23 +384,17 @@ int kinematicsToolFrame(const double *j, rot->x.z = M[2][0]; rot->y.z = M[2][1]; rot->z.z = M[2][2]; return 0; -} // kinematicsToolFrame() +} // tcpKinematicsToolFrame() -int kinematicsWorkFrame(const double *j, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int tcpKinematicsWorkFrame(const kins_params *p, const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) { + (void)p; (void)fflags; double Sw = sin(j[3]*TO_RAD); double Cw = cos(j[3]*TO_RAD); - // in tool kinematics the world axes are the tool axes, so the work is not - // being reported against the machine and there is nothing to turn - if (switchkins_type != 1) { - frame_square_with_machine(rot); - return 0; - } - // the A joint carries the work: its frame in machine coordinates // is a rotation about x by the joint value const double W[3][3] = {{1, 0, 0}, {0, Cw, Sw}, {0, -Sw, Cw}}; @@ -395,163 +404,186 @@ int kinematicsWorkFrame(const double *j, rot->x.z = W[2][0]; rot->y.z = W[2][1]; rot->z.z = W[2][2]; return 0; -} // kinematicsWorkFrame() +} // tcpKinematicsWorkFrame() -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int tcpKinematicsJacobian(const kins_params *p, const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { + (void)j; (void)iflags; - (void)fflags; + GEOMETRY(p); + (void)tc; (void)theta_1; (void)theta_2; - // START of custom variable declaration for Forward kinematics + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + + // The TCP inverse reads the rotary angles from its joint argument, + // where the machine is, and its own pose words for the same angles + // are the same numbers once the move is done. Its derivative is taken + // against the pose, which is what a consumer multiplies by. + double Sw = sin(pos->a*TO_RAD); + double Cw = cos(pos->a*TO_RAD); + double Ss = sin(pos->b*TO_RAD); + double Cs = cos(pos->b*TO_RAD); + double Sp = sin(pos->c*TO_RAD); + double Cp = cos(pos->c*TO_RAD); + double CvSs = Cv*Ss; + double SvSs = Sv*Ss; + double r = Cs + Sv*Sv*(1-Cs); + double t = Sv*Cv*(1-Cs); + // derivatives of the above over the secondary angle (Ss, r, s, t, CvSs, + // SvSs) and the primary angle (Sp, Cp), per degree + double dSs = Cs*TO_RAD; + double dr = -Ss*Cv*Cv*TO_RAD; + double ds = -Ss*Sv*Sv*TO_RAD; + double dt_ = Sv*Cv*Ss*TO_RAD; + double dCvSs = Cv*dSs; + double dSvSs = Sv*dSs; + double dSp = Cp*TO_RAD; + double dCp = -Sp*TO_RAD; + + double Qy = pos->tran.y; + double Qz = pos->tran.z; + // the two lever arms the table turns about + double Ay = Dray + Dy + Ly - Qy; + double Az = Draz + Dt + Lz - Qz; + int R, C; + + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } + } - // geometric offsets of the universal spindle head as defined in the ini file - double Ly = hal_get_real(haldata->y_pivot); - double Lz = hal_get_real(haldata->z_pivot); - double Dx = hal_get_real(haldata->x_offset); - double Dy = hal_get_real(haldata->y_offset); - double Dray = hal_get_real(haldata->y_rot_axis) - (Dy + Ly); - double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double tc = hal_get_real(haldata->pre_rot); - double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees + // j[0]: Qx plus terms in the head angles only + jac[0][0] = 1; + jac[0][4] = (Cp*dSvSs - Sp*dt_)*(Dt + Lz) - (Cp*dCvSs + Sp*dr)*Ly; + jac[0][5] = (dCp*SvSs - dSp*t)*(Dt + Lz) + dCp*Dx + - (dCp*CvSs + dSp*r)*Ly - Dy*dSp; + + // j[1]: -Cw*Ay - Az*Sw plus head terms + jac[1][1] = Cw; + jac[1][2] = Sw; + jac[1][3] = ( Sw*Ay - Az*Cw)*TO_RAD; + jac[1][4] = (Sp*dSvSs + Cp*dt_)*(Dt + Lz) - (dCvSs*Sp - Cp*dr)*Ly; + jac[1][5] = dCp*Dy + Dx*dSp + (dSp*SvSs + dCp*t)*(Dt + Lz) + - (CvSs*dSp - dCp*r)*Ly; + + // j[2]: -Cw*Az + Ay*Sw plus head terms + jac[2][1] = -Sw; + jac[2][2] = Cw; + jac[2][3] = ( Sw*Az + Ay*Cw)*TO_RAD; + jac[2][4] = (Dt + Lz)*ds + Ly*dt_; + + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; + return 0; +} // tcpKinematicsJacobian() - // tool-length offset if G43 is used (offset as defined in the tool editor) - double Dt = hal_get_real(haldata->tool_offset_z); +static int toolKinematicsJacobian(const kins_params *p, const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)j; + (void)pos; + (void)iflags; - // substitutions as used in mathematical documentation - // including degree -> radians angle conversion + // the head angles come from pins, so the inverse is linear in the pose + // and the rows are its coefficients + double tc = p->geometry[P_PRE_ROT]; + double nu = p->geometry[P_NUT]; // degrees + double theta_1 = p->geometry[P_PRIM]; // degrees + double theta_2 = p->geometry[P_SEC]; // degrees - // variables used in both, TCP and TOOL kinematics - double Sw = sin(j[3]*TO_RAD); - double Cw = cos(j[3]*TO_RAD); double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); double Stc = sin(tc); double Ctc = cos(tc); - - // in TCP we use the current positions of the spindle joints - // in TOOL we will use the angle values calculated in remap.py - double Ss = 0; - double Cs = 0; - double Sp = 0; - double Cp = 0; - double CvSs = 0; - double SvSs = 0; - double r = 0; - double s = 0; - double t = 0; - - // onLy used to be consistent with math in documentation - double Qx = pos->tran.x; - double Qy = pos->tran.y; - double Qz = pos->tran.z; - - // 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; + double Ss = sin(theta_2*TO_RAD); + double Cs = cos(theta_2*TO_RAD); + double Sp = sin(theta_1*TO_RAD); + double Cp = cos(theta_1*TO_RAD); + double CvSs = Cv*Ss; + double SvSs = Sv*Ss; + double r = Cs + Sv*Sv*(1-Cs); + double s = Cs + Cv*Cv*(1-Cs); + double t = Sv*Cv*(1-Cs); + int R, C; - case 1: // ========================= 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); - Sp = sin(j[5]*TO_RAD); - Cp = cos(j[5]*TO_RAD); - CvSs = Cv*Ss; - SvSs = Sv*Ss; - r = Cs + Sv*Sv*(1-Cs); - s = Cs + Cv*Cv*(1-Cs); - t = Sv*Cv*(1-Cs); + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } + } - j[0] = (Cp*SvSs - Sp*t)*(Dt + Lz) - + Cp*Dx - - (Cp*CvSs + Sp*r)*Ly - - Dy*Sp - - Dx - + Qx; + jac[0][0] = ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc); + jac[0][1] = -((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc); + jac[0][2] = (Cp*SvSs - Sp*t); - j[1] = Cp*Dy - + Dx*Sp - - Cw*(Dray + Dy + Ly - Qy) - + (Sp*SvSs + Cp*t)*(Dt + Lz) - - (CvSs*Sp - Cp*r)*Ly - - (Draz + Dt + Lz - Qz)*Sw - + Dray; + jac[1][0] = ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc); + jac[1][1] = -((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc); + jac[1][2] = (Sp*SvSs + Cp*t); - j[2] = (Dt + Lz)*s - + Ly*t - - Cw*(Draz + Dt + Lz - Qz) - + (Dray + Dy + Ly - Qy)*Sw - + Draz; + jac[2][0] = -(Ctc*SvSs - Stc*t); + jac[2][1] = (Stc*SvSs + Ctc*t); + jac[2][2] = s; - j[3] = pos->a; - j[4] = pos->b; - j[5] = pos->c; + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; + return 0; +} // toolKinematicsJacobian() + +static const kins_ops tcp_ops = { + .forward = tcpKinematicsForward, + .inverse = tcpKinematicsInverse, + .work = tcpKinematicsWorkFrame, + .tool = tcpKinematicsToolFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = tcpKinematicsJacobian, + .primary = 1, +}; + +// the tool kinematics report in tool axes, so the tool is square with the +// world by construction and nothing turns the work against it +static const kins_ops tool_ops = { + .forward = toolKinematicsForward, + .inverse = toolKinematicsInverse, + .work = kinsIdentityFrame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = toolKinematicsJacobian, +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "xyzacb_trsrn"; + kp->halprefix = "xyzacb_trsrn_kins"; + kp->required_coordinates = "xyzabc"; + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + kp->params = trsrn_params; + kp->nparams = sizeof(trsrn_params)/sizeof(trsrn_params[0]); + + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &tcp_ops); + switchkinsRegisterOps(2, &tool_ops); + return 0; +} // switchkinsSetup() - break; - - case 2: // ========================= 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); - Sp = sin(theta_1*TO_RAD); - Cp = cos(theta_1*TO_RAD); - CvSs = Cv*Ss; - SvSs = Sv*Ss; - r = Cs + Sv*Sv*(1-Cs); - s = Cs + Cv*Cv*(1-Cs); - t = Sv*Cv*(1-Cs); - - j[0] = Cp*Dx - - (Cp*CvSs + Sp*r)*Ly - + (Cp*SvSs - Sp*t)*Lz - + ((Cp*Cs - CvSs*Sp)*Ctc - - (Cp*CvSs + Sp*r)*Stc)*Qx - - ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc)*Qy - + (Cp*SvSs - Sp*t)*Qz - - Dy*Sp - - Dx; - - j[1] = Cp*Dy - - (CvSs*Sp - Cp*r)*Ly - + (Sp*SvSs + Cp*t)*Lz - + ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc)*Qx - - ((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc)*Qy - + (Sp*SvSs + Cp*t)*Qz - + Dx*Sp - - Dy - - Ly; - - j[2] = - (Ctc*SvSs - Stc*t)*Qx - + (Stc*SvSs + Ctc*t)*Qy - + Lz*s - + Qz*s - + Ly*t - - Lz; - - j[3] = pos->a; - j[4] = pos->b; - j[5] = pos->c; - - break; - } +// 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; - return 0; -} // kinematicsInverse() + if (switchkinsRunSetup(&kp, NULL)) { 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 371349d2365..9694b5b6f12 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -4,6 +4,11 @@ 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; @@ -14,172 +19,52 @@ author "David Mueller"; ;; #include -#include - - -static struct haldata { - // these should be parameters really but we want to be able to - // change them for demonstration purposes - hal_real_t x_pivot; - hal_real_t z_pivot; - hal_real_t x_offset; - hal_real_t y_offset; - hal_real_t x_rot_axis; - hal_real_t z_rot_axis; - hal_real_t pre_rot; - hal_real_t nut_angle; - hal_real_t prim_angle; - hal_real_t sec_angle; - - // Parameters used for xyzbca_trsrn kinematics: - - // 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; - - -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; -#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; - - 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; - 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(kinematicsTypeFlags); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsToolFrame); -EXPORT_SYMBOL(kinematicsWorkFrame); - -static rtapi_u32 switchkins_type; - -int kinematicsSwitchable() {return 1;} - -int kinematicsTypeFlags(int ktype) -{ - switch (ktype) { - case 0: return KINSTYPE_IDENTITY; - case 1: return KINSTYPE_PRIMARY; - default: return -1; - } -} - - -int kinematicsSwitch(int new_switchkins_type) +#include + +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); + +// The geometry of the universal spindle head, one pin each, shared by the +// TCP and TOOL kinematics; the maths reads it from the block (see +// kinematics.h) and the tool length from p->tool.tran.z. The two angle +// pins are what the TOOL kinematics uses in place of the head joints: +// the remap writes them. +static const kins_param_desc trsrn_params[] = { + { "tool-offset-z", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + { "x-pivot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-pivot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "x-rot-axis", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-rot-axis", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "pre-rot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "nut-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "primary-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "secondary-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, +}; +enum { P_TOOL, P_PIVOT, P_ZPIVOT, P_XO, P_YO, P_ROT_AXIS, P_ZROT_AXIS, + P_PRE_ROT, P_NUT, P_PRIM, P_SEC }; + +// geometric offsets of the universal spindle head as defined in the ini file +#define GEOMETRY(p) \ + const double Lx = (p)->geometry[P_PIVOT]; \ + const double Lz = (p)->geometry[P_ZPIVOT]; \ + const double Dx = (p)->geometry[P_XO]; \ + const double Dy = (p)->geometry[P_YO]; \ + const double Drax = (p)->geometry[P_ROT_AXIS] - Lx - Dx; \ + const double Draz = (p)->geometry[P_ZROT_AXIS] - Lz; \ + const double tc = (p)->geometry[P_PRE_ROT]; \ + const double nu = (p)->geometry[P_NUT]; /* degrees */ \ + const double theta_1 = (p)->geometry[P_PRIM]; /* degrees */ \ + const double theta_2 = (p)->geometry[P_SEC]; /* degrees */ \ + const double Dt = (p)->tool.tran.z /* tool-length offset if G43 is used */ + +// 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 kins_params *p, const double *j, EmcPose * pos, int tool_kins) { - 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 -} - -KINEMATICS_TYPE kinematicsType() -{ - 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)fflags; - (void)iflags; - - // START of custom variable declaration for Forward kinematics - - // geometric offsets of the universal spindle head as defined in the ini file - double Lx = hal_get_real(haldata->x_pivot); - double Lz = hal_get_real(haldata->z_pivot); - double Dx = hal_get_real(haldata->x_offset); - double Dy = hal_get_real(haldata->y_offset); - double Drax = hal_get_real(haldata->x_rot_axis) - Lx- Dx; - double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double tc = hal_get_real(haldata->pre_rot); - double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees - - // tool-length offset if G43 is used (offset as defined in the tool editor) - double Dt = hal_get_real(haldata->tool_offset_z); + GEOMETRY(p); // variables used in both, TCP and TOOL kinematics double Sw = sin(j[4]*TO_RAD); @@ -206,23 +91,7 @@ int kinematicsForward(const double *j, double Py = j[1]; double Pz = j[2]; - // 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); @@ -233,45 +102,39 @@ int kinematicsForward(const double *j, r = Cs + Sv*Sv*(1-Cs); s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - // onLy used to be consistent with math in documentation - Px = j[0]; - Py = j[1]; - Pz = j[2]; - - pos->tran.x = - Cp*Cw*Dx - + Cw*Dy*Sp - - Cw*(Drax - Px) - - (Cw*Sp*SvSs + Cp*Cw*t + Sw*s)*(Dt + Lz) - + (CvSs*Cw*Sp - Cp*Cw*r - Sw*t)*Lx - - (Draz - Pz)*Sw - + Drax - + Dx + + pos->tran.x = - Cp*Cw*Dx + + Cw*Dy*Sp + - Cw*(Drax - Px) + - (Cw*Sp*SvSs + Cp*Cw*t + Sw*s)*(Dt + Lz) + + (CvSs*Cw*Sp - Cp*Cw*r - Sw*t)*Lx + - (Draz - Pz)*Sw + + Drax + + Dx + Lx; - pos->tran.y = (Cp*SvSs - Sp*t)*(Dt + Lz) - - Cp*Dy - - (Cp*CvSs + Sp*r)*Lx - - Dx*Sp - + Dy + pos->tran.y = (Cp*SvSs - Sp*t)*(Dt + Lz) + - Cp*Dy + - (Cp*CvSs + Sp*r)*Lx + - Dx*Sp + + Dy + Py; - pos->tran.z = Cp*Dx*Sw - - Dy*Sp*Sw - - Cw*(Draz - Pz) - + (Sp*SvSs*Sw + Cp*Sw*t - Cw*s)*(Dt + Lz) - - (CvSs*Sp*Sw - Cp*Sw*r + Cw*t)*Lx - + (Drax - Px)*Sw - + Draz - + Dt - + Lz; + pos->tran.z = Cp*Dx*Sw + - Dy*Sp*Sw + - Cw*(Draz - Pz) + + (Sp*SvSs*Sw + Cp*Sw*t - Cw*s)*(Dt + Lz) + - (CvSs*Sp*Sw - Cp*Sw*r + Cw*t)*Lx + + (Drax - Px)*Sw + + Draz + + Dt + + Lz; pos->a = j[3]; 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); @@ -283,36 +146,30 @@ int kinematicsForward(const double *j, s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - pos->tran.x = - ((CvSs*Stc - Ctc*r)*Cp + (Ctc*CvSs + Cs*Stc)*Sp)*(Dx + Lx + Px) - + (CvSs*Stc - Ctc*r)*Dx - + ((Ctc*CvSs + Cs*Stc)*Cp - (CvSs*Stc - Ctc*r)*Sp)*(Dy + Py) - - (Ctc*CvSs + Cs*Stc)*Dy - - Ctc*Lx + pos->tran.x = - ((CvSs*Stc - Ctc*r)*Cp + (Ctc*CvSs + Cs*Stc)*Sp)*(Dx + Lx + Px) + + (CvSs*Stc - Ctc*r)*Dx + + ((Ctc*CvSs + Cs*Stc)*Cp - (CvSs*Stc - Ctc*r)*Sp)*(Dy + Py) + - (Ctc*CvSs + Cs*Stc)*Dy + - Ctc*Lx + (Stc*SvSs + Ctc*t)*(Lz + Pz); - - pos->tran.y = - ((Ctc*CvSs + Stc*r)*Cp + (Cs*Ctc - CvSs*Stc)*Sp)*(Dx + Lx + Px) - + (Ctc*CvSs + Stc*r)*Dx - + ((Cs*Ctc - CvSs*Stc)*Cp - (Ctc*CvSs + Stc*r)*Sp)*(Dy + Py) - - (Cs*Ctc - CvSs*Stc)*Dy - + (Ctc*SvSs - Stc*t)*(Lz + Pz) + pos->tran.y = - ((Ctc*CvSs + Stc*r)*Cp + (Cs*Ctc - CvSs*Stc)*Sp)*(Dx + Lx + Px) + + (Ctc*CvSs + Stc*r)*Dx + + ((Cs*Ctc - CvSs*Stc)*Cp - (Ctc*CvSs + Stc*r)*Sp)*(Dy + Py) + - (Cs*Ctc - CvSs*Stc)*Dy + + (Ctc*SvSs - Stc*t)*(Lz + Pz) + Lx*Stc; - - pos->tran.z = (Sp*SvSs + Cp*t)*(Dx + Lx + Px) - - (Cp*SvSs - Sp*t)*(Dy + Py) - + Dy*SvSs - + (Lz + Pz)*s - - Dx*t + pos->tran.z = (Sp*SvSs + Cp*t)*(Dx + Lx + Px) + - (Cp*SvSs - Sp*t)*(Dy + Py) + + Dy*SvSs + + (Lz + Pz)*s + - Dx*t - Lz; pos->a = j[3]; pos->b = j[4]; pos->c = j[5]; - - break; - - } // unused coordinates: pos->u = 0; @@ -320,114 +177,37 @@ int kinematicsForward(const double *j, pos->w = 0; return 0; -} // kinematicsForward() +} // trsrnForward() -// These modules do not link kins_util.c, so they cannot reach the shared -// TOOL_FRAME_SPINDLE: a kernel module has to resolve its own symbols. -static void frame_square_with_machine(PmRotationMatrix *rot) -{ - rot->x.x = 1; rot->y.x = 0; rot->z.x = 0; - rot->x.y = 0; rot->y.y = 1; rot->z.y = 0; - rot->x.z = 0; rot->y.z = 0; rot->z.z = 1; -} - -int kinematicsToolFrame(const double *j, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int tcpKinematicsForward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; - double nu = hal_get_real(haldata->nut_angle); // degrees - double Sv = sin(nu*TO_RAD); - double Cv = cos(nu*TO_RAD); - double Ss = sin(j[3]*TO_RAD); - double Cs = cos(j[3]*TO_RAD); - double Sp = sin(j[5]*TO_RAD); - double Cp = cos(j[5]*TO_RAD); - double r = Cs + Sv*Sv*(1-Cs); - double s = Cs + Cv*Cv*(1-Cs); - double t = Sv*Cv*(1-Cs); - int a, b, k; - - // identity kinematics, and tool kinematics where the world axes are the - // tool axes by construction, both leave the tool square with the machine - if (switchkins_type != 1) { - frame_square_with_machine(rot); - return 0; - } - - // the primary joint turns the head about z - const double Rp[3][3] = {{Cp, -Sp, 0}, {Sp, Cp, 0}, {0, 0, 1}}; - - // the nutating secondary joint - const double Rs[3][3] = {{r, -Cv*Ss, t}, - {Cv*Ss, Cs, -Sv*Ss}, - {t, Sv*Ss, s}}; - - double M[3][3]; - for (a = 0; a < 3; a++) { - for (b = 0; b < 3; b++) { - M[a][b] = 0; - for (k = 0; k < 3; k++) { M[a][b] += Rp[a][k] * Rs[k][b]; } - } - } - - rot->x.x = M[0][0]; rot->y.x = M[0][1]; rot->z.x = M[0][2]; - rot->x.y = M[1][0]; rot->y.y = M[1][1]; rot->z.y = M[1][2]; - rot->x.z = M[2][0]; rot->y.z = M[2][1]; rot->z.z = M[2][2]; - - return 0; -} // kinematicsToolFrame() - -int kinematicsWorkFrame(const double *j, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) + (void)iflags; + return trsrnForward(p, j, pos, 0); +} // tcpKinematicsForward() + +static int toolKinematicsForward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; - double Sw = sin(j[4]*TO_RAD); - double Cw = cos(j[4]*TO_RAD); - - // in tool kinematics the world axes are the tool axes, so the work is not - // being reported against the machine and there is nothing to turn - if (switchkins_type != 1) { - frame_square_with_machine(rot); - return 0; - } - - // the B joint carries the work: its frame in machine coordinates - // is a rotation about y by the joint value - const double W[3][3] = {{Cw, 0, -Sw}, {0, 1, 0}, {Sw, 0, Cw}}; - - rot->x.x = W[0][0]; rot->y.x = W[0][1]; rot->z.x = W[0][2]; - rot->x.y = W[1][0]; rot->y.y = W[1][1]; rot->z.y = W[1][2]; - rot->x.z = W[2][0]; rot->y.z = W[2][1]; rot->z.z = W[2][2]; - - return 0; -} // kinematicsWorkFrame() - -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) -{ (void)iflags; - (void)fflags; - - // START of custom variable declaration for Forward kinematics - - // geometric offsets of the universal spindle head as defined in the ini file - double Lx = hal_get_real(haldata->x_pivot); - double Lz = hal_get_real(haldata->z_pivot); - double Dx = hal_get_real(haldata->x_offset); - double Dy = hal_get_real(haldata->y_offset); - double Drax = hal_get_real(haldata->x_rot_axis) - Lx - Dx; - double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double tc = hal_get_real(haldata->pre_rot); - double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees + return trsrnForward(p, j, pos, 1); +} // toolKinematicsForward() - // tool-length offset if G43 is used (offset as defined in the tool editor) - double Dt = hal_get_real(haldata->tool_offset_z); +// The inverses read the rotary angles from the joint argument, where the +// machine is, as they always have. +static int trsrnInverse(const kins_params *p, const EmcPose * pos, double *j, int tool_kins) +{ + GEOMETRY(p); // variables used in both, TCP and TOOL kinematics double Sw = sin(j[4]*TO_RAD); @@ -451,29 +231,10 @@ int kinematicsInverse(const EmcPose * pos, // onLy used to be consistent with math in documentation double Qx = pos->tran.x; - double Qy = pos->tran.y; - double Qz = pos->tran.z; - - // END of custom variable declaration for Forward kinematics - + double Qy = pos->tran.y; + double Qz = pos->tran.z; - // 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); @@ -483,36 +244,34 @@ int kinematicsInverse(const EmcPose * pos, SvSs = Sv*Ss; r = Cs + Sv*Sv*(1-Cs); s = Cs + Cv*Cv*(1-Cs); - t = Sv*Cv*(1-Cs); - - j[0] = Cp*Dx - - Dy*Sp - - Cw*(Drax + Dx + Lx - Qx) - + (Sp*SvSs + Cp*t)*(Dt + Lz) - - (CvSs*Sp - Cp*r)*Lx - + (Draz + Dt + Lz - Qz)*Sw + t = Sv*Cv*(1-Cs); + + j[0] = Cp*Dx + - Dy*Sp + - Cw*(Drax + Dx + Lx - Qx) + + (Sp*SvSs + Cp*t)*(Dt + Lz) + - (CvSs*Sp - Cp*r)*Lx + + (Draz + Dt + Lz - Qz)*Sw + Drax; - j[1] = - (Cp*SvSs - Sp*t)*(Dt + Lz) - + Cp*Dy - + (Cp*CvSs + Sp*r)*Lx - + Dx*Sp - - Dy + j[1] = - (Cp*SvSs - Sp*t)*(Dt + Lz) + + Cp*Dy + + (Cp*CvSs + Sp*r)*Lx + + Dx*Sp + - Dy + Qy; - j[2] = (Dt + Lz)*s - + Lx*t - - Cw*(Draz + Dt + Lz - Qz) - - (Drax + Dx + Lx - Qx)*Sw + j[2] = (Dt + Lz)*s + + Lx*t + - Cw*(Draz + Dt + Lz - Qz) + - (Drax + Dx + Lx - Qx)*Sw + Draz; j[3] = pos->a; 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); @@ -523,40 +282,305 @@ int kinematicsInverse(const EmcPose * pos, r = Cs + Sv*Sv*(1-Cs); s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - - j[0] = Cp*Dx - - (CvSs*Sp - Cp*r)*Lx - + (Sp*SvSs + Cp*t)*Lz - - ((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc)*Qx - - ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc)*Qy - + (Sp*SvSs + Cp*t)*Qz - - Dy*Sp - - Dx + + j[0] = Cp*Dx + - (CvSs*Sp - Cp*r)*Lx + + (Sp*SvSs + Cp*t)*Lz + - ((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc)*Qx + - ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc)*Qy + + (Sp*SvSs + Cp*t)*Qz + - Dy*Sp + - Dx - Lx; - j[1] = Cp*Dy - + (Cp*CvSs + Sp*r)*Lx - - (Cp*SvSs - Sp*t)*Lz - + ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc)*Qx - + ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc)*Qy - - (Cp*SvSs - Sp*t)*Qz - + Dx*Sp + j[1] = Cp*Dy + + (Cp*CvSs + Sp*r)*Lx + - (Cp*SvSs - Sp*t)*Lz + + ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc)*Qx + + ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc)*Qy + - (Cp*SvSs - Sp*t)*Qz + + Dx*Sp - Dy; - - j[2] = (Stc*SvSs + Ctc*t)*Qx - + (Ctc*SvSs - Stc*t)*Qy - + Lz*s - + Qz*s - + Lx*t + j[2] = (Stc*SvSs + Ctc*t)*Qx + + (Ctc*SvSs - Stc*t)*Qy + + Lz*s + + Qz*s + + Lx*t - Lz; j[3] = pos->a; j[4] = pos->b; j[5] = pos->c; + } + + return 0; +} // trsrnInverse() - break; +static int tcpKinematicsInverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)s; + (void)iflags; + (void)fflags; + return trsrnInverse(p, pos, j, 0); +} // tcpKinematicsInverse() + +static int toolKinematicsInverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)s; + (void)iflags; + (void)fflags; + return trsrnInverse(p, pos, j, 1); +} // toolKinematicsInverse() + +// The head answers in the convention already, so the native rotation +// declared with these frames is TOOL_FRAME_SPINDLE. +static int tcpKinematicsToolFrame(const kins_params *p, const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + double nu = p->geometry[P_NUT]; // degrees + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + double Ss = sin(j[3]*TO_RAD); + double Cs = cos(j[3]*TO_RAD); + double Sp = sin(j[5]*TO_RAD); + double Cp = cos(j[5]*TO_RAD); + double r = Cs + Sv*Sv*(1-Cs); + double s = Cs + Cv*Cv*(1-Cs); + double t = Sv*Cv*(1-Cs); + int a, b, k; + + // the primary joint turns the head about z + const double Rp[3][3] = {{Cp, -Sp, 0}, {Sp, Cp, 0}, {0, 0, 1}}; + + // the nutating secondary joint + const double Rs[3][3] = {{r, -Cv*Ss, t}, + {Cv*Ss, Cs, -Sv*Ss}, + {t, Sv*Ss, s}}; + + double M[3][3]; + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + M[a][b] = 0; + for (k = 0; k < 3; k++) { M[a][b] += Rp[a][k] * Rs[k][b]; } + } } + rot->x.x = M[0][0]; rot->y.x = M[0][1]; rot->z.x = M[0][2]; + rot->x.y = M[1][0]; rot->y.y = M[1][1]; rot->z.y = M[1][2]; + rot->x.z = M[2][0]; rot->y.z = M[2][1]; rot->z.z = M[2][2]; + return 0; -} // kinematicsInverse() +} // tcpKinematicsToolFrame() + +static int tcpKinematicsWorkFrame(const kins_params *p, const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)p; + (void)fflags; + double Sw = sin(j[4]*TO_RAD); + double Cw = cos(j[4]*TO_RAD); + + // the B joint carries the work: its frame in machine coordinates + // is a rotation about y by the joint value + const double W[3][3] = {{Cw, 0, -Sw}, {0, 1, 0}, {Sw, 0, Cw}}; + + rot->x.x = W[0][0]; rot->y.x = W[0][1]; rot->z.x = W[0][2]; + rot->x.y = W[1][0]; rot->y.y = W[1][1]; rot->z.y = W[1][2]; + rot->x.z = W[2][0]; rot->y.z = W[2][1]; rot->z.z = W[2][2]; + + return 0; +} // tcpKinematicsWorkFrame() + +static int tcpKinematicsJacobian(const kins_params *p, const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)j; + (void)iflags; + GEOMETRY(p); + (void)tc; (void)theta_1; (void)theta_2; + + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + + // The TCP inverse reads the rotary angles from its joint argument, + // where the machine is, and its own pose words for the same angles + // are the same numbers once the move is done. Its derivative is taken + // against the pose, which is what a consumer multiplies by. + double Sw = sin(pos->b*TO_RAD); + double Cw = cos(pos->b*TO_RAD); + double Ss = sin(pos->a*TO_RAD); + double Cs = cos(pos->a*TO_RAD); + double Sp = sin(pos->c*TO_RAD); + double Cp = cos(pos->c*TO_RAD); + double CvSs = Cv*Ss; + double SvSs = Sv*Ss; + double r = Cs + Sv*Sv*(1-Cs); + double t = Sv*Cv*(1-Cs); + // derivatives of the above over the secondary angle (Ss, r, s, t, CvSs, + // SvSs) and the primary angle (Sp, Cp), per degree + double dSs = Cs*TO_RAD; + double dr = -Ss*Cv*Cv*TO_RAD; + double ds = -Ss*Sv*Sv*TO_RAD; + double dt_ = Sv*Cv*Ss*TO_RAD; + double dCvSs = Cv*dSs; + double dSvSs = Sv*dSs; + double dSp = Cp*TO_RAD; + double dCp = -Sp*TO_RAD; + + double Qx = pos->tran.x; + double Qz = pos->tran.z; + // the two lever arms the table turns about + double Ax = Drax + Dx + Lx - Qx; + double Az = Draz + Dt + Lz - Qz; + int R, C; + + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } + } + + // j[0]: -Cw*Ax + Az*Sw plus head terms + jac[0][0] = Cw; + jac[0][2] = -Sw; + jac[0][3] = (Sp*dSvSs + Cp*dt_)*(Dt + Lz) - (dCvSs*Sp - Cp*dr)*Lx; + jac[0][4] = ( Sw*Ax + Az*Cw)*TO_RAD; + jac[0][5] = dCp*Dx - Dy*dSp + (dSp*SvSs + dCp*t)*(Dt + Lz) + - (CvSs*dSp - dCp*r)*Lx; + + // j[1]: Qy plus head terms + jac[1][1] = 1; + jac[1][3] = -(Cp*dSvSs - Sp*dt_)*(Dt + Lz) + (Cp*dCvSs + Sp*dr)*Lx; + jac[1][5] = -(dCp*SvSs - dSp*t)*(Dt + Lz) + dCp*Dy + + (dCp*CvSs + dSp*r)*Lx + Dx*dSp; + + // j[2]: -Cw*Az - Ax*Sw plus head terms + jac[2][0] = Sw; + jac[2][2] = Cw; + jac[2][3] = (Dt + Lz)*ds + Lx*dt_; + jac[2][4] = ( Sw*Az - Ax*Cw)*TO_RAD; + + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; + return 0; +} // tcpKinematicsJacobian() + +static int toolKinematicsJacobian(const kins_params *p, const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)j; + (void)pos; + (void)iflags; + + // the head angles come from pins, so the inverse is linear in the pose + // and the rows are its coefficients + double tc = p->geometry[P_PRE_ROT]; + double nu = p->geometry[P_NUT]; // degrees + double theta_1 = p->geometry[P_PRIM]; // degrees + double theta_2 = p->geometry[P_SEC]; // degrees + + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + double Stc = sin(tc); + double Ctc = cos(tc); + double Ss = sin(theta_2*TO_RAD); + double Cs = cos(theta_2*TO_RAD); + double Sp = sin(theta_1*TO_RAD); + double Cp = cos(theta_1*TO_RAD); + double CvSs = Cv*Ss; + double SvSs = Sv*Ss; + double r = Cs + Sv*Sv*(1-Cs); + double s = Cs + Cv*Cv*(1-Cs); + double t = Sv*Cv*(1-Cs); + int R, C; + + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } + } + + jac[0][0] = -((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc); + jac[0][1] = -((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc); + jac[0][2] = (Sp*SvSs + Cp*t); + + jac[1][0] = ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc); + jac[1][1] = ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc); + jac[1][2] = -(Cp*SvSs - Sp*t); + + jac[2][0] = (Stc*SvSs + Ctc*t); + jac[2][1] = (Ctc*SvSs - Stc*t); + jac[2][2] = s; + + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; + return 0; +} // toolKinematicsJacobian() + +static const kins_ops tcp_ops = { + .forward = tcpKinematicsForward, + .inverse = tcpKinematicsInverse, + .work = tcpKinematicsWorkFrame, + .tool = tcpKinematicsToolFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = tcpKinematicsJacobian, + .primary = 1, +}; + +// the tool kinematics report in tool axes, so the tool is square with the +// world by construction and nothing turns the work against it +static const kins_ops tool_ops = { + .forward = toolKinematicsForward, + .inverse = toolKinematicsInverse, + .work = kinsIdentityFrame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = toolKinematicsJacobian, +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "xyzbca_trsrn"; + kp->halprefix = "xyzbca_trsrn_kins"; + kp->required_coordinates = "xyzabc"; + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + kp->params = trsrn_params; + kp->nparams = sizeof(trsrn_params)/sizeof(trsrn_params[0]); + + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &tcp_ops); + switchkinsRegisterOps(2, &tool_ops); + return 0; +} // switchkinsSetup() + +// 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; + + if (switchkinsRunSetup(&kp, NULL)) { return -1; } + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() diff --git a/tests/glcanon/test.sh b/tests/glcanon/test.sh index 2a33658aa7b..f7999bf093e 100755 --- a/tests/glcanon/test.sh +++ b/tests/glcanon/test.sh @@ -15,8 +15,9 @@ python3 test_camera_matrices.py >&2 if python3 -c 'import OpenGL' 2>/dev/null; then python3 test_workpiece.py >&2 + python3 test_workplane.py >&2 else - echo "skip: test_workpiece.py needs PyOpenGL (headless build)" >&2 + echo "skip: test_workpiece.py and test_workplane.py need PyOpenGL (headless build)" >&2 fi echo ok diff --git a/tests/glcanon/test_workplane.py b/tests/glcanon/test_workplane.py new file mode 100644 index 00000000000..db338fde130 --- /dev/null +++ b/tests/glcanon/test_workplane.py @@ -0,0 +1,183 @@ +#!/usr/bin/env python3 +"""The tilted work planes the preview draws: what the canon records from +G68.2 and the moves under it, and what the part draws from that. + +Needs the RIP environment (rs274 pulls the compiled gcode extension) but no +display and no GL context: nothing below calls into OpenGL. + + . scripts/rip-environment && runtests tests/glcanon +""" +import math +import unittest + +import rs274.glcanon as glcanon +from rs274 import glcanon_scene + + +def make_canon(): + return glcanon.GLCanon(colors={}, geometry="XYZ") + + +def frame(canon, origin, tilt_about_x=0.0, active=1): + """G68.2 as canon sees it: the origin and the rotation matrix, row by row.""" + c, s = math.cos(math.radians(tilt_about_x)), math.sin(math.radians(tilt_about_x)) + canon.set_g68_frame(origin[0], origin[1], origin[2], + 1, 0, 0, + 0, c, -s, + 0, s, c, active) + + +def move(canon, x, y, z): + canon.straight_feed(x, y, z, 0, 0, 0, 0, 0, 0) + + +class WorkPlaneRecordTest(unittest.TestCase): + def test_where_the_plane_sits(self): + canon = make_canon() + canon.set_g5x_offset(1, 100, 0, 0, 0, 0, 0, 0, 0, 0) + frame(canon, (0, 40, 20), tilt_about_x=30) + plane, = canon.workplanes + # the origin goes through the g5x offset like a move endpoint + self.assertAlmostEqual(plane.origin[0], 100) + self.assertAlmostEqual(plane.origin[1], 40) + self.assertAlmostEqual(plane.origin[2], 20) + # X is untouched by a tilt about X; Z leans back towards -Y + x, y, z = plane.axes + self.assertAlmostEqual(x[0], 1) + self.assertAlmostEqual(z[1], -math.sin(math.radians(30))) + self.assertAlmostEqual(z[2], math.cos(math.radians(30))) + self.assertFalse(plane.has_moves) + + def test_moves_extend_the_plane_in_its_own_coordinates(self): + canon = make_canon() + frame(canon, (0, 40, 20), tilt_about_x=30) + move(canon, -30, -20, 5) + move(canon, 30, 20, -4) + plane, = canon.workplanes + self.assertTrue(plane.has_moves) + lo, hi = plane.extents + for got, want in zip(lo + hi, (-30, -20, -4, 30, 20, 5)): + self.assertAlmostEqual(got, want) + # and a point of the plane comes back where the move went + p = plane.machine_point(30, 20, -4) + for a, b in zip(p, canon.lo[:3]): + self.assertAlmostEqual(a, b) + + def test_cancel_stops_the_recording(self): + canon = make_canon() + frame(canon, (0, 0, 0)) + move(canon, 1, 1, 0) + frame(canon, (0, 0, 0), active=0) + move(canon, 50, 50, 50) + plane, = canon.workplanes + self.assertAlmostEqual(plane.max_extents[0], 1) + + def test_restating_the_plane_is_one_plane(self): + canon = make_canon() + for i in range(3): + frame(canon, (0, 0, 0), tilt_about_x=30) + move(canon, i, 0, 0) + frame(canon, (0, 0, 0), tilt_about_x=45) + self.assertEqual(len(canon.workplanes), 2) + self.assertAlmostEqual(canon.workplanes[0].max_extents[0], 2) + + +class StatStub: + """The plane in effect as status reports it, in machine units (mm).""" + + def __init__(self, active=0, origin=(0, 0, 0), tilt_about_x=0.0): + c, s = math.cos(math.radians(tilt_about_x)), math.sin(math.radians(tilt_about_x)) + self.g68_active = active + self.g68_offset = tuple(origin) + (0,) * 6 + self.g68_rotation = (1, 0, 0, 0, c, -s, 0, s, c) + self.g92_offset = (0,) * 9 + self.g5x_offset = (0,) * 9 + self.rotation_xy = 0.0 + + +class CtxStub: + """What WorkPlanePart is allowed to read, and a record of what it drew.""" + + class Prim: + def __init__(self): + self.calls = [] + + def draw_lines(self, ctx, points, color, alpha=1.0): + self.calls.append((len(points), tuple(color), alpha)) + + def __init__(self, canon, stat=None): + self.canon = canon + self.stat = stat + self.colors = glcanon.GlCanonDraw.colors + self.prim = self.Prim() + + @staticmethod + def to_internal_units(pos): + return [v / 25.4 for v in pos[:3]] + list(pos[3:]) + + +class WorkPlanePartTest(unittest.TestCase): + def test_draws_the_outline_the_grid_and_the_axes(self): + canon = make_canon() + frame(canon, (0, 40, 20), tilt_about_x=30) + move(canon, -30, -20, 5) + move(canon, 30, 20, -4) + canon.calc_extents() + ctx = CtxStub(canon) + glcanon_scene.WorkPlanePart().draw(ctx) + n = glcanon_scene.WorkPlanePart.SUBDIVISIONS + # four edges, the inner lines each way, the origin cross, then one + # line per axis + self.assertEqual([c[0] for c in ctx.prim.calls], [8, 4 * (n - 1), 6, 2, 2, 2]) + self.assertEqual(ctx.prim.calls[0][1], tuple(glcanon_scene.WORKPLANE_COLOR)) + self.assertEqual([c[1] for c in ctx.prim.calls[3:]], + [tuple(glcanon.GlCanonDraw.colors[k]) for k in ('axis_x', 'axis_y', 'axis_z')]) + + def test_draws_nothing_without_planes(self): + for canon in (None, object(), make_canon()): + ctx = CtxStub(canon) + glcanon_scene.WorkPlanePart().draw(ctx) + self.assertEqual(ctx.prim.calls, []) + + def test_the_active_plane_is_the_program_record(self): + canon = make_canon() + frame(canon, (0, 40, 20), tilt_about_x=30) + move(canon, -30, -20, 5) + frame(canon, (0, 0, 0)) + move(canon, 1, 1, 1) + canon.calc_extents() + # the canon counts in inches, status in this machine's mm + stat = StatStub(active=1, origin=(0, 40 * 25.4, 20 * 25.4), tilt_about_x=30) + ctx = CtxStub(canon, stat) + self.assertIs(glcanon_scene.active_workplane(ctx), canon.workplanes[0]) + glcanon_scene.WorkPlanePart().draw(ctx) + # the other plane in the plane colour, then the active one in its own + colors = [c[1] for c in ctx.prim.calls] + self.assertEqual(colors[0], tuple(glcanon_scene.WORKPLANE_COLOR)) + self.assertEqual(colors[6], tuple(glcanon_scene.WORKPLANE_ACTIVE_COLOR)) + self.assertEqual(len(ctx.prim.calls), 12) + + def test_an_mdi_plane_is_drawn_on_its_own(self): + canon = make_canon() + canon.calc_extents() + ctx = CtxStub(canon, StatStub(active=1, origin=(10, 0, 0))) + plane = glcanon_scene.active_workplane(ctx) + self.assertEqual(plane.lineno, -1) + self.assertAlmostEqual(plane.origin[0], 10 / 25.4) + self.assertFalse(plane.has_moves) + glcanon_scene.WorkPlanePart().draw(ctx) + self.assertEqual(len(ctx.prim.calls), 6) + self.assertEqual(ctx.prim.calls[0][1], tuple(glcanon_scene.WORKPLANE_ACTIVE_COLOR)) + + def test_no_plane_in_effect(self): + canon = make_canon() + frame(canon, (0, 0, 0)) + canon.calc_extents() + ctx = CtxStub(canon, StatStub(active=0)) + self.assertIsNone(glcanon_scene.active_workplane(ctx)) + glcanon_scene.WorkPlanePart().draw(ctx) + self.assertEqual(len(ctx.prim.calls), 6) + + +if __name__ == '__main__': + unittest.main() diff --git a/tests/interp/g68-frame/expected b/tests/interp/g68-frame/expected new file mode 100644 index 00000000000..79e5b75b286 --- /dev/null +++ b/tests/interp/g68-frame/expected @@ -0,0 +1,57 @@ + 1 N..... USE_LENGTH_UNITS(CANON_UNITS_MM) + 2 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 3 N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 4 N..... SET_XY_ROTATION(0.0000) + 5 N..... SET_FEED_REFERENCE(CANON_XYZ) + 6 N..... ON_RESET() + 7 N..... COMMENT("the tilted work plane through the stand alone canon") + 8 N..... USE_LENGTH_UNITS(CANON_UNITS_MM) + 9 N..... COMMENT("interpreter: setting coordinate system origin") + 10 N..... SET_G5X_OFFSET(2, 100.0000, 200.0000, 300.0000, 0.0000, 0.0000, 0.0000) + 11 N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 12 N..... SET_XY_ROTATION(0.0000) + 13 N..... COMMENT("one plane, four ways. Three distinct angles, none of them right, so that") + 14 N..... COMMENT("the order the rotations compose in shows in the answer") + 15 N..... COMMENT("P0 turns about the frame as it goes, ZXZ by default") + 16 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.9592, -0.0781, 0.2717, 0.2285, 0.7800, -0.5826, -0.1664, 0.6209, 0.7660], 1) + 17 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 18 N..... COMMENT("the same plane about the fixed axes of the system it sits in, XYZ") + 19 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.9592, -0.0781, 0.2717, 0.2285, 0.7800, -0.5826, -0.1664, 0.6209, 0.7660], 1) + 20 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 21 N..... COMMENT("the same plane by three points: the first two give +X, the third the +Y side") + 22 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.9592, -0.0781, 0.2717, 0.2285, 0.7800, -0.5826, -0.1664, 0.6209, 0.7660], 1) + 23 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 24 N..... COMMENT("the same plane by two vectors, X nudged off square") + 25 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.9592, -0.0781, 0.2717, 0.2285, 0.7800, -0.5826, -0.1664, 0.6209, 0.7660], 1) + 26 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 27 N..... COMMENT("R turns the plane about its own Z") + 28 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.7134, -0.6459, 0.2717, 0.6561, 0.4797, -0.5826, 0.2460, 0.5939, 0.7660], 1) + 29 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 30 N..... COMMENT("an arc in the plane") + 31 N..... SET_FEED_RATE(100.0000) + 32 N..... STRAIGHT_FEED(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 33 N..... ARC_FEED(2.0000, 0.0000, 1.0000, 0.0000, -1, 0.0000, 0.0000, 0.0000, 0.0000) + 34 N..... COMMENT("G53 inside the plane goes to absolute coordinates") + 35 N..... STRAIGHT_TRAVERSE(-27.6365, -20.9503, -14.0466, 0.0000, 0.0000, 0.0000) + 36 N..... COMMENT("and #5021 reports them") + 37 N..... MESSAGE(" abs 100.000000 200.000000 300.000000 prog -27.636497 -20.950346 -14.046603") + 38 N..... COMMENT("a probe result comes back in plane coordinates: nothing to run here") + 39 N..... COMMENT("a tool length change moves the program coordinates along the plane axis that is world Z") + 40 N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 7.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000) + 41 N..... MESSAGE(" prog -29.358386 -25.107354 -19.408914") + 42 N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000) + 43 N..... COMMENT("G68.4 composes onto the plane, in its own axes") + 44 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.6477, -0.5920, 0.4796, 0.4862, -0.1635, -0.8584, 0.5865, 0.7892, 0.1820], 1) + 45 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 46 N..... COMMENT("G69 cancels") + 47 N..... SET_G68_FRAME(0.0000, 0.0000, 0.0000, [1.0000, 0.0000, 0.0000, 0.0000, 1.0000, 0.0000, 0.0000, 0.0000, 1.0000], 0) + 48 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 49 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 50 N..... SET_XY_ROTATION(0.0000) + 51 N..... SET_FEED_MODE(0, 0) + 52 N..... SET_FEED_RATE(0.0000) + 53 N..... STOP_SPINDLE_TURNING(0) + 54 N..... SET_SPINDLE_MODE(0 0.0000) + 55 N..... PROGRAM_END() + 56 N..... ON_RESET() + 57 N..... ON_RESET() diff --git a/tests/interp/g68-frame/g68.ngc b/tests/interp/g68-frame/g68.ngc new file mode 100644 index 00000000000..544f5a16e7a --- /dev/null +++ b/tests/interp/g68-frame/g68.ngc @@ -0,0 +1,46 @@ +% +(the tilted work plane through the stand alone canon) +g21 g90 +g10 l2 p2 x100 y200 z300 r0 +g55 +(one plane, four ways. Three distinct angles, none of them right, so that) +(the order the rotations compose in shows in the answer) +(P0 turns about the frame as it goes, ZXZ by default) +g68.2 x10 y20 z30 i25 j40 k-15 +g0 x1 y2 z3 +(the same plane about the fixed axes of the system it sits in, XYZ) +g68.2 p1 q123 x10 y20 z30 i39.025043525 j9.576578516 k13.400523974 +g0 x1 y2 z3 +(the same plane by three points: the first two give +X, the third the +Y side) +g68.2 p2 q0 x10 y20 z30 +g68.2 p2 q1 x0 y0 z0 +g68.2 p2 q2 x4.796086535 y1.142635331 z-0.831828377 +g68.2 p2 q3 x-0.234429999 y2.339990858 z1.862655459 +g0 x1 y2 z3 +(the same plane by two vectors, X nudged off square) +g68.2 p3 q1 x10 y20 z30 i0.959217579 j0.228526484 k-0.166364909 +g68.2 p3 q2 i0.271653782 j-0.582563416 k0.766044443 +g0 x1 y2 z3 +(R turns the plane about its own Z) +g68.2 x10 y20 z30 i25 j40 k-15 r37.5 +g0 x1 y2 z3 +(an arc in the plane) +g1 f100 x0 y0 z0 +g2 x2 y0 i1 j0 +(G53 inside the plane goes to absolute coordinates) +g53 g0 x100 y200 z300 +(and #5021 reports them) +(debug, abs #5021 #5022 #5023 prog #5420 #5421 #5422) +(a probe result comes back in plane coordinates: nothing to run here) +(a tool length change moves the program coordinates along the plane axis that is world Z) +g43.1 z7 +(debug, prog #5420 #5421 #5422) +g49 +(G68.4 composes onto the plane, in its own axes) +g68.4 p1 q123 i35 j-20 k10 +g0 x1 y2 z3 +(G69 cancels) +g69 +g0 x1 y2 z3 +m2 +% diff --git a/tests/interp/g68-frame/test.sh b/tests/interp/g68-frame/test.sh new file mode 100755 index 00000000000..c11ecd785a9 --- /dev/null +++ b/tests/interp/g68-frame/test.sh @@ -0,0 +1,3 @@ +#!/bin/bash +rs274 -g g68.ngc | sed 's/-0\.0000/0.0000/g' +exit "${PIPESTATUS[0]}" diff --git a/tests/kins-frames/checkresult b/tests/kins-frames/checkresult index 5fefd687bac..48c09ee0d98 100755 --- a/tests/kins-frames/checkresult +++ b/tests/kins-frames/checkresult @@ -1,3 +1,3 @@ #!/bin/sh -[ "$(grep -c 'frames agree' "$1")" = 5 ] \ +[ "$(grep -c 'frames agree' "$1")" = 6 ] \ && ! grep -q "FAIL" "$1" diff --git a/tests/kins-frames/framecheck.c b/tests/kins-frames/framecheck.c index a2be114cc68..bf284731c53 100644 --- a/tests/kins-frames/framecheck.c +++ b/tests/kins-frames/framecheck.c @@ -51,6 +51,9 @@ RTAPI_MP_INT(ktype, "switchkins type where the module models its own machine"); static int spin = -1; RTAPI_MP_INT(spin, "joint that turns the whole head about the machine's z, -1 for none"); +static int quill = -1; +RTAPI_MP_INT(quill, "joint that extends the tool along its own axis, -1 for none"); + static int r1 = -1, r2 = -1, r3 = -1; RTAPI_MP_INT(r1, "joint number of the first rotary to sweep"); RTAPI_MP_INT(r2, "joint number of the second rotary, -1 for none"); @@ -205,6 +208,16 @@ static void check(const double *j, int own_kinematics) && close3(&tool.z, 0, 0, 1), "the spindle stays square", j); } + if (quill >= 0) { + /* the joint runs the tool out along its own axis, away from the + holder, so the tip moves along the tool axis reversed: the one + tie between the reported frame and the forward transform on a + machine that turns nothing but the tool */ + response(j, quill, &d); + expect(close3(&d, -tool.z.x, -tool.z.y, -tool.z.z), + "the quill runs out along the tool axis", j); + } + if (spin >= 0) { memcpy(t, j, sizeof(t)); t[spin] = j[spin] + TURN; @@ -227,7 +240,7 @@ int rtapi_app_main(void) const int angles = sizeof(angle) / sizeof(angle[0]); double j[EMCMOT_MAX_JOINTS]; int a, b, c, t; - int checked = 0; + int checked = 0, own = 0; if (joints < 1 || joints > EMCMOT_MAX_JOINTS) { rtapi_print_msg(RTAPI_MSG_ERR, "framecheck: joints=%d\n", joints); @@ -245,6 +258,7 @@ int rtapi_app_main(void) memset(j, 0, sizeof(j)); if (!carries_tool) { j[0] = 10; j[1] = 20; j[2] = 30; } + own = 0; /* every kinematics the module offers, not just the one it starts in: the frames a switchable module reports are per type, and the @@ -253,6 +267,7 @@ int rtapi_app_main(void) if (kinematicsSwitchable() && kinematicsSwitch(t)) { break; } if (!supplies_frames(j)) { continue; } checked++; + if (t == ktype) { own = 1; } for (a = 0; a < angles; a++) { if (r1 >= 0) { j[r1] = angle[a]; } @@ -271,6 +286,16 @@ int rtapi_app_main(void) if (!kinematicsSwitchable()) { break; } } + /* the identity type a switchable module carries supplies frames of + its own, so a module that reports none for the machine it models + would otherwise pass on its neighbour's answers */ + if (checked && !own) { + rtapi_print_msg(RTAPI_MSG_ERR, + "framecheck: FAIL the module reports no frames for" + " kinematics type %d, the machine it models\n", ktype); + failures++; + } + if (!checked) { rtapi_print_msg(RTAPI_MSG_ERR, "framecheck: the module reports frames for no type\n"); diff --git a/tests/kins-frames/test.sh b/tests/kins-frames/test.sh index 4bb8e108765..cbc350a2eef 100755 --- a/tests/kins-frames/test.sh +++ b/tests/kins-frames/test.sh @@ -49,5 +49,8 @@ setp xyzbca_trsrn_kins.z-pivot 200 setp xyzbca_trsrn_kins.tool-offset-z 50" \ "joints=6 r1=3 r2=4 r3=5 spin=5 ktype=1" +run "5axiskins coordinates=XYZBCW" "setp 5axiskins.pivot-length 250" \ + "joints=6 carries_tool=1 r1=3 r2=4 spin=4 quill=5" + run "pumakins" "setp pumakins.A2 300" \ "joints=6 carries_tool=1 r1=0 r2=3 r3=4 spin=0" diff --git a/tests/kins-jacobian/checkresult b/tests/kins-jacobian/checkresult new file mode 100755 index 00000000000..b49a90b17c6 --- /dev/null +++ b/tests/kins-jacobian/checkresult @@ -0,0 +1,4 @@ +#!/bin/sh +[ "$(grep -c 'jacobian agrees' "$1")" = "$(grep -c '^=== ' "$1")" ] \ + && [ "$(grep -c '^=== ' "$1")" -ge 20 ] \ + && ! grep -q "FAIL" "$1" diff --git a/tests/kins-jacobian/jaccheck.c b/tests/kins-jacobian/jaccheck.c new file mode 100644 index 00000000000..0e5501f6943 --- /dev/null +++ b/tests/kins-jacobian/jaccheck.c @@ -0,0 +1,360 @@ +/* Check a kinematics module's Jacobian where it runs in service. + * + * Loaded after the module under test, so kinematicsForward(), + * kinematicsInverse() and kinematicsJacobian() resolve to it. A + * failed check fails the load, and a failed load fails the test. + * + * Two checks, neither of which reuses the module's own answer. + * + * Against the forward: perturb one joint, difference the forward to + * get how the pose responds, and multiply by the reported Jacobian. + * The result has to be that joint's unit vector, since the Jacobian + * is the derivative of the inverse and the two are inverse maps. The + * forward is a separate piece of code from the inverse, so this + * catches a transposed matrix, a wrong sign, a wrong column and a + * wrong unit, whether the module answered in closed form or by + * differencing. + * + * Against the inverse: difference the inverse here, with a different + * step, and compare entry by entry. This is the check for a machine + * whose forward is not one to one, the gantry with two joints on one + * letter, where the product above is not the identity. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2026 All rights reserved. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +MODULE_LICENSE("GPL"); +MODULE_DESCRIPTION("kinematics Jacobian checker"); + +static int joints = 3; +RTAPI_MP_INT(joints, "joint count the module under test was loaded for"); + +static int types = -1; +RTAPI_MP_INT(types, "how many switchkins types to check, from 0; -1 for all the module has"); + +static int r1 = -1, r2 = -1, r3 = -1; +RTAPI_MP_INT(r1, "joint number of the first joint to sweep"); +RTAPI_MP_INT(r2, "joint number of the second joint to sweep, -1 for none"); +RTAPI_MP_INT(r3, "joint number of the third joint to sweep, -1 for none"); + +#define MAX_ANGLES 8 +#define NO_ANGLE 9999 +static int angles[MAX_ANGLES] = { NO_ANGLE, NO_ANGLE, NO_ANGLE, NO_ANGLE, + NO_ANGLE, NO_ANGLE, NO_ANGLE, NO_ANGLE }; +RTAPI_MP_ARRAY_INT(angles, MAX_ANGLES, "values each swept joint takes; default 0,30,-25,90,180"); + +static int base[EMCMOT_MAX_JOINTS] = { 10, 20, 30 }; +RTAPI_MP_ARRAY_INT(base, EMCMOT_MAX_JOINTS, "joint values before the sweep, from joint 0"); + +static int frompose = 0; +RTAPI_MP_INT(frompose, "1 to read base and the sweep as pose coordinates and take the joints from the inverse"); + +static char *check = "both"; +RTAPI_MP_STRING(check, "fwd, inv or both: which checks to run"); + +static int tolexp = 6; +RTAPI_MP_INT(tolexp, "tolerance for the checks is 10 to the minus this"); + +/* switchkins.h is not an exported header, and a module rejects a type + it does not have, so the loop only needs an upper bound */ +#define MAX_TYPES 9 + +#define FWD_STEP 1e-5 /* joint units, for differencing the forward */ +#define INV_STEP 2e-3 /* pose units, for differencing the inverse; not + the step kins_util.c uses, on purpose */ + +static int comp_id = -1; +static int failures; +static int poses; +static double tolerance = 1e-6; +static int do_fwd = 1, do_inv = 1; + +static void expect(int ok, const char *what, const double *j, int m, int n) +{ + char pose[160]; + int i, k = 0; + + if (ok) { return; } + for (i = 0; i < joints && k < (int)sizeof(pose) - 12; i++) { + k += rtapi_snprintf(pose + k, sizeof(pose) - k, "%s%.4g", + i ? "," : "", j[i]); + } + rtapi_print_msg(RTAPI_MSG_ERR, "jaccheck: FAIL %s [%d][%d] at [%s]\n", + what, m, n, pose); + failures++; +} + +static double pose_coord(const EmcPose *p, int a) +{ + switch (a) { + 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 pose_add(EmcPose *p, int a, double d) +{ + switch (a) { + case 0: p->tran.x += d; break; + case 1: p->tran.y += d; break; + case 2: p->tran.z += d; break; + case 3: p->a += d; break; + case 4: p->b += d; break; + case 5: p->c += d; break; + case 6: p->u += d; break; + case 7: p->v += d; break; + default: p->w += d; break; + } +} + +/* how the pose responds to joint m: column m of the forward's derivative. + A forward that iterates starts from the pose it is handed, so both + calls start from the pose the joints are known to reach. */ +static int fwd_column(const double *j, int m, KINEMATICS_FORWARD_FLAGS ff, + const EmcPose *near, double *col) +{ + double t[EMCMOT_MAX_JOINTS]; + EmcPose lo = *near, hi = *near; + KINEMATICS_INVERSE_FLAGS inf = 0; + int a; + + memcpy(t, j, sizeof(t)); + + t[m] = j[m] - FWD_STEP; + if (kinematicsForward(t, &lo, &ff, &inf)) { return -1; } + t[m] = j[m] + FWD_STEP; + if (kinematicsForward(t, &hi, &ff, &inf)) { return -1; } + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + col[a] = (pose_coord(&hi, a) - pose_coord(&lo, a)) / (2 * FWD_STEP); + } + return 0; +} + +/* near is where the pose is expected to be, for a forward that iterates + from the pose it is handed; zero where nothing better is known */ +static void check_pose(const double *j, const EmcPose *near) +{ + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + double col[EMCMOT_MAX_AXIS]; + double qp[EMCMOT_MAX_JOINTS], qm[EMCMOT_MAX_JOINTS]; + EmcPose world = *near, p; + KINEMATICS_FORWARD_FLAGS ff = 0; + KINEMATICS_INVERSE_FLAGS inf = 0; + int m, n, a; + + m = kinematicsForward(j, &world, &ff, &inf); + if (m) { + rtapi_print_msg(RTAPI_MSG_ERR, + "jaccheck: forward started from [%.4g,%.4g,%.4g,%.4g,%.4g,%.4g]" + " and left [%.4g,%.4g,%.4g,%.4g,%.4g,%.4g]\n", + near->tran.x, near->tran.y, near->tran.z, near->a, near->b, near->c, + world.tran.x, world.tran.y, world.tran.z, world.a, world.b, world.c); + expect(0, "forward kinematics", j, m, -1); + return; + } + poses++; + + if (kinematicsJacobian(j, &world, jac, &inf)) { + /* say what the inverse makes of the same pose, since a module + that differences its inverse declines when that does not come + back to the joints it was given */ + memcpy(qp, j, sizeof(qp)); + if (kinematicsInverse(&world, qp, &inf, &ff)) { + rtapi_print_msg(RTAPI_MSG_ERR, "jaccheck: inverse fails at the pose\n"); + } else { + rtapi_print_msg(RTAPI_MSG_ERR, + "jaccheck: inverse gives [%.4g,%.4g,%.4g,%.4g,%.4g,%.4g] flags %lu\n", + qp[0], qp[1], qp[2], qp[3], qp[4], qp[5], inf); + } + expect(0, "jacobian declined", j, -1, -1); + return; + } + + /* rows the module has no joint for stay zero */ + for (m = joints; m < EMCMOT_MAX_JOINTS; m++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + expect(jac[m][a] == 0, "row past the joint count", j, m, a); + } + } + + if (do_fwd) { + for (m = 0; m < joints; m++) { + if (fwd_column(j, m, ff, &world, col)) { + expect(0, "forward kinematics near the pose", j, m, -1); + return; + } + for (n = 0; n < joints; n++) { + double s = 0; + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { s += jac[n][a] * col[a]; } + expect(fabs(s - (m == n ? 1.0 : 0.0)) < tolerance, + "jacobian times forward column", j, n, m); + } + } + } + + if (do_inv) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + p = world; + memcpy(qp, j, sizeof(qp)); + memcpy(qm, j, sizeof(qm)); + pose_add(&p, a, INV_STEP); + if (kinematicsInverse(&p, qp, &inf, &ff)) { + expect(0, "inverse kinematics near the pose", j, -1, a); + return; + } + pose_add(&p, a, -2 * INV_STEP); + if (kinematicsInverse(&p, qm, &inf, &ff)) { + expect(0, "inverse kinematics near the pose", j, -1, a); + return; + } + for (n = 0; n < joints; n++) { + double d = (qp[n] - qm[n]) / (2 * INV_STEP); + expect(fabs(d - jac[n][a]) < tolerance * (1 + fabs(d)), + "jacobian against the inverse", j, n, a); + } + } + } +} + +int rtapi_app_main(void) +{ + double j[EMCMOT_MAX_JOINTS]; + int angles_n; + int a, b, c, t, i; + int checked = 0; + + if (joints < 1 || joints > EMCMOT_MAX_JOINTS) { + rtapi_print_msg(RTAPI_MSG_ERR, "jaccheck: joints=%d\n", joints); + return -1; + } + /* the list given ends at the first untouched entry; none given means + the quarter and half turns where a sine changes sign or a cosine + vanishes, and the values in between */ + if (angles[0] == NO_ANGLE) { + static const int usual[] = { 0, 30, -25, 90, 180 }; + for (i = 0; i < (int)(sizeof(usual)/sizeof(usual[0])); i++) { angles[i] = usual[i]; } + } + for (angles_n = 0; angles_n < MAX_ANGLES; angles_n++) { + if (angles[angles_n] == NO_ANGLE) { break; } + } + for (tolerance = 1, i = 0; i < tolexp; i++) { tolerance *= 0.1; } + do_fwd = !strcmp(check, "fwd") || !strcmp(check, "both"); + do_inv = !strcmp(check, "inv") || !strcmp(check, "both"); + if (!do_fwd && !do_inv) { + rtapi_print_msg(RTAPI_MSG_ERR, "jaccheck: check=%s\n", check); + return -1; + } + + comp_id = hal_init("jaccheck"); + if (comp_id < 0) { return comp_id; } + + if (kinematicsType() == 0) { + rtapi_print_msg(RTAPI_MSG_ERR, "jaccheck: the module reports no type\n"); + hal_exit(comp_id); + return -1; + } + + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { j[i] = base[i]; } + + /* A switchable module's first forward after load restarts an + iterating forward from a stored pose that is still zero, which + for a hexapod is the singular pose it cannot leave; motion's first + cycle takes that failure and carries on. Take it here. */ + if (kinematicsSwitchable()) { + double q[EMCMOT_MAX_JOINTS]; + EmcPose seed; + KINEMATICS_FORWARD_FLAGS ff = 0; + KINEMATICS_INVERSE_FLAGS inf = 0; + ZERO_EMC_POSE(seed); + memcpy(q, j, sizeof(q)); + if (r1 >= 0) { q[r1] = angles[0]; } + if (r2 >= 0) { q[r2] = angles[0]; } + if (r3 >= 0) { q[r3] = angles[0]; } + if (frompose) { + for (i = 0; i < EMCMOT_MAX_AXIS; i++) { pose_add(&seed, i, q[i]); } + memset(q, 0, sizeof(q)); + kinematicsInverse(&seed, q, &inf, &ff); + } + kinematicsForward(q, &seed, &ff, &inf); + } + + /* every kinematics the module offers, since the answer is per type. + The module starts in type 0, and is not switched to it: a switch + restarts an iterating forward from a stored pose that is still + zero, which for a hexapod is the singular pose it cannot leave */ + for (t = 0; t < MAX_TYPES && (types < 0 || t < types); t++) { + if (kinematicsSwitchable() && t > 0 && kinematicsSwitch(t)) { break; } + checked++; + + for (a = 0; a < angles_n; a++) { + if (r1 >= 0) { j[r1] = angles[a]; } + for (b = 0; b < angles_n; b++) { + if (r2 >= 0) { j[r2] = angles[b]; } + for (c = 0; c < angles_n; c++) { + if (r3 >= 0) { j[r3] = angles[c]; } + if (frompose) { + /* base and sweep name a pose; the machine that + reaches it comes from the module's inverse */ + double q[EMCMOT_MAX_JOINTS]; + EmcPose want; + KINEMATICS_INVERSE_FLAGS inf = 0; + KINEMATICS_FORWARD_FLAGS ff = 0; + ZERO_EMC_POSE(want); + for (i = 0; i < EMCMOT_MAX_AXIS; i++) { pose_add(&want, i, j[i]); } + memset(q, 0, sizeof(q)); + if (kinematicsInverse(&want, q, &inf, &ff)) { + expect(0, "inverse kinematics at the base pose", j, -1, -1); + } else { + check_pose(q, &want); + } + } else { + EmcPose zero; + ZERO_EMC_POSE(zero); + check_pose(j, &zero); + } + if (r3 < 0) { break; } + } + if (r2 < 0) { break; } + } + if (r1 < 0) { break; } + } + + if (!kinematicsSwitchable()) { break; } + } + + if (failures) { + rtapi_print_msg(RTAPI_MSG_ERR, + "jaccheck: %d check(s) failed over %d pose(s)\n", + failures, poses); + hal_exit(comp_id); + return -1; + } + + rtapi_print("jaccheck: jacobian agrees for %d kinematics type(s), %d pose(s)\n", + checked, poses); + hal_ready(comp_id); + return 0; +} + +void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/tests/kins-jacobian/skip b/tests/kins-jacobian/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/kins-jacobian/skip @@ -0,0 +1,4 @@ +#!/bin/sh +# Builds a realtime component with halcompile, which needs the build +# tools present. Skip when testing installed packages. +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/kins-jacobian/test.sh b/tests/kins-jacobian/test.sh new file mode 100755 index 00000000000..bf98c0eaeae --- /dev/null +++ b/tests/kins-jacobian/test.sh @@ -0,0 +1,173 @@ +#!/bin/bash +set -e + +${SUDO} halcompile --install jaccheck.c >/dev/null + +# One hal file per module: they all define the same entry points, so +# only one can be loaded at a time. A run that leaves the sweep at its +# default takes each rotary through the quarter and half turns where a +# sine changes sign or a cosine vanishes; the arms and the parallel +# machines name their own, away from the poses they cannot hold. +# ONLY= in the environment runs the entries for that module alone +run() { + local hal + case "$1" in "${ONLY:-}"*) ;; *) return 0 ;; esac + hal=$(mktemp --suffix=.hal) + { printf 'loadrt %s\n' "$1" + printf '%s\n' "$2" + printf 'loadrt jaccheck %s\n' "$3" + } > "$hal" + echo "=== $1" + halrun -f "$hal" + rm -f "$hal" +} + +# identity, including a gantry: two joints on one letter is the case where +# the forward is not one to one, so it is checked against the inverse +run "trivkins coordinates=XYZ" "" "joints=3" +run "trivkins coordinates=XYZY kinstype=BOTH" "" "joints=4 check=inv" +run "trivkins coordinates=XYZABCUVW" "" "joints=9 r1=3 r2=5" +run "userkins" "" "joints=3" +run "millturn" "" "joints=4" + +# linear maps and one rotation +run "corexykins" "" "joints=9" +run "rotatekins" "" "joints=9 r1=5" +run "matrixkins" \ + "setp matrixkins.C_xy 0.02 +setp matrixkins.C_xz -0.01 +setp matrixkins.C_yx 0.03 +setp matrixkins.C_yz 0.015 +setp matrixkins.C_zx -0.02 +setp matrixkins.C_zy 0.01 +setp matrixkins.C_zz 1.001" \ + "joints=9" + +# tables and heads; offsets set so no term drops out +run "maxkins" \ + "setp maxkins.pivot-length 100" \ + "joints=9 r1=4 r2=5 base=10,20,30,0,0,0,7,0,3" + +run "5axiskins coordinates=XYZBCW" "" "joints=6 r1=3 r2=4 base=10,20,30,0,0,5" +run "5axiskins coordinates=XYZBCW sparm=identityfirst" "" "joints=6 r1=3 r2=4 base=10,20,30,0,0,5" + +run "xyzac-trt-kins coordinates=XYZAC" \ + "setp xyzac-trt-kins.y-offset 3 +setp xyzac-trt-kins.z-offset 11 +setp xyzac-trt-kins.tool-offset 7 +setp xyzac-trt-kins.x-rot-point 1 +setp xyzac-trt-kins.y-rot-point 2 +setp xyzac-trt-kins.z-rot-point 5" \ + "joints=5 r1=3 r2=4" + +run "xyzbc-trt-kins coordinates=XYZBC" \ + "setp xyzbc-trt-kins.x-offset 3 +setp xyzbc-trt-kins.z-offset 11 +setp xyzbc-trt-kins.tool-offset 7 +setp xyzbc-trt-kins.x-rot-point 1 +setp xyzbc-trt-kins.y-rot-point 2 +setp xyzbc-trt-kins.z-rot-point 5" \ + "joints=5 r1=3 r2=4" + +# and both with the rotation sense the chapter asks for +run "xyzac-trt-kins coordinates=XYZAC" \ + "setp xyzac-trt-kins.conventional-directions 1 +setp xyzac-trt-kins.y-offset 3 +setp xyzac-trt-kins.z-offset 11 +setp xyzac-trt-kins.tool-offset 7 +setp xyzac-trt-kins.x-rot-point 1 +setp xyzac-trt-kins.y-rot-point 2 +setp xyzac-trt-kins.z-rot-point 5" \ + "joints=5 r1=3 r2=4" + +run "xyzbc-trt-kins coordinates=XYZBC" \ + "setp xyzbc-trt-kins.conventional-directions 1 +setp xyzbc-trt-kins.x-offset 3 +setp xyzbc-trt-kins.z-offset 11 +setp xyzbc-trt-kins.tool-offset 7 +setp xyzbc-trt-kins.x-rot-point 1 +setp xyzbc-trt-kins.y-rot-point 2 +setp xyzbc-trt-kins.z-rot-point 5" \ + "joints=5 r1=3 r2=4" + +run "xyzab_tdr_kins" \ + "setp xyzab_tdr_kins.x-offset 3 +setp xyzab_tdr_kins.z-offset 11 +setp xyzab_tdr_kins.tool-offset-z 7 +setp xyzab_tdr_kins.x-rot-point 1 +setp xyzab_tdr_kins.y-rot-point 2 +setp xyzab_tdr_kins.z-rot-point 5" \ + "joints=5 r1=3 r2=4" + +# The nutating heads read their rotary angles from the joint argument of +# the inverse rather than from the pose, so differencing the inverse +# about a pose cannot see the coupling; the forward is the check here. +run "xyzacb_trsrn" \ + "setp xyzacb_trsrn_kins.nut-angle 45 +setp xyzacb_trsrn_kins.y-pivot 100 +setp xyzacb_trsrn_kins.z-pivot 200 +setp xyzacb_trsrn_kins.x-offset 5 +setp xyzacb_trsrn_kins.y-offset 7 +setp xyzacb_trsrn_kins.y-rot-axis 300 +setp xyzacb_trsrn_kins.z-rot-axis 400 +setp xyzacb_trsrn_kins.tool-offset-z 50 +setp xyzacb_trsrn_kins.pre-rot 0.3 +setp xyzacb_trsrn_kins.primary-angle 20 +setp xyzacb_trsrn_kins.secondary-angle 35" \ + "joints=6 r1=3 r2=4 r3=5 check=fwd" + +run "xyzbca_trsrn" \ + "setp xyzbca_trsrn_kins.nut-angle 45 +setp xyzbca_trsrn_kins.x-pivot 100 +setp xyzbca_trsrn_kins.z-pivot 200 +setp xyzbca_trsrn_kins.x-offset 5 +setp xyzbca_trsrn_kins.y-offset 7 +setp xyzbca_trsrn_kins.x-rot-axis 300 +setp xyzbca_trsrn_kins.z-rot-axis 400 +setp xyzbca_trsrn_kins.tool-offset-z 50 +setp xyzbca_trsrn_kins.pre-rot 0.3 +setp xyzbca_trsrn_kins.primary-angle 20 +setp xyzbca_trsrn_kins.secondary-angle 35" \ + "joints=6 r1=3 r2=4 r3=5 check=fwd" + +# polar +run "rosekins" "" "joints=3 r1=2 base=10,5,0 angles=30,-25,90,120" + +# arms. Straight or folded they are singular, so the sweep keeps clear +# of 0 and 180 on the elbow. genserkins iterates its inverse to a +# tolerance the differences would not see through, so it is checked +# against its forward only; pumakins and three21kins answer by differencing +# their own inverse and the forward is what proves the answer. +run "scarakins" "" "joints=6 r1=1 r2=3 r3=0 base=0,0,20,0,0,0 angles=30,-25,90,120,-60" +# scorbot's inverse returns the elbow-up arm, shoulder above elbow, so the +# poses have to be ones it can return: j1 above j2, and j2 within a quarter +# turn of level +run "scorbot-kins" "" "joints=5 r1=1 base=0,70,-20,0,0 angles=40,55,70,85" +run "scorbot-kins" "" "joints=5 r1=2 base=0,80,0,0,0 angles=-60,-30,0,20" +run "pumakins" "setp pumakins.D6 50" "joints=6 r1=1 r2=2 r3=4 base=15,0,0,10,0,20 angles=20,45,-35,70" +run "three21kins" "" "joints=6 r1=1 r2=2 r3=4 base=15,0,0,10,0,20 angles=20,45,-35,70" +run "genserkins" "" "joints=9 r1=1 r2=2 r3=4 base=15,0,0,10,0,20 angles=20,45,-35,70 check=fwd" +# and with a joint counted relative to the one before it +run "genserkins" "setp genserkins.unrotate-3 1" "joints=9 r1=1 r2=2 r3=4 base=15,0,0,10,0,20 angles=20,45,-35,70 check=fwd" + +# parallel machines. The struts cannot tilt the platform far, and the +# forward of the hexapod and the pentapod iterates to a tolerance, so the +# product check on those two is held to what that tolerance allows. The +# hexapod module runs its own forward for its GUI pins in every type, with +# whatever joint values that type has, and identity joint values are not +# strut lengths it can converge from; its identity types are the shared +# ones trivkins covers, so only its own type is checked. +run "tripodkins" \ + "setp tripodkins.Bx 2 +setp tripodkins.Cx 1 +setp tripodkins.Cy 2" \ + "joints=3 frompose=1 base=1,1,2" +run "lineardeltakins" "" "joints=9 frompose=1 base=20,30,-200" +run "rotarydeltakins" "" "joints=9 r1=0 r2=1 frompose=1 base=0,0,-12 angles=0,2,-3" +run "genhexkins" \ + "setp genhexkins.screw-lead 0" \ + "joints=6 r1=3 r2=4 r3=5 frompose=1 base=2,3,20 angles=0,5,-7,10 tolexp=3 types=1" +run "genhexkins" \ + "setp genhexkins.screw-lead 5" \ + "joints=6 r1=3 r2=4 r3=5 frompose=1 base=2,3,20 angles=0,5,-7,10 tolexp=3 types=1" +run "pentakins" "" "joints=5 r1=3 r2=4 frompose=1 base=10,20,0 angles=0,5,-7,10 tolexp=3" diff --git a/tests/kins-params/check.py b/tests/kins-params/check.py new file mode 100755 index 00000000000..a3133840f52 --- /dev/null +++ b/tests/kins-params/check.py @@ -0,0 +1,191 @@ +#!/usr/bin/env python3 +# The non-realtime half of the parameter block parity test. +# +# Evaluates the module paritycheck was loaded after through the +# non-realtime loader (libkinslimits, kinematicsUserInit and friends), +# which dlopens the module, asks it to describe itself through +# kinsDescribe(), fills a parameter block from the module's own pins and +# calls the same ops the realtime wrapper calls. The answers have to +# match what paritycheck published, to rounding, or the module is not +# the pure function of its parameters it claims to be. +# +# Usage: check.py MODULE JOINTS COORDS KTYPE FROMPOSE POSE JNT SPARM +# POSE and JNT are comma separated numbers, as given to paritycheck; +# COORDS and SPARM are a dash when the module was loaded without them. + +import ctypes +import os +import sys + +import hal + +EMC2_HOME = os.environ.get("EMC2_HOME") +# global, so the module the loader dlopens resolves its HAL and RTAPI +# symbols against the same library +def lib(name): + if EMC2_HOME: + return ctypes.CDLL(os.path.join(EMC2_HOME, "lib", name), mode=ctypes.RTLD_GLOBAL) + return ctypes.CDLL(name, mode=ctypes.RTLD_GLOBAL) + +class EmcPose(ctypes.Structure): + _fields_ = [(n, ctypes.c_double) for n in "xyzabcuvw"] + +MAX_JOINTS = 9 +AXES = 9 +Joints = ctypes.c_double * MAX_JOINTS +Jac = (ctypes.c_double * AXES) * MAX_JOINTS + +module, joints, coords, ktype, frompose = sys.argv[1], int(sys.argv[2]), sys.argv[3], int(sys.argv[4]), int(sys.argv[5]) +pose_in = [float(v) for v in sys.argv[6].split(",")] +jnt_in = [float(v) for v in sys.argv[7].split(",")] +# a dash stands for an absent value, since halcmd hands quotes through +if coords == "-": + coords = "" +sparm = sys.argv[8].encode() if len(sys.argv) > 8 and sys.argv[8] not in ("", "-") else None +# the orientation joints this machine is known to have, "primary,secondary"; +# "-" where it has no such pair, "no-frame" where it reports no tool frame +orient_in = sys.argv[9] if len(sys.argv) > 9 else "no-frame" +pose_in += [0.0] * (AXES - len(pose_in)) +jnt_in += [0.0] * (MAX_JOINTS - len(jnt_in)) + +halc = lib("liblinuxcnchal.so.0") +kins = lib("libkinslimits.so.0") + +kins.kinematicsUserInitSparm.restype = ctypes.c_void_p +kins.kinematicsUserInitSparm.argtypes = [ctypes.c_char_p, ctypes.c_int, ctypes.c_char_p, + ctypes.c_char_p, ctypes.c_int, ctypes.c_char_p] +for fn in ("kinematicsUserIsRtOnly", "kinematicsUserGetNumTypes"): + getattr(kins, fn).argtypes = [ctypes.c_void_p] +kins.kinematicsUserSetType.argtypes = [ctypes.c_void_p, ctypes.c_int] +kins.kinematicsUserInverse.argtypes = [ctypes.c_void_p, ctypes.POINTER(EmcPose), Joints] +kins.kinematicsUserForward.argtypes = [ctypes.c_void_p, Joints, ctypes.POINTER(EmcPose)] +kins.kinematicsUserJacobian.argtypes = [ctypes.c_void_p, ctypes.POINTER(EmcPose), Jac] +kins.kinematicsUserFree.argtypes = [ctypes.c_void_p] + +comp_id = halc.hal_init(b"kpcheck") +if comp_id < 0: + print("kins-params: FAIL hal_init") + sys.exit(1) +ctx = kins.kinematicsUserInitSparm(module.encode(), joints, coords.encode() if coords else None, sparm, + comp_id, b"kpcheck") +halc.hal_ready(comp_id) +failures = 0 + +def fail(what): + global failures + failures += 1 + print("kins-params: FAIL %s" % what) + +if not ctx or kins.kinematicsUserIsRtOnly(ctx): + fail("%s cannot be evaluated outside realtime" % module) + sys.exit(1) +if ktype and kins.kinematicsUserSetType(ctx, ktype): + fail("%s has no type %d in the block form" % (module, ktype)) + sys.exit(1) + +def pose_of(values): + p = EmcPose() + for n, v in zip("xyzabcuvw", values): + setattr(p, n, v) + return p + +def close(a, b): + return abs(a - b) <= 1e-9 * max(1.0, abs(a), abs(b)) + +def compare(what, ours, theirs): + if not close(ours, theirs): + fail("%s: loader %.12g, realtime %.12g" % (what, ours, theirs)) + +rc_fwd = hal.get_value("paritycheck.rc-fwd") +rc_inv = hal.get_value("paritycheck.rc-inv") +rc_jac = hal.get_value("paritycheck.rc-jac") + +q = Joints(*jnt_in) +qi = Joints(*jnt_in) +J = Jac() +if frompose: + P = pose_of(pose_in) + r_inv = kins.kinematicsUserInverse(ctx, ctypes.byref(P), qi) + F = pose_of(pose_in) + r_fwd = kins.kinematicsUserForward(ctx, qi, ctypes.byref(F)) + r_jac = kins.kinematicsUserJacobian(ctx, ctypes.byref(P), J) +else: + F = pose_of(pose_in) + r_fwd = kins.kinematicsUserForward(ctx, q, ctypes.byref(F)) + r_inv = kins.kinematicsUserInverse(ctx, ctypes.byref(F), qi) + r_jac = kins.kinematicsUserJacobian(ctx, ctypes.byref(F), J) + +# the same success or failure on both sides, then the same numbers +for what, ours, theirs in (("forward", r_fwd, rc_fwd), ("inverse", r_inv, rc_inv), ("jacobian", r_jac, rc_jac)): + if (ours != 0) != (theirs != 0): + fail("%s returned %d in the loader and %d in realtime" % (what, ours, theirs)) + +if r_fwd == 0 and rc_fwd == 0: + for n in "xyzabcuvw": + compare("forward %s" % n, getattr(F, n), hal.get_value("paritycheck.fwd-%s" % n)) +if r_inv == 0 and rc_inv == 0: + for j in range(joints): + compare("inverse joint %d" % j, qi[j], hal.get_value("paritycheck.inv-%d" % j)) +if r_jac == 0 and rc_jac == 0: + for j in range(joints): + for a, n in enumerate("xyzabcuvw"): + compare("jacobian [%d][%s]" % (j, n), J[j][a], hal.get_value("paritycheck.jac-%d-%s" % (j, n))) + +# the caller's tool wins over the module's pin: for a module with a tool +# entry, a length of the caller's must move the inverse, and handing the +# tool back to HAL must return it to what realtime found +kins.kinematicsUserSetTool.argtypes = [ctypes.c_void_p, ctypes.POINTER(EmcPose)] +tool_pin = None +for name in ("tool-offset", "tool-offset-z"): + try: + hal.get_value("%s.%s" % (module, name)) + tool_pin = name + except RuntimeError: + pass +if r_inv == 0 and rc_inv == 0 and tool_pin: + P = pose_of(pose_in) if frompose else F + T = pose_of([0.0] * AXES) + T.z = hal.get_value("%s.%s" % (module, tool_pin)) + 10.0 + kins.kinematicsUserSetTool(ctx, ctypes.byref(T)) + qt = Joints(*jnt_in) + if kins.kinematicsUserInverse(ctx, ctypes.byref(P), qt) != 0: + fail("inverse with the caller's tool") + elif all(close(qt[j], qi[j]) for j in range(joints)): + fail("the caller's tool did not move the inverse") + kins.kinematicsUserSetTool(ctx, None) + qt = Joints(*jnt_in) + if kins.kinematicsUserInverse(ctx, ctypes.byref(P), qt) != 0: + fail("inverse with the tool handed back") + else: + for j in range(joints): + compare("inverse joint %d after the tool is handed back" % j, qt[j], qi[j]) + +# the two rotaries that orient the tool, told apart by which carries the +# other. The sign of the secondary is what G53.1 P names, so a module that +# gets this wrong sends the machine to the other pose without saying so. +kins.kinematicsUserOrientJoints.argtypes = [ctypes.c_void_p, Joints, + ctypes.POINTER(ctypes.c_int), + ctypes.POINTER(ctypes.c_int)] +primary, secondary = ctypes.c_int(-1), ctypes.c_int(-1) +r_orient = kins.kinematicsUserOrientJoints(ctx, Joints(*jnt_in), + ctypes.byref(primary), ctypes.byref(secondary)) +class Rot(ctypes.Structure): + _fields_ = [(n, ctypes.c_double * 3) for n in "xyz"] +kins.kinematicsUserToolFrame.argtypes = [ctypes.c_void_p, Joints, ctypes.POINTER(Rot)] +frame = Rot() +has_frame = kins.kinematicsUserToolFrame(ctx, Joints(*jnt_in), ctypes.byref(frame)) == 0 +if r_orient == 0: + got = "%d,%d" % (primary.value, secondary.value) +else: + got = "-" if has_frame else "no-frame" +if got != orient_in: + fail("orientation joints are %s, expected %s" % (got, orient_in)) +else: + print("kins-params: %s type %d orientation joints %s" % (module, ktype, got)) + +kins.kinematicsUserFree(ctx) +halc.hal_exit(comp_id) + +if failures: + sys.exit(1) +print("kins-params: %s type %d agrees" % (module, ktype)) diff --git a/tests/kins-params/checkresult b/tests/kins-params/checkresult new file mode 100755 index 00000000000..d3eba1a4da0 --- /dev/null +++ b/tests/kins-params/checkresult @@ -0,0 +1,4 @@ +#!/bin/sh +[ "$(grep -c 'agrees' "$1")" = "$(grep -c '^=== ' "$1")" ] \ + && [ "$(grep -c '^=== ' "$1")" -ge 20 ] \ + && ! grep -q "FAIL" "$1" diff --git a/tests/kins-params/paritycheck.c b/tests/kins-params/paritycheck.c new file mode 100644 index 00000000000..9043df69dac --- /dev/null +++ b/tests/kins-params/paritycheck.c @@ -0,0 +1,148 @@ +/* + * paritycheck: the realtime half of the parameter block parity test. + * + * Loaded after a kinematics module, it evaluates the module through the + * classic entry points once, at load, and publishes the answers on HAL + * pins: the forward pose, the inverse joints and the Jacobian. check.py + * then evaluates the same module through the non-realtime loader, which + * goes through kinsDescribe() and the parameter block, and compares. + * + * Two flows. With frompose=0 the input is a joint set: the pose is the + * forward of it, the joints published are the inverse of that pose, and + * the Jacobian is taken there. With frompose=1 the input is a pose, for + * the parallel machines whose forward wants a seed: the joints are its + * inverse, the forward is run from the pose as seed, and the Jacobian is + * taken there. + * + * Module parameters + * joints joint count the module was loaded for + * ktype switchkins type to select first, 0 for none + * frompose 0 or 1, as above + * pose up to nine integers, the pose (frompose=1) or the forward + * seed (frompose=0) + * jnt up to sixteen integers, the joint set (frompose=0) or the + * inverse seed (frompose=1) + */ +#include +#include +#include +#include +#include + +MODULE_LICENSE("GPL"); + +static int joints = 3; +RTAPI_MP_INT(joints, "joint count the module under test was loaded for"); +static int ktype = 0; +RTAPI_MP_INT(ktype, "switchkins type to select first"); +static int frompose = 0; +RTAPI_MP_INT(frompose, "1 to take the pose as the input"); +static int pose[EMCMOT_MAX_AXIS] = { 0 }; +RTAPI_MP_ARRAY_INT(pose, EMCMOT_MAX_AXIS, "pose, x y z a b c u v w"); +static int jnt[EMCMOT_MAX_JOINTS] = { 10, 20, 30, 40, 50, 60, 70, 80, 90 }; +RTAPI_MP_ARRAY_INT(jnt, EMCMOT_MAX_JOINTS, "joint values, from joint 0"); + +static int comp_id = -1; + +static struct { + hal_real_t fwd[EMCMOT_MAX_AXIS]; + hal_real_t inv[EMCMOT_MAX_JOINTS]; + hal_real_t jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + hal_sint_t rc_fwd; + hal_sint_t rc_inv; + hal_sint_t rc_jac; +} *pins; + +static const char letter[EMCMOT_MAX_AXIS] = { 'x','y','z','a','b','c','u','v','w' }; + +static double *coord(EmcPose *p, int a) +{ + switch (a) { + 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; + } +} + +int rtapi_app_main(void) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + KINEMATICS_INVERSE_FLAGS iflags = 0; + double q[EMCMOT_MAX_JOINTS], qi[EMCMOT_MAX_JOINTS]; + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + EmcPose P, F, seed; + int a, j, res = 0; + + if (joints < 1 || joints > EMCMOT_MAX_JOINTS) { return -1; } + + comp_id = hal_init("paritycheck"); + if (comp_id < 0) { return comp_id; } + + pins = hal_malloc(sizeof(*pins)); + if (!pins) { hal_exit(comp_id); return -1; } + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->fwd[a], 0.0, + "paritycheck.fwd-%c", letter[a]); + } + for (j = 0; j < joints; j++) { + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->inv[j], 0.0, + "paritycheck.inv-%d", j); + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->jac[j][a], 0.0, + "paritycheck.jac-%d-%c", j, letter[a]); + } + } + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->rc_fwd, 0, "paritycheck.rc-fwd"); + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->rc_inv, 0, "paritycheck.rc-inv"); + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->rc_jac, 0, "paritycheck.rc-jac"); + if (res) { hal_exit(comp_id); return -1; } + + if (ktype > 0 && kinematicsSwitchable()) { + if (kinematicsSwitch(ktype)) { hal_exit(comp_id); return -1; } + } + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { *coord(&seed, a) = pose[a]; } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { q[j] = jnt[j]; qi[j] = jnt[j]; } + + // a switchable module's first forward after load restarts from the + // pose it saved, which is nothing yet; take that call here so the one + // measured starts from the seed like the loader's does + F = seed; + kinematicsForward(q, &F, &fflags, &iflags); + fflags = 0; iflags = 0; + + if (frompose) { + P = seed; + hal_set_si32(pins->rc_inv, kinematicsInverse(&P, qi, &iflags, &fflags)); + F = seed; + fflags = 0; iflags = 0; + hal_set_si32(pins->rc_fwd, kinematicsForward(qi, &F, &fflags, &iflags)); + iflags = 0; + hal_set_si32(pins->rc_jac, kinematicsJacobian(qi, &P, jac, &iflags)); + } else { + F = seed; + hal_set_si32(pins->rc_fwd, kinematicsForward(q, &F, &fflags, &iflags)); + iflags = 0; fflags = 0; + hal_set_si32(pins->rc_inv, kinematicsInverse(&F, qi, &iflags, &fflags)); + iflags = 0; + hal_set_si32(pins->rc_jac, kinematicsJacobian(qi, &F, jac, &iflags)); + } + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { hal_set_real(pins->fwd[a], *coord(&F, a)); } + for (j = 0; j < joints; j++) { + hal_set_real(pins->inv[j], qi[j]); + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { hal_set_real(pins->jac[j][a], jac[j][a]); } + } + + hal_ready(comp_id); + return 0; +} + +void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/tests/kins-params/skip b/tests/kins-params/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/kins-params/skip @@ -0,0 +1,4 @@ +#!/bin/sh +# Builds a realtime component with halcompile, which needs the build +# tools present. Skip when testing installed packages. +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/kins-params/test.sh b/tests/kins-params/test.sh new file mode 100755 index 00000000000..e429bf5029c --- /dev/null +++ b/tests/kins-params/test.sh @@ -0,0 +1,168 @@ +#!/bin/bash +set -e + +${SUDO} halcompile --install paritycheck.c >/dev/null + +# One hal file per module. paritycheck evaluates the module in realtime +# through the classic entry points and publishes the answers; check.py +# evaluates it through the non-realtime loader, kinsDescribe() and the +# parameter block, and compares. Where they disagree the module keeps +# state its table does not declare. +# ONLY= in the environment runs the entries for that module alone +run() { + local loadrt="$1" setp="$2" parms="$3" ktype="$4" + local module coords sparm joints frompose pose jnt orient rtparms hal tok + case "$loadrt" in "${ONLY:-}"*) ;; *) return 0 ;; esac + module=${loadrt%% *} + coords=""; sparm="" + for tok in $loadrt; do + case "$tok" in + coordinates=*) coords=${tok#coordinates=} ;; + sparm=*) sparm=${tok#sparm=} ;; + esac + done + joints=3; frompose=0; pose="0,0,0,0,0,0,0,0,0"; jnt="10,20,30,40,50,60,70,80,90" + # the two rotaries that orient the tool, "primary,secondary", or "-" + # where the machine has no such pair and "no-frame" where the module + # reports no tool frame at all, which most of the tree still does + orient="no-frame" + for tok in $parms; do + case "$tok" in + joints=*) joints=${tok#joints=} ;; + frompose=*) frompose=${tok#frompose=} ;; + pose=*) pose=${tok#pose=} ;; + jnt=*) jnt=${tok#jnt=} ;; + orient=*) orient=${tok#orient=} ;; + esac + done + # what the module under test is loaded with: everything but our own word + rtparms=$(printf ' %s ' "$parms" | sed 's/ orient=[^ ]*//g') + hal=$(mktemp --suffix=.hal) + { printf 'loadrt %s\n' "$loadrt" + printf '%s\n' "$setp" + printf 'loadrt paritycheck %s ktype=%s\n' "$rtparms" "${ktype:-0}" + # halcmd keeps quotes, so an absent value travels as a dash + printf 'loadusr -w python3 check.py %s %s %s %s %s %s %s %s %s\n' \ + "$module" "$joints" "${coords:--}" "${ktype:-0}" "$frompose" "$pose" "$jnt" "${sparm:--}" "$orient" + } > "$hal" + echo "=== $loadrt type ${ktype:-0}" + halrun -f "$hal" + rm -f "$hal" +} + +# identity, a gantry included +run "trivkins coordinates=XYZ" "" "joints=3 jnt=10,20,30 orient=-" +run "trivkins coordinates=XYZY kinstype=BOTH" "" "joints=4 jnt=10,20,30,20 orient=-" +run "trivkins coordinates=XYZABCUVW" "" "joints=9 orient=-" +run "userkins" "" "joints=3 jnt=10,20,30" +run "millturn" "" "joints=4 jnt=10,20,30,40 orient=-" +run "millturn" "" "joints=4 jnt=10,20,30,40" 1 + +# linear maps and one rotation +run "corexykins" "" "joints=9" +run "rotatekins" "" "joints=9" +run "matrixkins" \ + "setp matrixkins.C_xy 0.02 +setp matrixkins.C_xz -0.01 +setp matrixkins.C_yx 0.03 +setp matrixkins.C_yz 0.015 +setp matrixkins.C_zx -0.02 +setp matrixkins.C_zy 0.01 +setp matrixkins.C_zz 1.001" \ + "joints=9" + +# tables and heads, offsets set so no term drops out +run "maxkins" \ + "setp maxkins.pivot-length 100" \ + "joints=9 jnt=10,20,30,0,15,25,7,0,3" + +run "5axiskins coordinates=XYZBCW" "" "joints=6 jnt=10,20,30,15,25,5 orient=4,3" +run "5axiskins coordinates=XYZBCW sparm=identityfirst" "" "joints=6 jnt=10,20,30,15,25,5 orient=4,3" 1 + +run "xyzac-trt-kins coordinates=XYZAC" \ + "setp xyzac-trt-kins.y-offset 3 +setp xyzac-trt-kins.z-offset 11 +setp xyzac-trt-kins.tool-offset 7 +setp xyzac-trt-kins.x-rot-point 1 +setp xyzac-trt-kins.y-rot-point 2 +setp xyzac-trt-kins.z-rot-point 5" \ + "joints=5 jnt=10,20,30,15,25 orient=-" + +run "xyzbc-trt-kins coordinates=XYZBC" \ + "setp xyzbc-trt-kins.conventional-directions 1 +setp xyzbc-trt-kins.x-offset 3 +setp xyzbc-trt-kins.z-offset 11 +setp xyzbc-trt-kins.tool-offset 7 +setp xyzbc-trt-kins.x-rot-point 1 +setp xyzbc-trt-kins.y-rot-point 2 +setp xyzbc-trt-kins.z-rot-point 5" \ + "joints=5 jnt=10,20,30,15,25 orient=-" + +run "xyzab_tdr_kins" \ + "setp xyzab_tdr_kins.x-offset 3 +setp xyzab_tdr_kins.z-offset 11 +setp xyzab_tdr_kins.tool-offset-z 7 +setp xyzab_tdr_kins.x-rot-point 1 +setp xyzab_tdr_kins.y-rot-point 2 +setp xyzab_tdr_kins.z-rot-point 5" \ + "joints=5 jnt=10,20,30,15,25" 1 + +run "xyzacb_trsrn" \ + "setp xyzacb_trsrn_kins.nut-angle 45 +setp xyzacb_trsrn_kins.y-pivot 100 +setp xyzacb_trsrn_kins.z-pivot 200 +setp xyzacb_trsrn_kins.x-offset 5 +setp xyzacb_trsrn_kins.y-offset 7 +setp xyzacb_trsrn_kins.y-rot-axis 300 +setp xyzacb_trsrn_kins.z-rot-axis 400 +setp xyzacb_trsrn_kins.tool-offset-z 50 +setp xyzacb_trsrn_kins.pre-rot 0.3 +setp xyzacb_trsrn_kins.primary-angle 20 +setp xyzacb_trsrn_kins.secondary-angle 35" \ + "joints=6 jnt=10,20,30,15,25,35 orient=5,4" 1 + +run "xyzacb_trsrn" \ + "setp xyzacb_trsrn_kins.nut-angle 45 +setp xyzacb_trsrn_kins.y-pivot 100 +setp xyzacb_trsrn_kins.z-pivot 200 +setp xyzacb_trsrn_kins.pre-rot 0.3 +setp xyzacb_trsrn_kins.primary-angle 20 +setp xyzacb_trsrn_kins.secondary-angle 35" \ + "joints=6 jnt=10,20,30,15,25,35 orient=-" 2 + +run "xyzbca_trsrn" \ + "setp xyzbca_trsrn_kins.nut-angle 45 +setp xyzbca_trsrn_kins.x-pivot 100 +setp xyzbca_trsrn_kins.z-pivot 200 +setp xyzbca_trsrn_kins.x-offset 5 +setp xyzbca_trsrn_kins.y-offset 7 +setp xyzbca_trsrn_kins.x-rot-axis 300 +setp xyzbca_trsrn_kins.z-rot-axis 400 +setp xyzbca_trsrn_kins.tool-offset-z 50 +setp xyzbca_trsrn_kins.pre-rot 0.3 +setp xyzbca_trsrn_kins.primary-angle 20 +setp xyzbca_trsrn_kins.secondary-angle 35" \ + "joints=6 jnt=10,20,30,15,25,35 orient=5,3" 1 + +# polar +run "rosekins" "" "joints=3 jnt=10,5,30" + +# arms +run "scarakins" "" "joints=6 jnt=30,40,20,10,0,0" +run "scorbot-kins" "" "joints=5 jnt=40,60,-20,0,0" +run "pumakins" "setp pumakins.D6 50" "joints=6 jnt=15,20,-35,10,70,20 orient=-" +run "three21kins" "" "joints=6 jnt=15,20,-35,10,70,20" +run "genserkins" "" "joints=9 jnt=15,20,-35,10,70,20,0,0,0" +run "genserkins" "setp genserkins.unrotate-3 1" "joints=9 jnt=15,20,-35,10,70,20,0,0,0" + +# parallel machines, from a pose the forward can be seeded with +run "tripodkins" \ + "setp tripodkins.Bx 2 +setp tripodkins.Cx 1 +setp tripodkins.Cy 2" \ + "joints=3 frompose=1 pose=1,1,2" +run "lineardeltakins" "" "joints=9 frompose=1 pose=20,30,-200" +run "rotarydeltakins" "" "joints=9 frompose=1 pose=0,0,-12" +run "genhexkins" "setp genhexkins.screw-lead 0" "joints=6 frompose=1 pose=2,3,20,0,5,-7" +run "genhexkins" "setp genhexkins.screw-lead 5" "joints=6 frompose=1 pose=2,3,20,0,5,-7" +run "pentakins" "" "joints=5 frompose=1 pose=10,20,0,5,-7" diff --git a/tests/kins-switch-unsolved/README b/tests/kins-switch-unsolved/README new file mode 100644 index 00000000000..c3f263e4f7f --- /dev/null +++ b/tests/kins-switch-unsolved/README @@ -0,0 +1,9 @@ +A kinematics switch whose forward kinematics cannot solve the current +joint position must leave the type in force, the joints and the position +alone, report the failure and abort; motion goes on working in the +kinematics it kept. + +The hexapod module starts in identity kinematics here, so the joints are +put at a few units each as coordinates. Six struts that short are not a +platform position the hexapod forward can converge to, so G12.1 P1 asks +for exactly that switch. diff --git a/tests/kins-switch-unsolved/checkresult b/tests/kins-switch-unsolved/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/kins-switch-unsolved/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/kins-switch-unsolved/sim.hal b/tests/kins-switch-unsolved/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/kins-switch-unsolved/sim.hal @@ -0,0 +1,16 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/kins-switch-unsolved/test-ui.py b/tests/kins-switch-unsolved/test-ui.py new file mode 100755 index 00000000000..4335f689ac0 --- /dev/null +++ b/tests/kins-switch-unsolved/test-ui.py @@ -0,0 +1,112 @@ +#!/usr/bin/env python3 +# A kinematics switch the new forward cannot solve. Motion must keep the +# position it knows, say so, abort, and take a switch back afterwards. +import hal +import linuxcnc +import sys +import time + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def kins_type(): + return int(round(hal.get_value("motion.kins-type"))) + +def position(): + s.poll() + return tuple(round(v, 6) for v in s.position[:6]) + +def drain(): + said = [] + while True: + m = e.poll() + if not m: + return said + said.append(m[1]) + +def wait_idle(): + deadline = time.time() + 30 + while time.time() < deadline: + s.poll() + if s.interp_state == linuxcnc.INTERP_IDLE and not s.queue: + return + time.sleep(0.05) + error("timed out waiting for the interpreter") + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(30) + wait_idle() + time.sleep(0.2) # let the error channel and the status catch up + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() + +if kins_type() != 0: + error("starts in kinematics type %d, expected identity (0)" % kins_type()) + +def joints(): + s.poll() + return tuple(round(v, 6) for v in s.joint_position[:6]) + +# a known position, in identity: joints and coordinates are the same thing +mdi("G0 X1 Y2 Z3 A4 B5 C6") +before = position() +if before != (1, 2, 3, 4, 5, 6): + error("position before the switch is %s, expected (1, 2, 3, 4, 5, 6)" % (before,)) +drain() + +# the switch the hexapod forward cannot solve: six struts of a few units +# are no platform position. Nothing may change but the report. +mdi("G12.1 P1") +said = drain() +if not any("kinematicsForward failed" in m for m in said): + error("no report of the failed forward kinematics, got %s" % said) +if kins_type() != 0: + error("kinematics type %d after the failed switch, expected 0 still" % kins_type()) +if position() != before: + error("position after the failed switch is %s, expected %s unchanged" % (position(), before)) +if joints() != before: + error("joints after the failed switch are %s, expected %s unchanged" % (joints(), before)) +s.poll() +if s.task_state != linuxcnc.STATE_ON: + error("task state %d after the failed switch, expected still ON" % s.task_state) + +# motion goes on working in the kinematics it kept, and the module is +# back in it too: the joints follow the identity inverse, not the hexapod's +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +mdi("G0 X30 Y30 Z30 A30 B30 C30") +if joints() != (30, 30, 30, 30, 30, 30): + error("joints after the move are %s, expected all 30" % (joints(),)) +said = drain() +if said: + error("unexpected messages after the failed switch: %s" % said) + +# a switch to the kinematics in force is nothing to do +mdi("G13.1") +if kins_type() != 0: + error("kinematics type %d after G13.1, expected identity (0)" % kins_type()) +if position() != (30, 30, 30, 30, 30, 30): + error("position after G13.1 is %s, expected all 30" % (position(),)) +said = drain() +if said: + error("unexpected messages after the switch back: %s" % said) + +print("Exiting with %d errors" % errors) +c.state(linuxcnc.STATE_ESTOP) +sys.exit(1 if errors else 0) diff --git a/tests/kins-switch-unsolved/test.ini b/tests/kins-switch-unsolved/test.ini new file mode 100644 index 00000000000..c93bc09563f --- /dev/null +++ b/tests/kins-switch-unsolved/test.ini @@ -0,0 +1,130 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# switchkins-type 0 is identity, 1 is the hexapod, 2 is the userk template +KINEMATICS = genhexkins sparm=identityfirst +JOINTS = 6 + +[HAL] +HALFILE = sim.hal + +[TRAJ] +COORDINATES = XYZABC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 200 +MAX_LINEAR_VELOCITY = 346 +MAX_LINEAR_ACCELERATION = 800 +DEFAULT_LINEAR_ACCELERATION = 800 +MAX_ANGULAR_VELOCITY = 360 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Y] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_A] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_B] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_C] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[JOINT_0] +TYPE = LINEAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = ANGULAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/kins-switch-unsolved/test.sh b/tests/kins-switch-unsolved/test.sh new file mode 100755 index 00000000000..765cf14fed6 --- /dev/null +++ b/tests/kins-switch-unsolved/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/kins-switch-unsolved/tool.tbl b/tests/kins-switch-unsolved/tool.tbl new file mode 100644 index 00000000000..d793e2d60ed --- /dev/null +++ b/tests/kins-switch-unsolved/tool.tbl @@ -0,0 +1 @@ +T1 P1 D0.0 Z12.5 ; diff --git a/tests/kins-switch/README b/tests/kins-switch/README index e031f2cd0ff..e409e2b3cf9 100644 --- a/tests/kins-switch/README +++ b/tests/kins-switch/README @@ -7,5 +7,12 @@ block after the switch is planned in the kinematics that runs it, that the selection reaches the motion controller and the interpreter, that a negative P word and a kinematics the module does not provide are refused, that G13.1 cancels to the identity kinematics the module declares (type 1 -here, not 0), and that G13.1 in an ON_ABORT_COMMAND routine does not -swallow the rest of the routine. +here, not 0), that the tool length G43.4 puts in effect reaches the +kinematics with nothing netted to its pin and, the head being tilted, +moves the programmed point rather than the joints, with the interpreter +agreeing on where the point went, that under G43.5 a tool vector I J K +lands on the joints and the point the rotary words reach, is not +incremental, keeps the free joint on the pole and is refused with a +rotary word, as a zero vector, on the identity kinematics and outside +G43.5, and that G13.1 in an ON_ABORT_COMMAND routine does not swallow +the rest of the routine. diff --git a/tests/kins-switch/test-ui.py b/tests/kins-switch/test-ui.py index 19a85744a15..bca7225ba6c 100755 --- a/tests/kins-switch/test-ui.py +++ b/tests/kins-switch/test-ui.py @@ -72,6 +72,7 @@ def mdi(cmd): said = [] seen = [kins_type()] at_switch = None +holding = False strayed = [0.0] * JOINTS w_reached = 0.0 deadline = time.time() + 60 @@ -83,7 +84,13 @@ def mdi(cmd): seen.append(k) if k == 1 and at_switch is None: at_switch = now - if k == 1 and at_switch is not None: + holding = True + else: + holding = False + # the joints are held over the first stretch in identity, the one the + # W stroke runs in; later on the program applies a tool length in the + # tilted pose, which moves the joints on purpose + if holding: for j in CARRIED: strayed[j] = max(strayed[j], abs(now[j] - at_switch[j])) w_reached = max(w_reached, now[5]) @@ -114,10 +121,11 @@ def mdi(cmd): # the abort routine's G13.1 fires at estop reset, so the machine may stand # in identity (1) already when the program starts; either way the program -# itself walks 0, 1, 0, 1 +# itself walks 0, 1, 0, 1 through the selection codes, and the same +# again through G43.4, G49 and the codes between want = [0, 1, 0, 1] -if seen[-4:] != want: - error("motion.kins-type went %s, not ...%s" % (seen, want)) +if seen[-8:] != want + want: + error("motion.kins-type went %s, not ...%s" % (seen, want + want)) reported = [m[1].strip() for m in said if m[1].strip().startswith("KINSTYPE=")] want_reported = ["KINSTYPE=%d.000000" % k for k in want] @@ -126,10 +134,168 @@ def mdi(cmd): else: print("#<_kins_type> reported %s" % " ".join(reported)) -# ---- a negative kinematics number is refused ----------------------------- +# G43.4 switches to the primary kinematics (0) and applies the offset, +# G49 cancels both, and a plain G43 touches the offset only; a G49 that +# cancels a plain G43, or comes after the program selected a kinematics +# itself, leaves the selection alone +g434 = [m[1].strip() for m in said if m[1].strip().startswith("G434")] +want_g434 = ["G434 KINSTYPE=0.000000 TLOZ=12.500000", + "G434 KINSTYPE=1.000000 TLOZ=0.000000", + "G434 KINSTYPE=1.000000 TLOZ=12.500000", + "G434 KINSTYPE=0.000000 TLOZ=0.000000", + "G434 KINSTYPE=0.000000 TLOZ=0.000000", + "G434 KINSTYPE=0.000000 TLOZ=12.500000", + "G434 KINSTYPE=1.000000 TLOZ=0.000000"] +if g434 != want_g434: + error("G43.4/G49 reported %s" % (g434,)) +else: + print("G43.4 switched to primary with the offset, G49 cancelled both") +# ---- a tool length under a tilt keeps the joints ------------------------ +# +# Motion hands the module the offset G43 puts in effect, with nothing +# netted. The head is tilted, so the length lies along the tool axis +# instead of along Z: the point the joints stand on moves by the tilt +# term of the length, and the joints stay where they are. The +# interpreter works out the same point ahead of motion, so a move to +# where it thinks the machine is goes nowhere. + +import math c.mode(linuxcnc.MODE_MDI) c.wait_complete(30) +drain() + +def pose(): + s.poll() + return list(s.position[:3]) + +def tool_length_check(what, before_joints, before_pose, after_joints, after_pose, want): + if max(abs(after_joints[j] - before_joints[j]) for j in range(JOINTS)) > 1e-6: + error("%s moved the joints by %s" % (what, + " ".join("%.4f" % (after_joints[j] - before_joints[j]) for j in range(JOINTS)))) + got = [after_pose[j] - before_pose[j] for j in (0, 1, 2)] + if max(abs(g - w) for g, w in zip(got, want)) > 1e-3: + error("%s moved the point by %s, not %s" % (what, + " ".join("%.4f" % v for v in got), " ".join("%.4f" % v for v in want))) + said = [m[1].strip() for m in drain() if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR)] + if said: + error("%s raised %s" % (what, said)) + # the interpreter's point: a move of nothing from it goes nowhere + mdi("G91") + stayed = mdi("G0 X0 Y0 Z0") + mdi("G90") + if max(abs(stayed[j] - after_joints[j]) for j in range(JOINTS)) > 1e-6: + error("after %s the interpreter has the point elsewhere: a move of nothing moved the joints by %s" + % (what, " ".join("%.4f" % (stayed[j] - after_joints[j]) for j in range(JOINTS)))) + +mdi("G12.1 P0") +mdi("G0 X10 Y10 Z-5 B-22.5 C45") +mdi("G49") +errors_before = errors +before = mdi("G0 X10 Y10 Z-5") +before_pose = pose() +after = mdi("G43.4 H1") +pose_after = pose() +L = 12.5 # tool 1 in tool.tbl +b, cc = math.radians(-22.5), math.radians(45) +# the length along the tool axis, less the length along Z the offset stands for +want = [L * math.sin(math.pi - b) * math.cos(cc), + L * math.sin(math.pi - b) * math.sin(cc), + L * (1 + math.cos(math.pi - b))] +tool_length_check("G43.4 in the tilted pose", before, before_pose, after, pose_after, want) +# G49 would drop to identity and hold the joints where they are; a zero +# offset without a switch takes the length back out +back = mdi("G43.1 Z0") +tool_length_check("a zero tool length", after, pose_after, back, pose(), [-w for w in want]) +if errors == errors_before: + print("a tool length under a tilt moved the point, not the joints") + +# a point-to-point move ends on joints motion holds while the point +# stays; a tool length moves the point, and the joints are held on +errors_before = errors +p2p = mdi("G53.4 G0 X10 Y10 Z-5 B-22.5 C45") +p2p_pose = pose() +after = mdi("G43.1 Z%g" % L) +tool_length_check("a tool length after a point-to-point move", p2p, p2p_pose, after, pose(), want) +if errors == errors_before: + print("a tool length after a point-to-point move keeps the joints too") +mdi("G43.1 Z0") +mdi("G49") + +# ---- G43.5: the tool axis as a vector ------------------------------------ +# +# Under G43.5 a G0 or G1 line gives the direction of the tool axis as I J K +# and the interpreter finds the rotaries. The head's tool axis at B, C is +# (-sin B cos C, -sin B sin C, cos B), so the vector for the tilted pose +# above has to land on the joints and the point the rotary words reach. + +def refused(cmd, needle): + drain() + c.mdi(cmd) + c.wait_complete(30) + m = e.poll() + if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("%s was accepted" % cmd) + elif needle not in m[1]: + error("%s said %r, nothing about %r" % (cmd, m[1].strip(), needle)) + else: + print("refused as expected: %s" % m[1].strip()) + drain() + +errors_before = errors +mdi("G12.1 P0") +mdi("G0 X0 Y0 Z0 B0 C0") +mdi("G43.5 H1") +s.poll() +if 435 not in s.gcodes: + error("G43.5 is not among the active G-codes %s" % (s.gcodes,)) +by_words = mdi("G0 X10 Y10 Z-5 B-22.5 C45") +by_words_pose = pose() +mdi("G0 X0 Y0 Z0 B0 C0") +vec = (-math.sin(b) * math.cos(cc), -math.sin(b) * math.sin(cc), math.cos(b)) +by_vector = mdi("G0 X10 Y10 Z-5 I%.9f J%.9f K%.9f" % vec) +if max(abs(by_vector[j] - by_words[j]) for j in range(JOINTS)) > 1e-5: + error("the vector put the joints at %s, the rotary words at %s" + % (" ".join("%.4f" % v for v in by_vector), " ".join("%.4f" % v for v in by_words))) +if max(abs(p - q) for p, q in zip(pose(), by_words_pose)) > 1e-5: + error("the vector put the point at %s, the rotary words at %s" + % (" ".join("%.4f" % v for v in pose()), " ".join("%.4f" % v for v in by_words_pose))) +# a direction is not incremental +mdi("G91") +held = mdi("G1 X0 I%.9f J%.9f K%.9f F1000" % vec) +mdi("G90") +if max(abs(held[j] - by_vector[j]) for j in range(JOINTS)) > 1e-6: + error("the same vector under G91 moved the joints by %s" + % " ".join("%.4f" % (held[j] - by_vector[j]) for j in range(JOINTS))) +# the pole: the tool vertical leaves C where it is, and the point stays +pole = mdi("G0 K1") +if abs(pole[3]) > 1e-6 or abs(pole[4] - 45) > 1e-3: + error("the tool vertical put B, C at %.4f, %.4f, not 0, 45" % (pole[3], pole[4])) +if max(abs(p - q) for p, q in zip(pose()[:3], by_words_pose[:3])) > 1e-5: + error("the tool vertical moved the point to %s" % " ".join("%.4f" % v for v in pose())) +# a rotary offset renames the angles, the direction is the same: the same +# joints, called something else by the program +mdi("G10 L2 P1 C30") +mdi("G0 X0 Y0 Z0 B0 C0") +offset = mdi("G0 X10 Y10 Z-5 I%.9f J%.9f K%.9f" % vec) +mdi("G10 L2 P1 C0") +if max(abs(offset[j] - by_words[j]) for j in range(JOINTS)) > 1e-5: + error("under a C offset the vector put the joints at %s, not %s" + % (" ".join("%.4f" % v for v in offset), " ".join("%.4f" % v for v in by_words))) +if errors == errors_before: + print("G43.5 turned the tool along the vector, onto the joints the rotary words reach") +refused("G0 X0 K1 B5", "twice") +refused("G0 X0 I0 J0 K0", "zero") +mdi("G13.1") +refused("G0 X0 K1", "G12.1 first") +mdi("G43.4 H1") +refused("G0 X0 K1", "K word with no") +mdi("G43.5 H1") +mdi("G49") +refused("G0 X0 K1", "K word with no") + +# ---- a negative kinematics number is refused ----------------------------- + drain() c.mdi("G12.1 P-1") c.wait_complete(30) diff --git a/tests/kins-switch/test.ngc b/tests/kins-switch/test.ngc index 96c03dfef17..eb512ea6b18 100644 --- a/tests/kins-switch/test.ngc +++ b/tests/kins-switch/test.ngc @@ -19,4 +19,33 @@ g12.1 p0 ; it is the flag that decides, not the number g13.1 (debug, KINSTYPE=#<_kins_type>) +; G43.4 is G43 on the primary kinematics: it switches from identity to +; the fiveaxis kinematics (0) and applies the tool offset +g43.4 h1 +(debug, G434 KINSTYPE=#<_kins_type> TLOZ=#5083) +; G49 cancels the offset and undoes the switch G43.4 made +g49 +(debug, G434 KINSTYPE=#<_kins_type> TLOZ=#5083) +; a plain G43 applies the offset but does not switch +g43 h1 +(debug, G434 KINSTYPE=#<_kins_type> TLOZ=#5083) +g49 +; on a kinematics the program selected itself, a plain G43 and its G49 +; leave the selection alone +g12.1 p0 +g43 h1 +g49 +(debug, G434 KINSTYPE=#<_kins_type> TLOZ=#5083) +; and G43.4 on it followed by G13.1: the program took over, the G49 +; has nothing to undo +g43.4 h1 +g13.1 +g12.1 p0 +g49 +(debug, G434 KINSTYPE=#<_kins_type> TLOZ=#5083) +; and back once more, ending in identity +g43.4 h1 +(debug, G434 KINSTYPE=#<_kins_type> TLOZ=#5083) +g49 +(debug, G434 KINSTYPE=#<_kins_type> TLOZ=#5083) m2 diff --git a/tests/kins-switch/tool.tbl b/tests/kins-switch/tool.tbl index a5809a7ac2f..d793e2d60ed 100644 --- a/tests/kins-switch/tool.tbl +++ b/tests/kins-switch/tool.tbl @@ -1 +1 @@ -T1 P1 D0.0 Z0.0 ; +T1 P1 D0.0 Z12.5 ; diff --git a/tests/kins-tool-offset/README b/tests/kins-tool-offset/README new file mode 100644 index 00000000000..db4bd42a726 --- /dev/null +++ b/tests/kins-tool-offset/README @@ -0,0 +1,7 @@ +The kinematics module takes the tool offset from motion, not from a net. + +Runs xyzac-trt-kins under motion with nothing connected to its tool-offset +pin, applies a tool length through G43, and checks that the joints move as +the tool length requires. Then connects motion.tooloffset.z to the pin the +old way and checks that nothing changes, and that G49 takes the length back +out through motion alone. diff --git a/tests/kins-tool-offset/checkresult b/tests/kins-tool-offset/checkresult new file mode 100755 index 00000000000..24dc9aa53e3 --- /dev/null +++ b/tests/kins-tool-offset/checkresult @@ -0,0 +1,2 @@ +#!/bin/sh +exit 0 # test failure is indicated by test.sh exit value diff --git a/tests/kins-tool-offset/sim.hal b/tests/kins-tool-offset/sim.hal new file mode 100644 index 00000000000..81a0df64444 --- /dev/null +++ b/tests/kins-tool-offset/sim.hal @@ -0,0 +1,20 @@ +# the module under test, with nothing on its tool-offset pin +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +# offsets, so that the tool length reaches the joints through a rotation +setp xyzac-trt-kins.y-offset 20 +setp xyzac-trt-kins.z-offset 10 + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/kins-tool-offset/test-ui.py b/tests/kins-tool-offset/test-ui.py new file mode 100755 index 00000000000..dbd174caa66 --- /dev/null +++ b/tests/kins-tool-offset/test-ui.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python3 +# The kinematics module takes the tool offset from motion. +# +# xyzac-trt-kins runs with nothing connected to its tool-offset pin. A +# tool length applied with G43 must still reach the joints, since motion +# hands the offset to the module; connecting motion.tooloffset.z to the +# pin afterwards, the old way, must change nothing; and G49 must take the +# length back out again through motion alone. + +import linuxcnc +import hal +import subprocess +import sys +import os +import time + +TOOL_LENGTH = 25.0 +POSE = "G0 X10 Y20 Z30 A30 C45" +AWAY = "G0 X0 Y0 Z0 A0 C0" + +c = linuxcnc.command() +s = linuxcnc.stat() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.home(-1) +c.wait_complete() +c.mode(linuxcnc.MODE_MDI) + +errors = 0 + +def error(msg): + global errors + errors += 1 + print("*** ERROR " + msg) + +def mdi(*cmds): + for cmd in cmds: + c.mdi(cmd) + c.wait_complete(30) + +def joints(): + # the commanded joint positions once the move has settled: in position, + # nothing queued, and the same answer twice in a row, since the in + # position flag can go up a cycle before the last increment lands + deadline = time.time() + 30 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(5)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.1) + error("timed out waiting for the move") + return last + +def same(a, b, tol=1e-6): + return all(abs(x - y) <= tol for x, y in zip(a, b)) + +def show(what, j): + print("%-28s %s" % (what, " ".join("%.6f" % v for v in j))) + +# no tool: the pose with nothing applied +mdi("G49", POSE) +base = joints() +show("G49", base) + +# tool applied through motion, the pin still at its default +mdi("G43 H1", AWAY, POSE) +with_tool = joints() +show("G43 H1, pin unconnected", with_tool) +pin = hal.get_value("xyzac-trt-kins.tool-offset") +if pin != 0.0: + error("the tool-offset pin reads %g with nothing connected" % pin) +if same(base, with_tool): + error("the tool length did not reach the joints") + +# the table on rotaries at A30 C45: the tool length moves Y and Z joints, +# by a known amount, since the pivot geometry is the module's alone +tool_z = hal.get_value("motion.tooloffset.z") +if abs(tool_z - TOOL_LENGTH) > 1e-9: + error("motion.tooloffset.z is %g, expected %g" % (tool_z, TOOL_LENGTH)) +if abs(with_tool[0] - base[0]) > 1e-6: + error("the tool length moved joint 0, which the A rotation does not touch") + +# the old connection: nothing may change +subprocess.check_call(["halcmd", "net", ":tool-offset", + "motion.tooloffset.z", "xyzac-trt-kins.tool-offset"]) +mdi(AWAY, POSE) +with_net = joints() +show("G43 H1, pin connected", with_net) +pin = hal.get_value("xyzac-trt-kins.tool-offset") +if abs(pin - TOOL_LENGTH) > 1e-9: + error("the connected tool-offset pin reads %g" % pin) +if not same(with_tool, with_net): + error("connecting the pin changed the joints") + +# and back out, through motion, with the pin connected +mdi("G49", AWAY, POSE) +without = joints() +show("G49, pin connected", without) +if not same(base, without): + error("G49 did not take the tool length back out") + +for f in ("sim.var", "sim.var.bak"): + try: + os.unlink(f) + except OSError: + pass + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/kins-tool-offset/test.ini b/tests/kins-tool-offset/test.ini new file mode 100644 index 00000000000..bb7839671ed --- /dev/null +++ b/tests/kins-tool-offset/test.ini @@ -0,0 +1,112 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0x0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +PARAMETER_FILE = sim.var + +[EMCMOT] +EMCMOT = motmod +COMM_TIMEOUT = 4.0 +SERVO_PERIOD = 1000000 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[HAL] +HALFILE = sim.hal + +[TRAJ] +COORDINATES = XYZAC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 20 +MAX_LINEAR_VELOCITY = 200 +MAX_LINEAR_ACCELERATION = 2000 +NO_FORCE_HOMING = 1 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[KINS] +KINEMATICS = xyzac-trt-kins +JOINTS = 5 + +[AXIS_X] +MIN_LIMIT = -200 +MAX_LIMIT = 200 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 + +[AXIS_Y] +MIN_LIMIT = -200 +MAX_LIMIT = 200 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 + +[AXIS_Z] +MIN_LIMIT = -200 +MAX_LIMIT = 200 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 + +[AXIS_A] +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 + +[JOINT_0] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 +MIN_LIMIT = -200 +MAX_LIMIT = 200 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 +MIN_LIMIT = -200 +MAX_LIMIT = 200 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 +MIN_LIMIT = -200 +MAX_LIMIT = 200 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 +MIN_LIMIT = -100 +MAX_LIMIT = 100 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +HOME_SEQUENCE = 0 diff --git a/tests/kins-tool-offset/test.sh b/tests/kins-tool-offset/test.sh new file mode 100755 index 00000000000..a31b772a81c --- /dev/null +++ b/tests/kins-tool-offset/test.sh @@ -0,0 +1,2 @@ +#!/bin/bash -e +linuxcnc -r test.ini diff --git a/tests/kins-tool-offset/tool.tbl b/tests/kins-tool-offset/tool.tbl new file mode 100644 index 00000000000..acb961918d9 --- /dev/null +++ b/tests/kins-tool-offset/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z25 D6 ;the tool with a length diff --git a/tests/kins-twp/README b/tests/kins-twp/README new file mode 100644 index 00000000000..d114c3a2afc --- /dev/null +++ b/tests/kins-twp/README @@ -0,0 +1,11 @@ +The C kinematics against the tilted work plane maths. + +The tilted work plane maths for the two nutating-head machines was first +written in python, remap_funcs_twp.py, independently of the kinematics +modules, for the remap the configs used before the interpreter learned +the codes. It lives on here, one copy per machine, as the oracle. +This test loads each module in realtime and puts its tool frame next to +the python transformation matrix over a grid of head angles, and its +tool frame inverse next to the python candidate joint angles and virtual +rotation for a set of requested tool axes. Where they disagree, one of +the two has the sign or the order of a rotation wrong. diff --git a/tests/kins-twp/check.py b/tests/kins-twp/check.py new file mode 100755 index 00000000000..2b8d52dd683 --- /dev/null +++ b/tests/kins-twp/check.py @@ -0,0 +1,273 @@ +#!/usr/bin/env python3 +# The python half of the tilted work plane cross-check. +# +# Imports the machine's remap_funcs_twp.py, the maths the tilted work +# plane remap orients the head with, and drives twpcheck, loaded after +# the kinematics module, to get the module's answers to the same +# questions. Two comparisons: +# +# frames over a grid of primary angle, secondary angle, virtual +# rotation and table angle, the module's tool frame in +# machine coordinates against the python transformation +# matrix Rp * Rs * Rtc +# inverse for a set of requested tool axes, with the table held as +# the remap holds it, the joint angle pairs the module's +# kinematicsToolFrameInverse() finds against the pairs the +# python candidate search keeps, and the spin about the tool +# the module reports for the python's horizontal tool x +# against the python's own virtual rotation; then with nothing +# held, where the module may turn the table, each side judged +# by the other's maths +# +# Usage: check.py MACHINE CONFIGDIR INIFILE +# MACHINE is xyzacb or xyzbca; CONFIGDIR holds remap_funcs_twp.py; +# INIFILE is what that file reads its letters and limits from. + +import os +import sys +import time +from math import radians, degrees, pi, sin, cos, atan2 + +import numpy as np +import hal + +machine, cfgdir, inifile = sys.argv[1:4] +os.environ["INI_FILE_NAME"] = inifile +sys.path.insert(0, cfgdir) +import remap_funcs_twp as twp + +# joint numbers: the table, the secondary and the primary rotary +TABLE, SECONDARY, PRIMARY = {"xyzacb": (3, 4, 5), "xyzbca": (4, 3, 5)}[machine] +PREROT = "%s_trsrn_kins.pre-rot" % machine +TOL = 1e-9 +ANGLE_TOL = 1e-6 # degrees + +failures = 0 +def fail(what): + global failures + failures += 1 + print("kins-twp: FAIL %s: %s" % (machine, what)) + +class Log: + def debug(self, *a, **k): pass + def error(self, *a, **k): print("kins-twp: python error:", a[0] % tuple(a[1:]) if len(a) > 1 else a[0]) +log = Log() + +# ---- driving twpcheck + +request = 0 +def ask(j, axis=None, xdir=None, held=0): + """set the joints and the request, wait for the answer""" + global request + for i, v in enumerate(j): + hal.set_p("twpcheck.j-%d" % i, str(v)) + hal.set_p("twpcheck.held", str(held)) + for i, c in enumerate("xyz"): + hal.set_p("twpcheck.axis-%s" % c, str(axis[i] if axis is not None else 0.0)) + hal.set_p("twpcheck.xdir-%s" % c, str(xdir[i] if xdir is not None else 0.0)) + hal.set_p("twpcheck.have-x", "1" if xdir is not None else "0") + request += 1 + hal.set_p("twpcheck.request", str(request)) + deadline = time.time() + 5 + while hal.get_value("twpcheck.done") != request: + if time.time() > deadline: + print("kins-twp: FAIL twpcheck did not answer") + sys.exit(1) + time.sleep(0.002) + +def read_matrix(name): + return np.array([[hal.get_value("twpcheck.%s-%d%d" % (name, r, c)) for c in range(3)] + for r in range(3)]) + +def read_solutions(): + n = hal.get_value("twpcheck.nsol") + sols = [] + for k in range(max(n, 0)): + sols.append(([hal.get_value("twpcheck.sol-%d-%d" % (k, i)) for i in range(6)], + hal.get_value("twpcheck.spin-%d" % k), + hal.get_value("twpcheck.free-%d" % k))) + return n, sols + +def joints_at(table, secondary, primary): + j = [10.0, 20.0, 30.0, 0.0, 0.0, 0.0] + j[TABLE], j[SECONDARY], j[PRIMARY] = table, secondary, primary + return j + +# ---- the python's answers + +def py_matrix(primary_deg, secondary_deg, tc): + m = twp.kins_calc_transformation_matrix(radians(primary_deg), radians(secondary_deg), tc, + np.asmatrix(np.identity(4)), 'inv') + return np.array(m)[:3, :3] + +def py_pairs(z): + """the (primary, secondary) pairs in degrees the remap would keep for a + tool axis, following remap.py: every combination of the candidate + lists, kept where it reaches the axis""" + t1, t2 = twp.kins_calc_possible_joint_angles(log, np.array(z), None) + if t1 is None or t2 is None: + return [] + pairs = [] + for a in set(t1): + for b in set(t2): + m = py_matrix(degrees(a), degrees(b), 0.0) + if np.allclose(m[:, 2], z, atol=1e-6): + pairs.append((degrees(a), degrees(b))) + return pairs + +def same_angle(a, b): + d = (a - b + 180.0) % 360.0 - 180.0 + return abs(d) < ANGLE_TOL + +def same_pair(p, q): + return same_angle(p[0], q[0]) and same_angle(p[1], q[1]) + +def fmt(m): + return np.array2string(m, precision=6, suppress_small=True) + +# ---- frames +# +# The module's frame is the head's rotation from its joints alone, so it +# is compared with the python matrix at zero virtual rotation; whether the +# frame should carry the virtual rotation too is a convention question the +# test does not settle. + +frames = 0 +hal.set_p(PREROT, "0") +for table in (0.0, 20.0): + for primary in (0.0, 30.0, -25.0, 90.0, 180.0, -135.0): + for secondary in (0.0, 30.0, -25.0, 90.0, -90.0, 180.0): + ask(joints_at(table, secondary, primary)) + if hal.get_value("twpcheck.frame-rc") != 0: + fail("no frame at primary %g secondary %g" % (primary, secondary)) + continue + tool = read_matrix("tool") + want = py_matrix(primary, secondary, 0.0) + frames += 1 + if not np.allclose(tool, want, atol=TOL): + fail("tool frame differs at primary %g secondary %g table %g\n module:\n%s\n python:\n%s" + % (primary, secondary, table, fmt(tool), fmt(want))) + +# ---- inverse, the table held +# +# The remap holds the table and orients the head, so ask the module the +# same: the joint pairs must then be the python's, and the spin about the +# tool for the python's horizontal tool x must be the python's virtual +# rotation. + +def rz(a): + return np.array([[cos(a), -sin(a), 0.0], [sin(a), cos(a), 0.0], [0.0, 0.0, 1.0]]) + +def frames_at(j): + ask(j) + return read_matrix("work"), read_matrix("tool") + +def in_work(work, tool): + return work.T @ tool + +HOLD_TABLE = 1 << TABLE +REQUESTS = ((30.0, 30.0), (-25.0, 60.0), (120.0, -45.0), (180.0, 90.0), + (0.0, 0.0), (90.0, 135.0), (45.0, 170.0), (-100.0, -20.0)) + +requests = 0 +for primary, secondary in REQUESTS: + z = py_matrix(primary, secondary, 0.0)[:, 2] + pairs = py_pairs(list(z)) + seed = joints_at(0.0, 0.0, 0.0) + where = "axis %s (from primary %g secondary %g)" % (fmt(z), primary, secondary) + if not any(same_pair(p, (primary, secondary)) for p in pairs): + fail("the python does not find the pair (%g, %g) the axis was made from" % (primary, secondary)) + + ask(seed, axis=z, held=HOLD_TABLE) + n, sols = read_solutions() + requests += 1 + if n <= 0: + fail("with the table held, the module finds no solution for " + where) + continue + found = [(s[0][PRIMARY], s[0][SECONDARY]) for s in sols] + for s in sols: + j, spin, free = s + if abs(j[TABLE] - seed[TABLE]) > 1e-12: + fail("the held table moved for " + where) + if free != 0 and (primary, secondary) != (0.0, 0.0): + fail("with the table held a solution is still a family for " + where) + for p in pairs: + if not any(same_pair(p, f) for f in found): + fail("python pair (%.6f, %.6f) not among the module's %s for %s" + % (p[0], p[1], ["(%.6f, %.6f)" % f for f in found], where)) + for f in found: + if not any(same_pair(p, f) for p in pairs): + fail("module pair (%.6f, %.6f) not among the python's %s for %s" + % (f[0], f[1], ["(%.6f, %.6f)" % p for p in pairs], where)) + + # tool x as the python's virtual rotation places it, horizontal: the + # module, holding the table, must answer the same pair with that spin + for p in pairs: + tc = twp.kins_calc_virtual_rot_for_g683(radians(p[0]), radians(p[1])) + full = py_matrix(p[0], p[1], tc) + if abs(full[2, 0]) > 1e-9: + fail("python virtual rotation %g leaves tool x off horizontal for pair (%.6f, %.6f)" % (tc, p[0], p[1])) + ask(seed, axis=z, xdir=full[:, 0], held=HOLD_TABLE) + n, sols = read_solutions() + requests += 1 + match = [s for s in sols if same_pair((s[0][PRIMARY], s[0][SECONDARY]), p)] + if not match: + fail("with tool x given and the table held, pair (%.6f, %.6f) is gone from the module's answers" % p) + continue + spin = match[0][1] + if abs((spin - tc + pi) % (2 * pi) - pi) > 1e-6: + fail("module spin %.9f and python virtual rotation %.9f differ for pair (%.6f, %.6f)" + % (spin, tc, p[0], p[1])) + +# ---- inverse, nothing held +# +# The module may now turn the table, since it turns the tool against the +# work as surely as the head does, and reports one member of the family +# that results. Not the python's answer, so each is judged by the other's +# maths: a module solution must reach the axis through the python head +# matrix composed with the module's table frame, and with tool x given it +# must reach the whole frame. + +for primary, secondary in REQUESTS: + z = py_matrix(primary, secondary, 0.0)[:, 2] + pairs = py_pairs(list(z)) + seed = joints_at(0.0, 0.0, 0.0) + where = "axis %s (from primary %g secondary %g)" % (fmt(z), primary, secondary) + + ask(seed, axis=z) + n, sols = read_solutions() + requests += 1 + if n <= 0: + fail("the module finds no solution for " + where) + continue + for s in sols: + j, spin, free = s + work, tool = frames_at(j) + if not np.allclose(tool, py_matrix(j[PRIMARY], j[SECONDARY], 0.0), atol=TOL): + fail("module frame at its own solution differs from the python head matrix for " + where) + if not np.allclose(in_work(work, py_matrix(j[PRIMARY], j[SECONDARY], 0.0))[:, 2], z, atol=1e-6): + fail("module solution %s does not reach %s by the python head matrix" % (fmt(np.array(j)), where)) + for i in (0, 1, 2): + if abs(j[i] - seed[i]) > 1e-9: + fail("solution moved linear joint %d for %s" % (i, where)) + + for p in pairs: + tc = twp.kins_calc_virtual_rot_for_g683(radians(p[0]), radians(p[1])) + full = py_matrix(p[0], p[1], tc) + ask(seed, axis=z, xdir=full[:, 0]) + n, sols = read_solutions() + requests += 1 + if n <= 0: + fail("the module finds no solution with tool x given for pair (%.6f, %.6f)" % p) + continue + for s in sols: + j, spin, free = s + work, tool = frames_at(j) + achieved = in_work(work, py_matrix(j[PRIMARY], j[SECONDARY], 0.0) @ rz(spin)) + if not np.allclose(achieved, full, atol=1e-6): + fail("module solution %s spin %.6f does not reach the python frame for pair (%.6f, %.6f)\n achieved:\n%s\n wanted:\n%s" + % (fmt(np.array(j)), spin, p[0], p[1], fmt(achieved), fmt(full))) + +if failures: + sys.exit(1) +print("kins-twp: %s agrees, %d frames, %d requests" % (machine, frames, requests)) diff --git a/tests/kins-twp/checkresult b/tests/kins-twp/checkresult new file mode 100755 index 00000000000..011ea9232ad --- /dev/null +++ b/tests/kins-twp/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +[ "$(grep -c 'kins-twp: .* agrees' "$1")" = 2 ] \ + && ! grep -q "FAIL" "$1" diff --git a/tests/kins-twp/skip b/tests/kins-twp/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/kins-twp/skip @@ -0,0 +1,4 @@ +#!/bin/sh +# Builds a realtime component with halcompile, which needs the build +# tools present. Skip when testing installed packages. +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/kins-twp/test.sh b/tests/kins-twp/test.sh new file mode 100755 index 00000000000..d50c9bea68e --- /dev/null +++ b/tests/kins-twp/test.sh @@ -0,0 +1,26 @@ +#!/bin/bash +set -e + +${SUDO} halcompile --install twpcheck.c >/dev/null + +# One hal file per machine. twpcheck answers frame and inverse requests +# from check.py over HAL pins; check.py holds the python maths. +run() { + local machine=$1 hal + hal=$(mktemp --suffix=.hal) + { printf 'loadrt %s_trsrn\n' "$machine" + printf 'setp %s_trsrn_kins.nut-angle 45\n' "$machine" + printf 'loadrt twpcheck joints=6 ktype=1\n' + printf 'loadrt threads name1=t1 period1=1000000\n' + printf 'addf twpcheck t1\n' + printf 'start\n' + printf 'loadusr -w python3 check.py %s %s/%s %s/twp-%s.ini\n' \ + "$machine" "$PWD" "$machine" "$PWD" "$machine" + } > "$hal" + echo "=== $machine" + halrun -f "$hal" + rm -f "$hal" +} + +run xyzacb +run xyzbca diff --git a/tests/kins-twp/twp-xyzacb.ini b/tests/kins-twp/twp-xyzacb.ini new file mode 100644 index 00000000000..7d276d86bf8 --- /dev/null +++ b/tests/kins-twp/twp-xyzacb.ini @@ -0,0 +1,16 @@ +# what remap_funcs_twp.py reads: the primary and secondary letters, their +# limits, and the module name its pins hang off +[KINS] +KINEMATICS = xyzacb_trsrn + +[TWP] +PRIMARY = C +SECONDARY = B + +[AXIS_C] +MIN_LIMIT = -181 +MAX_LIMIT = 181 + +[AXIS_B] +MIN_LIMIT = -181 +MAX_LIMIT = 181 diff --git a/tests/kins-twp/twp-xyzbca.ini b/tests/kins-twp/twp-xyzbca.ini new file mode 100644 index 00000000000..b6bfd198e60 --- /dev/null +++ b/tests/kins-twp/twp-xyzbca.ini @@ -0,0 +1,16 @@ +# what remap_funcs_twp.py reads: the primary and secondary letters, their +# limits, and the module name its pins hang off +[KINS] +KINEMATICS = xyzbca_trsrn + +[TWP] +PRIMARY = C +SECONDARY = A + +[AXIS_C] +MIN_LIMIT = -181 +MAX_LIMIT = 181 + +[AXIS_A] +MIN_LIMIT = -181 +MAX_LIMIT = 181 diff --git a/tests/kins-twp/twpcheck.c b/tests/kins-twp/twpcheck.c new file mode 100644 index 00000000000..90a86025569 --- /dev/null +++ b/tests/kins-twp/twpcheck.c @@ -0,0 +1,165 @@ +/* + * twpcheck: the realtime half of the tilted work plane cross-check. + * + * Loaded after a kinematics module, it answers requests made over HAL + * pins: for the joint values on its inputs it reports the module's tool + * frame and work frame, and for the tool axis (and optionally tool x) + * on its inputs it reports what kinematicsToolFrameInverse() finds, the + * joint sets and the spin about the tool each needs, with the joints + * named on the held pin kept where they are. check.py drives it and + * holds the python maths the answers are compared with. + * + * A request is made by raising the request pin; done follows it when + * the answers are on the pins. + * + * Module parameters + * joints joint count the module was loaded for + * ktype switchkins type to select first, 0 for none + */ +#include +#include +#include +#include +#include +#include + +MODULE_LICENSE("GPL"); + +static int joints = 6; +RTAPI_MP_INT(joints, "joint count the module under test was loaded for"); +static int ktype = 0; +RTAPI_MP_INT(ktype, "switchkins type to select first"); + +static int comp_id = -1; + +#define NSOL TOOL_FRAME_MAX_SOLUTIONS + +static struct { + hal_real_t j[EMCMOT_MAX_JOINTS]; + hal_real_t axis[3]; + hal_real_t xdir[3]; + hal_bool_t have_x; + hal_uint_t held; /* bit per joint the inverse may not move */ + hal_uint_t request; + hal_uint_t done; + hal_real_t tool[3][3]; /* [row][column], columns are the frame's axes */ + hal_real_t work[3][3]; + hal_sint_t frame_rc; + hal_sint_t nsol; + hal_real_t sol[NSOL][EMCMOT_MAX_JOINTS]; + hal_real_t spin[NSOL]; + hal_sint_t free[NSOL]; +} *pins; + +static void publish(hal_real_t out[3][3], const PmRotationMatrix *m) +{ + hal_set_real(out[0][0], m->x.x); hal_set_real(out[0][1], m->y.x); hal_set_real(out[0][2], m->z.x); + hal_set_real(out[1][0], m->x.y); hal_set_real(out[1][1], m->y.y); hal_set_real(out[1][2], m->z.y); + hal_set_real(out[2][0], m->x.z); hal_set_real(out[2][1], m->y.z); hal_set_real(out[2][2], m->z.z); +} + +static void update(void *arg, long period) +{ + KINEMATICS_FORWARD_FLAGS ff = 0; + PmRotationMatrix tool, work; + PmCartesian axis, xdir; + double j[EMCMOT_MAX_JOINTS]; + double sols[NSOL * EMCMOT_MAX_JOINTS]; /* rows of joints doubles, packed */ + double spin[NSOL]; + int freed[NSOL]; + int i, k, n, rc; + (void)arg; + (void)period; + + if (hal_get_ui32(pins->request) == hal_get_ui32(pins->done)) { return; } + + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + j[i] = i < joints ? hal_get_real(pins->j[i]) : 0.0; + } + + rc = kinematicsToolFrame(j, &tool, &ff); + if (!rc) { rc = kinematicsWorkFrame(j, &work, &ff); } + hal_set_si32(pins->frame_rc, rc); + if (!rc) { + publish(pins->tool, &tool); + publish(pins->work, &work); + } + + axis.x = hal_get_real(pins->axis[0]); + axis.y = hal_get_real(pins->axis[1]); + axis.z = hal_get_real(pins->axis[2]); + xdir.x = hal_get_real(pins->xdir[0]); + xdir.y = hal_get_real(pins->xdir[1]); + xdir.z = hal_get_real(pins->xdir[2]); + n = -1; + if (axis.x != 0 || axis.y != 0 || axis.z != 0) { + n = kinematicsToolFrameInverse(&axis, hal_get_bool(pins->have_x) ? &xdir : NULL, + j, hal_get_ui32(pins->held), sols, NSOL, + freed, spin); + } + hal_set_si32(pins->nsol, n); + for (k = 0; k < NSOL; k++) { + for (i = 0; i < joints; i++) { + hal_set_real(pins->sol[k][i], k < n ? sols[k * joints + i] : 0.0); + } + hal_set_real(pins->spin[k], k < n ? spin[k] : 0.0); + hal_set_si32(pins->free[k], k < n ? freed[k] : 0); + } + + hal_set_ui32(pins->done, hal_get_ui32(pins->request)); +} + +int rtapi_app_main(void) +{ + static const char letter[3] = { 'x', 'y', 'z' }; + int i, k, r, res = 0; + + if (joints < 1 || joints > EMCMOT_MAX_JOINTS) { return -1; } + + comp_id = hal_init("twpcheck"); + if (comp_id < 0) { return comp_id; } + + pins = hal_malloc(sizeof(*pins)); + if (!pins) { hal_exit(comp_id); return -1; } + + for (i = 0; i < joints; i++) { + res += hal_pin_new_real(comp_id, HAL_IN, &pins->j[i], 0.0, "twpcheck.j-%d", i); + } + for (i = 0; i < 3; i++) { + res += hal_pin_new_real(comp_id, HAL_IN, &pins->axis[i], 0.0, "twpcheck.axis-%c", letter[i]); + res += hal_pin_new_real(comp_id, HAL_IN, &pins->xdir[i], 0.0, "twpcheck.xdir-%c", letter[i]); + } + res += hal_pin_new_bool(comp_id, HAL_IN, &pins->have_x, 0, "twpcheck.have-x"); + res += hal_pin_new_ui32(comp_id, HAL_IN, &pins->held, 0, "twpcheck.held"); + res += hal_pin_new_ui32(comp_id, HAL_IN, &pins->request, 0, "twpcheck.request"); + res += hal_pin_new_ui32(comp_id, HAL_OUT, &pins->done, 0, "twpcheck.done"); + for (r = 0; r < 3; r++) { + for (i = 0; i < 3; i++) { + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->tool[r][i], 0.0, "twpcheck.tool-%d%d", r, i); + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->work[r][i], 0.0, "twpcheck.work-%d%d", r, i); + } + } + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->frame_rc, 0, "twpcheck.frame-rc"); + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->nsol, 0, "twpcheck.nsol"); + for (k = 0; k < NSOL; k++) { + for (i = 0; i < joints; i++) { + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->sol[k][i], 0.0, "twpcheck.sol-%d-%d", k, i); + } + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->spin[k], 0.0, "twpcheck.spin-%d", k); + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->free[k], 0, "twpcheck.free-%d", k); + } + if (res) { hal_exit(comp_id); return -1; } + + if (ktype > 0 && kinematicsSwitchable()) { + if (kinematicsSwitch(ktype)) { hal_exit(comp_id); return -1; } + } + + if (hal_export_funct("twpcheck", update, NULL, 1, 0, comp_id)) { + hal_exit(comp_id); + return -1; + } + hal_ready(comp_id); + return 0; +} + +void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/tests/kins-twp/xyzacb/remap_funcs_twp.py b/tests/kins-twp/xyzacb/remap_funcs_twp.py new file mode 100644 index 00000000000..7c2ebd39961 --- /dev/null +++ b/tests/kins-twp/xyzacb/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/tests/kins-twp/xyzbca/remap_funcs_twp.py b/tests/kins-twp/xyzbca/remap_funcs_twp.py new file mode 100644 index 00000000000..abb24726b72 --- /dev/null +++ b/tests/kins-twp/xyzbca/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/tests/maxkins/README b/tests/maxkins/README new file mode 100644 index 00000000000..cdf964410db --- /dev/null +++ b/tests/maxkins/README @@ -0,0 +1,7 @@ +Tests that maxkins forward and inverse kinematics agree with each other. + +The machine is driven to a series of poses with the pivot length, the tool +length and every one of B, C, U, V and W away from zero, and the position +that comes back through the forward kinematics is compared with the one that +was commanded. A pose that survives the round trip is one where the two +directions of the transform describe the same machine. diff --git a/tests/maxkins/checkresult b/tests/maxkins/checkresult new file mode 100755 index 00000000000..24dc9aa53e3 --- /dev/null +++ b/tests/maxkins/checkresult @@ -0,0 +1,2 @@ +#!/bin/sh +exit 0 # test failure is indicated by test.sh exit value diff --git a/tests/maxkins/core_sim.hal b/tests/maxkins/core_sim.hal new file mode 100644 index 00000000000..daf9b8f4c25 --- /dev/null +++ b/tests/maxkins/core_sim.hal @@ -0,0 +1,28 @@ +# core HAL config file for simulation + +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +# add motion controller functions to servo thread +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +# create HAL signals for position commands from motion module +# loop position commands back to motion module feedback +net Xpos joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net Ypos joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net Zpos joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net Apos joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net Bpos joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net Cpos joint.5.motor-pos-cmd => joint.5.motor-pos-fb +net Upos joint.6.motor-pos-cmd => joint.6.motor-pos-fb +net Vpos joint.7.motor-pos-cmd => joint.7.motor-pos-fb +net Wpos joint.8.motor-pos-cmd => joint.8.motor-pos-fb + +# the geometry the kinematics works from. Both are away from zero so that +# the pivot to tip distance is not degenerate. +setp maxkins.pivot-length [MAX]B_PIVOT_LENGTH +setp maxkins.tool-length [MAX]TOOL_LENGTH + +# estop loopback +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in diff --git a/tests/maxkins/test-ui.py b/tests/maxkins/test-ui.py new file mode 100755 index 00000000000..d2b804cb83f --- /dev/null +++ b/tests/maxkins/test-ui.py @@ -0,0 +1,134 @@ +#!/usr/bin/env python3 + +import linuxcnc +import hal + +import time +import sys + + +def wait_for_linuxcnc_startup(status, timeout=10.0): + + """Poll the Status buffer waiting for it to look initialized, + rather than just allocated (all-zero). Returns on success, throws + RuntimeError on failure.""" + + start_time = time.time() + while time.time() - start_time < timeout: + status.poll() + if (status.angular_units == 0.0) \ + or (status.axis_mask == 0) \ + or (status.cycle_time == 0.0) \ + or (status.exec_state != linuxcnc.EXEC_DONE) \ + or (status.interp_state != linuxcnc.INTERP_IDLE) \ + or (status.inpos == False) \ + or (status.linear_units == 0.0) \ + or (status.max_acceleration == 0.0) \ + or (status.max_velocity == 0.0) \ + or (status.program_units == 0.0) \ + or (status.rapidrate == 0.0) \ + or (status.state != linuxcnc.RCS_DONE) \ + or (status.task_state != linuxcnc.STATE_ESTOP): + time.sleep(0.1) + else: + # looks good + return + + # timeout, throw an exception + raise RuntimeError("Timeout") + + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() +h = hal.component("dummy") +h.ready() + +# Wait for LinuxCNC to initialize itself so the Status buffer stabilizes. +wait_for_linuxcnc_startup(s) + +# Because the kinematics is non-trivial, a homing is needed. +# HOME_ABSOLUTE_ENCODER = 1 is used in the ini file. +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +for joint in range(9): + c.home(joint) +c.wait_complete() + +LETTERS = 'XYZABCUVW' + + +def absdelta(a, b): + '''Maximum absolute difference between components of two coordinate points''' + return max(abs(a[i] - b[i]) for i in range(len(a))) + + +def test_pose(pose): + '''Command LinuxCNC to go to 'pose', then check that the position that + comes back matches it. Motion runs the inverse kinematics to turn the + pose into joint values and the forward kinematics to turn the joint + feedback back into a position, so a pose that comes back unchanged is one + the two directions agree on. + ''' + words = ' '.join('%s%0.9f' % (letter, value) + for letter, value in zip(LETTERS, pose)) + + c.mode(linuxcnc.MODE_MDI) + c.mdi('G0 ' + words) + c.wait_complete() + + # The delay seems to be needed for trajectory to fully settle, otherwise + # there is about 1e-5 error between target and actual position. + time.sleep(0.05) + s.poll() + joints = s.joint_actual_position[:9] + position = s.actual_position[:9] + + print("Commanded %s, joints %s, position %s" % (pose, joints, position)) + + # Accuracy limit is set to 1e-9 here. + # For practical purposes, a numerical accuracy of 1e-6 would be perfectly + # acceptable. Current implementation using doubles achieves about 1e-14 + # precision. + if absdelta(position, pose) > 1e-9: + raise RuntimeError( + "Forward and inverse kinematics disagree: commanded %s, joints %s, got %s" + % (pose, joints, position)) + + +# X Y Z A B C U V W +POSES = [ + # The pose everything starts from. + ( 0, 0, 0, 0, 0, 0, 0, 0, 0), + # Linear axes only, with the head upright. + ( 10, 20, 30, 0, 0, 0, 0, 0, 0), + # B alone tilts the head, which moves the tip in X and Z. + ( 10, 20, 30, 0, 40, 0, 0, 0, 0), + # C alone turns the table under the tip. + ( 10, 20, 30, 0, 0, 30, 0, 0, 0), + # B and C together: the head correction is in the machine frame and the + # table rotation is not, so the order the two are applied in matters. + ( 10, 20, 30, 0, 40, 30, 0, 0, 0), + # U alone shifts the head along its own axis, which with B tilted has a + # component in both X and Z. + ( 10, 20, 30, 0, 40, 0, 15, 0, 0), + # V is a plain shift of the saddle. + ( 10, 20, 30, 0, 40, 0, 0, 8, 0), + # W extends the pivot to tip distance. + ( 10, 20, 30, 0, 40, 0, 0, 0, 12), + # Everything at once. + ( 50, 50, 50, 5, 40, 30, 15, 8, 12), + # And with the signs the other way round. + ( -50, -50, -50, -5, -40, -30, -15, -8, -12), + # Back to the origin, where the pose is the same whichever direction the + # axes are taken to run in. + ( 0, 0, 0, 0, 0, 0, 0, 0, 0), +] + +for conventional in (0, 1): + hal.set_p('maxkins.conventional-directions', str(conventional)) + print("conventional-directions %d" % conventional) + for pose in POSES: + test_pose(pose) + +sys.exit(0) diff --git a/tests/maxkins/test.ini b/tests/maxkins/test.ini new file mode 100644 index 00000000000..7cf23c05738 --- /dev/null +++ b/tests/maxkins/test.ini @@ -0,0 +1,210 @@ +[EMC] +# The version string for this INI file. +VERSION = 1.1 + +DEBUG = 0 + +[MAX] +# distance from the end of the reference tool to the centre of the head +# tilt axis, and the length of the tool on top of it +B_PIVOT_LENGTH = 100 +TOOL_LENGTH = 25 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[FILTER] +#No Content + +[RS274NGC] +PARAMETER_FILE = sim.var + +[EMCMOT] +EMCMOT = motmod +COMM_TIMEOUT = 4.0 +BASE_PERIOD = 0 +SERVO_PERIOD = 1000000 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[HAL] +HALUI = halui +HALFILE = core_sim.hal + +[HALUI] +#No Content + +[TRAJ] +COORDINATES = X Y Z A B C U V W +HOME = 0 0 0 0 0 0 0 0 0 +LINEAR_UNITS = mm +ANGULAR_UNITS = degree +DEFAULT_LINEAR_VELOCITY = 10 +MAX_LINEAR_VELOCITY = 100 +MAX_LINEAR_ACCELERATION = 1000 +DEFAULT_ANGULAR_VELOCITY = 10 +MAX_ANGULAR_VELOCITY = 100 +MAX_ANGULAR_ACCELERATION = 1000 + +[EMCIO] +TOOL_TABLE = tool.tbl +RANDOM_TOOLCHANGER = 0 + +[KINS] +KINEMATICS = maxkins +JOINTS = 9 + +[AXIS_X] +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 + +[JOINT_0] +TYPE = LINEAR +HOME = 0.000 +HOME_ABSOLUTE_ENCODER = 1 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 +BACKLASH = 0.000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +FERROR = 0.001 + +[AXIS_Y] +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 + +[JOINT_1] +TYPE = LINEAR +HOME = 0.000 +HOME_ABSOLUTE_ENCODER = 1 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 +BACKLASH = 0.000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +FERROR = 0.001 + +[AXIS_Z] +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 + +[JOINT_2] +TYPE = LINEAR +HOME = 0.000 +HOME_ABSOLUTE_ENCODER = 1 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 +BACKLASH = 0.000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +FERROR = 0.001 + +[AXIS_A] +MIN_LIMIT = -360 +MAX_LIMIT = 360 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 + +[JOINT_3] +TYPE = ANGULAR +HOME = 0.000 +HOME_ABSOLUTE_ENCODER = 1 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 +BACKLASH = 0.000 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +FERROR = 0.001 + +[AXIS_B] +MIN_LIMIT = -360 +MAX_LIMIT = 360 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 + +[JOINT_4] +TYPE = ANGULAR +HOME = 0.000 +HOME_ABSOLUTE_ENCODER = 1 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 +BACKLASH = 0.000 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +FERROR = 0.001 + +[AXIS_C] +MIN_LIMIT = -360 +MAX_LIMIT = 360 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 + +[JOINT_5] +TYPE = ANGULAR +HOME = 0.000 +HOME_ABSOLUTE_ENCODER = 1 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 +BACKLASH = 0.000 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +FERROR = 0.001 + +[AXIS_U] +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 + +[JOINT_6] +TYPE = LINEAR +HOME = 0.000 +HOME_ABSOLUTE_ENCODER = 1 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 +BACKLASH = 0.000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +FERROR = 0.001 + +[AXIS_V] +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 + +[JOINT_7] +TYPE = LINEAR +HOME = 0.000 +HOME_ABSOLUTE_ENCODER = 1 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 +BACKLASH = 0.000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +FERROR = 0.001 + +[AXIS_W] +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 + +[JOINT_8] +TYPE = LINEAR +HOME = 0.000 +HOME_ABSOLUTE_ENCODER = 1 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1000 +BACKLASH = 0.000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +FERROR = 0.001 diff --git a/tests/maxkins/test.sh b/tests/maxkins/test.sh new file mode 100755 index 00000000000..6edf34d77ca --- /dev/null +++ b/tests/maxkins/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash + +rm -f sim.var +linuxcnc -r test.ini diff --git a/tests/maxkins/tool.tbl b/tests/maxkins/tool.tbl new file mode 100644 index 00000000000..e69de29bb2d diff --git a/tests/ptp-iterative/README b/tests/ptp-iterative/README new file mode 100644 index 00000000000..456921354f4 --- /dev/null +++ b/tests/ptp-iterative/README @@ -0,0 +1,9 @@ +The point-to-point moves on a kinematics module that answers the inverse +by iterating, and the preview of them. + +genserkins takes the Jacobian at the joints it is handed, so the seed +decides whether there is an answer at all: this machine homes to a pose +that is nowhere near all zeros, and all zeros is the singular pose the +inverse cannot start from. The interpreter reads the joints the machine +stands in, and the preview reads them from the status buffer the way a GUI +does, so the same program previews and runs to the same place. diff --git a/tests/ptp-iterative/checkresult b/tests/ptp-iterative/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/ptp-iterative/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/ptp-iterative/sim.hal b/tests/ptp-iterative/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/ptp-iterative/sim.hal @@ -0,0 +1,16 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/ptp-iterative/test-ui.py b/tests/ptp-iterative/test-ui.py new file mode 100755 index 00000000000..7fb988a976e --- /dev/null +++ b/tests/ptp-iterative/test-ui.py @@ -0,0 +1,175 @@ +#!/usr/bin/env python3 +# A module that answers the inverse by iterating has to be started +# somewhere, and the pose this machine homes to is not the one it would be +# started from by default. The moves are checked twice: through the +# interpreter, which reads the joints from motion, and through the preview, +# which reads them from the status buffer the way a GUI does. +import gcode +import linuxcnc +import preview_helpers +import os +import sys +import time +from rs274.interpret import StatMixin + +JOINTS = 6 +PROGRAM = "test.ngc" + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + + +def drain(): + while e.poll(): + pass + + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(JOINTS)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.05) + error("timed out waiting for the move") + return last + + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(60) + return settled() + + +class PreviewCanon(StatMixin): + # Stay on the per-event canon protocol: the catch-all below would + # otherwise answer gcode.parse's probe for the move-batch one. + use_move_batches = False + + def __init__(self, stat, parameter): + StatMixin.__init__(self, stat, False) + self.parameter_file = parameter + self.points = [] + + def __getattr__(self, name): + if name.startswith("_"): + raise AttributeError(name) + return lambda *args, **kwargs: None + + def straight_traverse(self, *pos): + self.points.append(pos) + + def straight_feed(self, *pos): + self.points.append(pos) + + +# the canon protocol carries lengths in the interpreter's own units, which +# a GUI turns into the machine's; the angles are already there +def in_machine_units(pos): + s.poll() + scale = (s.linear_units or 1) * 25.4 + return [v * scale for v in pos[:3]] + list(pos[3:6]) + + +def preview(program=PROGRAM): + ini = linuxcnc.ini(os.environ["INI_FILE_NAME"]) + s.poll() + canon = PreviewCanon(s, ini.getstring("RS274NGC", "PARAMETER_FILE")) + codes = preview_helpers.create_unitcode_and_initcode(s, ini) + result, line = gcode.parse(program, canon, *codes) + if result > gcode.MIN_ERROR: + return None, "line %d: %s" % (line, gcode.strerror(result)) + return canon.points, None + + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() + +home = settled() +print("homed at %s" % " ".join("%.4f" % v for v in home)) +if abs(home[1] + 90) > 1e-6 or abs(home[4] - 90) > 1e-6: + error("the machine did not home to the pose the test is written for") + +# the preview runs first, from the pose the machine stands in, and its last +# point is where the program ends up +points, refused = preview() +if refused: + error("the preview refused %s, %s" % (PROGRAM, refused)) +elif not points: + error("the preview of %s reported no move at all" % PROGRAM) +previewed = points[-1] if points else None + +# the interpreter takes the same program, one line at a time +for line in open(PROGRAM): + line = line.strip() + if not line or line.startswith("m2"): + continue + reached = mdi(line) + print("%-20s %s" % (line, " ".join("%.4f" % v for v in reached))) + drain() + +if abs(reached[0] - 10) > 1e-6 or abs(reached[4] - 80) > 1e-6: + error("the program left joints 0 and 4 at %.6f and %.6f" + % (reached[0], reached[4])) +for j in (1, 2, 3, 5): + if abs(reached[j] - home[j]) > 1e-6: + error("the program moved joint %d from %.6f to %.6f" + % (j, home[j], reached[j])) + +# and both agree on where that is +s.poll() +if previewed: + for name, i, got in zip("XYZABC", range(6), in_machine_units(previewed)): + if abs(got - s.position[i]) > 1e-3: + error("the preview put %s at %.6f, the machine at %.6f" + % (name, got, s.position[i])) + print("preview and machine agree on %s" + % " ".join("%.4f" % v for v in in_machine_units(previewed))) + +# a preview taken now starts where the machine stands, so a program that +# names the joints it is already in asks for no move at all +after, refused = preview() +if refused: + error("the second preview refused %s, %s" % (PROGRAM, refused)) +if after and max(abs(a - b) for a, b in zip(in_machine_units(after[0]), s.position[:6])) > 1e-3: + error("the second preview started at %s, not at %s" + % (["%.4f" % v for v in in_machine_units(after[0])], + ["%.4f" % v for v in s.position[:6]])) + +# a point out of the arm's reach: the module says so through the HAL +# library it prints with, which has to be within reach of the process the +# preview runs in, or the answer is the process going down +# all joints at zero is the pose this arm cannot be inverted from, and the +# module says so through the HAL library it prints with. That library has +# to be within reach of the process the preview runs in: a GUI has it only +# underneath the interpreter it loaded, and out of reach the answer is the +# process going down rather than a refusal. +mdi("g53.7 g0 j0=0 j1=0 j2=0 j3=0 j4=0 j5=0") +out, refused = preview() +if not refused: + error("the preview answered from the pose the arm cannot be inverted from") +elif "invert" not in refused: + error("the preview said %r, which does not mention the inverse" % refused) +else: + print("the preview refused from the singular pose: %s" % refused) + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/ptp-iterative/test.ini b/tests/ptp-iterative/test.ini new file mode 100644 index 00000000000..bc942288e36 --- /dev/null +++ b/tests/ptp-iterative/test.ini @@ -0,0 +1,140 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +KINEMATICS = genserkins +JOINTS = 6 + +[HAL] +HALFILE = sim.hal +# the modified DH parameters of the RV-6SDL, as the melfa-sim config has them +HALCMD = setp genserkins.A-1 85 +HALCMD = setp genserkins.A-2 380 +HALCMD = setp genserkins.A-3 100 +HALCMD = setp genserkins.ALPHA-1 -1.570796326 +HALCMD = setp genserkins.ALPHA-3 -1.570796326 +HALCMD = setp genserkins.ALPHA-4 1.570796326 +HALCMD = setp genserkins.ALPHA-5 -1.570796326 +HALCMD = setp genserkins.D-0 350 +HALCMD = setp genserkins.D-3 425 +HALCMD = setp genserkins.D-5 235 + +[TRAJ] +COORDINATES = XYZABC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 100 +MAX_LINEAR_VELOCITY = 120 +MAX_LINEAR_ACCELERATION = 700 +DEFAULT_LINEAR_ACCELERATION = 300 +NO_FORCE_HOMING = 1 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Y] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Z] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_A] +MIN_LIMIT = -360 +MAX_LIMIT = 360 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_B] +MIN_LIMIT = -185 +MAX_LIMIT = 185 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_C] +MIN_LIMIT = -320 +MAX_LIMIT = 320 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[JOINT_0] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = ANGULAR +HOME = -90 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +HOME = 90 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 diff --git a/tests/ptp-iterative/test.ngc b/tests/ptp-iterative/test.ngc new file mode 100644 index 00000000000..0afcad5d314 --- /dev/null +++ b/tests/ptp-iterative/test.ngc @@ -0,0 +1,3 @@ +g53.7 g0 j4=80 +g53.7 g0 j0=10 +m2 diff --git a/tests/ptp-iterative/test.sh b/tests/ptp-iterative/test.sh new file mode 100755 index 00000000000..765cf14fed6 --- /dev/null +++ b/tests/ptp-iterative/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/ptp-iterative/tool.tbl b/tests/ptp-iterative/tool.tbl new file mode 100644 index 00000000000..2028da29213 --- /dev/null +++ b/tests/ptp-iterative/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z25 D6 diff --git a/tests/ptp-robot/README b/tests/ptp-robot/README new file mode 100644 index 00000000000..8fda2b1b350 --- /dev/null +++ b/tests/ptp-robot/README @@ -0,0 +1,6 @@ +The point-to-point moves on a serial robot. + +pumakins maps X, Y and Z to its first three joints, which turn rather +than slide, so G53.5 refuses the machine and says which joint gives it +away. G53.7 names joints by number and works, and G53.4 still takes a +Cartesian destination. diff --git a/tests/ptp-robot/checkresult b/tests/ptp-robot/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/ptp-robot/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/ptp-robot/sim.hal b/tests/ptp-robot/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/ptp-robot/sim.hal @@ -0,0 +1,16 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/ptp-robot/test-ui.py b/tests/ptp-robot/test-ui.py new file mode 100755 index 00000000000..c013a42f2ff --- /dev/null +++ b/tests/ptp-robot/test-ui.py @@ -0,0 +1,115 @@ +#!/usr/bin/env python3 +# A serial robot has no axis letter that names the joint it looks like, so +# the letter form of the point-to-point move is refused here and the joint +# form is the one that works. +import linuxcnc +import sys +import time + +JOINTS = 6 + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(JOINTS)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.05) + error("timed out waiting for the move") + return last + +def drain(): + while e.poll(): + pass + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(60) + return settled() + +def refused(cmd, expect): + c.mdi(cmd) + c.wait_complete(30) + m = e.poll() + if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("%s was accepted" % cmd) + return + if expect not in m[1]: + error("%s said %r, which does not mention %r" % (cmd, m[1].strip(), expect)) + else: + print("refused as expected: %s" % m[1].strip()) + drain() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() + +# the joint form moves the joints it names and leaves the rest alone +before = mdi("G53.7 G0 J0=0 J1=0 J2=0 J3=0 J4=0 J5=0") +after = mdi("G53.7 G0 J1=-20 J4=35") +print("G53.7 G0 J1=-20 J4=35 %s" % " ".join("%.4f" % v for v in after)) +drain() +if abs(after[1] + 20) > 1e-6 or abs(after[4] - 35) > 1e-6: + error("G53.7 left joints 1 and 4 at %.6f and %.6f" % (after[1], after[4])) +for j in (0, 2, 3, 5): + if abs(after[j] - before[j]) > 1e-6: + error("G53.7 moved joint %d from %.9f to %.9f" % (j, before[j], after[j])) + +# a wrist reaches the same point with the forearm turned half a revolution +# and the wrist joints reversed, so a joint driven through zero must not +# come back as the other set: the joints not named stay where they are +mdi("G53.7 G0 J0=0 J1=0 J2=0 J3=0 J4=0 J5=0") +held = mdi("G53.7 G0 J4=90") +for value in (-10, 90, -45): + now = mdi("G53.7 G0 J4=%d" % value) + print("G53.7 G0 J4=%-4d %s" % (value, " ".join("%.4f" % v for v in now))) + drain() + if abs(now[4] - value) > 1e-6: + error("G53.7 J4=%d left joint 4 at %.6f" % (value, now[4])) + for j in (0, 1, 2, 3, 5): + if abs(now[j] - held[j]) > 1e-6: + error("G53.7 J4=%d moved joint %d from %.6f to %.6f" + % (value, j, held[j], now[j])) + +# the letter form is refused whichever letter is used, because X names the +# first rotary joint here; the message says so and points at G53.7 +refused("G53.5 G0 X10", "joint 0") +refused("G53.5 G0 A10", "G53.7") +refused("G53.5 G0 Z0", "angular") + +# and the code that takes a Cartesian target still works: the point the +# robot is standing on is reachable by definition, so ask for it +s.poll() +here = list(s.position[:3]) +mdi("G53.7 G0 J1=0 J4=0") +# a serial robot reaches one point with more than one set of joints, so +# only the point is checked, not the pose it comes back in +back = mdi("G53.4 G0 X%.6f Y%.6f Z%.6f" % (here[0], here[1], here[2])) +drain() +s.poll() +if max(abs(a - b) for a, b in zip(s.position[:3], here)) > 1e-3: + error("G53.4 landed at %s, not at %s" + % (["%.4f" % v for v in s.position[:3]], ["%.4f" % v for v in here])) +print("G53.4 back to the same point %s" % " ".join("%.4f" % v for v in back)) + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/ptp-robot/test.ini b/tests/ptp-robot/test.ini new file mode 100644 index 00000000000..4570f1389e8 --- /dev/null +++ b/tests/ptp-robot/test.ini @@ -0,0 +1,134 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +KINEMATICS = pumakins +JOINTS = 6 + +[HAL] +HALFILE = sim.hal +HALCMD = setp pumakins.A2 300 +HALCMD = setp pumakins.A3 50 +HALCMD = setp pumakins.D3 70 +HALCMD = setp pumakins.D4 400 +HALCMD = setp pumakins.D6 80 + +[TRAJ] +COORDINATES = XYZABC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 100 +MAX_LINEAR_VELOCITY = 120 +MAX_LINEAR_ACCELERATION = 700 +DEFAULT_LINEAR_ACCELERATION = 300 +NO_FORCE_HOMING = 1 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Y] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Z] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_A] +MIN_LIMIT = -360 +MAX_LIMIT = 360 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_B] +MIN_LIMIT = -185 +MAX_LIMIT = 185 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_C] +MIN_LIMIT = -320 +MAX_LIMIT = 320 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[JOINT_0] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 diff --git a/tests/ptp-robot/test.sh b/tests/ptp-robot/test.sh new file mode 100755 index 00000000000..765cf14fed6 --- /dev/null +++ b/tests/ptp-robot/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/ptp-robot/tool.tbl b/tests/ptp-robot/tool.tbl new file mode 100644 index 00000000000..2028da29213 --- /dev/null +++ b/tests/ptp-robot/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z25 D6 diff --git a/tests/remap/introspect/expected b/tests/remap/introspect/expected index 2f33b4bbe08..206bc4d96af 100644 --- a/tests/remap/introspect/expected +++ b/tests/remap/introspect/expected @@ -29,8 +29,8 @@ speed= 3000.0 global parameter set in test.ngc: 47.11 parameter set via test.ini: 3.14159 locals: ['a_new_local'] -globals: ['_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] -params(): ['a_new_local', '_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] +globals: ['_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_orient_a', '_orient_b', '_orient_c', '_orient_valid', '_orient_x', '_orient_y', '_orient_z', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] +params(): ['a_new_local', '_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_orient_a', '_orient_b', '_orient_c', '_orient_valid', '_orient_x', '_orient_y', '_orient_z', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] 14 N..... MESSAGE(" after introspect: return value=2.718280 call_level= 0.000000") 15 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) 16 N..... SET_XY_ROTATION(0.0000) diff --git a/tests/tool-frame/test_tool_frame.c b/tests/tool-frame/test_tool_frame.c index 00103cb4678..9e38a5f6933 100644 --- a/tests/tool-frame/test_tool_frame.c +++ b/tests/tool-frame/test_tool_frame.c @@ -217,10 +217,29 @@ static int holds(const double *sols, int count, int njoints, return 0; } +/* the joints that turn the work, read off the work frame rather than + declared, so that a caller can hold the table without naming it */ +static void test_work_joints(void) +{ + const double seed[6] = {0, 0, 0, 10, 20, 30}; + unsigned int mask = 99; + + check(toolFrameWorkJoints(xyzacWork, 5, seed, &mask) == 0 && mask == ((1u << 3) | (1u << 4)), + "xyzac: both rotaries carry the work"); + check(toolFrameWorkJoints(identityFrame, 5, seed, &mask) == 0 && mask == 0, + "a head machine: nothing turns the work"); + check(toolFrameWorkJoints(mixedWork, 6, seed, &mask) == 0 && mask == (1u << 3), + "table and head: the table joint alone"); + check(toolFrameWorkJoints(NULL, 5, seed, &mask) == -1, + "no frame function is refused"); +} + int main(void) { PmRotationMatrix m, r; + test_work_joints(); + /* the supplied constants are usable as declarations */ check(toolFrameIsProper(&TOOL_FRAME_SPINDLE), "TOOL_FRAME_SPINDLE is proper"); check(toolFrameIsProper(&TOOL_FRAME_FLANGE), "TOOL_FRAME_FLANGE is proper"); diff --git a/tests/twp-native/README b/tests/twp-native/README new file mode 100644 index 00000000000..9a49937006d --- /dev/null +++ b/tests/twp-native/README @@ -0,0 +1,13 @@ +The tilted work plane on the xyzacb nutating-head sim, natively. + +G12.1 P1 selects the TCP kinematics, G68.2 defines a plane, and the +orientation moves are checked against the python maths in +tests/kins-twp/xyzacb: G53.1 lands the head on one of the oracle's +angle pairs and leaves the linear joints where they were all through the +move; G53.6 leaves the tool tip where it was; G53.3 ends at the point +asked for in the plane; a move along plane X goes along plane X in the +world; G68.3 reads the plane back off the oriented tool; G69 cancels. +Q1 lets the table take part. The point-to-point moves are checked too: +G53.4 G0 to a program point, G53.5 and G53.7 G0 to a slide position with +the head tilted, G53.4 G1 taking the time the straight move would in G94 and +in G93, and what they refuse. diff --git a/tests/twp-native/abort.ngc b/tests/twp-native/abort.ngc new file mode 100644 index 00000000000..217d67e407a --- /dev/null +++ b/tests/twp-native/abort.ngc @@ -0,0 +1,11 @@ +(a plane, then a long move to be stopped in the middle) +(the cancel at the end is one the read ahead reaches long before the machine does) +G21 G90 G94 +G12.1 P1 +G0 X0 Y0 Z0 A0 B0 C0 +G68.2 P1 Q123 I30 J20 K0 +G53.1 +G1 F300 Z-40 +G1 Z0 +G69 +M2 diff --git a/tests/twp-native/checkresult b/tests/twp-native/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/twp-native/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/twp-native/sim.hal b/tests/twp-native/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/twp-native/sim.hal @@ -0,0 +1,16 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/twp-native/test-ui.py b/tests/twp-native/test-ui.py new file mode 100755 index 00000000000..96108490911 --- /dev/null +++ b/tests/twp-native/test-ui.py @@ -0,0 +1,534 @@ +#!/usr/bin/env python3 +# The tilted work plane on the xyzacb nutating-head sim, natively: see README. + +import linuxcnc +import hal +import sys +import os +import time +import math +import numpy as np + +TOPDIR = os.path.abspath(os.path.join(os.path.dirname(__file__), "..", "..")) +sys.path.insert(0, os.path.join(TOPDIR, "tests", "kins-twp", "xyzacb")) +import remap_funcs_twp as twp + +JOINTS = 6 +TABLE, SECONDARY, PRIMARY = 3, 4, 5 # A, B, C +I4 = np.asmatrix(np.identity(4)) + +class Log: + def debug(self, *a): pass + def error(self, *a): print("oracle:", a) +log = Log() + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.home(-1) +c.wait_complete() +c.mode(linuxcnc.MODE_MDI) + +errors = 0 + +def error(msg): + global errors + errors += 1 + print("*** ERROR " + msg) + +def drain(): + while True: + m = e.poll() + if not m: + return + print("channel:", m) + if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reported: %s" % m[1]) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(JOINTS)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.05) + error("timed out waiting for the move") + return last + +def mdi(*cmds): + for cmd in cmds: + c.mdi(cmd) + c.wait_complete(60) + return settled() + +# run one command and sample joints and positions on the way +# status is a task-cycle snapshot with the feedback a servo cycle behind the +# command, so two equal polls inside one task cycle do not mean the move is +# over: the last sample is taken once the move has settled +def sampled(cmd): + c.mdi(cmd) + samples = [] + t0 = time.time() + while time.time() - t0 < 60: + s.poll() + samples.append(([s.joint_position[i] for i in range(JOINTS)], list(s.position))) + if s.inpos and not s.queue and len(samples) > 20 and samples[-1] == samples[-2]: + break + time.sleep(0.005) + c.wait_complete(60) + end = settled() + s.poll() + samples.append(([s.joint_position[i] for i in range(JOINTS)], list(s.position))) + return end, samples + +# a point-to-point move runs every joint on a straight line in joint space, +# all together: the fraction of the way each moving joint has gone is the +# same for all of them at every sample, never goes back, and reaches one +def joint_line(what, samples, start, end): + moving = [i for i in range(JOINTS) if abs(end[i] - start[i]) > 1e-6] + if not moving: + error("%s: no joint moved" % what) + return + last = 0.0 + for n, (j, p) in enumerate(samples): + fs = [(j[i] - start[i]) / (end[i] - start[i]) for i in moving] + f = sum(fs) / len(fs) + if max(abs(x - f) for x in fs) > 1e-3: + error("%s: joints out of step at sample %d of %d: %s" % (what, n, len(samples), fs)) + return + if f < last - 1e-6: + error("%s: the joint path ran backwards at sample %d of %d (%.4f after %.4f)" % (what, n, len(samples), f, last)) + return + if f < -1e-6 or f > 1 + 1e-6: + error("%s: a joint left its segment at sample %d of %d (fraction %.4f)" % (what, n, len(samples), f)) + return + last = f + if last < 1 - 1e-6: + error("%s: the last sample is short of the end (fraction %.4f)" % (what, last)) + print("%s: %d joints on one line in joint space through %d samples" % (what, len(moving), len(samples))) + +def show(what, j): + print("%-26s %s" % (what, " ".join("%.4f" % v for v in j))) + +def wrap(d): + return (d + 180.0) % 360.0 - 180.0 + +def close(a, b, tol=1e-6): + return all(abs(x - y) <= tol for x, y in zip(a, b)) + +# the pairs the oracle would keep for a tool axis, in machine coordinates, +# as (b, c) in degrees, and which is nearest to the head's present angles +def oracle_pairs(z): + t1s, t2s = twp.kins_calc_possible_joint_angles(log, list(z), None) + pairs = [] + for t1 in t1s or []: + for t2 in t2s or []: + m = twp.kins_calc_transformation_matrix(t1, t2, 0, I4, 'inv') + got = [m[0, 2], m[1, 2], m[2, 2]] + if close(got, z, 1e-6): + pairs.append((math.degrees(t2), math.degrees(t1))) + return pairs + +def nearest_pair(pairs, b_now, c_now): + return min(pairs, key=lambda p: abs(wrap(p[0] - b_now)) + abs(wrap(p[1] - c_now))) + +def tool_axis(joints): + # the tool axis in work coordinates: the head as the oracle models it, + # brought into the table's frame the way the module reports the work + m = twp.kins_calc_transformation_matrix(math.radians(joints[PRIMARY]), + math.radians(joints[SECONDARY]), 0, I4, 'inv') + zm = np.array([m[0, 2], m[1, 2], m[2, 2]]) + a = math.radians(joints[TABLE]) + W = np.array([[1, 0, 0], [0, math.cos(a), math.sin(a)], [0, -math.sin(a), math.cos(a)]]) + return W.T.dot(zm) + +def plane_axes(): + s.poll() + r = s.g68_rotation + return ([r[0], r[3], r[6]], [r[1], r[4], r[7]], [r[2], r[5], r[8]]) + +def rot_x(d): + r = math.radians(d) + return np.array([[1, 0, 0], [0, math.cos(r), -math.sin(r)], [0, math.sin(r), math.cos(r)]]) + +def rot_y(d): + r = math.radians(d) + return np.array([[math.cos(r), 0, math.sin(r)], [0, 1, 0], [-math.sin(r), 0, math.cos(r)]]) + +# --- the plane, and G53.1 with the table held --------------------------- +# no G53.2 has run yet, so the pose parameters must refuse to be read +c.mdi("G0 X#<_orient_x>") +c.wait_complete(30) +m = e.poll() +if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reading #<_orient_x> before the first G53.2 was accepted") +drain() +start = mdi("G12.1 P1", "G0 X0 Y0 Z0 A0 B0 C0") +show("start", start) +R = rot_y(20).dot(rot_x(30)) +mdi("G68.2 P1 Q123 I30 J20 K0") +after, samples = sampled("G53.1") +show("G53.1", after) +drain() +s.poll() +if not s.g68_active: + error("G68.2 did not leave a plane active") +if not close(plane_axes()[2], list(R[:, 2]), 1e-9): + error("status reports a different plane normal than the program defined") +pairs = oracle_pairs(list(R[:, 2])) +print("oracle pairs (b, c):", ["(%.4f, %.4f)" % p for p in pairs]) +want = nearest_pair(pairs, start[SECONDARY], start[PRIMARY]) +if not pairs or abs(wrap(after[SECONDARY] - want[0])) > 1e-3 or abs(wrap(after[PRIMARY] - want[1])) > 1e-3: + error("G53.1 landed on (%.4f, %.4f), the oracle's nearest pair is (%.4f, %.4f)" + % (after[SECONDARY], after[PRIMARY], want[0], want[1])) +if abs(after[TABLE] - start[TABLE]) > 1e-9: + error("G53.1 moved the table with Q0") +worst = 0.0 +where = (0, 0, start[0], len(samples)) +for n, smp in enumerate(samples): + for i in range(3): + d = abs(smp[0][i] - start[i]) + if d > worst: + worst = d + where = (n, i, smp[0][i], len(samples)) +print("linear joints moved at most %.9f through G53.1 (sample %d of %d, joint %d at %.9f); ended at %s" + % ((worst,) + (where[0], where[3], where[1], where[2]) + (" ".join("%.9f" % v for v in after[:3]),))) +if worst > 1e-6: + error("G53.1 moved a linear joint") +if not close(tool_axis(after), list(R[:, 2]), 1e-6): + error("the tool axis after G53.1 is not the plane normal") +joint_line("G53.1", samples, start, after) + +# P names the pose rather than its rank: P1 is the one with the secondary +# rotary positive and P2 the one with it negative, from wherever the +# machine is standing, while no P is the nearest and so does depend on it +poses = {} +for where in ("G0 A0 B0 C0", "G0 A0 B-40 C170"): + for word in ("", "P1", "P2"): + mdi("G69") + mdi(where) + mdi("G68.2 P1 Q123 I30 J20 K0") + got = mdi("G53.1 %s" % word) + poses.setdefault(word, []).append(got) + drain() +for word, sign in (("P1", 1), ("P2", -1)): + a, b = poses[word] + rot = lambda j: [j[TABLE], j[SECONDARY], j[PRIMARY]] + if max(abs(wrap(x - y)) for x, y in zip(rot(a), rot(b))) > 1e-4: + error("G53.1 %s landed differently from two starting poses: %s and %s" + % (word, rot(a), rot(b))) + if sign * a[SECONDARY] <= 0: + error("G53.1 %s put the secondary rotary at %.4f" % (word, a[SECONDARY])) +if abs(poses["P1"][0][SECONDARY] - poses["P2"][0][SECONDARY]) < 1e-6: + error("G53.1 P1 and P2 chose the same pose") +if abs(poses[""][0][SECONDARY] - poses[""][1][SECONDARY]) < 1e-6: + error("G53.1 with no P gave the same pose from both starts, so it is not the nearest") +print("G53.1 P1 %.4f, P2 %.4f, no P %.4f then %.4f (secondary rotary)" + % (poses["P1"][0][SECONDARY], poses["P2"][0][SECONDARY], + poses[""][0][SECONDARY], poses[""][1][SECONDARY])) +c.mdi("G53.1 P3") +c.wait_complete(30) +m = e.poll() +if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("G53.1 P3 was accepted") +drain() +mdi("G69") +mdi("G0 X0 Y0 Z0 A0 B0 C0") +mdi("G68.2 P1 Q123 I30 J20 K0") +after = mdi("G53.1") + +# --- moves in the plane go along the plane's axes in the world ---------- +# G53.1 swung the tool tip, so it is somewhere in the plane; a move to X10 +# travels along the plane's X by the difference +def in_plane(): + s.poll() + return R.T.dot(np.array(s.position[:3]) - np.array(s.g68_offset[:3])), list(s.position[:3]) +q0, p0 = in_plane() +mdi("G0 X10") +q1, p1 = in_plane() +d = [p1[i] - p0[i] for i in range(3)] +want = list((10 - q0[0]) * R[:, 0]) +if not close(d, want, 1e-3) or abs(q1[0] - 10) > 1e-3: + error("G0 X10 in the plane moved the tool by %s, expected %s" % (d, want)) +mdi("G0 Z5") +q2, p2 = in_plane() +d = [p2[i] - p1[i] for i in range(3)] +want = list((5 - q1[2]) * R[:, 2]) +if not close(d, want, 1e-3) or abs(q2[2] - 5) > 1e-3: + error("G0 Z5 in the plane moved the tool by %s, expected %s" % (d, want)) + +# --- G53.6 keeps the tool centre point -------------------------------- +mdi("G69") +R2 = rot_y(20).dot(rot_x(-30)) +mdi("G68.2 P1 Q123 I-30 J20 K0") +s.poll(); before = list(s.position) +after, samples = sampled("G53.6") +show("G53.6", after) +drain() +worst = max(abs(smp[1][i] - before[i]) for smp in samples for i in range(3)) +print("tool tip moved at most %.6f through G53.6" % worst) +if worst > 1e-3: + error("G53.6 moved the tool tip") +if not close(tool_axis(after), list(R2[:, 2]), 1e-6): + error("the tool axis after G53.6 is not the plane normal") + +# --- G53.2 solves without moving, the pose lands on the parameters ---- +# same plane as G53.6 above, but from a pose that is not the answer: +# G53.2 must not move, and the pose it publishes must put the tool on the +# plane normal, which is where G53.6 already stands, so the nearest +# solution is the present rotary position +stay = after +after2, samples = sampled("G53.2") +drain() +if not close(after2, stay, 1e-9): + error("G53.2 moved the machine: %s became %s" % (stay, after2)) +# read the pose back through the parameters: a move to the published +# rotary words is a move to the present B and C, with the table held +before3 = mdi("G0 B0 C0") +after3, samples = sampled("G0 B#<_orient_b> C#<_orient_c>") +show("G0 to #<_orient_b/c>", after3) +drain() +if abs(wrap(after3[SECONDARY] - stay[SECONDARY])) > 1e-3 or abs(wrap(after3[PRIMARY] - stay[PRIMARY])) > 1e-3: + error("#<_orient_b> #<_orient_c> held (%.4f, %.4f), G53.6 had reached (%.4f, %.4f)" + % (after3[SECONDARY], after3[PRIMARY], stay[SECONDARY], stay[PRIMARY])) +if not close(tool_axis(after3), list(R2[:, 2]), 1e-6): + error("the tool axis at the pose G53.2 published is not the plane normal") +# the numbered parameters carry the same pose +before4 = mdi("G0 B0 C0") +after4, samples = sampled("G0 B#5075 C#5076") +show("G0 to #5075/#5076", after4) +drain() +if abs(wrap(after4[SECONDARY] - stay[SECONDARY])) > 1e-3 or abs(wrap(after4[PRIMARY] - stay[PRIMARY])) > 1e-3: + error("#5075 #5076 held (%.4f, %.4f), G53.6 had reached (%.4f, %.4f)" + % (after4[SECONDARY], after4[PRIMARY], stay[SECONDARY], stay[PRIMARY])) +c.mdi("#5075 = 0") +c.wait_complete(30) +m = e.poll() +if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("writing #5075 was accepted; the G53.2 pose is not read-only") +drain() +c.mdi("G0 A#<_orient_a>") +c.wait_complete(30) +m = e.poll() +if m and m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("#<_orient_a> after G53.2: %s" % m[1]) +drain() + +# --- G53.3 goes to a point in the plane with the tool oriented ---------- +before = mdi("G69") +R3 = rot_y(-25).dot(rot_x(35)) +mdi("G68.2 P1 Q123 I35 J-25 K0") +after, samples = sampled("G53.3 X5 Y5 Z5") +show("G53.3", after) +drain() +joint_line("G53.3", samples, before, after) +s.poll() +prog = R3.T.dot(np.array(s.position[:3]) - np.array(s.g68_offset[:3])) +if not close(list(prog), [5, 5, 5], 1e-3): + error("G53.3 ended at %s in the plane, not 5 5 5" % list(prog)) +if not close(tool_axis(after), list(R3[:, 2]), 1e-6): + error("the tool axis after G53.3 is not the plane normal") + +# --- G68.3 reads the plane back off the tool --------------------------- +mdi("G69", "G68.3 X1 Y2 Z3") +drain() +s.poll() +x, y, z = plane_axes() +if not s.g68_active or not close(list(s.g68_offset[:3]), [1, 2, 3], 1e-9): + error("G68.3 did not set the origin asked for") +if not close(z, list(R3[:, 2]), 1e-6): + error("G68.3's normal %s is not the tool axis %s" % (z, list(R3[:, 2]))) +if abs(x[2]) > 1e-6: + error("G68.3's X is not parallel to the machine XY plane: %s" % x) +mdi("G69", "G68.3 R90") +xr, yr, zr = plane_axes() +if not close(xr, y, 1e-6): + error("G68.3 R90 did not turn the plane about its normal") + +# --- Q1 lets the table take part --------------------------------------- +mdi("G69", "G0 X0 Y0 Z0 A0 B0 C0") +R4 = rot_x(30) +mdi("G68.2 P1 Q123 I30 J0 K0") +after = mdi("G53.1 Q1") +show("G53.1 Q1", after) +drain() +if not close(tool_axis(after), list(R4[:, 2]), 1e-6): + error("the tool axis after G53.1 Q1 is not the plane normal") +# with the plane X requested as well and the table free, three joints +# place three constraints: the plane's X is reached by the machine rather +# than by the frame +m = twp.kins_calc_transformation_matrix(math.radians(after[PRIMARY]), + math.radians(after[SECONDARY]), 0, I4, 'inv') +a = math.radians(after[TABLE]) +W = np.array([[1, 0, 0], [0, math.cos(a), math.sin(a)], [0, -math.sin(a), math.cos(a)]]) +xm = np.array([m[0, 0], m[1, 0], m[2, 0]]) +if not close(list(W.T.dot(xm)), list(R4[:, 0]), 1e-6): + error("with Q1 the machine did not place the plane's X") + +# --- G53.4, G53.5 and G69 ------------------------------------------------ +before = mdi("G69") +after, samples = sampled("G53.4 G0 X0 Y0 Z0 A0 B0 C0") +show("G53.4 G0", after) +drain() +joint_line("G53.4 G0", samples, before, after) +s.poll() +if s.g68_active: + error("G69 left the plane active") +if not close(list(s.g68_rotation), [1, 0, 0, 0, 1, 0, 0, 0, 1], 1e-12): + error("G69 left a rotation in status") +if not close(list(s.position[:3]), [0, 0, 0], 1e-6) or any(abs(after[j]) > 1e-6 for j in (TABLE, SECONDARY, PRIMARY)): + error("G53.4 G0 did not bring the tool and the rotaries back to zero") + +# G53.7 takes joint values by joint number: with the head tilted, J2=-5 +# puts joint 2 at -5 whatever that does to the tool tip, and touches no +# other joint; the value is the joint's own, untouched by G20 +before = mdi("G0 B30") +show("before G53.7", before) +after = mdi("G20 G53.7 G0 J2=-5") +mdi("G21") +show("G53.7 G0 J2=-5", after) +drain() +if abs(after[2] + 5) > 1e-6: + error("G53.7 J2=-5 left joint 2 at %.6f" % after[2]) +for j in (0, 1, 3, 4, 5): + if abs(after[j] - before[j]) > 1e-6: + error("G53.7 J2=-5 moved joint %d from %.9f to %.9f" % (j, before[j], after[j])) +after = mdi("G53.7 G0 J2=0 J[2+2]=0") +if abs(after[2]) > 1e-6 or abs(after[SECONDARY]) > 1e-6: + error("G53.7 J2=0 J4=0 did not put joints 2 and 4 at zero") + +# G53.5 takes the same destination by axis letter, in program units: Z-5 +# is joint 2 in millimetres, and under G20 the same words are inches +before = mdi("G0 B30") +after = mdi("G53.5 G0 Z-5") +show("G53.5 G0 Z-5", after) +drain() +if abs(after[2] + 5) > 1e-6: + error("G53.5 Z-5 left joint 2 at %.6f" % after[2]) +for j in (0, 1, 3, 4, 5): + if abs(after[j] - before[j]) > 1e-6: + error("G53.5 Z-5 moved joint %d from %.9f to %.9f" % (j, before[j], after[j])) +after = mdi("G20 G53.5 G0 Z-1") +mdi("G21") +show("G20 G53.5 G0 Z-1", after) +if abs(after[2] + 25.4) > 1e-6: + error("an inch of G53.5 Z left joint 2 at %.6f, not -25.4" % after[2]) +after = mdi("G53.5 G0 Z0 B0") +if abs(after[2]) > 1e-6 or abs(after[SECONDARY]) > 1e-6: + error("G53.5 Z0 B0 did not put joints 2 and 4 at zero") + +# a point-to-point feed takes the time the straight move would: 10 mm at +# F600 is one second, and F30 in G93 is two +def timed(cmd): + c.mdi(cmd) + first = last = None + t0 = time.time() + s.poll(); start = [s.joint_position[i] for i in range(JOINTS)] + while time.time() - t0 < 60: + s.poll() + now = [s.joint_position[i] for i in range(JOINTS)] + if now != start: + if first is None: + first = time.time() + last = time.time() + start = now + elif first is not None and s.inpos and not s.queue and time.time() - last > 0.3: + break + time.sleep(0.005) + c.wait_complete(60) + settled() + return (last - first) if first else 0.0 +mdi("G0 X0 Y0 Z0 A0 B0 C0") +took = timed("G53.4 G1 X10 F600") +print("G53.4 G1 X10 F600 took %.3f s" % took) +if not 0.7 < took < 1.5: + error("a 10 mm point-to-point feed at F600 took %.3f s, not about one" % took) +took = timed("G93 G53.4 G1 X0 F30") +mdi("G94") +print("G93 G53.4 G1 X0 F30 took %.3f s" % took) +if not 1.6 < took < 2.6: + error("a point-to-point feed at G93 F30 took %.3f s, not about two" % took) +drain() + +# what the point-to-point codes refuse +for cmd in ("G53.4 G2 X1 I1", "G91 G53.7 G0 J0=1", "G53.7 G0 J9=1", "G53.7 G0 X1", + "G53.7 G0 J1", "G53.7 G0", "G0 J0=1", "G53.7 G0 J0.5=1", "G53.7 G0 J0=1 J0=2", + "G53.5 G0 J0=1", "G53.5 G0", "G91 G53.5 G0 X1", "G53.4 G1 F0 X1"): + c.mdi(cmd) + c.wait_complete(30) + m = e.poll() + if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("%s was accepted" % cmd) + else: + print("refused as expected:", m[1]) + c.mode(linuxcnc.MODE_MDI) +mdi("G90 G94 G0 X0 Y0 Z0 A0 B0 C0") + +# --- a plane refuses what would move the ground under it --------------- +# each refusal is an interpreter error, and the abort that follows cancels +# the plane, so it is defined afresh before every one +for cmd in ("G92 X1", "G55", "G10 L2 P1 X1"): + mdi("G68.2 P1 Q123 I30 J0 K0") + c.mdi(cmd) + c.wait_complete(30) + m = e.poll() + if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("%s was accepted while a plane is active" % cmd) + else: + print("refused as expected:", m[1]) + c.mode(linuxcnc.MODE_MDI) + s.poll() + if s.g68_active: + error("the abort after %s left the plane active" % cmd) +mdi("G69") + +# --- stopping a program that has a plane --------------------------------- +# The read ahead runs the program's own G69 long before the machine gets +# there, so the cancel is sitting in the queue when the stop button throws +# the queue away. The plane in status has to end up cancelled all the same, +# and a G69 typed afterwards has to be able to say so again. +c.mode(linuxcnc.MODE_AUTO) +c.wait_complete(30) +c.program_open("abort.ngc") +c.auto(linuxcnc.AUTO_RUN, 0) +deadline = time.time() + 30 +while time.time() < deadline: + s.poll() + if s.g68_active and s.current_line >= 8: + break + time.sleep(0.02) +s.poll() +if not s.g68_active: + error("the program never reported a plane to stop in the middle of") +c.abort() +c.wait_complete(30) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() +s.poll() +if s.g68_active: + error("stopping the program left the plane active in status") +if not close(list(s.g68_rotation), [1, 0, 0, 0, 1, 0, 0, 0, 1], 1e-12): + error("stopping the program left a rotation in status") +mdi("G69") +s.poll() +if s.g68_active: + error("G69 after the stop did not clear the plane") +print("a stopped program leaves no plane behind") +mdi("G0 X0 Y0 Z0 A0 B0 C0") +drain() + +for f in ("sim.var", "sim.var.bak"): + try: + os.unlink(f) + except OSError: + pass + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/twp-native/test.ini b/tests/twp-native/test.ini new file mode 100644 index 00000000000..a3a6bcf9e1a --- /dev/null +++ b/tests/twp-native/test.ini @@ -0,0 +1,147 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +KINEMATICS = xyzacb_trsrn +JOINTS = 6 + +# what the python oracle reads +[TWP] +PRIMARY = C +SECONDARY = B + +[HAL] +HALFILE = sim.hal +HALCMD = setp xyzacb_trsrn_kins.nut-angle 45 +HALCMD = setp xyzacb_trsrn_kins.y-pivot 100 +HALCMD = setp xyzacb_trsrn_kins.z-pivot 200 +HALCMD = setp xyzacb_trsrn_kins.x-offset 5 +HALCMD = setp xyzacb_trsrn_kins.y-offset 7 +HALCMD = setp xyzacb_trsrn_kins.y-rot-axis 300 +HALCMD = setp xyzacb_trsrn_kins.z-rot-axis 400 + +[TRAJ] +COORDINATES = XYZABC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 100 +MAX_LINEAR_VELOCITY = 120 +MAX_LINEAR_ACCELERATION = 700 +DEFAULT_LINEAR_ACCELERATION = 300 +NO_FORCE_HOMING = 1 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Y] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Z] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_A] +MIN_LIMIT = -360 +MAX_LIMIT = 360 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_B] +MIN_LIMIT = -185 +MAX_LIMIT = 185 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_C] +MIN_LIMIT = -320 +MAX_LIMIT = 320 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[JOINT_0] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MAX_JERK = 7000 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MAX_JERK = 7000 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MAX_JERK = 7000 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MAX_JERK = 9000 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MAX_JERK = 9000 +MIN_LIMIT = -185 +MAX_LIMIT = 185 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MAX_JERK = 9000 +MIN_LIMIT = -320 +MAX_LIMIT = 320 +HOME_SEQUENCE = 0 diff --git a/tests/twp-native/test.sh b/tests/twp-native/test.sh new file mode 100755 index 00000000000..765cf14fed6 --- /dev/null +++ b/tests/twp-native/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/twp-native/tool.tbl b/tests/twp-native/tool.tbl new file mode 100644 index 00000000000..2028da29213 --- /dev/null +++ b/tests/twp-native/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z25 D6