From 55c0f806e1f3edd3f64888ee2aa01e5e864e9563 Mon Sep 17 00:00:00 2001 From: david mueller Date: Sun, 23 Aug 2026 13:32:45 +1000 Subject: [PATCH 01/77] twp: split the machine maths out of the tilted work plane remap The remap carried the geometry of every supported machine as branches on the (primary, secondary) letter pair inside its four kinematics functions, so adding a machine meant adding a branch to each. The generic half is remap.py now and the machine half a remap_funcs_twp.py beside each config, imported by name: eleven functions the generic side asks a machine, which joint angles reach a tool orientation, how to build the transformation, the default tool x, what to write on the module pins. The split and the generic file are David Mueller's, from https://github.com/Sigma1912/LinuxCNC_Demo_Configs/tree/main/5axis-twp. Adapted: the ini is read through linuxcnc.ini, the kinematics switch is G12.1 rather than the deprecated motion.switchkins-type pin, the angles reach the module pins in degrees, pin names unchanged. Three fixes come with it: kins_calc_primary collected only the last candidate's primary angle, so P1 and P2 failed where a solution existed; candidate angles were compared in radians against limits in degrees; on xyzbca-trsrn G53.6, G68.3 and one G53.3 case reported success without activating the plane. Both configs verified over eight orientations under every code against the run before the change, tool vectors identical to 1e-9. --- .../python/remap.py | 1476 ++++++++--------- .../xyzacb-trsrn_twp/remap_funcs_twp.py | 347 ++++ .../xyzacb-trsrn_twp/xyzacb-trsrn.ini | 2 +- .../xyzbca-trsrn_twp/remap_funcs_twp.py | 350 ++++ .../xyzbca-trsrn_twp/xyzbca-trsrn.ini | 2 +- 5 files changed, 1386 insertions(+), 791 deletions(-) create mode 100644 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py create mode 100644 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py index f4f9506a846..05fc53261a1 100755 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py @@ -1,7 +1,7 @@ # This is a python remap for LinuxCNC implementing 'Tilted Work Plane' # G68.2, G68.3, G68.4 and related Gcodes G53.1, G53.3, G53.6, G69 # -# Copyright ()c) 2023 David Mueller +# Copyright ()c) 2025 David Mueller # # This program is free software: you can redistribute it and/or modify # it under the terms of the GNU General Public License as published by @@ -13,7 +13,22 @@ # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. # -# +''' +The remap does the following: + +- Parses the G68.[2,4] gcodes and constructs the requested tool orientation vectors (x,z). +- Writes and reads hal pins created and updated by'twp-helper-comp.py' (mostly for updating the gui). +- Parses the G53.[1,3,6] and uses the functions in 'remap_funcs_twp.py' to calculate all rotary joint position that result in the correct tool orientation (there may be more than just one). +- Selects the appropriate rotary angles that will respect rotary limits set in the ini file and also follow any orientation strategy requested by the operator using the 'P' word. +- Sets the kinematic modes +- Calculates new work offset values so the WCS origin after switching to TWP mode is in the requested physical position. +- Used MDI commands to: + - Move the rotary joints to the calculated positions + - Switch the WCS system to 'G59' and set the values of G59, G59.[1,2.3] to the calculated coordinates +- Parses the G69 gcodes, resets the relevant parameters and switches back to Identity kinematic mode +''' + + import sys import traceback import numpy as np @@ -23,7 +38,6 @@ from util import lineno, call_pydevd import hal - # logging import logging # this name will be printed first on each log message @@ -33,22 +47,41 @@ formatter = logging.Formatter('%(name)s %(levelname)s: %(message)s') handler.setFormatter(formatter) log.addHandler(handler) -# Manually force the log level for this module -log.setLevel(logging.ERROR) # One of DEBUG, INFO, WARNING, ERROR, CRITICAL - # set up parsing of the inifile import os import linuxcnc # get the path for the ini file used to start this config inifile = os.environ.get("INI_FILE_NAME") + +# adding the remap_funcs folder to the system path. The machine specific +# functions live beside the ini file, which is the working directory, and the +# parent is searched too so a config may keep them one level up and share them +# between variants. +cwd = os.getcwd() +parent = os.path.abspath(os.path.join(cwd, os.pardir)) +sys.path.insert(0, parent) +sys.path.insert(0, cwd) +from remap_funcs_twp import * + # instantiate the LinuxCNC ini-parser config = linuxcnc.ini(inifile) -## SPINDLE ROTARY JOINT LETTERS -# spindle primary joint +# debug setting +try: + debug_setting = config.getint('TWP', 'LOG_LEVEL', fallback=1) + if debug_setting > 4: debug_setting = 4 + if debug_setting < 0: debug_setting = 0 +except Exception as error: + debug_setting = 1 + log.warning("Unable to parse debug setting given in INI. Setting it to 1.") +debug_levels = (logging.CRITICAL, logging.ERROR, logging.WARNING, logging.INFO, logging.DEBUG) +log.setLevel(debug_levels[debug_setting]) + +## ROTARY JOINT LETTERS +# primary rotary joint (independent of the secondary joint) joint_letter_primary = config.getstring('TWP', 'PRIMARY', fallback="").capitalize() -# spindle secondary joint (ie the one closer to the tool) +# secondary rotary joint (dependent on the primary joint) joint_letter_secondary = config.getstring('TWP', 'SECONDARY', fallback="").capitalize() if not joint_letter_primary in ('A','B','C') or not joint_letter_secondary in ('A','B','C'): @@ -58,32 +91,28 @@ else: # get the MIN/MAX limits of the respective rotary joint letters category = 'AXIS_' + joint_letter_primary - primary_min_limit = config.getreal(category, 'MIN_LIMIT', fallback=0.0) - primary_max_limit = config.getreal(category, 'MAX_LIMIT', fallback=0.0) - log.info('Joint letter for primary is %s with MIN/MAX limits: %s,%s', joint_letter_primary, primary_min_limit, primary_max_limit) + primary_min_limit = radians(config.getreal(category, 'MIN_LIMIT', fallback=0.0)) + primary_max_limit = radians(config.getreal(category, 'MAX_LIMIT', fallback=0.0)) + log.info('Joint letter for primary is %s with MIN/MAX limits: %s,%s', + joint_letter_primary, degrees(primary_min_limit), degrees(primary_max_limit)) category = 'AXIS_' + joint_letter_secondary - secondary_min_limit = config.getreal(category, 'MIN_LIMIT', fallback=0.0) - secondary_max_limit = config.getreal(category, 'MAX_LIMIT', fallback=0.0) - log.info('Joint letter for secondary is %s with MIN/MAX Limits: %s,%s', joint_letter_secondary, secondary_min_limit, secondary_max_limit) - - -## CONNECTIONS TO THE KINEMATIC COMPONENT -# get the name of the kinematic component -kins_comp = config.getstring('KINS', 'KINEMATICS', fallback="") -# name of the hal pin that represents the nutation-angle -kins_nutation_angle = kins_comp + '_kins.nut-angle' -# name of the hal pin that represents the pre-rotation -kins_pre_rotation = kins_comp + '_kins.pre-rot' -# name of the hal pin that represents the primary joint orientation angle -kins_primary_rotation = kins_comp + '_kins.primary-angle' -# name of the hal pin that represents the secondary joint orientation angle -kins_secondary_rotation = kins_comp + '_kins.secondary-angle' + secondary_min_limit = radians(config.getreal(category, 'MIN_LIMIT', fallback=0.0)) + secondary_max_limit = radians(config.getreal(category, 'MAX_LIMIT', fallback=0.0)) + log.info('Joint letter for secondary is %s with MIN/MAX Limits: %s,%s', + joint_letter_secondary, degrees(secondary_min_limit), degrees(secondary_max_limit)) + ## CONNECTIONS TO THE HELPER COMPONENT twp_comp = 'twp-helper-comp.' twp_is_defined = twp_comp + 'twp-is-defined' twp_is_active = twp_comp + 'twp-is-active' +# Which rotary joint should be prioritized when calculating optimal joint rotation angles +try: + optimization_priority = config.getint('TWP', 'PRIORITY', fallback=1) +except Exception as error: + log.warning("Unable to parse orientation priority given in INI. Setting it to 1.") + optimization_priority = 1 # raise InterpreterException if execute() or read() fail throw_exceptions = 1 @@ -93,7 +122,7 @@ twp_matrix = np.asmatrix(np.identity(4)) # some g68.2 p-word modes require several calls to enter all the required parameters so we -# need a flag that indicates when the twp has been defined and is ready for g53.x +# need a flag that indicates when the twp has been defined and is ready for G53.n # [current p-word, number of calls required, (state of calls required for that p mode added by g68.2)] # note that we use string since boolean True == 1, which gives wrong results if we want # to count the elements that are True because it is counted as integer '1' @@ -105,592 +134,391 @@ current_work_offset_number = 1 saved_work_offset = [0,0,0] # orientation mode refers to the strategy used to choose from the different rotary angles for a given -# tool-z vector. The optimization is applied to the primary axis only with mode 0 (shortest path) being +# z-vector vector. The optimization is applied to the primary axis only with mode 0 (shortest path) being # the default. (0=shortest_path , 1=positive_rotation only, 2=negative_rotation only, ) orient_mode = 0 -# defines the kinematic model for (world <-> tool) coordinates of the machine at hand -# returns 4x4 transformation matrix for given angles and 4x4 input matrix -# NOTE: these matrices must be the same as the ones used to derive the kinematic model -def kins_tool_transformation(theta_1, theta_2, pre_rot, matrix_in, direction='fwd'): - global joint_letter_primary, joint_letter_secondary - global kins_nutation_angle - T_in = matrix_in - - ## Define 4x4 transformation for virtual rotation around tool-z to orient tool-x and -y - Stc = sin(pre_rot) - Ctc = cos(pre_rot) - Rtc=np.matrix([[ Ctc, -Stc, 0, 0], - [ Stc, Ctc, 0, 0], - [ 0 , 0 , 1, 0], - [ 0, 0 , 0, 1]]) - - ## Define 4x4 transformation for the primary joint - # get the basic 3x3 rotation matrix (returns array) - if joint_letter_primary == 'A': - Rp = Rx(theta_1) - elif joint_letter_primary == 'B': - Rp = Ry(theta_1) - elif joint_letter_primary == 'C': - Rp = Rz(theta_1) - # add fourth column on the right - Rp = np.hstack((Rp, [[0],[0],[0]])) - # expand to 4x4 array and make into a matrix - row_4 = [0,0,0,1] - Rp = np.vstack((Rp, row_4)) - Rp = np.asmatrix(Rp) - - ## Define 4x4 transformation matrix for the secondary joint - # get the basic 3x3 rotation matrix (returns array) - if joint_letter_secondary == 'A': - Rs = Rx(theta_2) - elif joint_letter_secondary == 'B': - Rs = Ry(theta_2) - elif joint_letter_secondary == 'C': - Rs = Rz(theta_2) - # add fourth column on the right - Rs = np.hstack((Rs, [[0],[0],[0]])) - # expand to 4x4 array and make into a matrix - row_4 = [0,0,0,1] - Rs = np.vstack((Rs, row_4)) - Rs = np.asmatrix(Rs) - - if (joint_letter_primary, joint_letter_secondary)== ('C', 'B'): - # Additional definitions for nutating joint - v = radians(hal.get_value(kins_nutation_angle)) - Sv = sin(v) - Cv = cos(v) - Ss = sin(theta_2) - Cs = cos(theta_2) - r = Cs + Sv*Sv*(1-Cs) - s = Cs + Cv*Cv*(1-Cs) - t = Sv*Cv*(1-Cs) - # define rotation matrix for the secondary spindle joint - Rs=np.matrix([[ Cs, -Cv*Ss, Sv*Ss, 0], - [ Cv*Ss, r, t, 0], - [ -Sv*Ss, t, s, 0], - [ 0, 0, 0, 1]]) - - elif (joint_letter_primary, joint_letter_secondary)== ('C', 'A'): - # Additional definitions for nutating joint - v = radians(hal.get_value(kins_nutation_angle)) - Sv = sin(v) - Cv = cos(v) - Ss = sin(theta_2) - Cs = cos(theta_2) - r = Cs + Sv*Sv*(1-Cs) - s = Cs + Cv*Cv*(1-Cs) - t = Sv*Cv*(1-Cs) - # define rotation matrix for the secondary spindle joint - Rs=np.matrix([[ r, -Cv*Ss, t, 0], - [ Cv*Ss, Cs, -Sv*Ss, 0], - [ t, Sv*Ss, s, 0], - [ 0, 0, 0, 1]]) - else: - log.error('No formula for this spindle kinematic (primary, secondary) %s, %s', joint_letter_primary, joint_letter_secondary) - - # calculate the transformation matrix for the forward tool kinematic - matrix_tool_fwd = np.transpose(Rtc)*np.transpose(Rs)*np.transpose(Rp)*T_in - # calculate the transformation matrix for the inverse tool kinematic - matrix_tool_inv = Rp*Rs*Rtc*T_in - if direction == 'fwd': - #log.debug("matrix tool fwd: \n", matrix_tool_fwd) - #log.debug("inv would have been: \n", matrix_tool_inv) - return matrix_tool_fwd - elif direction == 'inv': - #log.debug("matrix tool inv: \n", matrix_tool_inv) - #log.debug("fwd would have been: \n", matrix_tool_fwd) - return matrix_tool_inv - else: - return 0 +# define the basic rotation matrices +def Rx(th): + return np.array([[1, 0 , 0 ], + [0, cos(th), -sin(th)], + [0, sin(th), cos(th)]]) +def Ry(th): + return np.array([[ cos(th), 0, sin(th)], + [ 0 , 1, 0 ], + [-sin(th), 0, cos(th)]]) -# returns angle 'tc' required to rotate the x-axis of the tool-coords parallelto the machine-xy plane -# for given machine joint position angles. -# For G68.3 this is the default tool-x direction -# NOTE: this uses formulas derived from the transformation matrix in the inverse tool kinematic -def kins_calc_tool_rot_c_for_horizontal_x(self, theta_1, theta_2): - global joint_letter_primary, joint_letter_secondary - # The idea is that the tool-x vector is parallel to the machine xy-plane when the - # z component of the x-direction vector is equal to zero - # Mathematically we take the symbolic formula found in row 3, column 1 of the transformation - # matrix from the inverse tool-kinematics, equal that to zero and solve for 'tc'. - # this makes the x orientation of the tool coords horizontal and the user can set the - # rotation from there using g68.3 r - global kins_nutation_angle - v = radians(hal.get_value(kins_nutation_angle)) - Cv = cos(v) - Sv = sin(v) - Cs = cos(theta_2) - Ss = sin(theta_2) - Cp = cos(theta_1) - Sp = sin(theta_1) - if (joint_letter_primary, joint_letter_secondary)== ('C', 'B'): - t = Sv*Cv*(1-Cs) - tc = atan2((Sv*Ss),t) - elif (joint_letter_primary, joint_letter_secondary)== ('C', 'A'): - t = Sv*Cv*(1-Cs) - tc = atan2(-t,(Sv*Ss)) - else: - log.error('No formula for this spindle kinematic (primary, secondary) %s, %s', joint_letter_primary, joint_letter_secondary) - # note: tool-c rotation is done using a halpin that feeds into the kinematic component and the - # vismach model. In contrast to a gcode command where 'c' refers to a physical machine joint) - return tc +def Rz(th): + return np.array([[cos(th), -sin(th), 0], + [sin(th), cos(th), 0], + [0 , 0 , 1]]) -# calculates the secondary joint position for a given tool-vector -# secondary being the joint closest to the tool -# Note: this uses functions derived from the custom kinematic -def kins_calc_secondary(self, tool_z_req): - global joint_letter_primary, joint_letter_secondary - global secondary_min_limit, secondary_max_limit - global kins_nutation_angle - epsilon = 0.000001 - theta_2_list=[] - (Kzx, Kzy, Kzz) = (tool_z_req[0], tool_z_req[1], tool_z_req[2]) - - if (joint_letter_primary, joint_letter_secondary)== ('C', 'B'): - # This kinmatic has infinite results for the vertical tool orientation - # so we explicitly define the angles for that specific case - if Kzz > 1 - epsilon: - return [0] - else: - v = radians(hal.get_value(kins_nutation_angle)) - Sv = sin(v) - Cv = cos(v) - theta_2 = acos((Kzz - Cv*Cv)/(1 - Cv*Cv)) - elif (joint_letter_primary, joint_letter_secondary)== ('C', 'A'): - # This kinmatic has infinite results for the vertical tool orientation - # so we explicitly define the angles for that specific case - if Kzz > 1 - epsilon: - return [0] - else: - v = radians(hal.get_value(kins_nutation_angle)) - Sv = sin(v) - Cv = cos(v) - theta_2 = acos((Kzz - Cv*Cv)/(1 - Cv*Cv)) - else: - log.error('No formula for this spindle kinematic (primary, secondary) %s', (joint_letter_primary, joint_letter_secondary)) - # since we are using acos() we really have two solutions theta_2 and -theta_2 - for theta in [theta_2, -theta_2]: - log.debug('Checking if result %s is within secondary joint limits of %s and %s.', - degrees(theta), secondary_min_limit, secondary_max_limit) - if theta > secondary_min_limit and theta < secondary_max_limit: - log.debug('Adding %s to valid angles list.', degrees(theta)) - theta_2_list.append(theta) - log.debug('List of possible secondary angles: %s\n', theta_2_list) - return theta_2_list - - -# calculates the primary joint position for a given tool-vector -# Note: this uses functions derived from the custom kinematic -def kins_calc_primary(self, tool_z_req, theta_2_list): - global joint_letter_primary, joint_letter_secondary - global primary_min_limit, primary_max_limit - global kins_nutation_angle - epsilon = 0.000001 - theta_1_list=[] - (Kzx, Kzy, Kzz) = (tool_z_req[0], tool_z_req[1], tool_z_req[2]) - if (joint_letter_primary, joint_letter_secondary)== ('C', 'B'): - # This kinmatic has infinite results for the vertical tool orientation - # so we explicitly define the angles for that specific case - if Kzz > 1 - epsilon: - return [0] - else: - v = radians(hal.get_value(kins_nutation_angle)) - Sv = sin(v) - Cv = cos(v) - for i in range(len(theta_2_list)): - theta_2 = theta_2_list[i] - Ss = sin(theta_2) - Cs = cos(theta_2) - t = Sv*Cv*(1-Cs) - p = Sv * Ss - - theta_1 = asin((p*Kzy - t*Kzx)/(t*t + p*p)) - elif (joint_letter_primary, joint_letter_secondary)== ('C', 'A'): - # This kinmatic has infinite results for the vertical tool orientation - # so we explicitly define the angles for that specific case - if Kzz > 1 - epsilon: - return [0] - else: - v = radians(hal.get_value(kins_nutation_angle)) - Sv = sin(v) - Cv = cos(v) - for i in range(len(theta_2_list)): - theta_2 = theta_2_list[i] - Ss = sin(theta_2) - Cs = cos(theta_2) - t = Sv*Cv*(1-Cs) - p = Sv * Ss - q = (t*Kzy - p*Kzx)/(t*t + p*p) - theta_1 = asin(q) - else: - log.error('No formula for this spindle kinematic (primary, secondary) %s', (joint_letter_primary, joint_letter_secondary)) - # since we are using asin() we really have two solutions theta_1 and pi-theta_2 - for theta in [theta_1, transform_to_pipi(pi - theta_1)]: - log.debug('Checking if result %s is within secondary joint limits of %s and %s.', - degrees(theta), secondary_min_limit, secondary_max_limit) - if theta > secondary_min_limit and theta < secondary_max_limit: - log.debug('Adding %s to valid angles list.', degrees(theta)) - theta_1_list.append(theta) - log.debug('List of possible secondary angles: %s\n', theta_2_list) - return theta_1_list - - -# this is from 'mika-s.github.io' -# transforms a given angle to the interval of [-pi,pi] -def transform_to_pipi(input_angle): - revolutions = int((input_angle + np.sign(input_angle) * pi) / (2 * pi)) - p1 = truncated_remainder(input_angle + np.sign(input_angle) * pi, 2 * pi) - p2 = (np.sign(np.sign(input_angle) - + 2 * (np.sign(fabs((truncated_remainder(input_angle + pi, 2 * pi)) / (2 * pi))) - 1))) * pi - output_angle = p1 - p2 - return output_angle - - -# this is from 'mika-s.github.io' -# used by 'transform_to_pipi()' -def truncated_remainder(dividend, divisor): - divided_number = dividend / divisor - divided_number = -int(-divided_number) if divided_number < 0 else int(divided_number) - remainder = dividend - divisor * divided_number - return remainder - - -# returns a list of valid primary/secondary spindle joint positions for a given tool-orientation vector -# or 'None','None' if no valid position could be found -def kins_calc_jnt_angles(self, tool_z_req): - log.debug('tool_z_requested: %s', tool_z_req) + +def calc_euler_rot_matrix(th1, th2, th3, order): # expects radians + # returns the rotation matrices for given order and angles + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + debug_msg = (f' Euler order {order} requested with angles: ' + f'{degrees(th1):.4f}, {degrees(th2):.4f}, {degrees(th3):.4f}') + log.debug(debug_msg) + if order == '131': + matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Rx(th3)) + elif order=='121': + matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rx(th3)) + elif order=='212': + matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Ry(th3)) + elif order=='232': + matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Ry(th3)) + elif order=='323': + matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rz(th3)) + elif order=='313': + matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Rz(th3)) + elif order=='123': + matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rz(th3)) + elif order=='132': + matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Ry(th3)) + elif order=='213': + matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Rz(th3)) + elif order=='231': + matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Rx(th3)) + elif order=='321': + matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rx(th3)) + elif order=='312': + matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Ry(th3)) + #log.debug(' Returning euler rotation as matrix: \n %s', matrix) + return matrix + + +def calc_joint_angles(z_vector_req, x_vector_req): + # returns a list of valid primary/secondary rotary joint positions in radians for a given orientation vector + # returns an empty list if no valid position could be found + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + log.debug(' z_vector_requested: %s', z_vector_req) + log.debug(' x_vector_requested: %s', x_vector_req) # set the tolerance value epsilon = 0.0001 # create np.array so we can easily calculate differences and check elements - tool_z_req = np.array([tool_z_req[0], tool_z_req[1], tool_z_req[2]]) - # calculate secondary joint values using kinematic specific formula - theta_2_pair = kins_calc_secondary(self, tool_z_req) - # calculate primary joint values using kinematic specific formula - theta_1_pair = kins_calc_primary(self, tool_z_req, theta_2_pair) + z_vector_req = np.array([z_vector_req[0], z_vector_req[1], z_vector_req[2]]) + # calculate joint values using kinematic specific formula + try: + (theta_1_calcd, theta_2_calcd) = kins_calc_possible_joint_angles(log, z_vector_req, x_vector_req) + except Exception as error: + log.error('Remap_funcs: kins_calc_possible_joint_angles failure, %s', error) + + # remove any duplicate values from the results + theta_1_calcd = tuple(set(theta_1_calcd)) + theta_2_calcd = tuple(set(theta_2_calcd)) + log.debug(' Got possible angles theta_1: ' + ' '.join("{:.4f}°".format(degrees(theta)) for theta in theta_1_calcd)) + log.debug(' Got possible angles theta_2: ' + ' '.join("{:.4f}°".format(degrees(theta)) for theta in theta_2_calcd)) + if theta_1_calcd == None or theta_2_calcd == None: + return [] + angle_pairs_list = [] + # create a list of paired combinations of returned angles (theta_1 , theta_2) + for i in range(len(theta_1_calcd)): + for j in range(len(theta_2_calcd)): + angle_pairs_list.append((theta_1_calcd[i], theta_2_calcd[j])) + angle_pairs_list = list(set(angle_pairs_list)) + # iterate through the list and check if a particular pair actually produces the requested z-vector orientation joint_angles_list = [] - # iterate through all the possible combinations of (theta_1 , theta_2) - for i in range(len(theta_1_pair)): - for j in range(len(theta_2_pair)): - # rotate an identity matrix using the custom tool kinematic model and the (theta_1, theta_2) - matrix_in = np.asmatrix(np.identity(4)) - t_out = kins_tool_transformation(theta_1_pair[i], theta_2_pair[j], 0, matrix_in,'inv') - # the resulting tool-z vector for this pair of (theta_1, theta_2) is found in the third column - tool_z_would_be = np.array([t_out[0,2], t_out[1,2], t_out[2,2]]) - log.debug('tool_z_would_be: %s', tool_z_would_be) - # calculate the difference of the respective elements - tool_z_diff = tool_z_req - tool_z_would_be - # and check if all elements are within [-epsilon,epsilon] - match = np.all((tool_z_diff > -epsilon) & (tool_z_diff < epsilon)) - log.debug('Is the tool-Z-vector close enough ? %s', match) - if match: - # check if we already have this particular pair in the list - if not (theta_1_pair[i], theta_2_pair[j]) in joint_angles_list: - log.debug('Appending (theta_1_pair, theta_2_pair) %s', (degrees(theta_1_pair[i]), degrees(theta_2_pair[j]))) - joint_angles_list.append((theta_1_pair[i], theta_2_pair[j])) - log.info('Found valid joint angles: %s', joint_angles_list) - if joint_angles_list: - return joint_angles_list - #return joint_angles_list[-1] - else: - return None, None + for i in range(len(angle_pairs_list)): + debug_msg = (f' Checking angle pair {i}: ({angle_pairs_list[i][0]:.4f}, {angle_pairs_list[i][1]:.4f}) ' + f'({degrees(angle_pairs_list[i][0]):.4f}°, {degrees(angle_pairs_list[i][1]):.4f}°)') + log.debug(debug_msg) + # we start with an identity matrix (ie oriented to world) + matrix_in = np.asmatrix(np.identity(4)) + try: + direction = kins_calc_transformation_get_direction() + except Exception as error: + log.error('kins_calc_transformation_get_direction, %s', error) + try: + matrix_out = kins_calc_transformation_matrix(angle_pairs_list[i][0], angle_pairs_list[i][1], 0, matrix_in, direction) + except Exception as error: + log.error('kins_calc_transformation_matrix, %s', error) + # the resulting z-vector for this pair of (theta_1, theta_2) is found in the third column + z_vector_would_be = np.array([matrix_out[0,2], matrix_out[1,2], matrix_out[2,2]]) + # calculate the difference of the respective elements + z_vector_diff = z_vector_req - z_vector_would_be + log.debug(' z_vector_diff: %s', z_vector_diff) + # and check if all elements are within [-epsilon,epsilon] + match_z = np.all((z_vector_diff > -epsilon) & (z_vector_diff < epsilon)) + log.debug(' Is the z-vector-vector close enough ? %s', match_z) + if match_z: + joint_angles_list.append((angle_pairs_list[i][0], angle_pairs_list[i][1])) + for (theta_1, theta_2) in joint_angles_list: + log.debug(f'Returning valid joint angles found: {degrees(theta_1):.4f}°, {degrees(theta_2):.4f}°') + return joint_angles_list # returns radians + def calc_shortest_distance(pos, trgt, mode): - # calculate the shortest distance in [-180°, 180°] - # eg if pos=170° and trgt=-170° then dist will be 20° - # If the operator requests positive or negative rotation - # we may need to return the long distance instead - log.debug('Got (pos, trgt): %s', (pos, trgt)) + pos = degrees(pos) + trgt = degrees(trgt) + # calculate the shortest distance in [-180°, 180°] eg if pos=170° and trgt=-170° then dist will be 20° + # If the operator requests positive or negative rotation we may need to return the long distance instead + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) dist_short = (trgt - pos + 180) % 360 - 180 # calculate short and long distance if dist_short >= 0: # ie dist_long should be negative dist_long = -(360 - dist_short) else: dist_long = 360 + dist_short - log.debug('Calculated (dist_short, dist_long): %s', (dist_short, dist_long)) + log.debug(f' Calculated dist_short: {dist_short:.4f}°, dist_long: {dist_long:.4f}°') if mode == 1: # positive rotation only, ie we want a positive distance if dist_short >= 0: # ie we want this one dist = dist_short else: # ie we need to go the other way dist = dist_long - if mode == 2: # negative rotation only ie we want a positive distance - if dist_short >= 0: # ie we need to go the other way + elif mode == 2: # negative rotation only ie we want a positive distance + if dist_short > 0: # ie we need to go the other way dist = dist_long else: # ie we want this one dist = dist_short else: # mode = 0 ie we want the shortest distance either way dist = dist_short - log.debug('Distance returned: %s', dist) - return dist + log.debug(f'Returning distance: {dist:.4f}°') + return radians(dist) -# this takes a target angle in [-pi,pi] and finds the closest move within [min_limit, max_limit] -# from a given position in [min_limit, max_limit], returns the optimized target angle and the distance -# from the given position to that target angle -def calc_rotary_move_with_joint_limits(position, target, max_limit, min_limit, mode): - pos = degrees(position) - trgt = degrees(target) - log.debug('(Current_pos, target): %s', (pos, trgt)) +def calc_rotary_move_with_joint_limits(pos, trgt, max_limit, min_limit, mode): # expects radians + # this takes a target angle in [-pi,pi] and finds the closest move within [min_limit, max_limit] + # from a given position in [min_limit, max_limit], returns the optimized target angle and the distance + # from the given position to that target angle + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + log.debug(f' Current position: {degrees(pos):.4f}°, target position: {degrees(trgt):.4f}°') # calculate the shortest distance from position to target for the strategy given by # the operator (ie shortest (= default), positive rotation only, negative rotation only ) dist = calc_shortest_distance(pos, trgt, mode) # check that the result is within the rotary axis limits defined in the ini file if dist >= 0: # shortest way is in the positive direction if (pos + dist) <= max_limit: # if the limits allow we rotate the joint in the positive sense - log.debug('Max_limit OK, target changed to: %s', (pos + dist)) + log.debug(f' Max_limit OK, setting target to: {degrees(pos + dist):.4f}°') theta = pos + dist - else: # if positive limits would be exceeded we need to go the longer wey in the other direction + else: # if positive limits would be exceeded we need to go the longer way in the other direction + log.debug(f' Maximum axis limit of {degrees(max_limit):.4f} would be violated.') if mode == 0: - log.debug('Max_limit reached, target remains: %s', trgt) + dist = dist - 2*pi + log.debug(f' Changing target to: {degrees(trgt):.4f}°, distance to: {degrees(dist):.4f}°') theta = trgt else: # if the rotation direction was set by the operator then we can not change direction + log.debug(f' Unable to change direction because orient mode is set to {mode:.0f}.\n') theta = None - dist = None else: # shortest way is in the negative direction if (pos + dist) >= min_limit: # if the limits allow we rotate the joint in the negative sense - log.debug('Min_limit OK, target changed to: %s', (pos + dist)) + log.debug(f' Min_limit OK, setting target to: {degrees(pos + dist):.4f}°') theta = pos + dist else: # if negative limits would be exceeded we need to go the longer way int the other direction + log.debug(f' Minimum axis limit of {degrees(min_limit):.4f} would be violated.') if mode == 0: - log.debug('Min_limit reached, target remains: %s', trgt) + dist = dist + 2*pi + log.debug(f' Changing target to: {degrees(trgt):.4f}°, distance to: {degrees(dist):.4f}°') theta = trgt else: # if the rotation direction was set by the operator then we can not change direction + log.debug(f' Unable to change direction because orient mode is set to {mode:.0f}.\n') theta = None - dist = None + if theta is not None: + log.debug(f'Returning: angle {degrees(theta):.4f}° with distance {degrees(dist):.4f}° for requested mode {mode:.0f}\n') # we also attach the distance for this particular move and mode - log.debug('Angle and distance returned: %s, %s', theta, dist) - return theta, dist + return theta, dist # returns radians -# this takes a list of joint angle pairs in [-pi,pi] and optimizes them for shortest moves -# in (min_limit, max_linit) from the current joint positions using the orient_mode set by -# the operator: 0=shortest (default), 1=positive rotation only, 2=negative rotation only -def calc_angle_pairs_and_distances(self, possible_prim_sec_angle_pairs): +def calc_angle_pairs_and_distances(self, possible_prim_sec_angle_pairs): # expects radians + # this takes a list of joint angle pairs in [-pi,pi] and optimizes them for shortest moves + # in (min_limit, max_linit) from the current joint positions using the orient_mode set by + # the operator: 0=shortest (default), 1=positive rotation only, 2=negative rotation only + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) global primary_min_limit, primary_max_limit, secondary_min_limit, secondary_max_limit global orient_mode # get the current joint positions - prim_pos, sec_pos = get_current_rotary_positions(self) + prim_pos, sec_pos = get_current_rotary_positions(self) # returns radians # we want to return a list of angles that are optimized for the orient_mode and the # rotary axes limits as set in the ini file target_dist_list= [] for prim_trgt, sec_trgt in possible_prim_sec_angle_pairs: - # primary joint, here we apply the orient mode requested by the operator + # For the priortized joint we apply the orient mode requested by the operator + # the other we optimize for shortest move + if optimization_priority == 2: + primary_strategy = 0 + secondary_strategy = orient_mode + else: + primary_strategy = orient_mode + secondary_strategy = 0 + # primary joint prim_move, prim_dist = calc_rotary_move_with_joint_limits(prim_pos, prim_trgt, primary_max_limit, primary_min_limit, - orient_mode) - # secondary joint, here we want the shortest move (although we could also apply a strategy here) + primary_strategy) + # secondary joint sec_move, sec_dist = calc_rotary_move_with_joint_limits(sec_pos, sec_trgt, secondary_max_limit, secondary_min_limit, - 0) + secondary_strategy) # if a solution has been found for this particular pair then we add it to the list if not (prim_move == None) and not (sec_move == None): target_dist_list.append(((prim_move, sec_move),(prim_dist, sec_dist))) - log.debug('Assembled target_dist_list: %s',target_dist_list) - return target_dist_list + for ((prim_move, sec_move),(prim_dist, sec_dist)) in target_dist_list: + debug_msg = (f'Returning prim_move: {degrees(prim_move):.4f}°, sec_move: {degrees(sec_move):.4f}°, ' + f'prim_dist: {degrees(prim_dist):.4f}°, sec_dist: {degrees(sec_dist):.4f}°') + log.debug(debug_msg) + return target_dist_list # returns radians -# find the optimal joint move from current to target positions in the list -# for this we look at the primary joint move only -# orient_mode is 0=shortest, 1=positive rotation only, 2=negative rotation only -# For orient_mode=(1,2): If no move can be found within joint limits we return None def calc_optimal_joint_move(self, possible_prim_sec_angle_pairs): + # find the optimal joint move from current to target positions in the list + # orient_mode is 0=shortest, 1=positive rotation only, 2=negative rotation only + # For orient_mode=(1,2): If no move can be found within joint limits we return None + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) global orient_mode # this returns a list with all moves ((prim_move, sec_move),(prim_dist, sec_dist)) that # will result in correct tool orientation, stay within the rotary axis limits and respect the # orient_mode if set by the operator valid_joint_moves_and_distances = calc_angle_pairs_and_distances(self, possible_prim_sec_angle_pairs) + if len(valid_joint_moves_and_distances) < 1: + log.error(f' No valid joint moves found.') + return (None, None) # now we need to pick and return the (primary angle, secondary angle) that results in the - # shortest move of the primary joint + # shortest move of the prioritized joint (theta_1, theta_2) = (None, None) - dist = 3600 + joint = optimization_priority - 1 + dist = 10 # some large initial value for trgt_angles, dists in valid_joint_moves_and_distances: - if orient_mode == 0 and fabs(dists[0]) < fabs(dist): # shortest move requested + if orient_mode == 0 and fabs(dists[joint]) < fabs(dist): # shortest move requested (theta_1, theta_2) = trgt_angles dist = dists[0] - elif orient_mode == 1 and fabs(dists[0]) < fabs(dist) and dists[0] >= 0: # positive primary rotation only + elif orient_mode == 1 and fabs(dists[joint]) < fabs(dist) and dists[joint] >= 0: # positive primary rotation only (theta_1, theta_2) = trgt_angles dist = dists[0] - elif orient_mode == 2 and fabs(dists[0]) < fabs(dist) and dists[0] <= 0: # negative primary rotation only + elif orient_mode == 2 and fabs(dists[joint]) < fabs(dist) and dists[joint] <= 0: # negative primary rotation only (theta_1, theta_2) = trgt_angles dist = dists[0] - log.debug('Shortest move selected for (orient_mode, theta_1, theta_2): %s', (orient_mode, theta_1, theta_2)) - return theta_1, theta_2 - - -# calculates the required pre-rotation around tool-z so the tool-x matches the requested -# orientation after rotation of the spindle joints -def kins_calc_pre_rot(self, theta_1, theta_2, tool_x_req, tool_z_requested): - # tolerance setting for check if tool-x-vector needs to be rotated at all + if theta_1 is not None: + debug_msg = (f'Returning shortest move selected for orient_mode {orient_mode:.0f}: ' + f'primary: {degrees(theta_1):.4f}°, secondary: {degrees(theta_2):.4f}°\n') + log.debug(debug_msg) + return theta_1, theta_2 # returns radians + + +def calc_virtual_rotation(theta_1, theta_2, x_vector_req, z_vector_req, matrix_in, direction): # expects radians + # calculates a required virtual-rotation around tool- or work-z so the x-vector matches the requested + # orientation after rotation + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + # tolerance setting for check if x-vector-vector needs to be rotated at all epsilon = 0.00000001 - log.info("Tool-x-requested: %s", tool_x_req) - # we need to calculate the current tool-x vector with the given rotations using - # the transformation matrix from our custom tool kinematic - log.debug("joint angles (secondary, primary) in radians given: %s", (theta_2, theta_1)) - log.debug("joint angles (secondary, primary) in degrees given: %s", (theta_2*180/pi, theta_1*180/pi)) - # run the identity matrix through the tool kinematic transformation in the requested direction - # using the given joint angles and pre-rotation zero - matrix_in = np.asmatrix(np.identity(4)) - t_out = kins_tool_transformation(theta_1, theta_2, 0, matrix_in,'inv') - # the tool-x vector for the given machine joint rotations is found directly in the first column - tool_x_is = [t_out[0,0], t_out[1,0], t_out[2,0]] - log.debug("tool-x after machine rotation would be: %s", tool_x_is) - # we calculate the angular difference between the two vectors so we can 'pre-rotate' - # around tool-z to get the requested tool-x vector after machine rotation + log.info(" x-vector-requested: %s", x_vector_req) + debug_msg = (f' got joint angles: primary {theta_1:.4f} {degrees(theta_1):.4f}°, ' + f'secondary {theta_2:.4f}° {degrees(theta_2):.4f}°') + log.debug(debug_msg) + # run matrix_in through the kinematic transformation in the requested direction + # using the given joint angles and zero virtual-rotation + try: + matrix_out = kins_calc_transformation_matrix(theta_1, theta_2, 0, matrix_in, direction) + except Exception as error: + log.error('calc_virtual_rotation, %s', error) + # the x-vector for the given machine joint rotations is found directly in the first column + x_vector_is = [matrix_out[0,0], matrix_out[1,0], matrix_out[2,0]] + log.debug(" X-vector after machine rotation would be: %s", x_vector_is) + # we calculate the angular difference between the two vectors so we can add a virtual rotation + # around z-vector or work-z to match the requested x orientation after machine rotation # just to be sure we normalize the two vectors - tool_x_is = tool_x_is / np.linalg.norm(tool_x_is) - tool_x_req = tool_x_req / np.linalg.norm(tool_x_req) + x_vector_is = x_vector_is / np.linalg.norm(x_vector_is) + x_vector_req = x_vector_req / np.linalg.norm(x_vector_req) # check if the x-vector is already in the required orientation (ie parallel) - log.debug("check if vectors are parallel: %s", np.dot(tool_x_is,tool_x_req)) - if np.dot(tool_x_is,tool_x_req) > 1 - epsilon: - log.info("Tool x-vector already oriented, setting pre-rotation = 0") - # if we are already parallel then we don't need to pre-rotate - pre_rot = 0 + log.debug(" checking if vectors are parallel: %s", np.dot(x_vector_is,x_vector_req)) + if np.dot(x_vector_is, x_vector_req) > 1 - epsilon: + log.info(" X-vector already oriented, setting virtual-rotation = 0") + # if we are already parallel then we don't need to add a virtual rotation + virtual_rot = 0 else: # we can use the cross product to determine the direction we need to rotate - cross = np.cross(tool_x_req, tool_x_is) - log.debug("cross product (tool_x_req, tool_x_is): %s", cross) - log.info("Tool_z_requested: %s", tool_z_requested) - pre_rot = np.arccos(np.dot(tool_x_req, tool_x_is)) - log.debug('base pre_rot: %s', pre_rot) + cross = np.cross(x_vector_req, x_vector_is) + log.debug(" cross product (x_vector_req, x_vector_is): %s", cross) + virtual_rot = np.arccos(np.dot(x_vector_req, x_vector_is)) + log.debug(f' raw virtual_rot: {virtual_rot:.4f} {degrees(virtual_rot):.4f}°') # To find out which quadrant we need the angle to be in we create a list of them all - pre_rot_list = [pre_rot, -pre_rot, 2*pi-pre_rot, -(2*pi-pre_rot)] - log.debug('pre_rot_list: %s',pre_rot_list) - # then we run all of them through the kinematic model and see which gives us - # the requested tool-x-vector - for pre_rot in pre_rot_list: + virtual_rot_list = [virtual_rot, -virtual_rot, 2*pi-virtual_rot, -(2*pi-virtual_rot)] + log.debug(' Got possible virtual_rot angles: ' + ' '.join("{:.4f}°".format(degrees(angle)) for angle in virtual_rot_list)) + # then we run all of them through the kinematic model and see which gives us the requested x-vector-vector + for virtual_rot in virtual_rot_list: + log.debug(f' Checking virtual_rot = {degrees(virtual_rot):.4f}°') zeta = 0.0001 - # run the identity matrix through the tool kinematic transformation in the requested direction - # using the given joint angles and pre-rotation angle in the list - matrix_in = np.asmatrix(np.identity(4)) - t_out = kins_tool_transformation(theta_1, theta_2, pre_rot, matrix_in,'inv') - # the tool-x vector for the given primary and secondary rotations is found directly in the first column - tool_x_would_be = [t_out[0,0], t_out[1,0], t_out[2,0]] - log.debug('tool_x_would_be: %s', tool_x_would_be) + # run the identity matrix through the kinematic transformation in the requested direction + # using the given joint angles and virtual-rotation angle in the list + try: + matrix_out = kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, matrix_in, direction) + except Exception as error: + log.error('calc_virtual_rotation, %s', error) + # the oriented x-vector is found directly in the first column + x_vector_would_be = [matrix_out[0,0], matrix_out[1,0], matrix_out[2,0]] + log.debug(' x_vector_would_be: %s', x_vector_would_be) # calculate the difference of the respective elements - tool_x_diff = tool_x_req - tool_x_would_be + x_vector_diff = x_vector_req - x_vector_would_be # and check if all elements are within [-epsilon,epsilon] - match = np.all((tool_x_diff > -zeta) & (tool_x_diff < zeta)) - log.debug('Is the tool-X-vector close enough ? %s', match) + match = np.all((x_vector_diff > -zeta) & (x_vector_diff < zeta)) + log.debug(' Is the X-vector close enough ? %s', match) if match: # if we have a match we leave the loop and use this angle break - log.info("Pre-rotation calculated [deg]: %s", degrees(pre_rot)) - # return pre_rot in radians - return pre_rot - - -# transforms a 4x4 input matrix using the current tool transformation matrix -# (forward or inverse) using the kinematic model of the machine -def kins_calc_tool_transformation(self, matrix_in, theta_1=None, theta_2=None, pre_rot=None, direction='fwd'): - global kins_pre_rotation - # if no angle values have been passed we get the current joint positions - if theta_2 == None or theta_1 == None: - # read current spindle rotary angles and convert to radians - theta_1, theta_2 = get_current_rotary_positions(self) - else: - log.debug("got for secondary joint: %s", theta_2) - log.debug("got for primary joint: %s", theta_1) - # pre-rot is the virtual rotary axis around the tool-z axis to align the tool-x axis - # if no pre-rot angle is passed then we use the currently active value - if pre_rot == None: - pre_rot = hal.get_value(kins_pre_rotation ) - log.debug("current pre-rot: %s", pre_rot) - else: - log.debug("requested pre-rot value [DEG]): %s", degrees(pre_rot)) - # run the input matrix through the tool kinematic transformation in the requested direction - # using the current joint angles and pre-rotation as requested - matrix_out = kins_tool_transformation(theta_1, theta_2, pre_rot, matrix_in, direction) - return matrix_out - - -# define the basic rotation matrices, used for euler twp modes -def Rx(th): - return np.array([[1, 0 , 0 ], - [0, cos(th), -sin(th)], - [0, sin(th), cos(th)]]) - -def Ry(th): - return np.array([[ cos(th), 0, sin(th)], - [ 0 , 1, 0 ], - [-sin(th), 0, cos(th)]]) - -def Rz(th): - return np.array([[cos(th), -sin(th), 0], - [sin(th), cos(th), 0], - [0 , 0 , 1]]) + log.info(f'Returning virtual-rotation calculated {degrees(virtual_rot):.4f}°') + return virtual_rot # returns radians -# returns the rotation matrices for given order and angles -def twp_calc_euler_rot_matrix(th1, th2, th3, order): - log.debug("euler order requested: %s", order) - log.debug("angles given (th1, th2 , th3): %s", (th1, th2, th3)) - th1 = radians(th1) - th2 = radians(th2) - th3 = radians(th3) - if order == '131': - matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Rx(th3)) - elif order=='121': - matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rx(th3)) - elif order=='212': - matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Ry(th3)) - elif order=='232': - matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Ry(th3)) - elif order=='323': - matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rz(th3)) - elif order=='313': - matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Rz(th3)) - elif order=='123': - matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rz(th3)) - elif order=='132': - matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Ry(th3)) - elif order=='213': - matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Rz(th3)) - elif order=='231': - matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Rx(th3)) - elif order=='321': - matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rx(th3)) - elif order=='312': - matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Ry(th3)) - log.debug('euler rotation as matrix: \n %s', matrix) - return matrix +def calc_twp_matrix_from_joint_position(self, matrix_in, virtual_rot, direction): # expects radians + # transforms a 4x4 input matrix using the current transformation matrix + # (forward or inverse) using the kinematic model of the machine + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + global kins_virtual_rotation + # read current spindle rotary angles (radians) + theta_1, theta_2 = get_current_rotary_positions(self) + # virtual-rot is the virtual rotary axis around the z-vector or work-z axis to align the x-vector + log.debug(f" requested virtual-rot value {degrees(virtual_rot):.4f}°") + # run matrix_in through the kinematic transformation in the requested direction + # using the current joint angles and virtual-rotation as requested + try: + twp_matrix = kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, matrix_in, direction) + except Exception as error: + log.error('calc_twp_matrix_from_joint_position, %s', error) + return twp_matrix -# The tilted-work-plane is created in identity mode and must NOT be updated after a switch -def gui_update_twp(self): +def gui_update_twp(): + # The tilted-work-plane is created in identity mode and must NOT be updated after a switch + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) global twp_matrix, saved_work_offset # twp origin as vector (in world coords) from current work-offset to the origin of the twp - hal.set_p("twp-helper-comp.twp-ox-in",str(twp_matrix[0,3])) - hal.set_p("twp-helper-comp.twp-oy-in",str(twp_matrix[1,3])) - hal.set_p("twp-helper-comp.twp-oz-in",str(twp_matrix[2,3])) - # twp x-vector - hal.set_p("twp-helper-comp.twp-xx-in",str(twp_matrix[0,0])) - hal.set_p("twp-helper-comp.twp-xy-in",str(twp_matrix[1,0])) - hal.set_p("twp-helper-comp.twp-xz-in",str(twp_matrix[2,0])) - # twp z-vector - hal.set_p("twp-helper-comp.twp-zx-in",str(twp_matrix[0,2])) - hal.set_p("twp-helper-comp.twp-zy-in",str(twp_matrix[1,2])) - hal.set_p("twp-helper-comp.twp-zz-in",str(twp_matrix[2,2])) + try: + hal.set_p("twp-helper-comp.twp-ox-in",str(twp_matrix[0,3])) + hal.set_p("twp-helper-comp.twp-oy-in",str(twp_matrix[1,3])) + hal.set_p("twp-helper-comp.twp-oz-in",str(twp_matrix[2,3])) + # twp x-vector + hal.set_p("twp-helper-comp.twp-xx-in",str(twp_matrix[0,0])) + hal.set_p("twp-helper-comp.twp-xy-in",str(twp_matrix[1,0])) + hal.set_p("twp-helper-comp.twp-xz-in",str(twp_matrix[2,0])) + # twp z-vector + hal.set_p("twp-helper-comp.twp-zx-in",str(twp_matrix[0,2])) + hal.set_p("twp-helper-comp.twp-zy-in",str(twp_matrix[1,2])) + hal.set_p("twp-helper-comp.twp-zz-in",str(twp_matrix[2,2])) + except Exception as error: + log.error('gui_update_twp failed, %s', error) # publish the twp offset coordinates in world coordinates (ie identity) [work_offset_x, work_offset_y, work_offset_z] = saved_work_offset - log.debug("Setting work_offsets in the simulation: %s", (work_offset_x, work_offset_y, work_offset_z)) + log.debug(" Setting work_offsets in the simulation: %s", (work_offset_x, work_offset_y, work_offset_z)) # this is used to translate the rotated twp to the correct position # care must be taken that only the work_offsets in identity mode are sent as that is - # what the model uses. The visuals for the offsets are created then rotated according to - # the rotary joint position and then translated. + # what the model uses. The visuals for the offsets are created in the origin, + # then rotated according to the rotary joint position and then translated. # The twp has to be rotated out of the machine xy plane using the g68.2 parameters and is then # translated by the offset values of the identity mode. - hal.set_p("twp-helper-comp.twp-ox-world-in",str(work_offset_x)) - hal.set_p("twp-helper-comp.twp-oy-world-in",str(work_offset_y)) - hal.set_p("twp-helper-comp.twp-oz-world-in",str(work_offset_z)) + try: + hal.set_p("twp-helper-comp.twp-ox-world-in",str(work_offset_x)) + hal.set_p("twp-helper-comp.twp-oy-world-in",str(work_offset_y)) + hal.set_p("twp-helper-comp.twp-oz-world-in",str(work_offset_z)) + except Exception as error: + log.error('gui_update_twp failed, %s', error) # NOTE: Due to easier abort handling we currently restrict the use of twp to G54 # as LinuxCNC seems to revert to G54 as the default system def get_current_work_offset(self): + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) # get which offset is active (g54=1 .. g59.3=9) active_offset = int(self.params[5220]) current_work_offset_number = active_offset @@ -702,11 +530,12 @@ def get_current_work_offset(self): co_x = self.params[work_offset_x] co_y = self.params[work_offset_y] co_z = self.params[work_offset_z] - current_work_offset = [co_x, co_y, co_z] + current_work_offset = (co_x, co_y, co_z) return [current_work_offset_number, current_work_offset] def get_current_rotary_positions(self): + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) global joint_letter_primary, joint_letter_secondary if joint_letter_primary == 'A': theta_1 = radians(self.AA_current) @@ -714,7 +543,7 @@ def get_current_rotary_positions(self): theta_1 = radians(self.BB_current) elif joint_letter_primary == 'C': theta_1 = radians(self.CC_current) - log.debug('Current position Primary joint: %s', degrees(theta_1)) + log.debug(f' Current position Primary joint: {degrees(theta_1):.4f}°') # read current spindle rotary angles and convert to radians if joint_letter_secondary == 'A': theta_2 = radians(self.AA_current) @@ -722,45 +551,32 @@ def get_current_rotary_positions(self): theta_2 = radians(self.BB_current) elif joint_letter_secondary == 'C': theta_2 = radians(self.CC_current) - log.debug('Current position Secondary joint: %s', degrees(theta_2)) + log.debug(f' Current position Secondary joint: {degrees(theta_2):.4f}°') return theta_1, theta_2 -# forms a 4x4 transformation matrix from a given 1x3 point vector [x,y,z] -def point_to_matrix(point): - # start with a 4x4 identity matrix and add the point vector to the 4th column - matrix = np.identity(4) - [matrix[0,3], matrix[1,3], matrix[2,3]] = point - matrix = np.asmatrix(matrix) - return matrix - - -# extracts the point vector form a given 4x4 transformation matrix -def matrix_to_point(matrix): - point = (matrix[0,3],matrix[1,3],matrix[2,3]) - return point - - -def reset_twp_params(self): - global pre_rot, twp_matrix, twp_flag, twp_build_params - pre_rot = 0 +def reset_twp_params(): + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + global virtual_rot, twp_matrix, twp_flag, twp_build_params + virtual_rot = 0 # we must not change tool kins parameters when TOOL kins are active or we get sudden joint position changes - # ie don't do this: kins_comp_set_pre_rot(self,0)! + # ie don't do this: kins_comp_set_virtual_rot(0)! twp_flag = [] twp_build_params = {} - log.info("Resetting TWP-matrix") + log.info(" Resetting TWP-matrix") twp_matrix = np.asmatrix(np.identity(4)) -# Orient the tool to the current twp (with TCP for G53.1 or IDENTITY for G53.6) -# (some controllers offer an optional P-word to give preferred rotation directions this is not implemented yet) -# Note: To avoid that this python code is run prematurely by the read ahead we need a quebuster at the beginning but -# because we need self.execute() to switch the WCS properly this remap needs to be called from -# an ngc reamp that contains a quebuster before calling this code -# IMPORTANT: -# The correct kinematic mode (ie TCP for 53.1 / IDENTITY for G53.6) must be active when this code is called -# (ie do it in the ngc remap mentioned above!) -def g53x_core(self): - global saved_work_offset, twp_matrix, twp_flag, pre_rot + +def g53n_core(self): + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + # Orient the tool to the current twp (with TCP for G53.1 or IDENTITY for G53.6) + # Note: To avoid that this python code is run prematurely by the read ahead we need a quebuster at the + # beginning but because we need self.execute() to switch the WCS properly this remap needs to be called from + # an ngc reamp that contains a quebuster before calling this code. + # IMPORTANT: + # The correct kinematic mode (ie TCP for 53.1 / IDENTITY for G53.6) must be active when this code is called + # (ie do it in the ngc remap mentioned above!) + global saved_work_offset, twp_matrix, twp_flag, virtual_rot global joint_letter_primary, joint_letter_secondary, twp_error_status global orient_mode if self.task == 0: # ignore the preview interpreter @@ -769,112 +585,142 @@ def g53x_core(self): if not hal.get_value(twp_is_defined): # reset the twp parameters - reset_twp_params(self) - msg = "G53.x: No TWP defined." - log.debug(msg) + reset_twp_params() + msg = "G53.n: No TWP defined." + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - elif hal.get_value(twp_is_active): # reset the twp parameters - reset_twp_params(self) - msg = "G53.x: TWP already active" - log.debug(msg) + reset_twp_params() + msg = "G53.n: TWP already active" + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - # Check if any words have been passed with the respective G53.x command + # Check if any words have been passed with the respective G53.n command c = self.blocks[self.remap_level] p = c.p_number if c.p_flag else 0 x = c.i_number if c.i_flag else None y = c.j_number if c.j_flag else None z = c.k_number if c.k_flag else None - log.debug('G53.x Words passed: (P, X,Y,Z): %s', (p,x,y,z)) + log.debug(' G53.n Words passed: (P, X,Y,Z): %s', (p,x,y,z)) + if p not in [0,1,2]: - # reset the twp parameters - reset_twp_params(self) - msg = "G53.x : unrecognised P-Word found." - log.debug(msg) + # reset the twp parameters + reset_twp_params() + msg = "G53.n : unrecognised P-Word found." + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR orient_mode = p - # calculate the required rotary joint positions and pre_rotation for the requested tool-orientation + z_vector_requested = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] + x_vector_requested = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] + # calculate all possible pairs of (primary, secondary) angles to matches the requested orientation try: - tool_z_requested = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] - # calculate all possible pairs of (primary, secondary) angles so our tool-z vector matches the requested tool-z # angles are returned in [-pi,pi] - possible_prim_sec_angle_pairs = kins_calc_jnt_angles(self, tool_z_requested) - # An excepton will occur if the requested tool orientation cannot be achieved with the kinematic at hand + possible_prim_sec_angle_pairs = calc_joint_angles(z_vector_requested, x_vector_requested) # returns radians except Exception as error: - log.error('G53.x: Calculation failed, %s', error) - possible_prim_sec_angle_pairs = [] - if not possible_prim_sec_angle_pairs: - # reset the twp parameters - reset_twp_params(self) - msg = "G53.x ERROR: Requested tool orientation not reachable -> aborting G53.x" - log.debug(msg) + log.error('calc_joint_angles, %s', error) + # reset the twp parameters + reset_twp_params() + msg = ("G53.n ERROR: Calculation of joint angles has failed. -> aborting G53.n") + log.debug(' ' + msg) + emccanon.CANON_ERROR(msg) + yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed + yield INTERP_EXIT # w/o this the error does not abort a running gcode program + return INTERP_ERROR + + if possible_prim_sec_angle_pairs == []: + # reset the twp parameters + log.error('G53.n: No possible primary/secondary angle pairs found.') + reset_twp_params() + msg = "G53.n ERROR: Requested tool orientation not reachable -> aborting G53.n" + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR # this returns one pair of optimized angles in degrees, or (None, None) if no solution could be found - theta_1, theta_2 = calc_optimal_joint_move(self, possible_prim_sec_angle_pairs) - if theta_1 == None: + try: + theta_1, theta_2 = calc_optimal_joint_move(self, possible_prim_sec_angle_pairs) # returns radians + except Exception as error: + log.error('G53.n: Calculation of optimal joint move failed, %s', error) + if theta_1 == None or theta_2 == None: # reset the twp parameters - reset_twp_params(self) - msg = ("G53.x ERROR: Requested tool orientation not reachable -> aborting G53.x") - log.debug(msg) + reset_twp_params() + msg = ("G53.n ERROR: Requested tool orientation not reachable -> aborting G53.n") + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - theta_1 = radians(theta_1) - theta_2 = radians(theta_2) - # calculate the pre-rotation needed so our tool-x vector matches the requested tool-x vector - tool_x_requested = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] - pre_rot = kins_calc_pre_rot(self,theta_1, theta_2, tool_x_requested, tool_z_requested) - log.debug("Calculated pre-rotation (pre_rot) to match requested tool-x): %s", pre_rot) + # get the particular conditions to be met for the kinematic at hand + try: + (x_vector_requested, z_vector_requested, matrix_in, direction) = kins_calc_virtual_rot_get_values(x_vector_requested, + z_vector_requested, + twp_matrix) + except Exception as error: + log.error('G53.n: kins_calc_virtual_rot_get_values failed, %s', error) + # calculate the virtual-rotation needed + virtual_rot = calc_virtual_rotation(theta_1, + theta_2, + x_vector_requested, + z_vector_requested, + matrix_in, + direction) # returns radians + log.debug(f" Calculated virtual-rotation to match requested x-vector: {degrees(virtual_rot):.4f}°") + # mark twp-flag as active twp_flag = [0, 'active'] - gui_update_twp(self) - # set the pre-rotation value in the kinematic component - log.debug("G53.x: setting primary, secondary and pre_rotation angles in kinematic component: %s", (degrees(theta_1), degrees(theta_2), degrees(pre_rot))) - hal.set_p(kins_pre_rotation, str(pre_rot)) - hal.set_p(kins_primary_rotation, str(degrees(theta_1))) - hal.set_p(kins_secondary_rotation, str(degrees(theta_2))) - - # calculate the work offset in tool-coords - P = matrix_to_point(kins_calc_tool_transformation(self, point_to_matrix(saved_work_offset), theta_1, theta_2, pre_rot)) - # get the current twp_origin + gui_update_twp() + + # set the virtual-rotation value in the kinematic component + debug_msg = (f' G53.n: Setting angle values in kins comp to theta1: {degrees(theta_1):.4f}°, ' + f'theta2: {degrees(theta_2):.4f}°, virtual_rot: {degrees(virtual_rot):.4f}°') + log.debug(debug_msg) + try: + kins_set_values(theta_1, theta_2, virtual_rot) + except Exception as error: + log.error('G53.n: kins_set_values failed, %s', error) + + # calculate the work offset in transformed-coordinatess + log.debug(" G53.n: Saved work offset: %s", saved_work_offset) twp_offset = (twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]) - # calculate the twp offset in tool-coords - Q = matrix_to_point(kins_calc_tool_transformation(self, point_to_matrix(twp_offset), theta_1, theta_2, pre_rot)) - log.debug("G53.x: Setting transformed work-offsets for tool-kins in G59, G59.1, G59.2 and G59.3 to: %s ", P) + try: + new_offset = kins_calc_transformed_work_offset(saved_work_offset, twp_offset, theta_1, theta_2, virtual_rot) + except Exception as error: + log.error('G53.n: Calculation of kins_calc_transformed_work_offset failed, %s', error) + debug_msg = (f' G53.n: Setting transformed work-offsets for twp-kins in G59, G59.1, ' + f'G59.2 and G59.3 to: {new_offset[0]:.4f}, {new_offset[1]:.4f}, {new_offset[2]:.4f}') + log.debug(debug_msg) # set the dedicated TWP work offset values (G53, G53.1, G53.2, G53.3) - self.execute("G10 L2 P6 X%f Y%f Z%f " % (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]), lineno()) - self.execute("G10 L2 P7 X%f Y%f Z%f " % (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]), lineno()) - self.execute("G10 L2 P8 X%f Y%f Z%f " % (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]), lineno()) - self.execute("G10 L2 P9 X%f Y%f Z%f " % (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]), lineno()) - log.debug("G53.x: Moving (secondary and primary) joints to: %s", (degrees(theta_2), degrees(theta_1))) + self.execute("G10 L2 P6 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) + self.execute("G10 L2 P7 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) + self.execute("G10 L2 P8 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) + self.execute("G10 L2 P9 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) + + log.debug(f" G53.n: Moving primary joint to {degrees(theta_1):.4f}° and secondary joint to {degrees(theta_2):.4f}° ") if (x,y,z) == (None,None,None): - # Move rotary joints to align the tool with the requested twp - self.execute("G0 %s%f %s%f" % (joint_letter_secondary, degrees(theta_2), joint_letter_primary, degrees(theta_1)), lineno()) + # Move rotary joints to align the tool and the requested work plane + self.execute("G0 %s%f %s%f" % (joint_letter_primary, degrees(theta_1), joint_letter_secondary, degrees(theta_2)), lineno()) # switch to the dedicated TWP work offsets self.execute("G59", lineno()) - # activate TOOL kinematics + # activate TWP kinematics self.execute("G12.1 P2") if (x,y,z) != (None,None,None): - log.debug('G53.3 called') - self.execute("G0 X%s Y%s Z%s %s%f %s%f" % (x, y, z, joint_letter_secondary, degrees(theta_2), joint_letter_primary, degrees(theta_1)), lineno()) + log.debug(' G53.3 called') + self.execute("G0 X%s Y%s Z%s %s%f %s%f" % + (x, y, z, joint_letter_primary, degrees(theta_1), joint_letter_secondary, degrees(theta_2)), lineno()) # set twp-state to 'active' (2) self.execute("M68 E2 Q2") yield INTERP_EXECUTE_FINISH @@ -886,24 +732,25 @@ def g53x_core(self): # because we need self.execute() to switch the WCS properly this remap needs to be called from # an ngc that contains a quebuster before calling this code def g69_core(self): + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) global twp_flag, saved_work_offset_number, saved_work_offset if self.task == 0: # ignore the preview interpreter yield INTERP_EXECUTE_FINISH return INTERP_OK log.info('G69 called') # reset the twp parameters - reset_twp_params(self) - gui_update_twp(self) + reset_twp_params() + gui_update_twp() # set twp-state to 'undefined' (0) self.execute("M68 E2 Q0") yield INTERP_EXECUTE_FINISH return INTERP_OK -# define a virtual tilted-work-plane (twp) that is perpendicular to the current -# tool-orientation +# define a virtual tilted-work-plane (twp) that is perpendicular to the current tool-orientation def g683(self, **words): - global twp_matrix, pre_rot, twp_flag, saved_work_offset_number, saved_work_offset + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + global twp_matrix, virtual_rot, twp_flag, saved_work_offset_number, saved_work_offset if self.task == 0: # ignore the preview interpreter yield INTERP_EXECUTE_FINISH @@ -917,7 +764,7 @@ def g683(self, **words): if hal.get_value(twp_is_defined): # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg =("G68.3 ERROR: TWP already defined.") log.debug(msg) emccanon.CANON_ERROR(msg) @@ -931,7 +778,7 @@ def g683(self, **words): (n, offsets) = get_current_work_offset(self) if n != 1: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = "G68.3 ERROR: Must be in G54 to define TWP." log.debug(msg) emccanon.CANON_ERROR(msg) @@ -944,23 +791,31 @@ def g683(self, **words): x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested rotation of x-vector around the origin + r = radians(c.r_number) if c.r_flag else 0 twp_flag = [0, 1, 'empty'] # one call to define the twp in this mode - theta_1, theta_2 = get_current_rotary_positions(self) - # calculate tool-prerotation necessary to have tool-x vector in machine xy-plane - pre_rot = kins_calc_tool_rot_c_for_horizontal_x(self, theta_1, theta_2 ) - log.info("G68.3: Pre-Rotation calculated for x-vector in machine-xy plane [deg]: %s", pre_rot*180/pi) - # then we need the tool transformation matrix of the current tool orientation with the - # calculated pre-rotation to get the tool-x vector in the machine xy-plane - # for this we take the 4x4 identity matrix and pass it through the inverse tool kinematic - # transformation using the current rotary joint positions and calculated pre-rotation angle - # plus the requested angle of rotation for tool-x from the machine-xy plane + theta_1, theta_2 = get_current_rotary_positions(self) # radians + # calculate virtual rotation to have the oriented x-vector in the direction required for the kinematic at hand + try: + virtual_rot = kins_calc_virtual_rot_for_g683(theta_1, theta_2 ) + except Exception as error: + log.error('remap_func: kins_calc_virtual_rot_for_g683 failed, %s', error) + log.info("G68.3: virtual-Rotation calculated for x-vector in machine-xy plane [deg]: %s", degrees(virtual_rot)) + # then we need to calculate the transformation matrix of the current orientation with the including the + # calculated virtual-rotation. + # for this we take the 4x4 identity matrix and pass it through the kinematic transformation using the + # current rotary joint positions and the calculated virtual-rotation angle plus any additional angle + # passed in the R word of the G68.3 command start_matrix = np.asmatrix(np.identity(4)) - log.info('G68.3: Requested origin rotation [deg]: %s', r) - twp_matrix = kins_calc_tool_transformation(self, start_matrix, None, None, pre_rot + radians(r), 'inv') - log.debug("G68.3: Tool matrix with x-vector in machine xy-plane: \n%s", twp_matrix) + log.info('G68.3: Requested R-word rotation [deg]: %s', degrees(r)) + # the required transformation direction may depend on the kinematic at hand + try: + direction = kins_calc_transformation_get_direction() + except Exception as error: + log.error('kins_calc_transformation_get_direction, %s', error) + twp_matrix = calc_twp_matrix_from_joint_position(self, start_matrix, virtual_rot + r, direction) + log.debug("G68.3: TWP matrix with oriented x-vector: \n%s", twp_matrix) # put the requested origin into the twp_matrix (twp_matrix[0,3], twp_matrix[1,3], twp_matrix[2,3]) = (x, y, z) # update the build state of the twp call @@ -974,13 +829,14 @@ def g683(self, **words): self.execute("M68 E2 Q1") yield INTERP_EXECUTE_FINISH - gui_update_twp(self) + gui_update_twp() return INTERP_OK # definition of a virtual work-plane (twp) using different methods set by the 'p'-word def g682(self, **words): - global twp_matrix, pre_rot, twp_flag, twp_build_params, saved_work_offset_number, saved_work_offset + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + global twp_matrix, virtual_rot, twp_flag, twp_build_params, saved_work_offset_number, saved_work_offset if self.task == 0: # ignore the preview interpreter yield INTERP_EXECUTE_FINISH @@ -994,9 +850,9 @@ def g682(self, **words): if hal.get_value(twp_is_defined): # ie TWP has already been defined # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.2: TWP already defined.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1007,9 +863,9 @@ def g682(self, **words): (n, offsets) = get_current_work_offset(self) if n != 1: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = "G68.2 ERROR: Must be in G54 to define TWP." - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1018,7 +874,7 @@ def g682(self, **words): # collect the currently active work offset values (ie g54, g55 or other) saved_work_offset_number = n saved_work_offset = offsets - log.debug("G68.2: Saved work offsets %s", (n, saved_work_offset)) + log.debug(" G68.2: Saved work offsets %s", (n, saved_work_offset)) c = self.blocks[self.remap_level] p = c.p_number if c.p_flag else 0 @@ -1028,9 +884,9 @@ def g682(self, **words): q = str(int(c.q_number if c.q_flag else 313)) if q not in ['121','131','212','232','313','323']: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.2 (P0): No recognised Q-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1040,21 +896,24 @@ def g682(self, **words): x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 # parse the requested euler rotation angles - th1 = c.i_number if c.i_flag else 0 - th2 = c.j_number if c.j_flag else 0 - th3 = c.k_number if c.k_flag else 0 + th1 = radians(c.i_number) if c.i_flag else 0 + th2 = radians(c.j_number) if c.j_flag else 0 + th3 = radians(c.k_number) if c.k_flag else 0 # build the translation vector of the twp_matrix twp_origin = [[x], [y], [z]] - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - log.debug('G68.2 (P0): Twp_origin_rotation \n%s',twp_origin_rotation) + # create the rotation matrix for the requested origin rotation + try: + twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) + except Exception as error: + log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) + log.debug(' G68.2 (P0): Twp_origin_rotation \n%s',twp_origin_rotation) # build the rotation matrix for the requested euler rotation - twp_euler_rotation = twp_calc_euler_rot_matrix(th1, th2, th3, q) - log.debug('G68.2 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) + twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) + log.debug(' G68.2 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_euler_rotation) # combine rotation and translation and form the 4x4 twp-transformation matrix @@ -1072,34 +931,36 @@ def g682(self, **words): if q not in ['123','132','213','231','312','321']: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.2 P1: No recognised Q-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - # parse the requested origin x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 # parse the requested euler rotation angles - th1 = c.i_number if c.i_flag else 0 - th2 = c.j_number if c.j_flag else 0 - th3 = c.k_number if c.k_flag else 0 + th1 = radians(c.i_number) if c.i_flag else 0 + th2 = radians(c.j_number) if c.j_flag else 0 + th3 = radians(c.k_number) if c.k_flag else 0 # build the translation vector of the twp_matrix twp_origin = [[x], [y], [z]] - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - log.debug('G68.2 P1: Twp_origin_rotation \n%s',twp_origin_rotation) + # create the rotation matrix for the requested origin rotation + try: + twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) + except Exception as error: + log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) + log.debug(' G68.2 P1: Twp_origin_rotation \n%s',twp_origin_rotation) # build the rotation matrix for the requested euler rotation - twp_euler_rotation = twp_calc_euler_rot_matrix(th1, th2, th3, q) - log.debug('G68.2 P1: Twp_euler_rotation \n%s',twp_euler_rotation) + twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) + log.debug(' G68.2 P1: Twp_euler_rotation \n%s',twp_euler_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_euler_rotation) # combine rotation and translation and form the 4x4 twp-transformation matrix @@ -1111,21 +972,28 @@ def g682(self, **words): twp_flag[2] = 'done' elif p == 2: # twp defined py 3 points on the plane + # TODO implement operator errors as outlined in the twp README + #- G68.2 P2 (Q0),Q1,Q2,Q3 commands are not entered consecutively + #- two to the points entered in Q1,Q2,Q3 are identical + #- all three points entered in Q1,Q2,Q3 are on a line + #- the distance between a line defined by any two points entered in (Q1,Q2,Q3) and + #the remaining point is less than 10mm or 0.5inch (just some arbitrary values for now) + # if this is the first call for this mode reset the twp_flag flag if not twp_flag: twp_flag = [int(p), 4 , 'empty', 'empty', 'empty', 'empty'] # four calls needed twp_build_params = {'q0':[], 'q1':[], 'q2':[], 'q3':[]} # Point 1: defines the origin of the twp - # Point 2: direction from P1 to P2 defines the positive x direction on the twp (tool-x) - # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (tool-z) + # Point 2: direction from P1 to P2 defines the positive x direction on the twp (x-vector) + # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (z-vector) q = int(c.q_number if c.q_flag else 0) # this mode needs four calls to fill all required parameters if q == 0: # define new origin and rotation x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 twp_build_params['q0'] = [x,y,z,r] twp_flag[2] = 'done' elif q == 1: # define point 1 @@ -1148,9 +1016,9 @@ def g682(self, **words): twp_flag[5] = 'done' else: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.2 P2: No recognised Q-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1164,36 +1032,38 @@ def g682(self, **words): p1 = twp_build_params['q1'][0:3] p2 = twp_build_params['q2'] p3 = twp_build_params['q3'] - log.debug("G68.2 P2: Point 1: %s",p1) - log.debug("G68.2 P2: Point 2: %s",p2) - log.debug("G68.2 P2: Point 3: %s",p3) + log.debug(" G68.2 P2: Point 1: %s",p1) + log.debug(" G68.2 P2: Point 2: %s",p2) + log.debug(" G68.2 P2: Point 3: %s",p3) # build vectors x:P1->P2 and v2:P1->P3 twp_vect_x = [p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]] - log.debug("G68.2 P2: Twp_vect_x: \n%s",twp_vect_x) + log.debug(" G68.2 P2: Twp_vect_x: \n%s",twp_vect_x) v2 = [p3[0]-p1[0], p3[1]-p1[1], p3[2]-p1[2]] - log.debug("G68.2 P2 (v2): %s",v2) + log.debug(" G68.2 P2 (v2): %s",v2) # normalize the two vectors twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) v2 = v2 / np.linalg.norm(v2) - # we can use the cross product to calculate the tool-z vector + # we can use the cross product to calculate the z-vector vector # note: if P3 is on the right side of the vector P1->P2 - # then the tool-z will be below the twp (ie tool-z will be downwards) + # then the z-vector will be below the twp (ie z-vector will be downwards) twp_vect_z = np.cross(twp_vect_x , v2) - log.debug("G68.2 P2: Twp_vect_z %s",twp_vect_z) - # we can use the cross product to calculate the tool-y vector + log.debug(" G68.2 P2: Twp_vect_z %s",twp_vect_z) + # we can use the cross product to calculate the y vector twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug("G68.2 P2: Twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated tool-vectors + log.debug(" G68.2 P2: Twp_vect_y %s",twp_vect_y) + # build the rotation matrix of the twp_matrix from the calculated vectors # first stack the vectors (lists) and then flip diagonally (transpose) # so the vectors are now vertical twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug("G68.2 P2: Built the twp-rotation-matrix: \n%s", twp_vect_rotation) - # convert requested origin rotation to radians - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - log.debug('G68.2 P2: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) + log.debug(" G68.2 P2: Built the twp-rotation-matrix: \n%s", twp_vect_rotation) + # create the rotation matrix for the requested origin rotation + try: + twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) + except Exception as error: + log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) + log.debug(' G68.2 P2: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_rotation) # add the origin translation on the right @@ -1202,22 +1072,26 @@ def g682(self, **words): twp_row_4 = [0,0,0,1] twp_matrix = np.vstack((twp_matrix, twp_row_4)) twp_matrix = np.asmatrix(twp_matrix) - log.debug("G68.2 P2: Built twp-transformation-matrix: \n%s", twp_matrix) + log.debug(" G68.2 P2: Built twp-transformation-matrix: \n%s", twp_matrix) - elif p == 3: # two vectors (vector 1 defines the tool-x and vector 2 defines the tool-z) + elif p == 3: # two vectors (vector 1 defines the x-vector and vector 2 defines the z-vector) + # TODO implement operator errors as outlined in the twp README + #- G68.2 P3 Q1 and Q2 commands are not entered consecutively + #- one of the vectors is the zero vector + #- the enclosed angle between the 1. and 2. vector is <85° or >95° (re fanuc twp pdf) q = int(c.q_number if c.q_flag else 0) # if this is the first call for this mode reset the twp_flag flag if not twp_flag: - log.info('first call') + log.info(' first call') twp_flag = [int(p), 2 , 'empty', 'empty'] # two calls needed twp_build_params = {'q0':[], 'q1':[]} - log.debug('twp_build_params: %s', twp_build_params) + log.debug(' twp_build_params: %s', twp_build_params) if q == 0: # define new origin of the twp x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 # first vector (direction of x in the twp) i = c.i_number if c.i_flag else 0 j = c.j_number if c.j_flag else 0 @@ -1232,9 +1106,9 @@ def g682(self, **words): twp_flag[3] = 'done' else: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.2 P3: No recognised Q-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1249,39 +1123,39 @@ def g682(self, **words): log.debug("(x, y, z): %s", (x, y, z)) log.debug("(i, j, k): %s", (i, j, k)) log.debug("(i1, j1, k1): %s", (i1, j1, k1)) - # build unit vector defining tool-x direction + # build unit vector defining x-vector direction twp_vect_x = [i-x, j-y, k-z] twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) twp_vect_z = [i1, j1, k1] twp_vect_z = twp_vect_z / np.linalg.norm(twp_vect_z) orth = np.dot(twp_vect_x, twp_vect_z) - log.debug("orth check: %s", orth) + log.debug(" orth check: %s", orth) # the two vectors must be orthogonal - if orth != 0: - reset_twp_params(self) + if orth > 0.001: + reset_twp_params() msg = ("G68.2 P3: Vectors are not orthogonal.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - # we can use the cross product to calculate the tool-y vector + # we can use the cross product to calculate the y vector twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug("G68.2 P3: twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated tool-vectors + log.debug(" G68.2 P3: twp_vect_y %s",twp_vect_y) + # build the rotation matrix of the twp_matrix from the calculated vectors # first stack the vectors (lists) and then flip diagonally (transpose) # so the vectors are now vertical twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug("G68.2 P3: Built twp-rotation-matrix: \n%s", twp_vect_rotation) - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation + log.debug(" G68.2 P3: Built twp-rotation-matrix: \n%s", twp_vect_rotation) + # create the rotation matrix for the requested origin rotation try: - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - except Exception as e: - log.info('G68.2 P3: twp_origin_rotation failed, %s', e) - log.debug('G68.2 P3: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) + twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) + except Exception as error: + log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) + log.debug(' G68.2 P3: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_rotation) # add the origin translation on the right @@ -1291,41 +1165,45 @@ def g682(self, **words): twp_row_4 = [0,0,0,1] twp_matrix = np.vstack((twp_matrix, twp_row_4)) twp_matrix = np.asmatrix(twp_matrix) - log.debug("G68.2 P3: Built twp-transformation-matrix: \n%s", twp_matrix) + log.debug(" G68.2 P3: Built twp-transformation-matrix: \n%s", twp_matrix) + + # TODO implement G68.2 P4 as outlined in the fanuc twp pdf (the exact meaning of which is unclear to me) else: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.2: No recognised P-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - log.debug("G68.2: twp_flag: %s", twp_flag) - log.debug("G68.2: calls required: %s", twp_flag.count('done')) - log.debug("G68.2: number of calls made: %s", twp_flag.count('done')) + log.debug(" G68.2: twp_flag: %s", twp_flag) + log.debug(" G68.2: calls required: %s", twp_flag.count('done')) + log.debug(" G68.2: number of calls made: %s", twp_flag.count('done')) if twp_flag.count('done') == twp_flag[1]: - log.info('G68.2: requested rotation: %s', radians(r)) - log.info("G68.2: twp-tranformation-matrix: \n%s",twp_matrix) + log.info(' G68.2: requested rotation (degrees): %s', degrees(r)) + log.info(" G68.2: twp-tranformation-matrix: \n%s",twp_matrix) twp_origin = [twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]] - log.info("G68.2: twp origin: %s", twp_origin) + log.info(" G68.2: twp origin: %s", twp_origin) twp_vect_x = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] - log.info("G68.2: twp vector-x: %s", twp_vect_x) + log.info(" G68.2: twp vector-x: %s", twp_vect_x) twp_vect_z = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] - log.info("G68.2: twp vector-z: %s", twp_vect_z) + log.info(" G68.2: twp vector-z: %s", twp_vect_z) # set twp-state to 'defined' (1) self.execute("M68 E2 Q1") yield INTERP_EXECUTE_FINISH - gui_update_twp(self) + gui_update_twp() return INTERP_OK + # incremental definition of a virtual work-plane (twp) using different methods set by the 'p'-word def g684(self, **words): - global twp_matrix, pre_rot, twp_flag, twp_build_params, saved_work_offset_number, saved_work_offset + log.debug('Entering: %s', sys._getframe( ).f_code.co_name) + global twp_matrix, virtual_rot, twp_flag, twp_build_params, saved_work_offset_number, saved_work_offset if self.task == 0: # ignore the preview interpreter yield INTERP_EXECUTE_FINISH @@ -1339,9 +1217,9 @@ def g684(self, **words): if not hal.get_value(twp_is_active): # ie there is currently no TWP defined # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4: No TWP active to increment from. Run G68.2 or G68.3 first.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1352,18 +1230,16 @@ def g684(self, **words): # Must be in one of the dedicated offset systems for TWP if False: #n < 6: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4 ERROR: Must be in G59, G59.x to increment TWP.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - # store the current TWP to twp_matrix_current = np.matrix.copy(twp_matrix) - c = self.blocks[self.remap_level] p = c.p_number if c.p_flag else 0 @@ -1374,9 +1250,9 @@ def g684(self, **words): if q not in ['121','131','212','232','313','323']: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4 (P0): No recognised Q-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1386,21 +1262,24 @@ def g684(self, **words): x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 - # parse requested euler angles - th1 = c.i_number if c.i_flag else 0 - th2 = c.j_number if c.j_flag else 0 - th3 = c.k_number if c.k_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 + # parse the requested euler rotation angles + th1 = radians(c.i_number) if c.i_flag else 0 + th2 = radians(c.j_number) if c.j_flag else 0 + th3 = radians(c.k_number) if c.k_flag else 0 # build the translation vector of the twp_matrix twp_origin = [[x], [y], [z]] - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - log.debug('G68.4 (P0): Twp_origin_rotation \n%s',twp_origin_rotation) + # create the rotation matrix for the requested origin rotation + try: + twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) + except Exception as error: + log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) + log.debug(' G68.4 (P0): Twp_origin_rotation \n%s',twp_origin_rotation) # build the rotation matrix for the requested euler rotation - twp_euler_rotation = twp_calc_euler_rot_matrix(th1, th2, th3, q) - log.debug('G68.4 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) + twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) + log.debug(' G68.4 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_euler_rotation) # combine rotation and translation and form the 4x4 twp-transformation matrix @@ -1418,9 +1297,9 @@ def g684(self, **words): if q not in ['123','132','213','231','312','321']: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4 P1: No recognised Q-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1430,21 +1309,24 @@ def g684(self, **words): x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 # parse the requested euler rotation angles - th1 = c.i_number if c.i_flag else 0 - th2 = c.j_number if c.j_flag else 0 - th3 = c.k_number if c.k_flag else 0 + th1 = radians(c.i_number) if c.i_flag else 0 + th2 = radians(c.j_number) if c.j_flag else 0 + th3 = radians(c.k_number) if c.k_flag else 0 # build the translation vector of the twp_matrix twp_origin = [[x], [y], [z]] - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - log.debug('G68.4 P1: Twp_origin_rotation \n%s',twp_origin_rotation) + # create the rotation matrix for the requested origin rotation + try: + twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) + except Exception as error: + log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) + log.debug(' G68.4 P1: Twp_origin_rotation \n%s',twp_origin_rotation) # build the rotation matrix for the requested euler rotation - twp_euler_rotation = twp_calc_euler_rot_matrix(th1, th2, th3, q) - log.debug('G68.4 P1: Twp_euler_rotation \n%s',twp_euler_rotation) + twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) + log.debug(' G68.4 P1: Twp_euler_rotation \n%s',twp_euler_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_euler_rotation) # combine rotation and translation and form the 4x4 twp-transformation matrix @@ -1456,21 +1338,28 @@ def g684(self, **words): twp_flag[2] = 'done' elif p == 2: # twp defined py 3 points on the plane + # TODO implement operator errors as outlined in the twp README + #- G68.2 P2 (Q0),Q1,Q2,Q3 commands are not entered consecutively + #- two to the points entered in Q1,Q2,Q3 are identical + #- all three points entered in Q1,Q2,Q3 are on a line + #- the distance between a line defined by any two points entered in (Q1,Q2,Q3) and + #the remaining point is less than 10mm or 0.5inch (just some arbitrary values for now) + # if this is the first call for this mode reset the twp_flag flag if not twp_flag: twp_flag = [int(p), 4 , 'empty', 'empty', 'empty', 'empty'] # four calls needed twp_build_params = {'q0':[], 'q1':[], 'q2':[], 'q3':[]} # Point 1: defines the origin of the twp - # Point 2: direction from P1 to P2 defines the positive x direction on the twp (tool-x) - # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (tool-z) + # Point 2: direction from P1 to P2 defines the positive x direction on the twp (x-vector) + # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (z-vector) q = int(c.q_number if c.q_flag else 0) # this mode needs four calls to fill all required parameters if q == 0: # define new origin and rotation x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 twp_build_params['q0'] = [x,y,z,r] twp_flag[2] = 'done' elif q == 1: # define point 1 @@ -1493,9 +1382,9 @@ def g684(self, **words): twp_flag[5] = 'done' else: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4 P2: No recognised Q-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1509,38 +1398,38 @@ def g684(self, **words): p1 = twp_build_params['q1'][0:3] p2 = twp_build_params['q2'] p3 = twp_build_params['q3'] - log.debug("G68.4 P2: Point 1: %s",p1) - log.debug("G68.4 P2: Point 2: %s",p2) - log.debug("G68.4 P2: Point 3: %s",p3) + log.debug(" G68.4 P2: Point 1: %s",p1) + log.debug(" G68.4 P2: Point 2: %s",p2) + log.debug(" G68.4 P2: Point 3: %s",p3) # build vectors x:P1->P2 and v2:P1->P3 twp_vect_x = [p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]] - log.debug("G68.4 P2: Twp_vect_x: \n%s",twp_vect_x) + log.debug(" G68.4 P2: Twp_vect_x: \n%s",twp_vect_x) v2 = [p3[0]-p1[0], p3[1]-p1[1], p3[2]-p1[2]] - log.debug("G68.4 P2: (v2) %s", v2) + log.debug(" G68.4 P2: (v2) %s", v2) # normalize the two vectors twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) v2 = v2 / np.linalg.norm(v2) - # we can use the cross product to calculate the tool-z vector + # we can use the cross product to calculate the z-vector vector # note: if P3 is on the right side of the vector P1->P2 - # then the tool-z will be below the twp (ie tool-z will be downwards) + # then the z-vector will be below the twp (ie z-vector will be downwards) twp_vect_z = np.cross(twp_vect_x , v2) - log.debug("G68.4 P2: Twp_vect_z %s",twp_vect_z) - # we can use the cross product to calculate the tool-y vector + log.debug(" G68.4 P2: Twp_vect_z %s",twp_vect_z) + # we can use the cross product to calculate the y vector twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug("G68.4 P2: Twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated tool-vectors + log.debug(" G68.4 P2: Twp_vect_y %s",twp_vect_y) + # build the rotation matrix of the twp_matrix from the calculated vectors # first stack the vectors (lists) and then flip diagonally (transpose) # so the vectors are now vertical twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug("G68.4 P2: Built the twp-rotation-matrix: \n%s", twp_vect_rotation) - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation + log.debug(" G68.4 P2: Built the twp-rotation-matrix: \n%s", twp_vect_rotation) + # create the rotation matrix for the requested origin rotation try: - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - except Exception as e: - log.debug('G68.4 P2: twp_origin_rotation failed ', e) - log.debug('G68.4 P2: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) + twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) + except Exception as error: + log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) + log.debug(' G68.4 P2: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_rotation) # add the origin translation on the right @@ -1549,9 +1438,13 @@ def g684(self, **words): twp_row_4 = [0,0,0,1] twp_matrix = np.vstack((twp_matrix, twp_row_4)) twp_matrix = np.asmatrix(twp_matrix) - log.debug("G68.4 P2: Built twp-transformation-matrix: \n%s", twp_matrix) + log.debug(" G68.4 P2: Built twp-transformation-matrix: \n%s", twp_matrix) - elif p == 3: # two vectors (vector 1 defines the tool-x and vector 2 defines the tool-z) + elif p == 3: # two vectors (vector 1 defines the x-vector and vector 2 defines the z-vector) + # TODO implement operator errors as outlined in the twp README + #- G68.2 P3 Q1 and Q2 commands are not entered consecutively + #- one of the vectors is the zero vector + #- the enclosed angle between the 1. and 2. vector is <85° or >95° (re fanuc twp pdf) q = int(c.q_number if c.q_flag else 0) # if this is the first call for this mode reset the twp_flag flag if not twp_flag: @@ -1561,8 +1454,8 @@ def g684(self, **words): x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # parse the requested rotation of tool-x around the origin - r = c.r_number if c.r_flag else 0 + # parse the requested xy-rotation around the origin + r = radians(c.r_number) if c.r_flag else 0 # first vector (direction of x in the twp) i = c.i_number if c.i_flag else 0 j = c.j_number if c.j_flag else 0 @@ -1577,9 +1470,9 @@ def g684(self, **words): twp_flag[3] = 'done' else: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4 P3: No recognised Q-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program @@ -1594,40 +1487,43 @@ def g684(self, **words): log.debug("(x, y, z) %s", (x, y, z)) log.debug("(i, j, k) %s", (i, j, k)) log.debug("(i1, j1, k1) %s", (i1, j1, k1)) - # build unit vector defining tool-x direction + # build unit vector defining x-vector direction twp_vect_x = [i-x, j-y, k-z] twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) twp_vect_z = [i1, j1, k1] twp_vect_z = twp_vect_z / np.linalg.norm(twp_vect_z) orth = np.dot(twp_vect_x, twp_vect_z) - log.debug("orth check: %s", orth) + log.debug(" orth check: %s", orth) # the two vectors must be orthogonal if orth != 0: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() ## reset the parameter values #twp_flag = [int(p), 2 , 'empty', 'empty'] # two calls needed #twp_build_params = {'q0':[], 'q1':[]} msg = ("G68.4 P3: Vectors are not orthogonal.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - # we can use the cross product to calculate the tool-y vector + # we can use the cross product to calculate the y vector twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug("G68.4 P3: twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated tool-vectors + log.debug(" G68.4 P3: twp_vect_y %s",twp_vect_y) + # build the rotation matrix of the twp_matrix from the calculated vectors # first stack the vectors (lists) and then flip diagonally (transpose) # so the vectors are now vertical twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug("G68.4 P3: Built twp-rotation-matrix: \n%s", twp_vect_rotation) - # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation - twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') - log.debug('G68.4 P3: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) + log.debug(" G68.4 P3: Built twp-rotation-matrix: \n%s", twp_vect_rotation) + # create the rotation matrix for the requested origin rotation + try: + twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) + except Exception as error: + log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) + log.debug(' G68.4 P3: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_rotation) # add the origin translation on the right @@ -1637,40 +1533,42 @@ def g684(self, **words): twp_row_4 = [0,0,0,1] twp_matrix = np.vstack((twp_matrix, twp_row_4)) twp_matrix = np.asmatrix(twp_matrix) - log.debug("G68.4 P3: Built twp-transformation-matrix: \n%s", twp_matrix) + log.debug(" G68.4 P3: Built twp-transformation-matrix: \n%s", twp_matrix) + + # TODO implement G68.4 P4 as outlined in the fanuc twp pdf (the exact meaning of which is unclear to me) else: # reset the twp parameters - reset_twp_params(self) + reset_twp_params() msg = ("G68.4: No recognised P-Word found.") - log.debug(msg) + log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR - log.debug("G68.4: twp_flag: %s", twp_flag) - log.debug("G68.4: calls required: %s", twp_flag.count('done')) - log.debug("G68.4: number of calls made: %s", twp_flag.count('done')) + log.debug(" G68.4: twp_flag: %s", twp_flag) + log.debug(" G68.4: calls required: %s", twp_flag.count('done')) + log.debug(" G68.4: number of calls made: %s", twp_flag.count('done')) if twp_flag.count('done') == twp_flag[1]: - log.info('G68.4: requested rotation %s', radians(r)) - log.info("G68.4: twp_matrix_current: \n%s", twp_matrix_current) - log.info("G68.4: incremental twp_matrix requested: \n%s",twp_matrix) - log.info("G68.4: calculating new twp_matrix...") + log.info(' G68.4: requested rotation (degrees) %s', degrees(r)) + log.info(" G68.4: twp_matrix_current: \n%s", twp_matrix_current) + log.info(" G68.4: incremental twp_matrix requested: \n%s",twp_matrix) + log.info(" G68.4: calculating new twp_matrix...") twp_matrix_new = twp_matrix_current * twp_matrix - log.info("G68.4: twp_matrix_new: \n%s",twp_matrix_new) + log.info(" G68.4: twp_matrix_new: \n%s",twp_matrix_new) twp_origin = [twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]] - log.info("G68.4: twp origin: %s", twp_origin) + log.info(" G68.4: twp origin: %s", twp_origin) twp_vect_x = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] - log.info("G68.4: twp vector-x: %s", twp_vect_x) + log.info(" G68.4: twp vector-x: %s", twp_vect_x) twp_vect_z = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] - log.info("G68.4: twp vector-z: %s", twp_vect_z) - log.info("G68.4: incremented twp_matrix: \n%s", twp_matrix_new) + log.info(" G68.4: twp vector-z: %s", twp_vect_z) + log.info(" G68.4: incremented twp_matrix: \n%s", twp_matrix_new) twp_matrix = twp_matrix_new # set twp-state to 'defined' (1) self.execute("M68 E2 Q1") yield INTERP_EXECUTE_FINISH - gui_update_twp(self) + gui_update_twp() return INTERP_OK diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py new file mode 100644 index 00000000000..7c2b498b7b6 --- /dev/null +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py @@ -0,0 +1,347 @@ +# 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 + + +# 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((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((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) + 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 (theta_1_calcd, theta_2_calcd) # returns radians + + +# calculate the transformed work offset used after 53.n +def kins_calc_transformed_work_offset(current_offset, twp_offset, theta_1, theta_2, virtual_rot): + P = matrix_to_point(kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, point_to_matrix(current_offset))) + # calculate the twp offset in transformed-coordinates + Q = matrix_to_point(kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, point_to_matrix(twp_offset))) + transformed_offset = (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]) + return transformed_offset + +# pass required values to the kinematics component +# the module takes the virtual rotation in radians and the two joint angles in +# degrees, the same units the joints themselves are in +def kins_set_values(theta_1, theta_2, virtual_rot): # expects radians + hal.set_p(kins_virtual_rotation, str(virtual_rot)) + hal.set_p(kins_primary_rotation, str(degrees(theta_1))) + hal.set_p(kins_secondary_rotation, str(degrees(theta_2))) + + +# returns angle required to orient the x-vector parallel to the machine-xy plane +# for given machine joint position angles. +# For G68.3 this is the default tool-x direction +# NOTE: this uses formulas derived from the transformation matrix in the inverse tool kinematic +# TODO I don't actually know if this is the correct x orientation for G68.3' +def kins_calc_virtual_rot_for_g683(theta_1, theta_2): + # The idea is that the oriented x-vector is parallel to the machine xy-plane when the + # z component of the x-direction vector is equal to zero + # Mathematically we take the symbolic formula found in row 3, column 1 of the transformation + # matrix from the inverse tool-kinematics, equal that to zero and solve for 'tc'. + # this makes the x-vector of the oriented coords horizontal and the user can set the + # rotation from there using g68.3 r + global kins_nutation_angle + v = radians(hal.get_value(kins_nutation_angle)) + Cv = cos(v) + Sv = sin(v) + Cs = cos(theta_2) + Ss = sin(theta_2) + Cp = cos(theta_1) + Sp = sin(theta_1) + t = Sv*Cv*(1-Cs) + tc = atan2((Sv*Ss),t) + # note: rotation is done using a halpin that feeds into the kinematic component and the + # vismach model. In contrast to a gcode command where 'c' refers to a physical machine joint) + return tc # returns radians + + +# return the start values required to calculate the virtual rotation +def kins_calc_virtual_rot_get_values(x_vector_requested, z_vector_requested, twp_matrix): + x_vector_requested = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] + z_vector_requested = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] + matrix_in = np.asmatrix(np.identity(4)) + direction = 'inv' + return (x_vector_requested, z_vector_requested, matrix_in, direction) + + +# If the operator has requested a rotation by passing an R word in the 68.n command we need to +# create a rotation matrix that represents a rotation around the Z-axis of the TWP plane +def kins_calc_twp_origin_rot_matrix(r): # expects radians + # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation + twp_origin_rot_matrix = calc_euler_rot_matrix(0, r, 0, '131') + + return twp_origin_rot_matrix + + +# This returns which transformation to use when checking calculated angles +# and when calculating the twp_matrix for G68.3 +def kins_calc_transformation_get_direction(): + return 'inv' + + +# returns the pin name for the virtual rotation in the kinematics component +def kins_get_current_virtual_rot(): + current_virtual_rot = hal.get_value(kins_virtual_rotation) + return current_virtual_rot # returns radians + + + + + + + +# forms a 4x4 transformation matrix from a given 1x3 point vector [x,y,z] +def point_to_matrix(point): + # start with a 4x4 identity matrix and add the point vector to the 4th column + matrix = np.identity(4) + [matrix[0,3], matrix[1,3], matrix[2,3]] = point + matrix = np.asmatrix(matrix) + return matrix + +# extracts the point vector form a given 4x4 transformation matrix +def matrix_to_point(matrix): + point = (matrix[0,3],matrix[1,3],matrix[2,3]) + return point + + +# this is from 'mika-s.github.io' +# transforms a given angle to the interval of [-pi,pi] +def transform_to_pipi(input_angle): + def truncated_remainder(dividend, divisor): + divided_number = dividend / divisor + divided_number = -int(-divided_number) if divided_number < 0 else int(divided_number) + remainder = dividend - divisor * divided_number + return remainder + + revolutions = int((input_angle + np.sign(input_angle) * pi) / (2 * pi)) + p1 = truncated_remainder(input_angle + np.sign(input_angle) * pi, 2 * pi) + p2 = (np.sign(np.sign(input_angle) + + 2 * (np.sign(fabs((truncated_remainder(input_angle + pi, 2 * pi)) / (2 * pi))) - 1))) * pi + output_angle = p1 - p2 + return output_angle + + +# define the basic rotation matrices, used for euler twp modes +def Rx(th): + return np.array([[1, 0 , 0 ], + [0, cos(th), -sin(th)], + [0, sin(th), cos(th)]]) + +def Ry(th): + return np.array([[ cos(th), 0, sin(th)], + [ 0 , 1, 0 ], + [-sin(th), 0, cos(th)]]) + +def Rz(th): + return np.array([[cos(th), -sin(th), 0], + [sin(th), cos(th), 0], + [0 , 0 , 1]]) + + +# returns the rotation matrices for given order and angles +def calc_euler_rot_matrix(th1, th2, th3, order): + if order == '131': + matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Rx(th3)) + elif order=='121': + matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rx(th3)) + elif order=='212': + matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Ry(th3)) + elif order=='232': + matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Ry(th3)) + elif order=='323': + matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rz(th3)) + elif order=='313': + matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Rz(th3)) + elif order=='123': + matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rz(th3)) + elif order=='132': + matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Ry(th3)) + elif order=='213': + matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Rz(th3)) + elif order=='231': + matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Rx(th3)) + elif order=='321': + matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rx(th3)) + elif order=='312': + matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Ry(th3)) + return matrix diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini index 06b9cd5d23f..2be585e6ef8 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini @@ -39,7 +39,7 @@ SUBROUTINE_PATH = ../remap_subs:../demos REMAP = G53.1 modalgroup=1 argspec=p ngc=g531remap REMAP = G53.3 modalgroup=1 argspec=pxyz ngc=g533remap REMAP = G53.6 modalgroup=1 argspec=p ngc=g536remap - REMAP = M530 modalgroup=10 python=g53x_core + REMAP = M530 modalgroup=10 python=g53n_core REMAP = G68.2 modalgroup=1 argspec=pqxyzijkr python=g682 REMAP = G68.3 modalgroup=1 argspec=xyzr python=g683 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py new file mode 100644 index 00000000000..b1629e7d012 --- /dev/null +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py @@ -0,0 +1,350 @@ +# 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 + + +# 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 = (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((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) + 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 (theta_1_calcd, theta_2_calcd) # returns radians + + +# calculate the transformed work offset used after 53.n +def kins_calc_transformed_work_offset(current_offset, twp_offset, theta_1, theta_2, virtual_rot): + P = matrix_to_point(kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, point_to_matrix(current_offset))) + # calculate the twp offset in transformed-coordinates + Q = matrix_to_point(kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, point_to_matrix(twp_offset))) + transformed_offset = (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]) + return transformed_offset + +# pass required values to the kinematics component +# the module takes the virtual rotation in radians and the two joint angles in +# degrees, the same units the joints themselves are in +def kins_set_values(theta_1, theta_2, virtual_rot): # expects radians + hal.set_p(kins_virtual_rotation, str(virtual_rot)) + hal.set_p(kins_primary_rotation, str(degrees(theta_1))) + hal.set_p(kins_secondary_rotation, str(degrees(theta_2))) + + +# returns angle required to orient the x-vector parallel to the machine-xy plane +# for given machine joint position angles. +# For G68.3 this is the default tool-x direction +# NOTE: this uses formulas derived from the transformation matrix in the inverse tool kinematic +# TODO I don't actually know if this is the correct x orientation for G68.3' +def kins_calc_virtual_rot_for_g683(theta_1, theta_2): + # The idea is that the oriented x-vector is parallel to the machine xy-plane when the + # z component of the x-direction vector is equal to zero + # Mathematically we take the symbolic formula found in row 3, column 1 of the transformation + # matrix from the inverse tool-kinematics, equal that to zero and solve for 'tc'. + # this makes the x-vector of the oriented coords horizontal and the user can set the + # rotation from there using g68.3 r + global kins_nutation_angle + v = radians(hal.get_value(kins_nutation_angle)) + Cv = cos(v) + Sv = sin(v) + Cs = cos(theta_2) + Ss = sin(theta_2) + Cp = cos(theta_1) + Sp = sin(theta_1) + t = Sv*Cv*(1-Cs) + tc = atan2(-t,(Sv*Ss)) + # note: rotation is done using a halpin that feeds into the kinematic component and the + # vismach model. In contrast to a gcode command where 'c' refers to a physical machine joint) + return tc # returns radians + + +# return the start values required to calculate the virtual rotation +def kins_calc_virtual_rot_get_values(x_vector_requested, z_vector_requested, twp_matrix): + x_vector_requested = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] + z_vector_requested = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] + matrix_in = np.asmatrix(np.identity(4)) + direction = 'inv' + return (x_vector_requested, z_vector_requested, matrix_in, direction) + + +# If the operator has requested a rotation by passing an R word in the 68.n command we need to +# create a rotation matrix that represents a rotation around the Z-axis of the TWP plane +def kins_calc_twp_origin_rot_matrix(r): # expects radians + # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation + twp_origin_rot_matrix = calc_euler_rot_matrix(0, r, 0, '131') + + return twp_origin_rot_matrix + + +# This returns which transformation to use when checking calculated angles +# and when calculating the twp_matrix for G68.3 +def kins_calc_transformation_get_direction(): + return 'inv' + + +# returns the pin name for the virtual rotation in the kinematics component +def kins_get_current_virtual_rot(): + current_virtual_rot = hal.get_value(kins_virtual_rotation) + return current_virtual_rot # returns radians + + + + + + + +# forms a 4x4 transformation matrix from a given 1x3 point vector [x,y,z] +def point_to_matrix(point): + # start with a 4x4 identity matrix and add the point vector to the 4th column + matrix = np.identity(4) + [matrix[0,3], matrix[1,3], matrix[2,3]] = point + matrix = np.asmatrix(matrix) + return matrix + +# extracts the point vector form a given 4x4 transformation matrix +def matrix_to_point(matrix): + point = (matrix[0,3],matrix[1,3],matrix[2,3]) + return point + + +# this is from 'mika-s.github.io' +# transforms a given angle to the interval of [-pi,pi] +def transform_to_pipi(input_angle): + def truncated_remainder(dividend, divisor): + divided_number = dividend / divisor + divided_number = -int(-divided_number) if divided_number < 0 else int(divided_number) + remainder = dividend - divisor * divided_number + return remainder + + revolutions = int((input_angle + np.sign(input_angle) * pi) / (2 * pi)) + p1 = truncated_remainder(input_angle + np.sign(input_angle) * pi, 2 * pi) + p2 = (np.sign(np.sign(input_angle) + + 2 * (np.sign(fabs((truncated_remainder(input_angle + pi, 2 * pi)) / (2 * pi))) - 1))) * pi + output_angle = p1 - p2 + return output_angle + + +# define the basic rotation matrices, used for euler twp modes +def Rx(th): + return np.array([[1, 0 , 0 ], + [0, cos(th), -sin(th)], + [0, sin(th), cos(th)]]) + +def Ry(th): + return np.array([[ cos(th), 0, sin(th)], + [ 0 , 1, 0 ], + [-sin(th), 0, cos(th)]]) + +def Rz(th): + return np.array([[cos(th), -sin(th), 0], + [sin(th), cos(th), 0], + [0 , 0 , 1]]) + + +# returns the rotation matrices for given order and angles +def calc_euler_rot_matrix(th1, th2, th3, order): + if order == '131': + matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Rx(th3)) + elif order=='121': + matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rx(th3)) + elif order=='212': + matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Ry(th3)) + elif order=='232': + matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Ry(th3)) + elif order=='323': + matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rz(th3)) + elif order=='313': + matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Rz(th3)) + elif order=='123': + matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rz(th3)) + elif order=='132': + matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Ry(th3)) + elif order=='213': + matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Rz(th3)) + elif order=='231': + matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Rx(th3)) + elif order=='321': + matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rx(th3)) + elif order=='312': + matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Ry(th3)) + return matrix diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini index d9ae382fefc..d3032855aee 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini @@ -39,7 +39,7 @@ SUBROUTINE_PATH = ../remap_subs:../demos REMAP = G53.1 modalgroup=1 argspec=p ngc=g531remap REMAP = G53.3 modalgroup=1 argspec=pxyz ngc=g533remap REMAP = G53.6 modalgroup=1 argspec=p ngc=g536remap - REMAP = M530 modalgroup=10 python=g53x_core + REMAP = M530 modalgroup=10 python=g53n_core REMAP = G68.2 modalgroup=1 argspec=pqxyzijkr python=g682 REMAP = G68.3 modalgroup=1 argspec=xyzr python=g683 From eb2b66e8b14726f1a5f34ae86533e49634717b1c Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 25 Aug 2026 01:03:48 +1000 Subject: [PATCH 02/77] twp: keep the arc functions inside their domain A tool vector in a principal plane raised "math domain error" from asin() in kins_calc_primary: the vector is a column of a product of rotations, a unit vector only to rounding, and a zero component lands the argument a rounding error outside plus or minus one. Sweeping every reachable orientation in five degree steps gave 47 failures of 5184 at the configured nutation, and every failure is a vector with a zero component, what G68.2 with I0 or J0 asks for. Clamp an argument within rounding of the limit and leave anything further out to raise, since that is an unreachable orientation rather than an artefact. kins_calc_possible_joint_angles then fell through to an unassigned variable on that failure; it returns no solution, which the caller handles. --- .../xyzacb-trsrn_twp/remap_funcs_twp.py | 32 +++++++++++++++++-- .../xyzbca-trsrn_twp/remap_funcs_twp.py | 32 +++++++++++++++++-- 2 files changed, 60 insertions(+), 4 deletions(-) diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py index 7c2b498b7b6..7c2ebd39961 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py @@ -21,6 +21,29 @@ 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 @@ -138,7 +161,7 @@ def kins_calc_primary(log, z_vector_req, x_vector_req, theta_2_list=[]): Cs = cos(theta_2) t = Sv*Cv*(1-Cs) p = Sv * Ss - theta_1 = asin((p*Kzy - t*Kzx)/(t*t + p*p)) + 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.') @@ -168,7 +191,7 @@ def kins_calc_secondary(log, z_vector_req, x_vector_req): log.error('remap_funcs: Requested orientation not reachable with the current nutation angle.') return None else: - theta_2 = acos((Kzz - Cv*Cv)/(1 - Cv*Cv)) + 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: @@ -182,12 +205,17 @@ def kins_calc_possible_joint_angles(log, z_vector_req, x_vector_req): 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 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py index b1629e7d012..abb24726b72 100644 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py @@ -21,6 +21,29 @@ 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 @@ -138,7 +161,7 @@ def kins_calc_primary(log, z_vector_req, x_vector_req, theta_2_list=[]): Cs = cos(theta_2) t = Sv*Cv*(1-Cs) p = Sv * Ss - q = (t*Kzy - p*Kzx)/(t*t + p*p) + 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)]: @@ -169,7 +192,7 @@ def kins_calc_secondary(log, z_vector_req, x_vector_req): log.error('remap_funcs: Requested orientation not reachable with the current nutation angle.') return None else: - theta_2 = acos((Kzz - Cv*Cv)/(1 - Cv*Cv)) + 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.') @@ -185,12 +208,17 @@ def kins_calc_possible_joint_angles(log, z_vector_req, x_vector_req): 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 From 3ddf67a7f2fc00147bf1ce184ee5c904ba09e68e Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:24 +1000 Subject: [PATCH 03/77] kinematics: add an optional Jacobian entry point A feed is a speed in the work frame and the machine delivers a speed at each joint; on any kinematics but the identity the two are related by where the machine is, and nothing in the interface answered that, so a limit taken from the joints had nowhere to get it. kinematicsJacobian() answers it: jac[j][a] is how joint j responds to a unit rate of pose coordinate a, rows joints, columns in EmcPose order. It is the derivative of the inverse, since that is what every consumer multiplies by and every module has one, in joint units per pose unit so nothing is converted. A module supplies nothing: kinsJacobianFromInverse() takes central differences of its inverse, eighteen calls a pose. switchkins answers for every type, exactly for an identity type and by differences for one that registers nothing; switchkinsRegisterJacobian() takes a closed form. kinsJacobianFromMappedAxes() turns the derivative of a computed position into rows for the modules that finish in position_to_mapped_joints(). Nothing in motion calls it yet; that is the realtime seam of the limits work, and it waits on the closed forms. --- docs/src/motion/kinematics-conventions.adoc | 112 +++++++++++++++++- src/emc/kinematics/kinematics.h | 95 +++++++++++++++ src/emc/kinematics/kins_util.c | 123 ++++++++++++++++++++ src/emc/kinematics/switchkins.c | 40 +++++++ src/emc/kinematics/switchkins.h | 11 ++ src/emc/kinematics/trivkins.c | 9 ++ 6 files changed, 384 insertions(+), 6 deletions(-) diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 063ee63e794..4596796ca03 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -279,12 +279,11 @@ 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 @@ -377,6 +376,101 @@ 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. If a module registers no Jacobian, the shared code computes one by +central differences: it calls the module's inverse eighteen times, stepping +each pose coordinate a small amount to either side on the solution branch +the inverse flags select, and differences the results. The answer is as +precise as the inverse itself; behind an inverse that iterates, that is the +iteration's convergence tolerance divided by the step. Modules built on `switchkins.c` answer this way for every type that +registers nothing; an identity type answers exactly. + +This cost matters because `kinematicsJacobian()` is meant to run in the +servo thread, where checking a feed limit calls it once per cycle: each call +then costs eighteen inverses. On the closed-form inverses in the tree that +is under two microseconds. `genserkins`, the one module whose inverse +iterates, would cost near eighty microseconds per call this way, against +under two for the geometric Jacobian it registers instead. + +So a module whose inverse iterates should register a closed-form Jacobian, +as below. Nothing enforces this. A module that registers nothing still gets +a correct Jacobian, just one that costs eighteen inverses and carries the +iteration's precision, which behind an iterating inverse is too slow for the +servo thread. + +A module with a closed form registers it with `switchkinsRegisterJacobian()`. +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:writing-a-module]] == Writing a Module @@ -406,6 +500,12 @@ 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 diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index ba285cb8861..900fe5aa474 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -16,6 +16,7 @@ #define __LINUXCNC_KINEMATICS_H #include "emcpos.h" /* EmcPose */ +#include "emcmotcfg.h" /* EMCMOT_MAX_JOINTS, EMCMOT_MAX_AXIS */ #include "rtapi_bool.h" /* @@ -360,6 +361,90 @@ 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); + extern int kinematicsSwitchable(void); extern int kinematicsSwitch(int switchkins_type); //NOTE: switchable kinematics may require Interp::Synch @@ -414,6 +499,11 @@ extern int xyzacKinematicsWorkFrame(const double *joints, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags); +extern int xyzacKinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + extern int xyzbcKinematicsForward(const double *joints, EmcPose * pos, @@ -433,4 +523,9 @@ extern int xyzbcKinematicsWorkFrame(const double *joints, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags); +extern int xyzbcKinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + //********************************************************************* diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index fa20010d1d2..324392e6864 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -1040,3 +1040,126 @@ 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]) +{ + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][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<= kins_count) { + return -1; + } + // 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; @@ -354,6 +374,20 @@ int switchkinsRegisterFrames(int ktype, KT kwork, KT ktool, return 0; } // switchkinsRegisterFrames() +int switchkinsRegisterJacobian(int ktype, KJ kjac) +{ + if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterJacobian: BAD switchkins_type" + " <%d> (must be 0..%d)\n", + ktype, SWITCHKINS_MAX_TYPES - 1); + register_error = 1; + return -1; + } + kjacs[ktype] = kjac; + return 0; +} // switchkinsRegisterJacobian() + int switchkinsRegisterToolFrameInverse(int ktype, KTI kinv) { if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { @@ -402,11 +436,13 @@ EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsWorkFrame); EXPORT_SYMBOL(kinematicsToolFrameInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(switchkinsRegister); EXPORT_SYMBOL(switchkinsRegisterFrames); EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); EXPORT_SYMBOL(switchkinsDeclare); EXPORT_SYMBOL(kinematicsTypeFlags); +EXPORT_SYMBOL(switchkinsRegisterJacobian); MODULE_LICENSE("GPL"); static int comp_id; @@ -443,6 +479,10 @@ 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 diff --git a/src/emc/kinematics/switchkins.h b/src/emc/kinematics/switchkins.h index d325ce75e48..77caca90629 100644 --- a/src/emc/kinematics/switchkins.h +++ b/src/emc/kinematics/switchkins.h @@ -73,4 +73,15 @@ 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); + +// 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); #endif // } diff --git a/src/emc/kinematics/trivkins.c b/src/emc/kinematics/trivkins.c index 4b3685dc6d6..f04d9642622 100644 --- a/src/emc/kinematics/trivkins.c +++ b/src/emc/kinematics/trivkins.c @@ -52,6 +52,14 @@ int kinematicsWorkFrame(const double *joints, return identityKinematicsWorkFrame(joints, rot, fflags); } +int kinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + return identityKinematicsJacobian(joints, pos, jac, iflags); +} + static KINEMATICS_TYPE ktype = -1; KINEMATICS_TYPE kinematicsType() @@ -72,6 +80,7 @@ EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsWorkFrame); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; From 56c1d43d25407827f454b10e764450efe56a6dfd Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:24 +1000 Subject: [PATCH 04/77] trtfuncs, 5axiskins, maxkins: supply the Jacobian Each inverse is a rotation of the pose about the table or the pivot plus offsets, so the derivative is the same rotation for the linear columns and the rotation advanced a quarter turn, times the lever, for the rotary ones. The tables and 5axiskins build the position and hand it to position_to_mapped_joints(), so they fill a matrix of the position's derivative and let kinsJacobianFromMappedAxes() place the rows, which keeps duplicate letters right. maxkins has fixed joint numbers and fills its rows directly. --- src/emc/kinematics/5axiskins.c | 42 ++++++++++++ src/emc/kinematics/maxkins.c | 44 ++++++++++++ src/emc/kinematics/trtfuncs.c | 101 ++++++++++++++++++++++++++++ src/emc/kinematics/xyzac-trt-kins.c | 2 + src/emc/kinematics/xyzbc-trt-kins.c | 2 + 5 files changed, 191 insertions(+) diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index aa0af3cfb8b..6e7923d58f4 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -162,6 +162,46 @@ static int fiveaxis_KinematicsInverse(const EmcPose * pos, return 0; } // fiveaxis_kinematicsInverse() +static int fiveaxis_KinematicsJacobian(const double *joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)joints; + (void)iflags; + rtapi_real pivot_length = hal_get_real(haldata->pivot_length); + const double R = pivot_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; + + memset(dP, 0, sizeof(dP)); + + // 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 kinsJacobianFromMappedAxes(fiveaxis_max_joints, + (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // fiveaxis_KinematicsJacobian() + int fiveaxis_KinematicsSetup(const int comp_id, const char* coordinates, kparms* kp) @@ -264,11 +304,13 @@ int switchkinsSetup(kparms* kp, *kinv1 = fiveaxis_KinematicsInverse; switchkinsDeclare(0, KINSTYPE_IDENTITY); switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterJacobian(1, fiveaxis_KinematicsJacobian); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); *kset0 = fiveaxis_KinematicsSetup; *kfwd0 = fiveaxis_KinematicsForward; *kinv0 = fiveaxis_KinematicsInverse; + switchkinsRegisterJacobian(0, fiveaxis_KinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; diff --git a/src/emc/kinematics/maxkins.c b/src/emc/kinematics/maxkins.c index d2623e0ad62..a5725480094 100644 --- a/src/emc/kinematics/maxkins.c +++ b/src/emc/kinematics/maxkins.c @@ -19,6 +19,7 @@ #include #include #include +#include #include #include /* these decls */ @@ -121,6 +122,48 @@ int kinematicsInverse(const EmcPose * pos, return 0; } +int kinematicsJacobian(const double *joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + rtapi_real pivot_length = hal_get_real(haldata->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 + pos->w; + int j; + + (void)joints; + (void)iflags; + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); + + // kinematicsInverse() 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; +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -130,6 +173,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; diff --git a/src/emc/kinematics/trtfuncs.c b/src/emc/kinematics/trtfuncs.c index 0cb4b5eb7aa..b445524b6e9 100644 --- a/src/emc/kinematics/trtfuncs.c +++ b/src/emc/kinematics/trtfuncs.c @@ -299,6 +299,57 @@ int xyzacKinematicsToolFrame(const double *joints, return 0; } // xyzacKinematicsToolFrame() +int xyzacKinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)joints; + (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 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 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; + + rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + + memset(dP, 0, sizeof(dP)); + + // the computed position P of xyzacKinematicsInverse(), 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 kinsJacobianFromMappedAxes(trtfuncs_max_joints, + (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // xyzacKinematicsJacobian() + int xyzbcKinematicsForward(const double *joints, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, @@ -443,3 +494,53 @@ int xyzbcKinematicsToolFrame(const double *joints, *rot = TOOL_FRAME_SPINDLE; return 0; } // xyzbcKinematicsToolFrame() + +int xyzbcKinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)joints; + (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 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 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; + + rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + + memset(dP, 0, sizeof(dP)); + + // see the comment in xyzacKinematicsJacobian(); 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 kinsJacobianFromMappedAxes(trtfuncs_max_joints, + (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // xyzbcKinematicsJacobian() diff --git a/src/emc/kinematics/xyzac-trt-kins.c b/src/emc/kinematics/xyzac-trt-kins.c index 13fd6bd79ec..b8bb47bbc1f 100644 --- a/src/emc/kinematics/xyzac-trt-kins.c +++ b/src/emc/kinematics/xyzac-trt-kins.c @@ -43,6 +43,7 @@ int switchkinsSetup(kparms* kp, &TOOL_FRAME_SPINDLE); switchkinsDeclare(0, KINSTYPE_IDENTITY); switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterJacobian(1, xyzacKinematicsJacobian); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); *kset0 = trtKinematicsSetup; // trt: xyzac,xyzbc @@ -51,6 +52,7 @@ int switchkinsSetup(kparms* kp, switchkinsRegisterFrames(0, xyzacKinematicsWorkFrame, xyzacKinematicsToolFrame, &TOOL_FRAME_SPINDLE); + switchkinsRegisterJacobian(0, xyzacKinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; diff --git a/src/emc/kinematics/xyzbc-trt-kins.c b/src/emc/kinematics/xyzbc-trt-kins.c index 73ea69bf820..7b61a69e301 100644 --- a/src/emc/kinematics/xyzbc-trt-kins.c +++ b/src/emc/kinematics/xyzbc-trt-kins.c @@ -43,6 +43,7 @@ int switchkinsSetup(kparms* kp, &TOOL_FRAME_SPINDLE); switchkinsDeclare(0, KINSTYPE_IDENTITY); switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterJacobian(1, xyzbcKinematicsJacobian); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); *kset0 = trtKinematicsSetup; // trt: xyzac,xyzbc @@ -51,6 +52,7 @@ int switchkinsSetup(kparms* kp, switchkinsRegisterFrames(0, xyzbcKinematicsWorkFrame, xyzbcKinematicsToolFrame, &TOOL_FRAME_SPINDLE); + switchkinsRegisterJacobian(0, xyzbcKinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; From 1e780265eb4531cfea7c0ec30d6ad11d66b33a95 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:25 +1000 Subject: [PATCH 05/77] corexykins, rotatekins, rosekins, matrixkins, millturn, userkins: supply the Jacobian The belt sum and difference, the rotation and its quarter turn, the polar radius and angle, the calibration matrix itself, and the two templates' joint to axis assignments. These are the short ones; they are here so that no module in the tree answers by differencing when its inverse is a few lines. --- src/emc/kinematics/corexykins.c | 19 ++++++++++++++++++ src/emc/kinematics/rosekins.c | 22 ++++++++++++++++++++ src/emc/kinematics/rotatekins.c | 23 +++++++++++++++++++++ src/hal/components/matrixkins.comp | 28 ++++++++++++++++++++++++++ src/hal/components/millturn.comp | 32 ++++++++++++++++++++++++++++++ src/hal/components/userkins.comp | 22 ++++++++++++++++++++ 6 files changed, 146 insertions(+) diff --git a/src/emc/kinematics/corexykins.c b/src/emc/kinematics/corexykins.c index f592ff52e9f..6d3ac4e75ab 100644 --- a/src/emc/kinematics/corexykins.c +++ b/src/emc/kinematics/corexykins.c @@ -6,6 +6,7 @@ #include #include +#include #include #include @@ -49,6 +50,23 @@ int kinematicsInverse(const EmcPose *pos return 0; } +int kinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + int j; + (void)joints; + (void)pos; + (void)iflags; + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][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; +} + int kinematicsHome(EmcPose *world ,double *joint ,KINEMATICS_FORWARD_FLAGS *fflags @@ -65,6 +83,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; diff --git a/src/emc/kinematics/rosekins.c b/src/emc/kinematics/rosekins.c index adfe763a33a..ac7a11159f5 100644 --- a/src/emc/kinematics/rosekins.c +++ b/src/emc/kinematics/rosekins.c @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -26,6 +27,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); #ifndef hypot @@ -112,6 +114,26 @@ int kinematicsInverse(const EmcPose * pos, return 0; } +int kinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + double x = pos->tran.x, y = pos->tran.y; + double r2 = x*x + y*y; + double r = sqrt(r2); + (void)joints; + (void)iflags; + // on the axis the angle is undefined and its rate unbounded + if (r2 <= 0) { return -1; } + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][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; +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; diff --git a/src/emc/kinematics/rotatekins.c b/src/emc/kinematics/rotatekins.c index 838c9178154..47f95bee2cd 100644 --- a/src/emc/kinematics/rotatekins.c +++ b/src/emc/kinematics/rotatekins.c @@ -15,6 +15,7 @@ #include #include #include +#include #include #include /* these decls */ @@ -60,6 +61,27 @@ int kinematicsInverse(const EmcPose * pos, return 0; } +int kinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + double c_rad = pos->c*M_PI/180; + double cc = cos(c_rad), sc = sin(c_rad); + int j; + (void)joints; + (void)iflags; + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][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; +} + /* implemented for these kinematics as giving joints preference */ int kinematicsHome(EmcPose * world, double *joint, @@ -81,6 +103,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); int comp_id; diff --git a/src/hal/components/matrixkins.comp b/src/hal/components/matrixkins.comp index aac6c04d913..8bf76899c8e 100644 --- a/src/hal/components/matrixkins.comp +++ b/src/hal/components/matrixkins.comp @@ -229,6 +229,7 @@ error: KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsForward); KINEMATICS_TYPE kinematicsType() @@ -321,3 +322,30 @@ int kinematicsInverse(const EmcPose * pos, return 0; } + +int kinematicsJacobian(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] = hal_get_real(haldata->C_xx); + jac[0][1] = hal_get_real(haldata->C_xy); + jac[0][2] = hal_get_real(haldata->C_xz); + jac[1][0] = hal_get_real(haldata->C_yx); + jac[1][1] = hal_get_real(haldata->C_yy); + jac[1][2] = hal_get_real(haldata->C_yz); + jac[2][0] = hal_get_real(haldata->C_zx); + jac[2][1] = hal_get_real(haldata->C_zy); + jac[2][2] = hal_get_real(haldata->C_zz); + for (r = 3; r < 9; r++) { jac[r][r] = 1; } + return 0; +} diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index 45ec650ad96..abb217f7a3a 100644 --- a/src/hal/components/millturn.comp +++ b/src/hal/components/millturn.comp @@ -100,6 +100,7 @@ EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsTypeFlags); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsForward); static rtapi_u32 switchkins_type; @@ -224,3 +225,34 @@ int kinematicsInverse(const EmcPose * pos, return 0; } // kinematicsInverse() + +int kinematicsJacobian(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 derivative of kinematicsInverse() for each type: which joint + // follows which pose coordinate, and in which sense + switch (switchkins_type) { + case 0: + jac[0][0] = 1; + jac[1][1] = 1; + jac[2][2] = 1; + jac[3][3] = 1; + break; + case 1: + jac[2][0] = 1; + jac[1][1] = -1; + jac[0][2] = 1; + jac[3][3] = 1; + break; + } + return 0; +} // kinematicsJacobian() diff --git a/src/hal/components/userkins.comp b/src/hal/components/userkins.comp index a7af5d29a75..ac0c003369d 100644 --- a/src/hal/components/userkins.comp +++ b/src/hal/components/userkins.comp @@ -128,6 +128,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsForward); KINEMATICS_TYPE kinematicsType() @@ -194,3 +195,24 @@ int kinematicsInverse(const EmcPose * pos, return 0; } // kinematicsInverse() + +int kinematicsJacobian(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; + // How each joint responds to each pose coordinate, the derivative of + // kinematicsInverse(): for this template joint 0 follows x, joint 1 + // follows y and joint 2 follows z, each one for one. See kinematics.h. + 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; +} // kinematicsJacobian() From 451fdd48bdca0bc2387e5e9177083a302320e514 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:25 +1000 Subject: [PATCH 06/77] xyzab_tdr_kins, xyzacb_trsrn, xyzbca_trsrn: supply the Jacobian The dual rotary table is its TCP inverse differentiated: the rotation matrix for the linear columns, and each term with A or B advanced a quarter turn for the rotary ones. The nutating heads are differentiated the same way, term by term through the secondary angle's r, s and t and the primary angle's sine and cosine. Their TCP inverse reads the rotary angles from the joint argument rather than from the pose, the two being the same numbers once a move is done; the derivative is taken against the pose, which is what a consumer multiplies by, and is the coupling the machine has. The TOOL type takes its angles from pins, so its inverse is linear in the pose and its rows are the coefficients. --- src/hal/components/xyzab_tdr_kins.comp | 62 +++++++++++ src/hal/components/xyzacb_trsrn.comp | 136 +++++++++++++++++++++++++ src/hal/components/xyzbca_trsrn.comp | 136 +++++++++++++++++++++++++ 3 files changed, 334 insertions(+) diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index dd0350e44f7..2ee61e6a9b8 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -95,6 +95,7 @@ EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsTypeFlags); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsForward); static rtapi_u32 switchkins_type; @@ -265,3 +266,64 @@ int kinematicsInverse(const EmcPose * pos, return 0; } // kinematicsInverse() + +int kinematicsJacobian(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 = 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 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 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; } + } + + switch (switchkins_type) { + case 0: // ====================== IDENTITY kinematics JACOBIAN ==================== + jac[0][0] = 1; + jac[1][1] = 1; + jac[2][2] = 1; + jac[3][3] = 1; + jac[4][4] = 1; + break; + case 1: // ========================= TCP kinematics JACOBIAN ====================== + // the TCP inverse above 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; + break; + } + return 0; +} // kinematicsJacobian() diff --git a/src/hal/components/xyzacb_trsrn.comp b/src/hal/components/xyzacb_trsrn.comp index 321d45584e5..67fd97715a8 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -91,6 +91,7 @@ EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsTypeFlags); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsWorkFrame); @@ -555,3 +556,138 @@ int kinematicsInverse(const EmcPose * pos, return 0; } // kinematicsInverse() + +int kinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)j; + (void)iflags; + + // the same geometry as kinematicsInverse(), read the same way + 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 + double Dt = hal_get_real(haldata->tool_offset_z); + + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + double Stc = sin(tc); + double Ctc = cos(tc); + + // 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 = 0, Cs = 0, Sp = 0, Cp = 0; + double CvSs = 0, SvSs = 0, r = 0, s = 0, t = 0; + // derivatives of the above over the secondary angle (Ss, r, s, t, CvSs, + // SvSs) and the primary angle (Sp, Cp), per degree + double dSs = 0, dr = 0, ds = 0, dt_ = 0, dCvSs = 0, dSvSs = 0; + double dSp = 0, dCp = 0; + + double Qy = pos->tran.y; + double Qz = pos->tran.z; + double Ay, Az; // the two lever arms the table turns about + int R, C; + + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } + } + + switch (switchkins_type) { + + case 0: // ========================= IDENTITY kinematics JACOBIAN ==================== + for (R = 0; R < 6; R++) { jac[R][R] = 1; } + break; + + case 1: // ========================= TCP kinematics JACOBIAN + Ss = sin(pos->b*TO_RAD); + Cs = cos(pos->b*TO_RAD); + Sp = sin(pos->c*TO_RAD); + Cp = cos(pos->c*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); + + dSs = Cs*TO_RAD; + dr = -Ss*Cv*Cv*TO_RAD; + ds = -Ss*Sv*Sv*TO_RAD; + dt_ = Sv*Cv*Ss*TO_RAD; + dCvSs = Cv*dSs; + dSvSs = Sv*dSs; + dSp = Cp*TO_RAD; + dCp = -Sp*TO_RAD; + + Ay = Dray + Dy + Ly - Qy; + Az = Draz + Dt + Lz - Qz; + + // 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; + break; + + case 2: // ========================= TOOL kinematics JACOBIAN + // the head angles come from pins, so the inverse is linear in + // the pose and the rows are its coefficients + 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); + + 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); + + 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); + + jac[2][0] = -(Ctc*SvSs - Stc*t); + jac[2][1] = (Stc*SvSs + Ctc*t); + jac[2][2] = s; + + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; + break; + } + return 0; +} // kinematicsJacobian() diff --git a/src/hal/components/xyzbca_trsrn.comp b/src/hal/components/xyzbca_trsrn.comp index 371349d2365..10126165eb1 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -93,6 +93,7 @@ EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsTypeFlags); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsWorkFrame); @@ -560,3 +561,138 @@ int kinematicsInverse(const EmcPose * pos, return 0; } // kinematicsInverse() + +int kinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)j; + (void)iflags; + + // the same geometry as kinematicsInverse(), read the same way + 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 + double Dt = hal_get_real(haldata->tool_offset_z); + + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + double Stc = sin(tc); + double Ctc = cos(tc); + + // 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 = 0, Cs = 0, Sp = 0, Cp = 0; + double CvSs = 0, SvSs = 0, r = 0, s = 0, t = 0; + // derivatives of the above over the secondary angle (Ss, r, s, t, CvSs, + // SvSs) and the primary angle (Sp, Cp), per degree + double dSs = 0, dr = 0, ds = 0, dt_ = 0, dCvSs = 0, dSvSs = 0; + double dSp = 0, dCp = 0; + + double Qx = pos->tran.x; + double Qz = pos->tran.z; + double Ax, Az; // the two lever arms the table turns about + int R, C; + + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } + } + + switch (switchkins_type) { + + case 0: // ========================= IDENTITY kinematics JACOBIAN ==================== + for (R = 0; R < 6; R++) { jac[R][R] = 1; } + break; + + case 1: // ========================= TCP kinematics JACOBIAN + Ss = sin(pos->a*TO_RAD); + Cs = cos(pos->a*TO_RAD); + Sp = sin(pos->c*TO_RAD); + Cp = cos(pos->c*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); + + dSs = Cs*TO_RAD; + dr = -Ss*Cv*Cv*TO_RAD; + ds = -Ss*Sv*Sv*TO_RAD; + dt_ = Sv*Cv*Ss*TO_RAD; + dCvSs = Cv*dSs; + dSvSs = Sv*dSs; + dSp = Cp*TO_RAD; + dCp = -Sp*TO_RAD; + + Ax = Drax + Dx + Lx - Qx; + Az = Draz + Dt + Lz - Qz; + + // 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; + break; + + case 2: // ========================= TOOL kinematics JACOBIAN + // the head angles come from pins, so the inverse is linear in + // the pose and the rows are its coefficients + 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); + + 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; + break; + } + return 0; +} // kinematicsJacobian() From f9f50424e370b2adb6b2fbf15cd48b536bcae0cb Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:25 +1000 Subject: [PATCH 07/77] tripodkins, lineardeltakins, rotarydeltakins, genhexkins, pentakins: supply the Jacobian A strut or rod changes length by the component of its moving end's motion along it, so the rows of the parallel machines are unit vectors and moments rather than differentiated formulas. The tripod's rows are the strut directions; the linear delta's are the rod directions scaled by the rise; the rotary delta's follow from the foot staying a shin from each knee, so the foot and the knee agree along the leg. The hexapod's rows are the ones its own Newton step already builds, with the rotary columns taken through the matrix that carries roll, pitch and yaw rates to the angular velocity; with a screw lead set, whose correction is a function of the pose too, it falls back to differencing. The pentapod differentiates InvKins() the same way, in effector coordinates. --- src/emc/kinematics/genhexkins.c | 69 ++++++++++++++++++++++++++ src/emc/kinematics/lineardeltakins.c | 26 ++++++++++ src/emc/kinematics/pentakins.c | 74 ++++++++++++++++++++++++++++ src/emc/kinematics/rotarydeltakins.c | 52 +++++++++++++++++++ src/emc/kinematics/tripodkins.c | 31 ++++++++++++ 5 files changed, 252 insertions(+) diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index 2966c377852..c634611ee62 100644 --- a/src/emc/kinematics/genhexkins.c +++ b/src/emc/kinematics/genhexkins.c @@ -544,6 +544,73 @@ static int genhexKinematicsInverse(const EmcPose * pos, return 0; } //genhexKinematicsInverse() +/************************ genhexKinematicsJacobian() ***********************/ +/* 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. */ + +static int genhexKinematicsJacobian(const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + PmCartesian aw, RMatrix_a, strut, u, moment; + PmRotationMatrix RMatrix; + PmRpy rpy; + PmCartesian E[3]; + double sb, cb, sc, cc; + int i, m; + + genhex_read_hal_pins(); + + /* the screw lead correction is a function of the pose too, and this + does not differentiate it; difference the inverse instead */ + if (hal_get_real(haldata->screw_lead) != 0.0) { + return kinsJacobianFromInverse(genhexKinematicsInverse, NUM_STRUTS, + joints, pos, iflags, jac); + } + + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); + + 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); + + /* 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; + + for (i = 0; i < NUM_STRUTS; i++) { + double len; + + pmMatCartMult(&RMatrix, &a[i], &RMatrix_a); + pmCartCartAdd(&pos->tran, &RMatrix_a, &aw); + pmCartCartSub(&aw, &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; +} // genhexKinematicsJacobian() + // 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] = { @@ -707,6 +774,7 @@ int switchkinsSetup(kparms* kp, *kinv1 = genhexKinematicsInverse; switchkinsDeclare(0, KINSTYPE_IDENTITY); switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterJacobian(1, genhexKinematicsJacobian); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); kp->fwd_iterates_mask = 0x1; //genhexkins switchkins_type==0 @@ -715,6 +783,7 @@ int switchkinsSetup(kparms* kp, *kset0 = genhexKinematicsSetup; *kfwd0 = genhexKinematicsForward; *kinv0 = genhexKinematicsInverse; + switchkinsRegisterJacobian(0, genhexKinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; diff --git a/src/emc/kinematics/lineardeltakins.c b/src/emc/kinematics/lineardeltakins.c index 353e9234562..d894bb46adb 100644 --- a/src/emc/kinematics/lineardeltakins.c +++ b/src/emc/kinematics/lineardeltakins.c @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -48,6 +49,30 @@ int kinematicsInverse(const EmcPose *pos, double *joints, return kinematics_inverse(pos, joints); } +int kinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { + double x = pos->tran.x, y = pos->tran.y, z = pos->tran.z; + int i, j; + (void)iflags; + set_geometry(hal_get_real(haldata->r), hal_get_real(haldata->l)); + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][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) ? Ax : (i == 1) ? Bx : Cx; + double ty = (i == 0) ? Ay : (i == 1) ? By : 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; +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -85,4 +110,5 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); diff --git a/src/emc/kinematics/pentakins.c b/src/emc/kinematics/pentakins.c index 18487be3134..551b79a4260 100644 --- a/src/emc/kinematics/pentakins.c +++ b/src/emc/kinematics/pentakins.c @@ -50,6 +50,7 @@ #include #include #include +#include #include #include /* these decls, KINEMATICS_FORWARD_FLAGS */ @@ -399,6 +400,78 @@ int kinematicsInverse(const EmcPose * pos, return 0; } +int kinematicsJacobian(const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + PmRotationMatrix R; + PmRpy rpy; + PmCartesian P, d, xyz, wa, wb, dxyz[5]; + int i, col; + + (void)joints; + (void)iflags; + pentakins_read_hal_pins(); + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); + + /* 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); + + /* 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; + + for (i = 0; i < NUM_STRUTS; i++) { + double rho, A, B, len; + + pmCartCartSub(&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 - za[i]; + B = rho - 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; +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -408,6 +481,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); diff --git a/src/emc/kinematics/rotarydeltakins.c b/src/emc/kinematics/rotarydeltakins.c index 8c83ebdec4f..92e8a763a99 100644 --- a/src/emc/kinematics/rotarydeltakins.c +++ b/src/emc/kinematics/rotarydeltakins.c @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -51,6 +52,56 @@ int kinematicsInverse(const EmcPose *pos, double *joints, return kinematics_inverse(pos, joints); } +int kinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { + int i, j; + (void)iflags; + set_geometry(hal_get_real(haldata->pfr), hal_get_real(haldata->tl), hal_get_real(haldata->sl), hal_get_real(haldata->fr)); + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][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 kinematics_forward() + // writes it, and q the hip angle that swings it. + for (i = 0; i < 3; i++) { + double q = D2R(joints[i]); + double reach = platformradius - footradius + 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 = thighlength * sin(q); + break; + case 1: + kx = reach * 0.5 * sqrt(3); ky = reach * 0.5; + dkx = -thighlength * sin(q) * 0.5 * sqrt(3); + dky = -thighlength * sin(q) * 0.5; + break; + default: + kx = -reach * 0.5 * sqrt(3); ky = reach * 0.5; + dkx = thighlength * sin(q) * 0.5 * sqrt(3); + dky = -thighlength * sin(q) * 0.5; + break; + } + kz = -thighlength * sin(q); + dkz = -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; +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -92,4 +143,5 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); diff --git a/src/emc/kinematics/tripodkins.c b/src/emc/kinematics/tripodkins.c index 990b7997297..ab5c7081b19 100644 --- a/src/emc/kinematics/tripodkins.c +++ b/src/emc/kinematics/tripodkins.c @@ -65,6 +65,7 @@ #include /* RTAPI realtime OS API */ #include /* RTAPI realtime module decls */ #include +#include #include #include /* these decls */ @@ -218,6 +219,35 @@ int kinematicsInverse(const EmcPose * pos, #undef Dz } +int kinematicsJacobian(const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + rtapi_real Bx = hal_get_real(haldata->bx); + rtapi_real Cx = hal_get_real(haldata->cx); + rtapi_real Cy = hal_get_real(haldata->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; + + (void)iflags; + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][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; + } + return 0; +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -356,6 +386,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); From 10bda8165486b08d380dd4409980d87247e29bd3 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:25 +1000 Subject: [PATCH 08/77] scarakins, scorbot-kins: supply the Jacobian Both inverses are chains of a few closed form steps, and the derivative follows the chain: for the scara the squared reach fixes the elbow and the bearing less the outer arm's angle fixes the shoulder; for the scorbot the distance to the wrist fixes the isosceles triangle the shoulder and elbow make. Each declines at the poses where its own inverse has no derivative, the arm straight or folded. --- src/emc/kinematics/scarakins.c | 52 +++++++++++++++++++++++ src/emc/kinematics/scorbot-kins.c | 70 +++++++++++++++++++++++++++++++ 2 files changed, 122 insertions(+) diff --git a/src/emc/kinematics/scarakins.c b/src/emc/kinematics/scarakins.c index 3b0e69ee7e4..db3ad15b737 100644 --- a/src/emc/kinematics/scarakins.c +++ b/src/emc/kinematics/scarakins.c @@ -179,6 +179,56 @@ static int scaraKinematicsInverse(const EmcPose * world, return (0); } // scaraKinematicsInverse() +static int scaraKinematicsJacobian(const double * joint, + const EmcPose * world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)iflags; + rtapi_real D2 = hal_get_real(haldata->d2); + rtapi_real D4 = hal_get_real(haldata->d4); + rtapi_real D6 = hal_get_real(haldata->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; + + if (rsq <= 0 || fabs(sin(q1)) < 1e-12) { + /* the arm folded or straight out: the elbow rate is unbounded */ + return -1; + } + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); + + /* 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; +} // scaraKinematicsJacobian() + #define DEFAULT_D1 490 #define DEFAULT_D2 340 #define DEFAULT_D3 50 @@ -233,11 +283,13 @@ int switchkinsSetup(kparms* kp, *kinv1 = scaraKinematicsInverse; switchkinsDeclare(0, KINSTYPE_IDENTITY); switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterJacobian(1, scaraKinematicsJacobian); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); *kset0 = scaraKinematicsSetup; *kfwd0 = scaraKinematicsForward; *kinv0 = scaraKinematicsInverse; + switchkinsRegisterJacobian(0, scaraKinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; diff --git a/src/emc/kinematics/scorbot-kins.c b/src/emc/kinematics/scorbot-kins.c index bd8868a063d..828f7b4ec43 100644 --- a/src/emc/kinematics/scorbot-kins.c +++ b/src/emc/kinematics/scorbot-kins.c @@ -41,6 +41,7 @@ #include #include #include +#include #include #include @@ -294,6 +295,74 @@ int kinematicsInverse( } +int kinematicsJacobian( + const double *joints, + const EmcPose *pose, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags +) { + // kinematicsInverse() 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; + + (void)joints; + (void)iflags; + if (rho2 <= 0) { return -1; } + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); + + // j0 = atan2(y, x) + jac[0][0] = -y/rho2 * TO_DEG; + jac[0][1] = x/rho2 * TO_DEG; + + 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; + + 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; + } + + // 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); + } + + 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); + } + + 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]; + } + + 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; +} + KINEMATICS_TYPE kinematicsType(void) { return KINEMATICS_BOTH; } @@ -302,6 +371,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; From c2feb437b292d74d9e3f829e0cd247fada4fee6a Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:25 +1000 Subject: [PATCH 09/77] genserkins: supply the Jacobian from its geometric one compute_jinv() already gives 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 the matrix of the axes each one turns about, for the RPY convention go_rpy_mat_convert() uses. The Jacobian is that product, with the unit conversions and the unrotate coupling applied in the order the inverse applies them, and the u, v, w pass-through as ones. Having the conversion written once in the module is worth more than the closed form itself, since every consumer would otherwise guess it. --- src/emc/kinematics/genserfuncs.c | 106 +++++++++++++++++++++++++++++++ src/emc/kinematics/genserkins.c | 2 + src/emc/kinematics/genserkins.h | 5 ++ 3 files changed, 113 insertions(+) diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index 5600ab2be1f..02703735b2a 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -37,6 +37,7 @@ #include #endif #include +#include #include #include "libposemath/gotypes.h" /* go_result, go_integer */ #include "libposemath/gomath.h" /* go_pose */ @@ -313,6 +314,111 @@ 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 genserKinematicsInverse(). + + 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. */ +int genserKinematicsJacobian(const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)iflags; + genser_struct *genser = KINS_PTR; + 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, a, m, retval; + +#ifndef ULAPI + genser_kin_init(); + if (!genser_hal_inited) { + rtapi_print_msg(RTAPI_MSG_ERR, + "genserKinematicsJacobian: not initialized\n"); + return -1; + } +#endif + + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][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 = hal_get_si32(haldata->unrotate[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 = hal_get_si32(haldata->unrotate[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 (total_joints > 6) jac[6][6] = 1; + if (total_joints > 7) jac[7][7] = 1; + if (total_joints > 8) jac[8][8] = 1; + + return 0; +} // genserKinematicsJacobian() + /* main function called by emc2 for forward Kins */ int genserKinematicsForward(const double *joint, EmcPose * world, diff --git a/src/emc/kinematics/genserkins.c b/src/emc/kinematics/genserkins.c index fa8d30598dc..94a325cbcf6 100644 --- a/src/emc/kinematics/genserkins.c +++ b/src/emc/kinematics/genserkins.c @@ -74,11 +74,13 @@ int switchkinsSetup(kparms* kp, *kinv1 = genserKinematicsInverse; switchkinsDeclare(0, KINSTYPE_IDENTITY); switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterJacobian(1, genserKinematicsJacobian); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); *kset0 = genserKinematicsSetup; *kfwd0 = genserKinematicsForward; *kinv0 = genserKinematicsInverse; + switchkinsRegisterJacobian(0, genserKinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; diff --git a/src/emc/kinematics/genserkins.h b/src/emc/kinematics/genserkins.h index 3aa0756fc5a..b74b826d2ec 100644 --- a/src/emc/kinematics/genserkins.h +++ b/src/emc/kinematics/genserkins.h @@ -142,6 +142,11 @@ extern int compute_jfwd(go_link * link_params, extern int compute_jinv(go_matrix * Jfwd, go_matrix * Jinv); +extern int genserKinematicsJacobian(const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + extern int genserKinematicsForward(const double *joint, EmcPose * world, const KINEMATICS_FORWARD_FLAGS * fflags, From fb9020b99b2a5a0849e003fdd047829796923e59 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 21 Sep 2026 21:35:56 +1000 Subject: [PATCH 10/77] pumakins, three21kins: supply the Jacobian from the Denavit-Hartenberg chain The rule the chapter states is that every module supplies a closed-form Jacobian, and the two arms whose inverse is a chain of arc tangents were the exception, answering through eighteen differenced inverses. That costs 3 us instead of 0.3 on this machine, which is not the point; the point is what the differences do near the wrist singularity, where the inverse flips its branch flags a step away from the pose and the difference reads two branches at once, so a consumer capping a feed or a jog sees a magnitude that means nothing. kinsJacobianFromDhArm() in kins_util.c takes the arm's table in Craig's convention, alpha, a and d per link and the tool point along the last z, composes the chain at the given joints, and builds the geometric Jacobian: each joint's axis crossed with the vector from it to the tool point for the point's rate, the axis itself for the angular rate. The six by six is inverted by Gauss-Jordan with row pivoting, -1 where a pivot is too small to trust, and the angular columns go through the matrix of the axes the roll, pitch and yaw rates turn about, as genserkins already does with its own chain. pumakins is Craig's PUMA 560 table with D6 the tool point; three21kins the same with A1 and D1 setting the shoulder off the base and D2 and D3 both along the upper arm's axis. Both register it for their arm type. tests/kins-jacobian already checks both arms against the differenced forward and the differenced inverse to 1e-6; a table entry one degree off fails the forward check, so the closed form is what the test sees. --- docs/src/motion/kinematics-conventions.adoc | 11 +- src/emc/kinematics/kinematics.h | 23 ++++ src/emc/kinematics/kins_util.c | 118 ++++++++++++++++++++ src/emc/kinematics/pumakins.c | 21 ++++ src/emc/kinematics/three21kins.c | 21 ++++ 5 files changed, 191 insertions(+), 3 deletions(-) diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 4596796ca03..5740fbc7012 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -462,9 +462,14 @@ servo thread. A module with a closed form registers it with `switchkinsRegisterJacobian()`. 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. +the pose. Every module in the tree supplies one. The two arms whose inverse is +a chain of arc tangents, `pumakins` and `three21kins`, take theirs from +their Denavit-Hartenberg chain through `kinsJacobianFromDhArm()`, which any +six-joint serial arm can call with its own table, written in the modified +convention of Craig's Introduction to Robotics: each joint's axis crossed +with the vector from it to the tool point gives the point's rate, the axis +itself the angular rate, and the six by six that makes is inverted and +taken through the roll, pitch and yaw rates the pose words are. 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 diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index 900fe5aa474..c4ef1f45ba0 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -439,6 +439,29 @@ extern int kinsJacobianFromMappedAxes(int max_joints, const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); +/* The Jacobian of a serial arm of six revolute joints from its + Denavit-Hartenberg chain. Two conventions carry that name and put the + four parameters on different links; this is the modified one of John J. + Craig, Introduction to Robotics: Mechanics and Control, where link i is + Rx(alpha[i]) Tx(a[i]) Rz(joint[i]) Tz(d[i]), the joint turning about the + z of the frame Rx and Tx leave it in. (The original 1955 convention is + Rz(theta) Tz(d) Tx(a) Rx(alpha), and a table written for it does not fit + here.) The tool point `tool` lies along the z of the last frame, and + the pose of that point is reported as X Y Z and the RPY of the last + frame, R = Rz(C) Ry(B) Rx(A), as pmMatRpyConvert() does. + Each joint's axis crossed with the vector from it to the tool point + gives the point's rate per radian of the joint, the axis itself the + angular rate; that 6x6 inverted is the joint rate per unit of twist, and + the RPY rates reach the twist through the matrix of the axes each one + turns about, which is where world's B and C come in. alpha and joint in + degrees, a, d and tool in the module's length unit. Rows 0 to 5 of jac + are filled, the rest zero. Returns 0, or -1 at a singular pose, where no + finite joint rate follows the pose. */ +extern int kinsJacobianFromDhArm(const double alpha[6], const double a[6], + const double d[6], const double *joint, + double tool, const EmcPose *world, + 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, diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index 324392e6864..27773c9edb1 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -1138,6 +1138,124 @@ int kinsJacobianFromMappedAxes(int max_joints, return 0; } // kinsJacobianFromMappedAxes() +/* r = r * Rx(angle), r = r * Rz(angle): a rotation composed on the right */ +static void kj_rot_x(double r[3][3], double angle) +{ + double c = cos(angle), s = sin(angle); + int i; + for (i = 0; i < 3; i++) { + double y = r[i][1], z = r[i][2]; + r[i][1] = y * c + z * s; + r[i][2] = -y * s + z * c; + } +} + +static void kj_rot_z(double r[3][3], double angle) +{ + double c = cos(angle), s = sin(angle); + int i; + for (i = 0; i < 3; i++) { + double x = r[i][0], y = r[i][1]; + r[i][0] = x * c + y * s; + r[i][1] = -x * s + y * c; + } +} + +/* m = inverse of the 6x6 m, by Gauss-Jordan with row pivoting; -1 where a + pivot is too small to trust, the arm singular */ +static int kj_invert6(double m[6][6]) +{ + double inv[6][6]; + int i, j, k, piv; + + for (i = 0; i < 6; i++) { + for (j = 0; j < 6; j++) { inv[i][j] = (i == j) ? 1.0 : 0.0; } + } + for (k = 0; k < 6; k++) { + double big = fabs(m[k][k]); + piv = k; + for (i = k + 1; i < 6; i++) { + if (fabs(m[i][k]) > big) { big = fabs(m[i][k]); piv = i; } + } + if (big < 1e-9) { return -1; } + if (piv != k) { + for (j = 0; j < 6; j++) { + double t = m[k][j]; m[k][j] = m[piv][j]; m[piv][j] = t; + t = inv[k][j]; inv[k][j] = inv[piv][j]; inv[piv][j] = t; + } + } + { + double f = 1.0 / m[k][k]; + for (j = 0; j < 6; j++) { m[k][j] *= f; inv[k][j] *= f; } + } + for (i = 0; i < 6; i++) { + double f = m[i][k]; + if (i == k || f == 0.0) { continue; } + for (j = 0; j < 6; j++) { m[i][j] -= f * m[k][j]; inv[i][j] -= f * inv[k][j]; } + } + } + memcpy(m, inv, sizeof(inv)); + return 0; +} + +int kinsJacobianFromDhArm(const double alpha[6], const double a[6], + const double d[6], const double *joint, + double tool, const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) +{ + double r[3][3] = { {1, 0, 0}, {0, 1, 0}, {0, 0, 1} }; + double o[3] = { 0, 0, 0 }; + double z[6][3], p[6][3], e[3]; + double jfwd[6][6], E[3][3]; + double sb, cb, sc, cc; + int i, j, m; + + if (!alpha || !a || !d || !joint || !world || !jac) { return -1; } + kj_zero(jac); + + /* the chain: after Rx(alpha) Tx(a) the frame's z is joint i's axis and + its origin a point on it; Rz(joint) Tz(d) then carry on to the next */ + for (i = 0; i < 6; i++) { + kj_rot_x(r, alpha[i] * PM_PI / 180); + for (j = 0; j < 3; j++) { o[j] += r[j][0] * a[i]; } + for (j = 0; j < 3; j++) { z[i][j] = r[j][2]; p[i][j] = o[j]; } + kj_rot_z(r, joint[i] * PM_PI / 180); + for (j = 0; j < 3; j++) { o[j] += r[j][2] * d[i]; } + } + /* the tool point, along the last z */ + for (j = 0; j < 3; j++) { e[j] = o[j] + r[j][2] * tool; } + + /* the point's rate and the angular rate per radian of each joint */ + for (i = 0; i < 6; i++) { + double v[3] = { e[0] - p[i][0], e[1] - p[i][1], e[2] - p[i][2] }; + jfwd[0][i] = z[i][1] * v[2] - z[i][2] * v[1]; + jfwd[1][i] = z[i][2] * v[0] - z[i][0] * v[2]; + jfwd[2][i] = z[i][0] * v[1] - z[i][1] * v[0]; + for (j = 0; j < 3; j++) { jfwd[3 + j][i] = z[i][j]; } + } + if (kj_invert6(jfwd) != 0) { return -1; } + + /* E: the roll axis carried by pitch and yaw, the pitch axis carried + by yaw, 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 < 6; i++) { + /* linear pose words: the joint comes out in radians per unit */ + for (j = 0; j < 3; j++) { jac[i][j] = jfwd[i][j] * (180 / PM_PI); } + /* angular pose words through E: degrees per degree */ + for (m = 0; m < 3; m++) { + double s = 0; + for (j = 0; j < 3; j++) { s += jfwd[i][3 + j] * E[j][m]; } + jac[i][3 + m] = s; + } + } + return 0; +} // kinsJacobianFromDhArm() + int identityKinematicsJacobian(const double *joint, const EmcPose *world, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index 049d384b424..aace42d7b37 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -211,6 +211,25 @@ static int pumaKinematicsForward(const double * joint, return 0; } +/* The Jacobian from the arm's Denavit-Hartenberg chain, the PUMA 560 + table of Craig's Introduction to Robotics in his modified convention, + which is the one the forward above encodes: the shoulder turns about + the base z, the upper arm and forearm + about axes at right angles to it, the wrist about three axes meeting at + its centre, and D6 carries the tool point out along the flange z. */ +static int pumaKinematicsJacobian(const double * joint, + const EmcPose * world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)iflags; + const double alpha[6] = { 0, -90, 0, -90, 90, -90 }; + const double a[6] = { 0, 0, hal_get_real(haldata->a2), hal_get_real(haldata->a3), 0, 0 }; + const double d[6] = { 0, 0, hal_get_real(haldata->d3), hal_get_real(haldata->d4), 0, 0 }; + + return kinsJacobianFromDhArm(alpha, a, d, joint, hal_get_real(haldata->d6), world, jac); +} // pumaKinematicsJacobian() + static int pumaKinematicsToolFrame(const double * joint, PmRotationMatrix * rot, const KINEMATICS_FORWARD_FLAGS * fflags) @@ -439,6 +458,7 @@ int switchkinsSetup(kparms* kp, switchkinsRegisterFrames(1, pumaKinematicsWorkFrame, pumaKinematicsToolFrame, &TOOL_FRAME_FLANGE); + switchkinsRegisterJacobian(1, pumaKinematicsJacobian); switchkinsDeclare(0, KINSTYPE_IDENTITY); switchkinsDeclare(1, KINSTYPE_PRIMARY); } else { @@ -451,6 +471,7 @@ int switchkinsSetup(kparms* kp, switchkinsRegisterFrames(0, pumaKinematicsWorkFrame, pumaKinematicsToolFrame, &TOOL_FRAME_FLANGE); + switchkinsRegisterJacobian(0, pumaKinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; diff --git a/src/emc/kinematics/three21kins.c b/src/emc/kinematics/three21kins.c index 2a3dfe08ee5..30c7f938e15 100644 --- a/src/emc/kinematics/three21kins.c +++ b/src/emc/kinematics/three21kins.c @@ -190,6 +190,25 @@ static int three21KinematicsForward(const double * joint, return 0; } +/* The Jacobian from the arm's Denavit-Hartenberg chain: the PUMA table + with the shoulder set A1 out along the first link and D1 up the base, + D2 and D3 both along the axis the upper arm turns about, and D6 carrying + the tool point out along the flange z. */ +static int three21KinematicsJacobian(const double * joint, + const EmcPose * world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)iflags; + const double alpha[6] = { 0, -90, 0, -90, 90, -90 }; + const double a[6] = { 0, hal_get_real(haldata->a1), hal_get_real(haldata->a2), + hal_get_real(haldata->a3), 0, 0 }; + const double d[6] = { hal_get_real(haldata->d1), hal_get_real(haldata->d2), + hal_get_real(haldata->d3), hal_get_real(haldata->d4), 0, 0 }; + + return kinsJacobianFromDhArm(alpha, a, d, joint, hal_get_real(haldata->d6), world, jac); +} // three21KinematicsJacobian() + static int three21KinematicsInverse(const EmcPose * world, double * joint, const KINEMATICS_INVERSE_FLAGS * iflags, @@ -396,6 +415,7 @@ int switchkinsSetup(kparms* kp, *kset1 = three21KinematicsSetup; *kfwd1 = three21KinematicsForward; *kinv1 = three21KinematicsInverse; + switchkinsRegisterJacobian(1, three21KinematicsJacobian); switchkinsDeclare(0, KINSTYPE_IDENTITY); switchkinsDeclare(1, KINSTYPE_PRIMARY); } else { @@ -403,6 +423,7 @@ int switchkinsSetup(kparms* kp, *kset0 = three21KinematicsSetup; *kfwd0 = three21KinematicsForward; *kinv0 = three21KinematicsInverse; + switchkinsRegisterJacobian(0, three21KinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; From df7587578e9bcecad9477060a3be1605807ab59c Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:25 +1000 Subject: [PATCH 11/77] tests: check the Jacobian of every module where it runs A realtime component loaded after the module under test, reaching it through the exported entry points; a failed check fails the load. Every module in the tree, every switchkins type, both direction settings on the tables. Two checks, neither reusing the module's own answer. Against the forward: perturb one joint, difference the forward, multiply by the Jacobian and expect that joint's unit vector, which catches a transposed matrix, a wrong sign, column or unit whichever way the module answered. Against the inverse: difference it here with a different step and compare entry by entry, the check for the gantry, whose forward is not one to one. Verified by mutation, one per module or shared routine, every one caught. The forward check is also a round trip of each module and found four whose forward and inverse disagreed, fixed in the commits before this one. maxkins, which disagrees away from c = 0 and u = 0, is checked against its inverse; the nutating heads read their angles from the inverse's joint argument, so they are checked against the forward. --- tests/kins-jacobian/checkresult | 4 + tests/kins-jacobian/jaccheck.c | 360 ++++++++++++++++++++++++++++++++ tests/kins-jacobian/skip | 4 + tests/kins-jacobian/test.sh | 173 +++++++++++++++ 4 files changed, 541 insertions(+) create mode 100755 tests/kins-jacobian/checkresult create mode 100644 tests/kins-jacobian/jaccheck.c create mode 100755 tests/kins-jacobian/skip create mode 100755 tests/kins-jacobian/test.sh 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..57a3a2e45e3 --- /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 from their +# Denavit-Hartenberg chain and both checks prove 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" From d48d5e80392e84e6cff1dd60218355433466b30a Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 11 Sep 2026 15:43:09 +0800 Subject: [PATCH 12/77] kinematics.h: C linkage guards, and the include guard closed at the end of the file The definitions are C and the header had no linkage guards, so the first C++ include of it, emc_nml.hh in many translation units, gave the declarations C++ linkage and references to the C definitions (toolFrameInWork, the TRT tables) no longer link. The header now wraps its declarations in extern "C" for a C++ includer, as a C header does. Its include guard also closed before the trt declarations at the end of the file, so a second include, which motion.h and emc_nml.hh together make, declared trtKinematicsSetup and the xyzac and xyzbc entry points twice. The guard now closes at the end of the file. --- src/emc/kinematics/kinematics.h | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index c4ef1f45ba0..5597d7a14a6 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -19,6 +19,10 @@ #include "emcmotcfg.h" /* EMCMOT_MAX_JOINTS, EMCMOT_MAX_AXIS */ #include "rtapi_bool.h" +#ifdef __cplusplus +extern "C" { +#endif + /* The type of kinematics used. @@ -497,7 +501,6 @@ 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, @@ -552,3 +555,8 @@ extern int xyzbcKinematicsJacobian(const double *joints, const KINEMATICS_INVERSE_FLAGS *iflags); //********************************************************************* +#ifdef __cplusplus +} +#endif + +#endif // __LINUXCNC_KINEMATICS_H From 7e287d6521f5b76b93de27176c434589b73eab92 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 11 Sep 2026 19:55:55 +0800 Subject: [PATCH 13/77] interp: G43.4 and G49 as spellings over the kinematics switch G43.4 is G43 on the module's working kinematics: it switches to the kinstype declared KINSTYPE_PRIMARY, then applies the offset, as if the switch line had run and drained. G49 clears the offset and drops to the identity kinstype, but only when the offset in effect is still G43.4's: a G12.1 or G13.1 in between, or a plain G43, means the program took the kinematics over and G49 leaves it alone. A switchable module without a primary rejects G43.4 at read time; without an identity it keeps the plain cancel; with no kinematics attached (sai, preview) both are plain. Already on the target kinstype there is no switch and no drain. The modal group 8 label follows the kinstype motion reports, which is the only authority, and a flag value of -1, "no information", matches every flag. The switch inside G49 exposed a latent deadlock: the startup code runs at task init, before the main loop can service INTERP_EXECUTE_FINISH, so a switch requested there waited forever, as G12.1 in the startup code did. Both now queue the switch without the wait while the startup code runs; no motion exists yet and the switch lands before the first move. tests/kins-switch walks G43.4, G49, G43 and G12.1 through the cases; docs in g-code.adoc and switchkins.adoc. --- docs/src/gcode/g-code.adoc | 49 +++++++++++++ docs/src/gcode/overview.adoc | 2 +- docs/src/motion/switchkins.adoc | 39 +++++++---- src/emc/rs274ngc/interp_array.cc | 2 +- src/emc/rs274ngc/interp_base.hh | 4 ++ src/emc/rs274ngc/interp_check.cc | 4 +- src/emc/rs274ngc/interp_convert.cc | 104 ++++++++++++++++++++-------- src/emc/rs274ngc/interp_internal.hh | 3 + src/emc/rs274ngc/interp_setup.cc | 2 + src/emc/rs274ngc/interp_write.cc | 13 +++- src/emc/rs274ngc/rs274ngc_interp.hh | 1 + src/emc/rs274ngc/rs274ngc_pre.cc | 5 ++ src/emc/rs274ngc/rs274ngc_return.hh | 1 + src/emc/task/emctask.cc | 5 ++ tests/kins-switch/test-ui.py | 17 +++++ tests/kins-switch/test.ngc | 29 ++++++++ tests/kins-switch/tool.tbl | 2 +- 17 files changed, 231 insertions(+), 51 deletions(-) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 7e27083cfad..8162e52c993 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -85,6 +85,7 @@ 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 |<> |Cancel Tool Length Offset |<> |Local Coordinate System Offset |<> |Move in Machine Coordinates @@ -1717,11 +1718,59 @@ 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:g49]] == G49 Cancel Tool Length Compensation(((G49 Cancel Tool Length Offset))) * 'G49' - cancels tool length compensation +'G49' also switches a switchable kinematics module back to its identity +kinematics when it cancels a 'G43.4', undoing the switch that made. It +leaves a kinematics selected by 'G12.1' or 'G13.1' alone, as it does +after a plain 'G43', and a module that declares no identity kinematics +gets the plain cancel. + 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. diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index 85ff1a127a5..2b8a6829a91 100644 --- a/docs/src/gcode/overview.adoc +++ b/docs/src/gcode/overview.adoc @@ -973,7 +973,7 @@ 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, G49 |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/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 8c1fea13e24..fe1fd05eed9 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -218,8 +218,12 @@ 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, and the 'G49' that cancels it switches back to identity. +See the G-code documentation for 'G43.4' and 'G49', and for 'G12.1' +and 'G13.1', for the full description. === M-code commands @@ -491,26 +495,31 @@ 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: - -. *KINSTYPE_IDENTITY* no transform: the joints are the world -. *KINSTYPE_PRIMARY* the module's working transform - 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. +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. After calling switchkinsSetup(), rtapi_app_main() checks the supplied parameters, creates a HAL component, and then invokes diff --git a/src/emc/rs274ngc/interp_array.cc b/src/emc/rs274ngc/interp_array.cc index 6bfaa513b4e..829c1777337 100644 --- a/src/emc/rs274ngc/interp_array.cc +++ b/src/emc/rs274ngc/interp_array.cc @@ -91,7 +91,7 @@ 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,-1,-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, 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 1eeb8625c1a..40992b41268 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -286,8 +286,8 @@ 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), + _("H word with no G43, G43.4 or G76 to use it")); } if (block->i_flag) { /* could still be useless if yz_plane arc */ diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 93577daa852..9456c1d56d5 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)) @@ -4136,12 +4136,13 @@ 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)) && 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); @@ -4451,8 +4452,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)); @@ -6478,6 +6481,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 @@ -6518,9 +6560,21 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu CHKS((settings->cutter_comp_side != CUTTER_COMP::OFF), (_("Cannot change tool offset with cutter radius compensation on"))); + if (g_code == G_43_4) { + 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. + CHKS(primary < 0 && kins_type_info_available(), NCE_NO_PRIMARY_KINEMATICS_TYPE); + if (primary >= 0) { 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; + } if (g_code == G_49) { idx = 0; - } else if (g_code == G_43) { + } else if (g_code == G_43 || g_code == G_43_4) { 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) { @@ -6600,7 +6654,7 @@ 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, or G49"); } USE_TOOL_LENGTH_OFFSET(tool_offset); @@ -6641,6 +6695,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; } @@ -6698,19 +6762,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 @@ -6718,16 +6769,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 @@ -6744,6 +6788,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_internal.hh b/src/emc/rs274ngc/interp_internal.hh index b833a94e778..4f82414d158 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -248,6 +248,7 @@ enum GCodes G_43 = 430, G_43_1 = 431, G_43_2 = 432, + G_43_4 = 434, G_49 = 490, G_50 = 500, G_51 = 510, @@ -760,6 +761,7 @@ 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 toolchange_flag; // flag indicating we just had a tool change bool home_flag; // flag indicating a G28.2 homing cycle just ran int input_index; // channel queried @@ -863,6 +865,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_setup.cc b/src/emc/rs274ngc/interp_setup.cc index d0611d9da9f..f8a456f2c32 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -118,6 +118,7 @@ setup::setup() : input_flag(0), kinsSwitch_flag(0), kins_type(0), + kins_by_g43_4(false), toolchange_flag(0), home_flag(0), input_index(0), @@ -202,6 +203,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_write.cc b/src/emc/rs274ngc/interp_write.cc index b6b2e7d53cf..b61982ea4f8 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,11 +110,17 @@ 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, 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)) ? G_43_4 : G_43) : G_49; settings->active_g_codes[10] = (settings->retract_mode == RETRACT_MODE::OLD_Z) ? G_98 : G_99; // Three modes: G_64, G_61, G_61_1 or CANON_CONTINUOUS/EXACT_PATH/EXACT_STOP settings->active_g_codes[11] = diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index cfbc3f34720..6ee52ae4e44 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(); diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index 3b4d2a40f57..df0f7081a7e 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -1307,6 +1307,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; +} + /***********************************************************************/ 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/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/tests/kins-switch/test-ui.py b/tests/kins-switch/test-ui.py index 9209eb2aab5..7e999b9aa76 100755 --- a/tests/kins-switch/test-ui.py +++ b/tests/kins-switch/test-ui.py @@ -130,6 +130,23 @@ def mdi(cmd): else: print("the lines after the selections reported %s" % " | ".join(reported)) +# 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 PIN=0.000000 TLOZ=12.500000", + "G434 KINSTYPE=1.000000 PIN=1.000000 TLOZ=0.000000", + "G434 KINSTYPE=1.000000 PIN=1.000000 TLOZ=12.500000", + "G434 KINSTYPE=0.000000 PIN=0.000000 TLOZ=0.000000", + "G434 KINSTYPE=0.000000 PIN=0.000000 TLOZ=0.000000", + "G434 KINSTYPE=0.000000 PIN=0.000000 TLOZ=12.500000", + "G434 KINSTYPE=1.000000 PIN=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 negative kinematics number is refused ----------------------------- c.mode(linuxcnc.MODE_MDI) diff --git a/tests/kins-switch/test.ngc b/tests/kins-switch/test.ngc index 6ff7f64423d..61dd3c76e9f 100644 --- a/tests/kins-switch/test.ngc +++ b/tests/kins-switch/test.ngc @@ -21,4 +21,33 @@ g12.1 p0 ; it is the flag that decides, not the number g13.1 (debug, KINSTYPE=#<_kins_type> PIN=#<_hal[motion.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> PIN=#<_hal[motion.kins-type]> TLOZ=#5083) +; G49 cancels the offset and undoes the switch G43.4 made +g49 +(debug, G434 KINSTYPE=#<_kins_type> PIN=#<_hal[motion.kins-type]> TLOZ=#5083) +; a plain G43 applies the offset but does not switch +g43 h1 +(debug, G434 KINSTYPE=#<_kins_type> PIN=#<_hal[motion.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> PIN=#<_hal[motion.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> PIN=#<_hal[motion.kins-type]> TLOZ=#5083) +; and back once more, ending in identity +g43.4 h1 +(debug, G434 KINSTYPE=#<_kins_type> PIN=#<_hal[motion.kins-type]> TLOZ=#5083) +g49 +(debug, G434 KINSTYPE=#<_kins_type> PIN=#<_hal[motion.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 ; From 082552d36aa5128ebc1183b39126930375f12d5d Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 23 Sep 2026 21:15:52 +1000 Subject: [PATCH 14/77] emccanon: B and C moves use the angular minimum displacement applyMinDisplacement() drops an axis delta below the smallest move motion can take. A used the angular threshold, B and C the linear one, so on an inch machine a B or C move of up to 25.4 CART_FUZZ degrees was dropped where the same A move was kept. --- src/emc/task/emccanon.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index bb6c2deafb3..0461f233b6d 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -623,8 +623,8 @@ static void applyMinDisplacement(double &dx, if(!axis_valid(1) || dy < tiny_linear) dy = 0.0; if(!axis_valid(2) || dz < tiny_linear) dz = 0.0; if(!axis_valid(3) || da < tiny_angular) da = 0.0; - if(!axis_valid(4) || db < tiny_linear) db = 0.0; - if(!axis_valid(5) || dc < tiny_linear) dc = 0.0; + if(!axis_valid(4) || db < tiny_angular) db = 0.0; + if(!axis_valid(5) || dc < tiny_angular) dc = 0.0; if(!axis_valid(6) || du < tiny_linear) du = 0.0; if(!axis_valid(7) || dv < tiny_linear) dv = 0.0; if(!axis_valid(8) || dw < tiny_linear) dw = 0.0; From 96fd7f68d8160fc9a2b35c8b2357204f92776529 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 23 Sep 2026 21:23:47 +1000 Subject: [PATCH 15/77] Honor [AXIS_] TYPE in the interpreter and canon The interpreter and canon decided by letter what an axis is: X Y Z U V W lengths, A B C angles. [AXIS_] TYPE was read by the AXIS GUI alone, so a V configured ANGULAR on a mm machine moved 25.4 times too far under G20, and an A configured LINEAR did not scale at all. Both now read one table, axis_kinds.hh: TYPE per letter, with the letter's default, and [TRAJ] FEED_AXES, the axes F is measured along, X Y Z by default. Every G20/G21 conversion of a position, offset, stored position (#5161, #5181, #5211, #5221 onward), tool offset and applied tool offset goes by type, in the interpreter and in canon. X Y Z must stay LINEAR. The move length the feed applies to is, in the interpreter (G93) and in canon alike: the feed axes that move, else the other linear axes, else the angular axes in degrees. With the defaults that is XYZ, else UVW, else ABC, as before. Motion still measures a line by letter, so canon scales the rates it sends by motion's length over its own, and the move takes the time canon planned. The ratio is exactly 1 whenever both measure the same axes of one kind, which the defaults always do. WRAPPED_ROTARY and LOCKING_INDEXER_JOINT apply to any ANGULAR axis, U V W included, and are ignored on a LINEAR one as they were on U V W. With every TYPE and FEED_AXES at its default the output is unchanged: all 632 programs under tests/ and nc_files/, through rs274 with no INI and with the axis_mm, axis and axis_9axis sims, give the same canon calls byte for byte before and after. --- docs/src/config/ini-config.adoc | 17 +- docs/src/gcode/machining-center.adoc | 30 +- src/Makefile | 1 + src/emc/ini/axis_kinds.hh | 120 +++++ src/emc/rs274ngc/interp_check.cc | 4 +- src/emc/rs274ngc/interp_convert.cc | 477 ++++++++++--------- src/emc/rs274ngc/interp_find.cc | 162 +++++-- src/emc/rs274ngc/interp_internal.cc | 31 +- src/emc/rs274ngc/interp_internal.hh | 11 +- src/emc/rs274ngc/interp_queue.cc | 26 +- src/emc/rs274ngc/interp_queue.hh | 2 +- src/emc/rs274ngc/interp_setup.cc | 10 +- src/emc/rs274ngc/interpmodule.cc | 24 +- src/emc/rs274ngc/rs274ngc_pre.cc | 61 +-- src/emc/rs274ngc/units.h | 4 + src/emc/task/emccanon.cc | 679 ++++++++++----------------- 16 files changed, 870 insertions(+), 789 deletions(-) create mode 100644 src/emc/ini/axis_kinds.hh diff --git a/docs/src/config/ini-config.adoc b/docs/src/config/ini-config.adoc index 9f932eb01cb..b5c3c5af8bb 100644 --- a/docs/src/config/ini-config.adoc +++ b/docs/src/config/ini-config.adoc @@ -933,12 +933,18 @@ Finally, no amount of tweaking will speed up a tool path with lots of small, tig For the common 'trivkins kinematics', joint numbers are assigned in sequence according to the trivkins parameter 'coordinates='. So, for trivkins 'coordinates=xz', joint0 corresponds to X and joint1 corresponds to Z. See the kinematics man page ('$ man kins') for information on trivkins and other kinematics modules. +* `FEED_AXES = X Y Z` - (((FEED AXES))) The axes the programmed feed rate F is measured along. + All of them must be `LINEAR` axes; the default is X Y Z. + A move goes at F along the straight line through the feed axes that move, and every other axis in the move arrives at the same time. + A move in which no feed axis moves goes at F along the other linear axes that move, and a move of angular axes only goes at F in degrees per minute. + With the defaults this is the RS274NGC rule: X Y Z, else U V W, else A B C. + With `FEED_AXES = X Y Z U`, for example, a move of X and U together goes at F along the line through both. * `LINEAR_UNITS =` _ - (((LINEAR UNITS))) Specifies the 'machine units' for linear axes. Possible choices are mm or inch. This does not affect the linear units in NC code (the G20 and G21 words do this). * `ANGULAR_UNITS =` __ - (((ANGULAR UNITS))) Specifies the 'machine units' for rotational axes. Possible choices are 'deg', 'degree' (360 per circle), 'rad', 'radian' (2*π per circle), 'grad', or 'gon' (400 per circle). - This does not affect the angular units of NC code. In RS274NGC, A-, B- and C- words are always expressed in degrees. + This does not affect the angular units of NC code. In RS274NGC, the words of an `ANGULAR` axis, A, B and C by default, are always expressed in degrees. * `DEFAULT_LINEAR_VELOCITY = 0.0167` - The initial rate for jogs of linear axes, in machine units per second. The value shown in 'AXIS' equals machine units per minute. * `DEFAULT_LINEAR_ACCELERATION = 2.0` - In machines with nontrivial kinematics, the acceleration used for "teleop" (Cartesian space) jogs, in 'machine units' per second per second. @@ -1020,8 +1026,11 @@ The __ specifies one of: X Y Z A B C U V W * `TYPE = LINEAR` - (enum) The type of this axis, either `LINEAR` or `ANGULAR`. Required if this axis is not a default axis type. The default axis types are X,Y,Z,U,V,W = LINEAR and A,B,C = ANGULAR. - This setting is effective with the AXIS GUI but note that other - GUI's may handle things differently. + X, Y and Z are always `LINEAR`. + A `LINEAR` axis is a length: its words, offsets, stored positions and tool offsets follow G20 and G21 and `[TRAJ]LINEAR_UNITS`. + An `ANGULAR` axis is an angle in degrees whatever G20 or G21 says, in `[TRAJ]ANGULAR_UNITS` for the machine, and may be a `WRAPPED_ROTARY` or have a `LOCKING_INDEXER_JOINT`. + The feed of a move depends on the type too, see `FEED_AXES` in the <>. + The AXIS GUI shows the axis in the units of its type; other GUIs may not. * `MAX_VELOCITY = 1.2` - (real) Maximum velocity for this axis in <> per second. * `MAX_ACCELERATION = 20.0` - (real) Maximum acceleration for this axis in machine units per second squared. * `MAX_JERK = 0.0` - (real) Maximum jerk for this axis in machine units per second cubed. @@ -1037,11 +1046,13 @@ The __ specifies one of: X Y Z A B C U V W For a rotary axis (A,B,C typ) with unlimited rotation having no `MAX_LIMIT` for that axis in the `[AXIS_``]` section a value of 1e99 is used. * `WRAPPED_ROTARY = 1` - (bool) When this is set to 1 for an ANGULAR axis the axis will move 0-359.999 degrees. Positive Numbers will move the axis in a positive direction and negative numbers will move the axis in the negative direction. + It is ignored on a `LINEAR` axis. * `LOCKING_INDEXER_JOINT = 4` - (int) This value selects a joint to use for a locking indexer for the specified axis __. In this example, the joint is 4 which would correspond to the B axis for a XYZAB system with trivkins (identity) kinematics. When set, a G0 move for this axis will initiate an unlock with the `joint.4.unlock pin` then wait for the `joint.4.is-unlocked` pin then move the joint at the rapid rate for that joint. After the move the `joint.4.unlock` will be false and motion will wait for `joint.4.is-unlocked` to go false. Moving with other joints is not allowed when moving a locked rotary joint. + It is ignored on a `LINEAR` axis. To create the unlock pins, use the motmod parameter: + [source,ini] diff --git a/docs/src/gcode/machining-center.adoc b/docs/src/gcode/machining-center.adoc index 27df8428626..818880163f5 100644 --- a/docs/src/gcode/machining-center.adoc +++ b/docs/src/gcode/machining-center.adoc @@ -179,14 +179,17 @@ rate is interpreted as follows (unless 'inverse time feed' or 'feed per revolution' modes are being used, in which case see section <>). -. If any of XYZ are moving, F is in units per minute in the XYZ - cartesian system, and all other axes (ABCUVW) move so as to start and - stop in coordinated fashion. -. Otherwise, if any of UVW are moving, F is in units per minute in the - UVW cartesian system, and all other axes (ABC) move so as to start and - stop in coordinated fashion. +. If any of the feed axes are moving, F is in units per minute in the + cartesian system of the feed axes, and all other axes move so as to + start and stop in coordinated fashion. The feed axes are X Y Z unless + `[TRAJ]FEED_AXES` in the INI file names others. +. Otherwise, if any of the other linear axes are moving, F is in units + per minute in their cartesian system, and the angular axes move so as + to start and stop in coordinated fashion. These are U V W unless + `[AXIS_]TYPE` in the INI file says otherwise. . Otherwise, the move is pure rotary motion and the F word is in rotary - units in the ABC 'pseudo-cartesian' system. + units in the 'pseudo-cartesian' system of the angular axes, A B C + unless `[AXIS_]TYPE` says otherwise. === Cooling @@ -208,11 +211,14 @@ the previous programmed move, as though it was in exact path mode. === Units (((units))) -Units used for distances along the X, Y, and Z axes may be measured in -millimeters or inches. Units for all other quantities involved in -machine control cannot be changed. Different quantities use different -specific units. Spindle speed is measured in revolutions per minute. -The positions of rotational axes are measured in degrees. Feed rates +Units used for distances along the X, Y, and Z axes, and along any other +linear axis, may be measured in millimeters or inches. Units for all +other quantities involved in machine control cannot be changed. +Different quantities use different specific units. Spindle speed is +measured in revolutions per minute. The positions of rotational axes are +measured in degrees. An axis is linear or rotational as +`[AXIS_]TYPE` in the INI file says: U V W linear and A B C +rotational unless it says otherwise. Feed rates are expressed in current length units per minute, or degrees per minute, or length units per spindle revolution, as described in section <>. diff --git a/src/Makefile b/src/Makefile index c05d9071f95..3fab1c5dae2 100644 --- a/src/Makefile +++ b/src/Makefile @@ -403,6 +403,7 @@ SRCHEADERS := \ emc/linuxcnc.h \ emc/kinematics/kinematics.h \ emc/nml_intf/emcmotcfg.h \ + emc/ini/axis_kinds.hh \ emc/ini/inifile.hh \ emc/ini/inifile.h \ emc/nml_intf/emcpos.h \ diff --git a/src/emc/ini/axis_kinds.hh b/src/emc/ini/axis_kinds.hh new file mode 100644 index 00000000000..8c343d4bd00 --- /dev/null +++ b/src/emc/ini/axis_kinds.hh @@ -0,0 +1,120 @@ +/******************************************************************** +* Description: axis_kinds.hh +* Which of the nine axes are lengths and which are angles, and which +* axes the programmed feed is measured along, as the INI file says. +* The interpreter and canon both read it, so that they measure a move +* the same way. +* +* License: GPL Version 2 +********************************************************************/ +#ifndef AXIS_KINDS_HH +#define AXIS_KINDS_HH + +#include +#include +#include +#include +#include + +#define AXIS_KINDS_ALL 0x1ffu /* X Y Z A B C U V W, bit 0 is X */ +#define AXIS_KINDS_ABC 0x038u /* angular unless the INI says otherwise */ +#define AXIS_KINDS_XYZ 0x007u /* the feed group unless the INI says otherwise */ + +struct AxisKinds { + unsigned angular; /* a bit per axis whose [AXIS_] TYPE is ANGULAR */ + unsigned feed; /* a bit per axis in [TRAJ] FEED_AXES */ +}; + +static const char axis_kinds_letters[] = "XYZABCUVW"; + +inline AxisKinds axisKindsDefault() +{ + return AxisKinds{AXIS_KINDS_ABC, AXIS_KINDS_XYZ}; +} + +inline bool axisKindsAngular(const AxisKinds &k, int axis) +{ + return (k.angular >> axis) & 1; +} + +/* Read [AXIS_] TYPE for every letter, the default being the + letter's (A B C angular, the others linear), and [TRAJ] FEED_AXES, the + default XYZ. X Y Z are always linear: arcs, cutter compensation and + tool length take them as lengths. Returns 0, or -1 with *err set when + a TYPE is neither LINEAR nor ANGULAR, X Y or Z is ANGULAR, or FEED_AXES + names a letter that is not a linear axis. */ +inline int axisKindsRead(const linuxcnc::IniFile &ini, AxisKinds *k, std::string *err) +{ + *k = axisKindsDefault(); + for (int i = 0; i < 9; i++) { + char section[] = "AXIS_X"; + section[5] = axis_kinds_letters[i]; + auto type = ini.findString("TYPE", section); + if (!type) { continue; } + if (*type == "ANGULAR") { + k->angular |= 1u << i; + } else if (*type == "LINEAR") { + k->angular &= ~(1u << i); + } else { + *err = "[" + std::string(section) + "] TYPE must be LINEAR or ANGULAR, not " + *type; + return -1; + } + } + if (k->angular & AXIS_KINDS_XYZ) { + *err = "[AXIS_X], [AXIS_Y] and [AXIS_Z] TYPE must be LINEAR"; + return -1; + } + auto feed = ini.findString("FEED_AXES", "TRAJ"); + if (!feed) { return 0; } + k->feed = 0; + for (char ch : *feed) { + if (ch == ' ' || ch == '\t') { continue; } + const char *at = strchr(axis_kinds_letters, toupper((unsigned char)ch)); + if (!at || !*at) { + *err = std::string("[TRAJ] FEED_AXES: ") + ch + " is not an axis letter"; + return -1; + } + int i = at - axis_kinds_letters; + if (axisKindsAngular(*k, i)) { + *err = std::string("[TRAJ] FEED_AXES: ") + axis_kinds_letters[i] + " is an ANGULAR axis; the feed group holds linear axes only"; + return -1; + } + k->feed |= 1u << i; + } + if (!k->feed) { + *err = "[TRAJ] FEED_AXES names no axis"; + return -1; + } + return 0; +} + +/* The axes a move is measured along, given a bit per axis that moves: the + feed group when any of it moves, else the other linear axes, else the + angular axes, in degrees. Every axis of the chosen set counts, moving + or not. With the defaults this is XYZ, else UVW, else ABC. */ +inline unsigned axisKindsMeasured(const AxisKinds &k, unsigned moving) +{ + unsigned linear = ~k.angular & ~k.feed & AXIS_KINDS_ALL; + if (moving & k.feed) { return k.feed; } + if (moving & linear) { return linear; } + return k.angular & ~k.feed & AXIS_KINDS_ALL; +} + +/* Whether a set from axisKindsMeasured() is angles. */ +inline bool axisKindsMeasuredAngular(const AxisKinds &k, unsigned set) +{ + return (set & k.angular) != 0; +} + +/* The Euclidean length of the deltas d over the axes of set, summed in + axis order. */ +inline double axisKindsLength(unsigned set, const double d[9]) +{ + double sum = 0.0; + for (int i = 0; i < 9; i++) { + if (set & (1u << i)) { sum += d[i] * d[i]; } + } + return sqrt(sum); +} + +#endif diff --git a/src/emc/rs274ngc/interp_check.cc b/src/emc/rs274ngc/interp_check.cc index 40992b41268..fc5ae57586e 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -489,7 +489,7 @@ int Interp::check_spindle_sync_feed(setup_pointer settings, //!< pointer to mac double length = 0.0; for (int ax = 0; ax < 9; ax++) { - if (ax >= 3 && ax <= 5) + if (axisKindsAngular(settings->axis_kinds, ax)) continue; /* rotary */ length += delta[ax] * delta[ax]; } @@ -501,7 +501,7 @@ int Interp::check_spindle_sync_feed(setup_pointer settings, //!< pointer to mac double required_rate = fabs(pitch) * speed; for (int ax = 0; ax < 9; ax++) { - if (ax >= 3 && ax <= 5) + if (axisKindsAngular(settings->axis_kinds, ax)) continue; if (delta[ax] == 0.0) continue; diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 9456c1d56d5..15fdb41a7f8 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -1641,18 +1641,30 @@ 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((block->a_flag && settings->a_axis_wrapped && + CHKS((block->a_flag && settings->axis_wrapped[3] && (block->a_number <= -360.0 || block->a_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->a_number, 'A'); - CHKS((block->b_flag && settings->b_axis_wrapped && + CHKS((block->b_flag && settings->axis_wrapped[4] && (block->b_number <= -360.0 || block->b_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->b_number, 'B'); - CHKS((block->c_flag && settings->c_axis_wrapped && + CHKS((block->c_flag && settings->axis_wrapped[5] && (block->c_number <= -360.0 || block->c_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->c_number, 'C'); + CHKS((block->u_flag && settings->axis_wrapped[6] && + (block->u_number <= -360.0 || block->u_number >= 360.0)), + (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), + block->u_number, 'U'); + CHKS((block->v_flag && settings->axis_wrapped[7] && + (block->v_number <= -360.0 || block->v_number >= 360.0)), + (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), + block->v_number, 'V'); + CHKS((block->w_flag && settings->axis_wrapped[8] && + (block->w_number <= -360.0 || block->w_number >= 360.0)), + (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), + block->w_number, 'W'); pars = settings->parameters; if ((g_code == G_52) || (g_code == G_92)) { pars[G92_APPLIED] = 1.0; @@ -1760,12 +1772,12 @@ int Interp::convert_axis_offsets(int g_code, //!< g_code being executed (mus pars[5211] = PROGRAM_TO_USER_LEN(settings->axis_offset_x); pars[5212] = PROGRAM_TO_USER_LEN(settings->axis_offset_y); pars[5213] = PROGRAM_TO_USER_LEN(settings->axis_offset_z); - pars[5214] = PROGRAM_TO_USER_ANG(settings->AA_axis_offset); - pars[5215] = PROGRAM_TO_USER_ANG(settings->BB_axis_offset); - pars[5216] = PROGRAM_TO_USER_ANG(settings->CC_axis_offset); - pars[5217] = PROGRAM_TO_USER_LEN(settings->u_axis_offset); - pars[5218] = PROGRAM_TO_USER_LEN(settings->v_axis_offset); - pars[5219] = PROGRAM_TO_USER_LEN(settings->w_axis_offset); + pars[5214] = PROGRAM_TO_USER_AX(3, settings->AA_axis_offset); + pars[5215] = PROGRAM_TO_USER_AX(4, settings->BB_axis_offset); + pars[5216] = PROGRAM_TO_USER_AX(5, settings->CC_axis_offset); + pars[5217] = PROGRAM_TO_USER_AX(6, settings->u_axis_offset); + pars[5218] = PROGRAM_TO_USER_AX(7, settings->v_axis_offset); + pars[5219] = PROGRAM_TO_USER_AX(8, settings->w_axis_offset); } else if ((g_code == G_92_1) || (g_code == G_92_2)) { pars[5210] = 0.0; @@ -1810,27 +1822,27 @@ int Interp::convert_axis_offsets(int g_code, //!< g_code being executed (mus settings->current_z = settings->current_z + settings->axis_offset_z - USER_TO_PROGRAM_LEN(pars[5213]); settings->AA_current = - settings->AA_current + settings->AA_axis_offset - USER_TO_PROGRAM_ANG(pars[5214]); + settings->AA_current + settings->AA_axis_offset - USER_TO_PROGRAM_AX(3, pars[5214]); settings->BB_current = - settings->BB_current + settings->BB_axis_offset - USER_TO_PROGRAM_ANG(pars[5215]); + settings->BB_current + settings->BB_axis_offset - USER_TO_PROGRAM_AX(4, pars[5215]); settings->CC_current = - settings->CC_current + settings->CC_axis_offset - USER_TO_PROGRAM_ANG(pars[5216]); + settings->CC_current + settings->CC_axis_offset - USER_TO_PROGRAM_AX(5, pars[5216]); settings->u_current = - settings->u_current + settings->u_axis_offset - USER_TO_PROGRAM_LEN(pars[5217]); + settings->u_current + settings->u_axis_offset - USER_TO_PROGRAM_AX(6, pars[5217]); settings->v_current = - settings->v_current + settings->v_axis_offset - USER_TO_PROGRAM_LEN(pars[5218]); + settings->v_current + settings->v_axis_offset - USER_TO_PROGRAM_AX(7, pars[5218]); settings->w_current = - settings->w_current + settings->w_axis_offset - USER_TO_PROGRAM_LEN(pars[5219]); + settings->w_current + settings->w_axis_offset - USER_TO_PROGRAM_AX(8, pars[5219]); settings->axis_offset_x = USER_TO_PROGRAM_LEN(pars[5211]); settings->axis_offset_y = USER_TO_PROGRAM_LEN(pars[5212]); settings->axis_offset_z = USER_TO_PROGRAM_LEN(pars[5213]); - settings->AA_axis_offset = USER_TO_PROGRAM_ANG(pars[5214]); - settings->BB_axis_offset = USER_TO_PROGRAM_ANG(pars[5215]); - settings->CC_axis_offset = USER_TO_PROGRAM_ANG(pars[5216]); - settings->u_axis_offset = USER_TO_PROGRAM_LEN(pars[5217]); - settings->v_axis_offset = USER_TO_PROGRAM_LEN(pars[5218]); - settings->w_axis_offset = USER_TO_PROGRAM_LEN(pars[5219]); + settings->AA_axis_offset = USER_TO_PROGRAM_AX(3, pars[5214]); + settings->BB_axis_offset = USER_TO_PROGRAM_AX(4, pars[5215]); + settings->CC_axis_offset = USER_TO_PROGRAM_AX(5, pars[5216]); + settings->u_axis_offset = USER_TO_PROGRAM_AX(6, pars[5217]); + settings->v_axis_offset = USER_TO_PROGRAM_AX(7, pars[5218]); + settings->w_axis_offset = USER_TO_PROGRAM_AX(8, pars[5219]); SET_G92_OFFSET(settings->axis_offset_x, settings->axis_offset_y, @@ -2418,12 +2430,12 @@ int Interp::convert_coordinate_system(int g_code, //!< g_code called (mus settings->origin_offset_x = USER_TO_PROGRAM_LEN(parameters[5201 + (origin * 20)]); settings->origin_offset_y = USER_TO_PROGRAM_LEN(parameters[5202 + (origin * 20)]); settings->origin_offset_z = USER_TO_PROGRAM_LEN(parameters[5203 + (origin * 20)]); - settings->AA_origin_offset = USER_TO_PROGRAM_ANG(parameters[5204 + (origin * 20)]); - settings->BB_origin_offset = USER_TO_PROGRAM_ANG(parameters[5205 + (origin * 20)]); - settings->CC_origin_offset = USER_TO_PROGRAM_ANG(parameters[5206 + (origin * 20)]); - settings->u_origin_offset = USER_TO_PROGRAM_LEN(parameters[5207 + (origin * 20)]); - settings->v_origin_offset = USER_TO_PROGRAM_LEN(parameters[5208 + (origin * 20)]); - settings->w_origin_offset = USER_TO_PROGRAM_LEN(parameters[5209 + (origin * 20)]); + settings->AA_origin_offset = USER_TO_PROGRAM_AX(3, parameters[5204 + (origin * 20)]); + settings->BB_origin_offset = USER_TO_PROGRAM_AX(4, parameters[5205 + (origin * 20)]); + settings->CC_origin_offset = USER_TO_PROGRAM_AX(5, parameters[5206 + (origin * 20)]); + settings->u_origin_offset = USER_TO_PROGRAM_AX(6, parameters[5207 + (origin * 20)]); + settings->v_origin_offset = USER_TO_PROGRAM_AX(7, parameters[5208 + (origin * 20)]); + settings->w_origin_offset = USER_TO_PROGRAM_AX(8, parameters[5209 + (origin * 20)]); settings->rotation_xy = parameters[5210 + (origin * 20)]; SET_G5X_OFFSET(origin, @@ -3083,28 +3095,43 @@ int Interp::convert_savehome(int code, block_pointer /*block*/, setup_pointer s) 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 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); - double u = PROGRAM_TO_USER_LEN(s->u_current + s->tool_offset.u + s->u_origin_offset + s->u_axis_offset); - double v = PROGRAM_TO_USER_LEN(s->v_current + s->tool_offset.v + s->v_origin_offset + s->v_axis_offset); - double w = PROGRAM_TO_USER_LEN(s->w_current + s->tool_offset.w + s->w_origin_offset + s->w_axis_offset); - - if(s->a_axis_wrapped) { + double a = PROGRAM_TO_USER_AX(3, s->AA_current + s->tool_offset.a + s->AA_origin_offset + s->AA_axis_offset); + double b = PROGRAM_TO_USER_AX(4, s->BB_current + s->tool_offset.b + s->BB_origin_offset + s->BB_axis_offset); + double c = PROGRAM_TO_USER_AX(5, s->CC_current + s->tool_offset.c + s->CC_origin_offset + s->CC_axis_offset); + double u = PROGRAM_TO_USER_AX(6, s->u_current + s->tool_offset.u + s->u_origin_offset + s->u_axis_offset); + double v = PROGRAM_TO_USER_AX(7, s->v_current + s->tool_offset.v + s->v_origin_offset + s->v_axis_offset); + double w = PROGRAM_TO_USER_AX(8, s->w_current + s->tool_offset.w + s->w_origin_offset + s->w_axis_offset); + + if(s->axis_wrapped[3]) { a = fmod(a, 360.0); if(a<0) a += 360.0; } - if(s->b_axis_wrapped) { + if(s->axis_wrapped[4]) { b = fmod(b, 360.0); if(b<0) b += 360.0; } - if(s->c_axis_wrapped) { + if(s->axis_wrapped[5]) { c = fmod(c, 360.0); if(c<0) c += 360.0; } + if(s->axis_wrapped[6]) { + u = fmod(u, 360.0); + if(u<0) u += 360.0; + } + + if(s->axis_wrapped[7]) { + v = fmod(v, 360.0); + if(v<0) v += 360.0; + } + + if(s->axis_wrapped[8]) { + w = fmod(w, 360.0); + if(w<0) w += 360.0; + } + if(code == G_28_1) { p[5161] = x; p[5162] = y; @@ -3290,12 +3317,18 @@ int Interp::convert_home(int move, //!< G-code, must be G_28 or G_30 // move indexers first, one at a time // JOINTS_AXES settings->*_indexer_jnum == -1 means notused - if (AA_end != settings->AA_current && (-1 != settings->a_indexer_jnum) ) - issue_straight_index(3,settings->a_indexer_jnum, AA_end, block->line_number, settings); - if (BB_end != settings->BB_current && (-1 != settings->b_indexer_jnum) ) - issue_straight_index(4,settings->b_indexer_jnum, BB_end, block->line_number, settings); - if (CC_end != settings->CC_current && (-1 != settings->c_indexer_jnum) ) - issue_straight_index(5,settings->c_indexer_jnum, CC_end, block->line_number, settings); + if (AA_end != settings->AA_current && (-1 != settings->axis_indexer_jnum[3]) ) + issue_straight_index(3,settings->axis_indexer_jnum[3], AA_end, block->line_number, settings); + if (BB_end != settings->BB_current && (-1 != settings->axis_indexer_jnum[4]) ) + issue_straight_index(4,settings->axis_indexer_jnum[4], BB_end, block->line_number, settings); + if (CC_end != settings->CC_current && (-1 != settings->axis_indexer_jnum[5]) ) + issue_straight_index(5,settings->axis_indexer_jnum[5], CC_end, block->line_number, settings); + if (u_end != settings->u_current && (-1 != settings->axis_indexer_jnum[6]) ) + issue_straight_index(6,settings->axis_indexer_jnum[6], u_end, block->line_number, settings); + if (v_end != settings->v_current && (-1 != settings->axis_indexer_jnum[7]) ) + issue_straight_index(7,settings->axis_indexer_jnum[7], v_end, block->line_number, settings); + if (w_end != settings->w_current && (-1 != settings->axis_indexer_jnum[8]) ) + issue_straight_index(8,settings->axis_indexer_jnum[8], w_end, block->line_number, settings); // Create a state tag and dump it to canon write_canon_state_tag(block, settings); @@ -3318,12 +3351,12 @@ int Interp::convert_home(int move, //!< G-code, must be G_28 or G_30 find_relative(USER_TO_PROGRAM_LEN(parameters[5161]), USER_TO_PROGRAM_LEN(parameters[5162]), USER_TO_PROGRAM_LEN(parameters[5163]), - USER_TO_PROGRAM_ANG(parameters[5164]), - USER_TO_PROGRAM_ANG(parameters[5165]), - USER_TO_PROGRAM_ANG(parameters[5166]), - USER_TO_PROGRAM_LEN(parameters[5167]), - USER_TO_PROGRAM_LEN(parameters[5168]), - USER_TO_PROGRAM_LEN(parameters[5169]), + USER_TO_PROGRAM_AX(3, parameters[5164]), + USER_TO_PROGRAM_AX(4, parameters[5165]), + USER_TO_PROGRAM_AX(5, parameters[5166]), + USER_TO_PROGRAM_AX(6, parameters[5167]), + USER_TO_PROGRAM_AX(7, parameters[5168]), + USER_TO_PROGRAM_AX(8, parameters[5169]), &end_x_home, &end_y_home, &end_z_home, &AA_end_home, &BB_end_home, &CC_end_home, &u_end_home, &v_end_home, &w_end_home, settings); @@ -3331,12 +3364,12 @@ int Interp::convert_home(int move, //!< G-code, must be G_28 or G_30 find_relative(USER_TO_PROGRAM_LEN(parameters[5181]), USER_TO_PROGRAM_LEN(parameters[5182]), USER_TO_PROGRAM_LEN(parameters[5183]), - USER_TO_PROGRAM_ANG(parameters[5184]), - USER_TO_PROGRAM_ANG(parameters[5185]), - USER_TO_PROGRAM_ANG(parameters[5186]), - USER_TO_PROGRAM_LEN(parameters[5187]), - USER_TO_PROGRAM_LEN(parameters[5188]), - USER_TO_PROGRAM_LEN(parameters[5189]), + USER_TO_PROGRAM_AX(3, parameters[5184]), + USER_TO_PROGRAM_AX(4, parameters[5185]), + USER_TO_PROGRAM_AX(5, parameters[5186]), + USER_TO_PROGRAM_AX(6, parameters[5187]), + USER_TO_PROGRAM_AX(7, parameters[5188]), + USER_TO_PROGRAM_AX(8, parameters[5189]), &end_x_home, &end_y_home, &end_z_home, &AA_end_home, &BB_end_home, &CC_end_home, &u_end_home, &v_end_home, &w_end_home, settings); @@ -3375,12 +3408,18 @@ int Interp::convert_home(int move, //!< G-code, must be G_28 or G_30 // move indexers first, one at a time // JOINTS_AXES settings->*_indexer_jnum == -1 means notused - if (AA_end != settings->AA_current && (-1 != settings->a_indexer_jnum) ) - issue_straight_index(3,settings->a_indexer_jnum, AA_end, block->line_number, settings); - if (BB_end != settings->BB_current && (-1 != settings->b_indexer_jnum) ) - issue_straight_index(4,settings->b_indexer_jnum, BB_end, block->line_number, settings); - if (CC_end != settings->CC_current && (-1 != settings->c_indexer_jnum) ) - issue_straight_index(5,settings->c_indexer_jnum, CC_end, block->line_number, settings); + if (AA_end != settings->AA_current && (-1 != settings->axis_indexer_jnum[3]) ) + issue_straight_index(3,settings->axis_indexer_jnum[3], AA_end, block->line_number, settings); + if (BB_end != settings->BB_current && (-1 != settings->axis_indexer_jnum[4]) ) + issue_straight_index(4,settings->axis_indexer_jnum[4], BB_end, block->line_number, settings); + if (CC_end != settings->CC_current && (-1 != settings->axis_indexer_jnum[5]) ) + issue_straight_index(5,settings->axis_indexer_jnum[5], CC_end, block->line_number, settings); + if (u_end != settings->u_current && (-1 != settings->axis_indexer_jnum[6]) ) + issue_straight_index(6,settings->axis_indexer_jnum[6], u_end, block->line_number, settings); + if (v_end != settings->v_current && (-1 != settings->axis_indexer_jnum[7]) ) + issue_straight_index(7,settings->axis_indexer_jnum[7], v_end, block->line_number, settings); + if (w_end != settings->w_current && (-1 != settings->axis_indexer_jnum[8]) ) + issue_straight_index(8,settings->axis_indexer_jnum[8], w_end, block->line_number, settings); STRAIGHT_TRAVERSE(block->line_number, end_x, end_y, end_z, AA_end, BB_end, CC_end, @@ -3400,6 +3439,25 @@ int Interp::convert_home(int move, //!< G-code, must be G_28 or G_30 /****************************************************************************/ +// G20/G21 on the axes past X Y Z that are lengths: A B C U V W as their +// [AXIS_] TYPE says +static void scale_linear_axes(setup_pointer settings, double factor) +{ + double *current[6] = {&settings->AA_current, &settings->BB_current, &settings->CC_current, + &settings->u_current, &settings->v_current, &settings->w_current}; + double *axis_offset[6] = {&settings->AA_axis_offset, &settings->BB_axis_offset, &settings->CC_axis_offset, + &settings->u_axis_offset, &settings->v_axis_offset, &settings->w_axis_offset}; + double *origin_offset[6] = {&settings->AA_origin_offset, &settings->BB_origin_offset, &settings->CC_origin_offset, + &settings->u_origin_offset, &settings->v_origin_offset, &settings->w_origin_offset}; + + for (int n = 0; n < 6; n++) { + if (axisKindsAngular(settings->axis_kinds, n + 3)) { continue; } + *current[n] = (*current[n] * factor); + *axis_offset[n] = (*axis_offset[n] * factor); + *origin_offset[n] = (*origin_offset[n] * factor); + } +} + /*! convert_length_units Returned Value: int @@ -3447,7 +3505,7 @@ int Interp::convert_length_units(int g_code, //!< g_code being executed (mus settings->program_x = (settings->program_x * INCH_PER_MM); settings->program_y = (settings->program_y * INCH_PER_MM); settings->program_z = (settings->program_z * INCH_PER_MM); - qc_scale(INCH_PER_MM); + qc_scale(INCH_PER_MM, settings->axis_kinds); settings->cutter_comp_radius *= INCH_PER_MM; settings->axis_offset_x = (settings->axis_offset_x * INCH_PER_MM); settings->axis_offset_y = (settings->axis_offset_y * INCH_PER_MM); @@ -3456,15 +3514,7 @@ int Interp::convert_length_units(int g_code, //!< g_code being executed (mus settings->origin_offset_y = (settings->origin_offset_y * INCH_PER_MM); settings->origin_offset_z = (settings->origin_offset_z * INCH_PER_MM); - settings->u_current = (settings->u_current * INCH_PER_MM); - settings->v_current = (settings->v_current * INCH_PER_MM); - settings->w_current = (settings->w_current * INCH_PER_MM); - settings->u_axis_offset = (settings->u_axis_offset * INCH_PER_MM); - settings->v_axis_offset = (settings->v_axis_offset * INCH_PER_MM); - settings->w_axis_offset = (settings->w_axis_offset * INCH_PER_MM); - settings->u_origin_offset = (settings->u_origin_offset * INCH_PER_MM); - settings->v_origin_offset = (settings->v_origin_offset * INCH_PER_MM); - settings->w_origin_offset = (settings->w_origin_offset * INCH_PER_MM); + scale_linear_axes(settings, INCH_PER_MM); settings->tool_offset.tran.x = GET_EXTERNAL_TOOL_LENGTH_XOFFSET(); settings->tool_offset.tran.y = GET_EXTERNAL_TOOL_LENGTH_YOFFSET(); @@ -3490,7 +3540,7 @@ int Interp::convert_length_units(int g_code, //!< g_code being executed (mus settings->program_x = (settings->program_x * MM_PER_INCH); settings->program_y = (settings->program_y * MM_PER_INCH); settings->program_z = (settings->program_z * MM_PER_INCH); - qc_scale(MM_PER_INCH); + qc_scale(MM_PER_INCH, settings->axis_kinds); settings->cutter_comp_radius *= MM_PER_INCH; settings->axis_offset_x = (settings->axis_offset_x * MM_PER_INCH); settings->axis_offset_y = (settings->axis_offset_y * MM_PER_INCH); @@ -3499,15 +3549,7 @@ int Interp::convert_length_units(int g_code, //!< g_code being executed (mus settings->origin_offset_y = (settings->origin_offset_y * MM_PER_INCH); settings->origin_offset_z = (settings->origin_offset_z * MM_PER_INCH); - settings->u_current = (settings->u_current * MM_PER_INCH); - settings->v_current = (settings->v_current * MM_PER_INCH); - settings->w_current = (settings->w_current * MM_PER_INCH); - settings->u_axis_offset = (settings->u_axis_offset * MM_PER_INCH); - settings->v_axis_offset = (settings->v_axis_offset * MM_PER_INCH); - settings->w_axis_offset = (settings->w_axis_offset * MM_PER_INCH); - settings->u_origin_offset = (settings->u_origin_offset * MM_PER_INCH); - settings->v_origin_offset = (settings->v_origin_offset * MM_PER_INCH); - settings->w_origin_offset = (settings->w_origin_offset * MM_PER_INCH); + scale_linear_axes(settings, MM_PER_INCH); settings->tool_offset.tran.x = GET_EXTERNAL_TOOL_LENGTH_XOFFSET(); settings->tool_offset.tran.y = GET_EXTERNAL_TOOL_LENGTH_YOFFSET(); @@ -4500,36 +4542,31 @@ int Interp::convert_motion(int motion, //!< g_code for a line, arc, canned cyc block_pointer block, //!< pointer to a block of RS274 instructions setup_pointer settings) //!< pointer to machine settings { - int ai = block->a_flag && (-1 != settings->a_indexer_jnum); - int bi = block->b_flag && (-1 != settings->b_indexer_jnum); - int ci = block->c_flag && (-1 != settings->c_indexer_jnum); - + const bool axis_flag[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}; + int indexed = -1; // the first axis word on a locking indexer - if (motion != G_0) { - CHKS((ai), (_("Indexing axis %c can only be moved with G0")), 'A'); - CHKS((bi), (_("Indexing axis %c can only be moved with G0")), 'B'); - CHKS((ci), (_("Indexing axis %c can only be moved with G0")), 'C'); + for (int n = 8; n >= 3; n--) { + if (axis_flag[n] && -1 != settings->axis_indexer_jnum[n]) { indexed = n; } + } + for (int n = 3; n < 9 && motion != G_0; n++) { + CHKS((axis_flag[n] && -1 != settings->axis_indexer_jnum[n]), + (_("Indexing axis %c can only be moved with G0")), axis_kinds_letters[n]); + } + for (int n = 3; n < 9; n++) { + if (!axis_flag[n] || -1 == settings->axis_indexer_jnum[n]) { continue; } + for (int other = 0; other < 9; other++) { + CHKS((other != n && axis_flag[other]), + (_("Indexing axis %c can only be moved alone")), axis_kinds_letters[n]); + } } - - int xyzuvw_flag = (block->x_flag || block->y_flag || block->z_flag || - block->u_flag || block->v_flag || block->w_flag); - - CHKS((ai && (xyzuvw_flag || block->b_flag || block->c_flag)), - (_("Indexing axis %c can only be moved alone")), 'A'); - CHKS((bi && (xyzuvw_flag || block->a_flag || block->c_flag)), - (_("Indexing axis %c can only be moved alone")), 'B'); - CHKS((ci && (xyzuvw_flag || block->a_flag || block->b_flag)), - (_("Indexing axis %c can only be moved alone")), 'C'); if (!is_a_cycle(motion)) settings->cycle_il_flag = false; - if (ai || bi || ci) { - int anum=-1,jnum=-1; - if ( ai) {anum = 3; jnum = settings->a_indexer_jnum;} - else if (bi) {anum = 4; jnum = settings->b_indexer_jnum;} - else if (ci) {anum = 5; jnum = settings->c_indexer_jnum;} - CHP(convert_straight_indexer(anum, jnum, block, settings)); + if (indexed != -1) { + CHP(convert_straight_indexer(indexed, settings->axis_indexer_jnum[indexed], block, settings)); } else if ((motion == G_0) || (motion == G_1) || (motion == G_33) || (motion == G_33_1) || (motion == G_76)) { CHP(convert_straight(motion, block, settings)); } else if ((motion == G_3) || (motion == G_2)) { @@ -4717,17 +4754,17 @@ int Interp::convert_setup_tool(block_pointer block, setup_pointer settings) { if(block->z_flag) settings->tool_table[idx].offset.tran.z = PROGRAM_TO_USER_LEN(block->z_number); if(block->a_flag) - settings->tool_table[idx].offset.a = PROGRAM_TO_USER_ANG(block->a_number); + settings->tool_table[idx].offset.a = PROGRAM_TO_USER_AX(3, block->a_number); if(block->b_flag) - settings->tool_table[idx].offset.b = PROGRAM_TO_USER_ANG(block->b_number); + settings->tool_table[idx].offset.b = PROGRAM_TO_USER_AX(4, block->b_number); if(block->c_flag) - settings->tool_table[idx].offset.c = PROGRAM_TO_USER_ANG(block->c_number); + settings->tool_table[idx].offset.c = PROGRAM_TO_USER_AX(5, block->c_number); if(block->u_flag) - settings->tool_table[idx].offset.u = PROGRAM_TO_USER_LEN(block->u_number); + settings->tool_table[idx].offset.u = PROGRAM_TO_USER_AX(6, block->u_number); if(block->v_flag) - settings->tool_table[idx].offset.v = PROGRAM_TO_USER_LEN(block->v_number); + settings->tool_table[idx].offset.v = PROGRAM_TO_USER_AX(7, block->v_number); if(block->w_flag) - settings->tool_table[idx].offset.w = PROGRAM_TO_USER_LEN(block->w_number); + settings->tool_table[idx].offset.w = PROGRAM_TO_USER_AX(8, block->w_number); } else { int to_fixture = block->l_number == 11; int destination_system = to_fixture? 9 : settings->origin_index; // maybe 9 (g59.3) should be user configurable? @@ -4744,12 +4781,12 @@ int Interp::convert_setup_tool(block_pointer block, setup_pointer settings) { tx += USER_TO_PROGRAM_LEN(settings->parameters[5211]); ty += USER_TO_PROGRAM_LEN(settings->parameters[5212]); tz += USER_TO_PROGRAM_LEN(settings->parameters[5213]); - ta += USER_TO_PROGRAM_ANG(settings->parameters[5214]); - tb += USER_TO_PROGRAM_ANG(settings->parameters[5215]); - tc += USER_TO_PROGRAM_ANG(settings->parameters[5216]); - tu += USER_TO_PROGRAM_LEN(settings->parameters[5217]); - tv += USER_TO_PROGRAM_LEN(settings->parameters[5218]); - tw += USER_TO_PROGRAM_LEN(settings->parameters[5219]); + ta += USER_TO_PROGRAM_AX(3, settings->parameters[5214]); + tb += USER_TO_PROGRAM_AX(4, settings->parameters[5215]); + tc += USER_TO_PROGRAM_AX(5, settings->parameters[5216]); + tu += USER_TO_PROGRAM_AX(6, settings->parameters[5217]); + tv += USER_TO_PROGRAM_AX(7, settings->parameters[5218]); + tw += USER_TO_PROGRAM_AX(8, settings->parameters[5219]); } @@ -4796,17 +4833,17 @@ int Interp::convert_setup_tool(block_pointer block, setup_pointer settings) { if(block->z_flag) settings->tool_table[idx].offset.tran.z = PROGRAM_TO_USER_LEN(tz - block->z_number); if(block->a_flag) - settings->tool_table[idx].offset.a = PROGRAM_TO_USER_ANG(ta - block->a_number); + settings->tool_table[idx].offset.a = PROGRAM_TO_USER_AX(3, ta - block->a_number); if(block->b_flag) - settings->tool_table[idx].offset.b = PROGRAM_TO_USER_ANG(tb - block->b_number); + settings->tool_table[idx].offset.b = PROGRAM_TO_USER_AX(4, tb - block->b_number); if(block->c_flag) - settings->tool_table[idx].offset.c = PROGRAM_TO_USER_ANG(tc - block->c_number); + settings->tool_table[idx].offset.c = PROGRAM_TO_USER_AX(5, tc - block->c_number); if(block->u_flag) - settings->tool_table[idx].offset.u = PROGRAM_TO_USER_LEN(tu - block->u_number); + settings->tool_table[idx].offset.u = PROGRAM_TO_USER_AX(6, tu - block->u_number); if(block->v_flag) - settings->tool_table[idx].offset.v = PROGRAM_TO_USER_LEN(tv - block->v_number); + settings->tool_table[idx].offset.v = PROGRAM_TO_USER_AX(7, tv - block->v_number); if(block->w_flag) - settings->tool_table[idx].offset.w = PROGRAM_TO_USER_LEN(tw - block->w_number); + settings->tool_table[idx].offset.w = PROGRAM_TO_USER_AX(8, tw - block->w_number); } if(block->r_flag) settings->tool_table[idx].diameter = PROGRAM_TO_USER_LEN(block->r_number) * 2.; @@ -4952,15 +4989,24 @@ int Interp::convert_setup(block_pointer block, //!< pointer to a block of RS27 p_int = settings->origin_index; } - CHKS((block->l_number == 20 && block->a_flag && settings->a_axis_wrapped && + CHKS((block->l_number == 20 && block->a_flag && settings->axis_wrapped[3] && (block->a_number <= -360.0 || block->a_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->a_number, 'A'); - CHKS((block->l_number == 20 && block->b_flag && settings->b_axis_wrapped && + CHKS((block->l_number == 20 && block->b_flag && settings->axis_wrapped[4] && (block->b_number <= -360.0 || block->b_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->b_number, 'B'); - CHKS((block->l_number == 20 && block->c_flag && settings->c_axis_wrapped && + CHKS((block->l_number == 20 && block->c_flag && settings->axis_wrapped[5] && (block->c_number <= -360.0 || block->c_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->c_number, 'C'); + CHKS((block->l_number == 20 && block->u_flag && settings->axis_wrapped[6] && + (block->u_number <= -360.0 || block->u_number >= 360.0)), + (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->u_number, 'U'); + CHKS((block->l_number == 20 && block->v_flag && settings->axis_wrapped[7] && + (block->v_number <= -360.0 || block->v_number >= 360.0)), + (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->v_number, 'V'); + CHKS((block->l_number == 20 && block->w_flag && settings->axis_wrapped[8] && + (block->w_number <= -360.0 || block->w_number >= 360.0)), + (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->w_number, 'W'); CHKS((settings->cutter_comp_side != CUTTER_COMP::OFF && p_int == settings->origin_index), (_("Cannot change the active coordinate system with cutter radius compensation on"))); @@ -5034,45 +5080,45 @@ int Interp::convert_setup(block_pointer block, //!< pointer to a block of RS27 if (block->a_flag) { a = block->a_number; - if (block->l_number == 20) a = ca + USER_TO_PROGRAM_ANG(parameters[5204 + (p_int * 20)]) - a; - parameters[5204 + (p_int * 20)] = PROGRAM_TO_USER_ANG(a); + if (block->l_number == 20) a = ca + USER_TO_PROGRAM_AX(3, parameters[5204 + (p_int * 20)]) - a; + parameters[5204 + (p_int * 20)] = PROGRAM_TO_USER_AX(3, a); } else - a = USER_TO_PROGRAM_ANG(parameters[5204 + (p_int * 20)]); + a = USER_TO_PROGRAM_AX(3, parameters[5204 + (p_int * 20)]); if (block->b_flag) { b = block->b_number; - if (block->l_number == 20) b = cb + USER_TO_PROGRAM_ANG(parameters[5205 + (p_int * 20)]) - b; - parameters[5205 + (p_int * 20)] = PROGRAM_TO_USER_ANG(b); + if (block->l_number == 20) b = cb + USER_TO_PROGRAM_AX(4, parameters[5205 + (p_int * 20)]) - b; + parameters[5205 + (p_int * 20)] = PROGRAM_TO_USER_AX(4, b); } else - b = USER_TO_PROGRAM_ANG(parameters[5205 + (p_int * 20)]); + b = USER_TO_PROGRAM_AX(4, parameters[5205 + (p_int * 20)]); if (block->c_flag) { c = block->c_number; - if (block->l_number == 20) c = cc + USER_TO_PROGRAM_ANG(parameters[5206 + (p_int * 20)]) - c; - parameters[5206 + (p_int * 20)] = PROGRAM_TO_USER_ANG(c); + if (block->l_number == 20) c = cc + USER_TO_PROGRAM_AX(5, parameters[5206 + (p_int * 20)]) - c; + parameters[5206 + (p_int * 20)] = PROGRAM_TO_USER_AX(5, c); } else - c = USER_TO_PROGRAM_ANG(parameters[5206 + (p_int * 20)]); + c = USER_TO_PROGRAM_AX(5, parameters[5206 + (p_int * 20)]); if (block->u_flag) { u = block->u_number; - if (block->l_number == 20) u = cu + USER_TO_PROGRAM_LEN(parameters[5207 + (p_int * 20)]) - u; - parameters[5207 + (p_int * 20)] = PROGRAM_TO_USER_LEN(u); + if (block->l_number == 20) u = cu + USER_TO_PROGRAM_AX(6, parameters[5207 + (p_int * 20)]) - u; + parameters[5207 + (p_int * 20)] = PROGRAM_TO_USER_AX(6, u); } else - u = USER_TO_PROGRAM_LEN(parameters[5207 + (p_int * 20)]); + u = USER_TO_PROGRAM_AX(6, parameters[5207 + (p_int * 20)]); if (block->v_flag) { v = block->v_number; - if (block->l_number == 20) v = cv + USER_TO_PROGRAM_LEN(parameters[5208 + (p_int * 20)]) - v; - parameters[5208 + (p_int * 20)] = PROGRAM_TO_USER_LEN(v); + if (block->l_number == 20) v = cv + USER_TO_PROGRAM_AX(7, parameters[5208 + (p_int * 20)]) - v; + parameters[5208 + (p_int * 20)] = PROGRAM_TO_USER_AX(7, v); } else - v = USER_TO_PROGRAM_LEN(parameters[5208 + (p_int * 20)]); + v = USER_TO_PROGRAM_AX(7, parameters[5208 + (p_int * 20)]); if (block->w_flag) { w = block->w_number; - if (block->l_number == 20) w = cw + USER_TO_PROGRAM_LEN(parameters[5209 + (p_int * 20)]) - w; - parameters[5209 + (p_int * 20)] = PROGRAM_TO_USER_LEN(w); + if (block->l_number == 20) w = cw + USER_TO_PROGRAM_AX(8, parameters[5209 + (p_int * 20)]) - w; + parameters[5209 + (p_int * 20)] = PROGRAM_TO_USER_AX(8, w); } else - w = USER_TO_PROGRAM_LEN(parameters[5209 + (p_int * 20)]); + w = USER_TO_PROGRAM_AX(8, parameters[5209 + (p_int * 20)]); if (p_int == settings->origin_index) { /* system is currently used */ @@ -5413,12 +5459,12 @@ int Interp::convert_stop(block_pointer block, //!< pointer to a block of RS27 settings->origin_offset_x = USER_TO_PROGRAM_LEN(settings->parameters[5221]); settings->origin_offset_y = USER_TO_PROGRAM_LEN(settings->parameters[5222]); settings->origin_offset_z = USER_TO_PROGRAM_LEN(settings->parameters[5223]); - settings->AA_origin_offset = USER_TO_PROGRAM_ANG(settings->parameters[5224]); - settings->BB_origin_offset = USER_TO_PROGRAM_ANG(settings->parameters[5225]); - settings->CC_origin_offset = USER_TO_PROGRAM_ANG(settings->parameters[5226]); - settings->u_origin_offset = USER_TO_PROGRAM_LEN(settings->parameters[5227]); - settings->v_origin_offset = USER_TO_PROGRAM_LEN(settings->parameters[5228]); - settings->w_origin_offset = USER_TO_PROGRAM_LEN(settings->parameters[5229]); + settings->AA_origin_offset = USER_TO_PROGRAM_AX(3, settings->parameters[5224]); + settings->BB_origin_offset = USER_TO_PROGRAM_AX(4, settings->parameters[5225]); + settings->CC_origin_offset = USER_TO_PROGRAM_AX(5, settings->parameters[5226]); + settings->u_origin_offset = USER_TO_PROGRAM_AX(6, settings->parameters[5227]); + settings->v_origin_offset = USER_TO_PROGRAM_AX(7, settings->parameters[5228]); + settings->w_origin_offset = USER_TO_PROGRAM_AX(8, settings->parameters[5229]); settings->rotation_xy = settings->parameters[5230]; settings->current_x -= settings->origin_offset_x; @@ -5755,37 +5801,20 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 } int Interp::convert_straight_indexer(int axis, int jnum, block_pointer block, setup_pointer settings) { - double end_x, end_y, end_z; - double AA_end, BB_end, CC_end; - double u_end, v_end, w_end; - - find_ends(block, settings, &end_x, &end_y, &end_z, - &AA_end, &BB_end, &CC_end, &u_end, &v_end, &w_end); - - CHKS((end_x != settings->current_x || - end_y != settings->current_y || - end_z != settings->current_z || - u_end != settings->u_current || - v_end != settings->v_current || - w_end != settings->w_current || - (axis != 3 && AA_end != settings->AA_current) || - (axis != 4 && BB_end != settings->BB_current) || - (axis != 5 && CC_end != settings->CC_current)), - _("BUG: An axis incorrectly moved along with an indexer")); - - switch(axis) { - case 3: - issue_straight_index(axis, jnum, AA_end, block->line_number, settings); - break; - case 4: - issue_straight_index(axis, jnum, BB_end, block->line_number, settings); - break; - case 5: - issue_straight_index(axis, jnum, CC_end, block->line_number, settings); - break; - default: - ERS((_("BUG: trying to index incorrect axis"))); + double end[9]; + + find_ends(block, settings, &end[0], &end[1], &end[2], + &end[3], &end[4], &end[5], &end[6], &end[7], &end[8]); + + const double current[9] = {settings->current_x, settings->current_y, settings->current_z, + settings->AA_current, settings->BB_current, settings->CC_current, + settings->u_current, settings->v_current, settings->w_current}; + CHKS((axis < 3 || axis > 8), (_("BUG: trying to index incorrect axis"))); + for (int n = 0; n < 9; n++) { + CHKS((n != axis && end[n] != current[n]), + _("BUG: An axis incorrectly moved along with an indexer")); } + issue_straight_index(axis, jnum, end[axis], block->line_number, settings); return INTERP_OK; } @@ -5799,15 +5828,16 @@ int Interp::issue_straight_index(int axis, int jnum, double target, int lineno, if (save_mode != CANON_EXACT_PATH) SET_MOTION_CONTROL_MODE(CANON_EXACT_PATH, 0); - double AA_end = axis == 3? target: settings->AA_current; - double BB_end = axis == 4? target: settings->BB_current; - double CC_end = axis == 5? target: settings->CC_current; + double end[9] = {settings->current_x, settings->current_y, settings->current_z, + settings->AA_current, settings->BB_current, settings->CC_current, + settings->u_current, settings->v_current, settings->w_current}; + end[axis] = target; // tell canon that this is a special indexing move UNLOCK_ROTARY(lineno, jnum); - STRAIGHT_TRAVERSE(lineno, settings->current_x, settings->current_y, settings->current_z, - AA_end, BB_end, CC_end, - settings->u_current, settings->v_current, settings->w_current); + STRAIGHT_TRAVERSE(lineno, end[0], end[1], end[2], + end[3], end[4], end[5], + end[6], end[7], end[8]); LOCK_ROTARY(lineno, jnum); // restore path mode @@ -5816,9 +5846,12 @@ int Interp::issue_straight_index(int axis, int jnum, double target, int lineno, SET_NAIVECAM_TOLERANCE(save_cam_tolerance); } - settings->AA_current = AA_end; - settings->BB_current = BB_end; - settings->CC_current = CC_end; + settings->AA_current = end[3]; + settings->BB_current = end[4]; + settings->CC_current = end[5]; + settings->u_current = end[6]; + settings->v_current = end[7]; + settings->w_current = end[8]; return INTERP_OK; } @@ -6435,12 +6468,12 @@ int Interp::convert_tool_change(setup_pointer settings) //!< pointer to machine find_relative(USER_TO_PROGRAM_LEN(settings->parameters[5181]), USER_TO_PROGRAM_LEN(settings->parameters[5182]), USER_TO_PROGRAM_LEN(settings->parameters[5183]), - USER_TO_PROGRAM_ANG(settings->parameters[5184]), - USER_TO_PROGRAM_ANG(settings->parameters[5185]), - USER_TO_PROGRAM_ANG(settings->parameters[5186]), - USER_TO_PROGRAM_LEN(settings->parameters[5187]), - USER_TO_PROGRAM_LEN(settings->parameters[5188]), - USER_TO_PROGRAM_LEN(settings->parameters[5189]), + USER_TO_PROGRAM_AX(3, settings->parameters[5184]), + USER_TO_PROGRAM_AX(4, settings->parameters[5185]), + USER_TO_PROGRAM_AX(5, settings->parameters[5186]), + USER_TO_PROGRAM_AX(6, settings->parameters[5187]), + USER_TO_PROGRAM_AX(7, settings->parameters[5188]), + USER_TO_PROGRAM_AX(8, settings->parameters[5189]), &end_x, &end_y, &end_z, &AA_end, &BB_end, &CC_end, &u_end, &v_end, &w_end, settings); @@ -6448,12 +6481,18 @@ int Interp::convert_tool_change(setup_pointer settings) //!< pointer to machine // move indexers first, one at a time // JOINTS_AXES settings->*_indexer_jnum == -1 means notused - if (AA_end != settings->AA_current && (-1 != settings->a_indexer_jnum) ) - issue_straight_index(3,settings->a_indexer_jnum, AA_end, -1, settings); - if (BB_end != settings->BB_current && (-1 != settings->b_indexer_jnum) ) - issue_straight_index(4,settings->b_indexer_jnum, BB_end, -1, settings); - if (CC_end != settings->CC_current && (-1 != settings->c_indexer_jnum) ) - issue_straight_index(5,settings->c_indexer_jnum, CC_end, -1, settings); + if (AA_end != settings->AA_current && (-1 != settings->axis_indexer_jnum[3]) ) + issue_straight_index(3,settings->axis_indexer_jnum[3], AA_end, -1, settings); + if (BB_end != settings->BB_current && (-1 != settings->axis_indexer_jnum[4]) ) + issue_straight_index(4,settings->axis_indexer_jnum[4], BB_end, -1, settings); + if (CC_end != settings->CC_current && (-1 != settings->axis_indexer_jnum[5]) ) + issue_straight_index(5,settings->axis_indexer_jnum[5], CC_end, -1, settings); + if (u_end != settings->u_current && (-1 != settings->axis_indexer_jnum[6]) ) + issue_straight_index(6,settings->axis_indexer_jnum[6], u_end, -1, settings); + if (v_end != settings->v_current && (-1 != settings->axis_indexer_jnum[7]) ) + issue_straight_index(7,settings->axis_indexer_jnum[7], v_end, -1, settings); + if (w_end != settings->w_current && (-1 != settings->axis_indexer_jnum[8]) ) + issue_straight_index(8,settings->axis_indexer_jnum[8], w_end, -1, settings); STRAIGHT_TRAVERSE(-1, end_x, end_y, end_z, AA_end, BB_end, CC_end, @@ -6601,12 +6640,12 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu tool_offset.tran.x = USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.tran.x); tool_offset.tran.y = USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.tran.y); tool_offset.tran.z = USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.tran.z); - tool_offset.a = USER_TO_PROGRAM_ANG(settings->tool_table[idx].offset.a); - tool_offset.b = USER_TO_PROGRAM_ANG(settings->tool_table[idx].offset.b); - tool_offset.c = USER_TO_PROGRAM_ANG(settings->tool_table[idx].offset.c); - tool_offset.u = USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.u); - tool_offset.v = USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.v); - tool_offset.w = USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.w); + tool_offset.a = USER_TO_PROGRAM_AX(3, settings->tool_table[idx].offset.a); + tool_offset.b = USER_TO_PROGRAM_AX(4, settings->tool_table[idx].offset.b); + tool_offset.c = USER_TO_PROGRAM_AX(5, settings->tool_table[idx].offset.c); + tool_offset.u = USER_TO_PROGRAM_AX(6, settings->tool_table[idx].offset.u); + tool_offset.v = USER_TO_PROGRAM_AX(7, settings->tool_table[idx].offset.v); + tool_offset.w = USER_TO_PROGRAM_AX(8, settings->tool_table[idx].offset.w); settings->g43_with_zero_offset = !(tool_offset.tran.x || tool_offset.tran.y || tool_offset.tran.z || tool_offset.a || tool_offset.b || tool_offset.c || @@ -6636,12 +6675,12 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu tool_offset.tran.x += USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.tran.x); tool_offset.tran.y += USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.tran.y); tool_offset.tran.z += USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.tran.z); - tool_offset.a += USER_TO_PROGRAM_ANG(settings->tool_table[idx].offset.a); - tool_offset.b += USER_TO_PROGRAM_ANG(settings->tool_table[idx].offset.b); - tool_offset.c += USER_TO_PROGRAM_ANG(settings->tool_table[idx].offset.c); - tool_offset.u += USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.u); - tool_offset.v += USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.v); - tool_offset.w += USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.w); + tool_offset.a += USER_TO_PROGRAM_AX(3, settings->tool_table[idx].offset.a); + tool_offset.b += USER_TO_PROGRAM_AX(4, settings->tool_table[idx].offset.b); + tool_offset.c += USER_TO_PROGRAM_AX(5, settings->tool_table[idx].offset.c); + tool_offset.u += USER_TO_PROGRAM_AX(6, settings->tool_table[idx].offset.u); + tool_offset.v += USER_TO_PROGRAM_AX(7, settings->tool_table[idx].offset.v); + tool_offset.w += USER_TO_PROGRAM_AX(8, settings->tool_table[idx].offset.w); } else { if(block->x_flag) tool_offset.tran.x += block->x_number; if(block->y_flag) tool_offset.tran.y += block->y_number; @@ -6688,12 +6727,12 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu settings->parameters[5081] = PROGRAM_TO_USER_LEN(tool_offset.tran.x); settings->parameters[5082] = PROGRAM_TO_USER_LEN(tool_offset.tran.y); settings->parameters[5083] = PROGRAM_TO_USER_LEN(tool_offset.tran.z); - settings->parameters[5084] = PROGRAM_TO_USER_ANG(tool_offset.a); - settings->parameters[5085] = PROGRAM_TO_USER_ANG(tool_offset.b); - settings->parameters[5086] = PROGRAM_TO_USER_ANG(tool_offset.c); - settings->parameters[5087] = PROGRAM_TO_USER_LEN(tool_offset.u); - settings->parameters[5088] = PROGRAM_TO_USER_LEN(tool_offset.v); - settings->parameters[5089] = PROGRAM_TO_USER_LEN(tool_offset.w); + settings->parameters[5084] = PROGRAM_TO_USER_AX(3, tool_offset.a); + settings->parameters[5085] = PROGRAM_TO_USER_AX(4, tool_offset.b); + settings->parameters[5086] = PROGRAM_TO_USER_AX(5, tool_offset.c); + settings->parameters[5087] = PROGRAM_TO_USER_AX(6, tool_offset.u); + settings->parameters[5088] = PROGRAM_TO_USER_AX(7, tool_offset.v); + settings->parameters[5089] = PROGRAM_TO_USER_AX(8, 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 diff --git a/src/emc/rs274ngc/interp_find.cc b/src/emc/rs274ngc/interp_find.cc index 7a12d49d878..5748f8ccd92 100644 --- a/src/emc/rs274ngc/interp_find.cc +++ b/src/emc/rs274ngc/interp_find.cc @@ -199,7 +199,7 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 } if(block->a_flag) { - if(s->a_axis_wrapped) { + if(s->axis_wrapped[3]) { CHP(unwrap_rotary(AA_p, block->a_number, block->a_number - s->AA_origin_offset - s->AA_axis_offset - s->tool_offset.a, s->AA_current, 'A')); @@ -211,7 +211,7 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 } if(block->b_flag) { - if(s->b_axis_wrapped) { + if(s->axis_wrapped[4]) { CHP(unwrap_rotary(BB_p, block->b_number, block->b_number - s->BB_origin_offset - s->BB_axis_offset - s->tool_offset.b, s->BB_current, 'B')); @@ -223,7 +223,7 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 } if(block->c_flag) { - if(s->c_axis_wrapped) { + if(s->axis_wrapped[5]) { CHP(unwrap_rotary(CC_p, block->c_number, block->c_number - s->CC_origin_offset - s->CC_axis_offset - s->tool_offset.c, s->CC_current, 'C')); @@ -235,19 +235,37 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 } if(block->u_flag) { - *u_p = block->u_number - s->u_origin_offset - s->u_axis_offset - s->tool_offset.u; + if(s->axis_wrapped[6]) { + CHP(unwrap_rotary(u_p, block->u_number, + block->u_number - s->u_origin_offset - s->u_axis_offset - s->tool_offset.u, + s->u_current, 'U')); + } else { + *u_p = block->u_number - s->u_origin_offset - s->u_axis_offset - s->tool_offset.u; + } } else { *u_p = s->u_current; } if(block->v_flag) { - *v_p = block->v_number - s->v_origin_offset - s->v_axis_offset - s->tool_offset.v; + if(s->axis_wrapped[7]) { + CHP(unwrap_rotary(v_p, block->v_number, + block->v_number - s->v_origin_offset - s->v_axis_offset - s->tool_offset.v, + s->v_current, 'V')); + } else { + *v_p = block->v_number - s->v_origin_offset - s->v_axis_offset - s->tool_offset.v; + } } else { *v_p = s->v_current; } if(block->w_flag) { - *w_p = block->w_number - s->w_origin_offset - s->w_axis_offset - s->tool_offset.w; + if(s->axis_wrapped[8]) { + CHP(unwrap_rotary(w_p, block->w_number, + block->w_number - s->w_origin_offset - s->w_axis_offset - s->tool_offset.w, + s->w_current, 'W')); + } else { + *w_p = block->w_number - s->w_origin_offset - s->w_axis_offset - s->tool_offset.w; + } } else { *w_p = s->w_current; } @@ -294,7 +312,7 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 } if(block->a_flag) { - if(s->a_axis_wrapped) { + if(s->axis_wrapped[3]) { CHP(unwrap_rotary(AA_p, block->a_number, block->a_number, s->AA_current, 'A')); } else { *AA_p = block->a_number; @@ -304,7 +322,7 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 } if(block->b_flag) { - if(s->b_axis_wrapped) { + if(s->axis_wrapped[4]) { CHP(unwrap_rotary(BB_p, block->b_number, block->b_number, s->BB_current, 'B')); } else { *BB_p = block->b_number; @@ -314,7 +332,7 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 } if(block->c_flag) { - if(s->c_axis_wrapped) { + if(s->axis_wrapped[5]) { CHP(unwrap_rotary(CC_p, block->c_number, block->c_number, s->CC_current, 'C')); } else { *CC_p = block->c_number; @@ -323,9 +341,33 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 *CC_p = s->CC_current; } - *u_p = (block->u_flag) ? block->u_number : s->u_current; - *v_p = (block->v_flag) ? block->v_number : s->v_current; - *w_p = (block->w_flag) ? block->w_number : s->w_current; + if(block->u_flag) { + if(s->axis_wrapped[6]) { + CHP(unwrap_rotary(u_p, block->u_number, block->u_number, s->u_current, 'U')); + } else { + *u_p = block->u_number; + } + } else { + *u_p = s->u_current; + } + if(block->v_flag) { + if(s->axis_wrapped[7]) { + CHP(unwrap_rotary(v_p, block->v_number, block->v_number, s->v_current, 'V')); + } else { + *v_p = block->v_number; + } + } else { + *v_p = s->v_current; + } + if(block->w_flag) { + if(s->axis_wrapped[8]) { + CHP(unwrap_rotary(w_p, block->w_number, block->w_number, s->w_current, 'W')); + } else { + *w_p = block->w_number; + } + } else { + *w_p = s->w_current; + } } else { /* mode is DISTANCE_MODE::INCREMENTAL */ @@ -431,7 +473,7 @@ int Interp::find_relative(double x1, //!< absolute x position *y2 -= settings->axis_offset_y; *z2 = z1 - settings->origin_offset_z - settings->axis_offset_z - settings->tool_offset.tran.z; - if(settings->a_axis_wrapped) { + if(settings->axis_wrapped[3]) { CHP(unwrap_rotary(AA_2, AA_1, AA_1 - settings->AA_origin_offset - settings->AA_axis_offset - settings->tool_offset.a, settings->AA_current, 'A')); @@ -439,7 +481,7 @@ int Interp::find_relative(double x1, //!< absolute x position *AA_2 = AA_1 - settings->AA_origin_offset - settings->AA_axis_offset - settings->tool_offset.a; } - if(settings->b_axis_wrapped) { + if(settings->axis_wrapped[4]) { CHP(unwrap_rotary(BB_2, BB_1, BB_1 - settings->BB_origin_offset - settings->BB_axis_offset - settings->tool_offset.b, settings->BB_current, 'B')); @@ -447,7 +489,7 @@ int Interp::find_relative(double x1, //!< absolute x position *BB_2 = BB_1 - settings->BB_origin_offset - settings->BB_axis_offset - settings->tool_offset.b; } - if(settings->c_axis_wrapped) { + if(settings->axis_wrapped[5]) { CHP(unwrap_rotary(CC_2, CC_1, CC_1 - settings->CC_origin_offset - settings->CC_axis_offset - settings->tool_offset.c, settings->CC_current, 'C')); @@ -455,9 +497,29 @@ int Interp::find_relative(double x1, //!< absolute x position *CC_2 = CC_1 - settings->CC_origin_offset - settings->CC_axis_offset - settings->tool_offset.c; } - *u_2 = u_1 - settings->u_origin_offset - settings->u_axis_offset - settings->tool_offset.u; - *v_2 = v_1 - settings->v_origin_offset - settings->v_axis_offset - settings->tool_offset.v; - *w_2 = w_1 - settings->w_origin_offset - settings->w_axis_offset - settings->tool_offset.w; + if(settings->axis_wrapped[6]) { + CHP(unwrap_rotary(u_2, u_1, + u_1 - settings->u_origin_offset - settings->u_axis_offset - settings->tool_offset.u, + settings->u_current, 'U')); + } else { + *u_2 = u_1 - settings->u_origin_offset - settings->u_axis_offset - settings->tool_offset.u; + } + + if(settings->axis_wrapped[7]) { + CHP(unwrap_rotary(v_2, v_1, + v_1 - settings->v_origin_offset - settings->v_axis_offset - settings->tool_offset.v, + settings->v_current, 'V')); + } else { + *v_2 = v_1 - settings->v_origin_offset - settings->v_axis_offset - settings->tool_offset.v; + } + + if(settings->axis_wrapped[8]) { + CHP(unwrap_rotary(w_2, w_1, + w_1 - settings->w_origin_offset - settings->w_axis_offset - settings->tool_offset.w, + settings->w_current, 'W')); + } else { + *w_2 = w_1 - settings->w_origin_offset - settings->w_axis_offset - settings->tool_offset.w; + } return INTERP_OK; } @@ -504,12 +566,12 @@ int Interp::find_current_in_system(setup_pointer s, int system, *x -= USER_TO_PROGRAM_LEN(p[5201 + system * 20]); *y -= USER_TO_PROGRAM_LEN(p[5202 + system * 20]); *z -= USER_TO_PROGRAM_LEN(p[5203 + system * 20]); - *a -= USER_TO_PROGRAM_ANG(p[5204 + system * 20]); - *b -= USER_TO_PROGRAM_ANG(p[5205 + system * 20]); - *c -= USER_TO_PROGRAM_ANG(p[5206 + system * 20]); - *u -= USER_TO_PROGRAM_LEN(p[5207 + system * 20]); - *v -= USER_TO_PROGRAM_LEN(p[5208 + system * 20]); - *w -= USER_TO_PROGRAM_LEN(p[5209 + system * 20]); + *a -= USER_TO_PROGRAM_AX(3, p[5204 + system * 20]); + *b -= USER_TO_PROGRAM_AX(4, p[5205 + system * 20]); + *c -= USER_TO_PROGRAM_AX(5, p[5206 + system * 20]); + *u -= USER_TO_PROGRAM_AX(6, p[5207 + system * 20]); + *v -= USER_TO_PROGRAM_AX(7, p[5208 + system * 20]); + *w -= USER_TO_PROGRAM_AX(8, p[5209 + system * 20]); rotate(x, y, -p[5210 + system * 20]); @@ -517,12 +579,12 @@ int Interp::find_current_in_system(setup_pointer s, int system, *x -= USER_TO_PROGRAM_LEN(p[5211]); *y -= USER_TO_PROGRAM_LEN(p[5212]); *z -= USER_TO_PROGRAM_LEN(p[5213]); - *a -= USER_TO_PROGRAM_ANG(p[5214]); - *b -= USER_TO_PROGRAM_ANG(p[5215]); - *c -= USER_TO_PROGRAM_ANG(p[5216]); - *u -= USER_TO_PROGRAM_LEN(p[5217]); - *v -= USER_TO_PROGRAM_LEN(p[5218]); - *w -= USER_TO_PROGRAM_LEN(p[5219]); + *a -= USER_TO_PROGRAM_AX(3, p[5214]); + *b -= USER_TO_PROGRAM_AX(4, p[5215]); + *c -= USER_TO_PROGRAM_AX(5, p[5216]); + *u -= USER_TO_PROGRAM_AX(6, p[5217]); + *v -= USER_TO_PROGRAM_AX(7, p[5218]); + *w -= USER_TO_PROGRAM_AX(8, p[5219]); } return INTERP_OK; @@ -583,12 +645,12 @@ int Interp::find_current_in_system_without_tlo(setup_pointer s, int system, *x -= USER_TO_PROGRAM_LEN(p[5201 + system * 20]); *y -= USER_TO_PROGRAM_LEN(p[5202 + system * 20]); *z -= USER_TO_PROGRAM_LEN(p[5203 + system * 20]); - *a -= USER_TO_PROGRAM_ANG(p[5204 + system * 20]); - *b -= USER_TO_PROGRAM_ANG(p[5205 + system * 20]); - *c -= USER_TO_PROGRAM_ANG(p[5206 + system * 20]); - *u -= USER_TO_PROGRAM_LEN(p[5207 + system * 20]); - *v -= USER_TO_PROGRAM_LEN(p[5208 + system * 20]); - *w -= USER_TO_PROGRAM_LEN(p[5209 + system * 20]); + *a -= USER_TO_PROGRAM_AX(3, p[5204 + system * 20]); + *b -= USER_TO_PROGRAM_AX(4, p[5205 + system * 20]); + *c -= USER_TO_PROGRAM_AX(5, p[5206 + system * 20]); + *u -= USER_TO_PROGRAM_AX(6, p[5207 + system * 20]); + *v -= USER_TO_PROGRAM_AX(7, p[5208 + system * 20]); + *w -= USER_TO_PROGRAM_AX(8, p[5209 + system * 20]); rotate(x, y, -p[5210 + system * 20]); @@ -596,12 +658,12 @@ int Interp::find_current_in_system_without_tlo(setup_pointer s, int system, *x -= USER_TO_PROGRAM_LEN(p[5211]); *y -= USER_TO_PROGRAM_LEN(p[5212]); *z -= USER_TO_PROGRAM_LEN(p[5213]); - *a -= USER_TO_PROGRAM_ANG(p[5214]); - *b -= USER_TO_PROGRAM_ANG(p[5215]); - *c -= USER_TO_PROGRAM_ANG(p[5216]); - *u -= USER_TO_PROGRAM_LEN(p[5217]); - *v -= USER_TO_PROGRAM_LEN(p[5218]); - *w -= USER_TO_PROGRAM_LEN(p[5219]); + *a -= USER_TO_PROGRAM_AX(3, p[5214]); + *b -= USER_TO_PROGRAM_AX(4, p[5215]); + *c -= USER_TO_PROGRAM_AX(5, p[5216]); + *u -= USER_TO_PROGRAM_AX(6, p[5217]); + *v -= USER_TO_PROGRAM_AX(7, p[5218]); + *w -= USER_TO_PROGRAM_AX(8, p[5219]); } return INTERP_OK; @@ -661,12 +723,16 @@ double Interp::find_straight_length(double x2, //!< X-coordinate of end point ) { #define tiny 1e-7 - if ( (fabs(x1-x2) > tiny) || (fabs(y1-y2) > tiny) || (fabs(z1-z2) > tiny) ) - return sqrt(pow((x2 - x1), 2) + pow((y2 - y1), 2) + pow((z2 - z1), 2)); - else if ( (fabs(u_1-u_2) > tiny) || (fabs(v_1-v_2) > tiny) || (fabs(w_1-w_2) > tiny) ) - return sqrt(pow((u_2 - u_1), 2) + pow((v_2 - v_1), 2) + pow((w_2 - w_1), 2)); - else - return sqrt(pow((AA_2 - AA_1), 2) + pow((BB_2 - BB_1), 2) + pow((CC_2 - CC_1), 2)); + // along the feed group when it moves, else the other linear axes, else + // the angular ones: XYZ, else UVW, else ABC with the default axis types + const double d[9] = {x2 - x1, y2 - y1, z2 - z1, + AA_2 - AA_1, BB_2 - BB_1, CC_2 - CC_1, + u_2 - u_1, v_2 - v_1, w_2 - w_1}; + unsigned moving = 0; + for (int n = 0; n < 9; n++) { + if (fabs(d[n]) > tiny) { moving |= 1u << n; } + } + return axisKindsLength(axisKindsMeasured(_setup.axis_kinds, moving), d); } /****************************************************************************/ diff --git a/src/emc/rs274ngc/interp_internal.cc b/src/emc/rs274ngc/interp_internal.cc index ff4ba82cb86..468827192a3 100644 --- a/src/emc/rs274ngc/interp_internal.cc +++ b/src/emc/rs274ngc/interp_internal.cc @@ -441,36 +441,53 @@ Called by: Interp::read int Interp::set_probe_data(setup_pointer settings) //!< pointer to machine settings { - double a, b, c; + double a, b, c, u, v, w; refresh_actual_position(settings); settings->parameters[5061] = GET_EXTERNAL_PROBE_POSITION_X(); settings->parameters[5062] = GET_EXTERNAL_PROBE_POSITION_Y(); settings->parameters[5063] = GET_EXTERNAL_PROBE_POSITION_Z(); a = GET_EXTERNAL_PROBE_POSITION_A(); - if(settings->a_axis_wrapped) { + if(settings->axis_wrapped[3]) { a = fmod(a, 360.0); if(a<0) a += 360.0; } settings->parameters[5064] = a; b = GET_EXTERNAL_PROBE_POSITION_B(); - if(settings->b_axis_wrapped) { + if(settings->axis_wrapped[4]) { b = fmod(b, 360.0); if(b<0) b += 360.0; } settings->parameters[5065] = b; c = GET_EXTERNAL_PROBE_POSITION_C(); - if(settings->c_axis_wrapped) { + if(settings->axis_wrapped[5]) { c = fmod(c, 360.0); if(c<0) c += 360.0; } settings->parameters[5066] = c; - settings->parameters[5067] = GET_EXTERNAL_PROBE_POSITION_U(); - settings->parameters[5068] = GET_EXTERNAL_PROBE_POSITION_V(); - settings->parameters[5069] = GET_EXTERNAL_PROBE_POSITION_W(); + u = GET_EXTERNAL_PROBE_POSITION_U(); + if(settings->axis_wrapped[6]) { + u = fmod(u, 360.0); + if(u<0) u += 360.0; + } + settings->parameters[5067] = u; + + v = GET_EXTERNAL_PROBE_POSITION_V(); + if(settings->axis_wrapped[7]) { + v = fmod(v, 360.0); + if(v<0) v += 360.0; + } + settings->parameters[5068] = v; + + w = GET_EXTERNAL_PROBE_POSITION_W(); + if(settings->axis_wrapped[8]) { + w = fmod(w, 360.0); + if(w<0) w += 360.0; + } + settings->parameters[5069] = w; settings->parameters[5070] = (double) GET_EXTERNAL_PROBE_TRIPPED_VALUE(); // was an undocumented feature?: settings->parameters[5067] = GET_EXTERNAL_PROBE_VALUE(); diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index 4f82414d158..1fc8700848d 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -30,6 +30,7 @@ #include "interp_fwd.hh" #include "interp_base.hh" #include "tooldata/tooldata.hh" +#include #define _(s) gettext(s) @@ -824,13 +825,9 @@ struct setup int tool_change_with_spindle_on; double parameter_g73_peck_clearance; double parameter_g83_peck_clearance; - int a_axis_wrapped; - int b_axis_wrapped; - int c_axis_wrapped; - - int a_indexer_jnum; - int b_indexer_jnum; - int c_indexer_jnum; + AxisKinds axis_kinds; // [AXIS_] TYPE, [TRAJ] FEED_AXES + int axis_wrapped[9]; // per axis, X 0 to W 8; angular axes only + int axis_indexer_jnum[9]; // -1 where the axis has no locking indexer bool lathe_diameter_mode; //Lathe diameter mode (g07/G08) bool mdi_interrupt; diff --git a/src/emc/rs274ngc/interp_queue.cc b/src/emc/rs274ngc/interp_queue.cc index 921450caf45..ee4ccde31d6 100644 --- a/src/emc/rs274ngc/interp_queue.cc +++ b/src/emc/rs274ngc/interp_queue.cc @@ -385,7 +385,16 @@ void enqueue_M_USER_COMMAND (int index, double p_number, double q_number) { qc().push_back(q); } -void qc_scale(double scale) { +// the axes past X Y Z that are lengths, as their [AXIS_] TYPE says +static void scale_linear(const AxisKinds &kinds, double &a, double &b, double &c, + double &u, double &v, double &w, double scale) { + double *axis[6] = {&a, &b, &c, &u, &v, &w}; + for (int n = 0; n < 6; n++) { + if (!axisKindsAngular(kinds, n + 3)) { *axis[n] *= scale; } + } +} + +void qc_scale(double scale, const AxisKinds &kinds) { if(qc().empty()) { if(debug_qc) printf("not scaling because qc is empty\n"); @@ -405,25 +414,22 @@ void qc_scale(double scale) { q.data.arc_feed.end3 *= scale; q.data.arc_feed.center1 *= scale; q.data.arc_feed.center2 *= scale; - q.data.arc_feed.u *= scale; - q.data.arc_feed.v *= scale; - q.data.arc_feed.w *= scale; + scale_linear(kinds, q.data.arc_feed.a, q.data.arc_feed.b, q.data.arc_feed.c, + q.data.arc_feed.u, q.data.arc_feed.v, q.data.arc_feed.w, scale); break; case QSTRAIGHT_FEED: q.data.straight_feed.x *= scale; q.data.straight_feed.y *= scale; q.data.straight_feed.z *= scale; - q.data.straight_feed.u *= scale; - q.data.straight_feed.v *= scale; - q.data.straight_feed.w *= scale; + scale_linear(kinds, q.data.straight_feed.a, q.data.straight_feed.b, q.data.straight_feed.c, + q.data.straight_feed.u, q.data.straight_feed.v, q.data.straight_feed.w, scale); break; case QSTRAIGHT_TRAVERSE: q.data.straight_traverse.x *= scale; q.data.straight_traverse.y *= scale; q.data.straight_traverse.z *= scale; - q.data.straight_traverse.u *= scale; - q.data.straight_traverse.v *= scale; - q.data.straight_traverse.w *= scale; + scale_linear(kinds, q.data.straight_traverse.a, q.data.straight_traverse.b, q.data.straight_traverse.c, + q.data.straight_traverse.u, q.data.straight_traverse.v, q.data.straight_traverse.w, scale); break; default: ; diff --git a/src/emc/rs274ngc/interp_queue.hh b/src/emc/rs274ngc/interp_queue.hh index bb203411d9c..e1706955b8c 100644 --- a/src/emc/rs274ngc/interp_queue.hh +++ b/src/emc/rs274ngc/interp_queue.hh @@ -144,6 +144,6 @@ void set_endpoint(double x, double y); void set_endpoint_zx(double z, double x); int move_endpoint_and_flush(setup_pointer settings, double x, double y); void qc_reset(void); -void qc_scale(double scale); +void qc_scale(double scale, const AxisKinds &kinds); #endif diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index f8a456f2c32..384baee3cdf 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -178,13 +178,9 @@ setup::setup() : tool_change_with_spindle_on(0), parameter_g73_peck_clearance(0.0), parameter_g83_peck_clearance(0.0), - a_axis_wrapped(0), - b_axis_wrapped(0), - c_axis_wrapped(0), - - a_indexer_jnum(0), - b_indexer_jnum(0), - c_indexer_jnum(0), + axis_kinds(axisKindsDefault()), + axis_wrapped{}, + axis_indexer_jnum{-1, -1, -1, -1, -1, -1, -1, -1, -1}, lathe_diameter_mode(0), mdi_interrupt(0), diff --git a/src/emc/rs274ngc/interpmodule.cc b/src/emc/rs274ngc/interpmodule.cc index d27624bcb2b..640a7c1ed6a 100644 --- a/src/emc/rs274ngc/interpmodule.cc +++ b/src/emc/rs274ngc/interpmodule.cc @@ -589,40 +589,40 @@ static inline void set_w_origin_offset(Interp &interp, double value) { interp._setup.w_origin_offset = value; } static inline int get_a_axis_wrapped (Interp &interp) { - return interp._setup.a_axis_wrapped; + return interp._setup.axis_wrapped[3]; } static inline void set_a_axis_wrapped(Interp &interp, int value) { - interp._setup.a_axis_wrapped = value; + interp._setup.axis_wrapped[3] = value; } static inline int get_a_indexer (Interp &interp) { - return interp._setup.a_indexer_jnum; + return interp._setup.axis_indexer_jnum[3]; } static inline void set_a_indexer(Interp &interp, int value) { - interp._setup.a_indexer_jnum = value; + interp._setup.axis_indexer_jnum[3] = value; } static inline int get_b_axis_wrapped (Interp &interp) { - return interp._setup.b_axis_wrapped; + return interp._setup.axis_wrapped[4]; } static inline void set_b_axis_wrapped(Interp &interp, int value) { - interp._setup.b_axis_wrapped = value; + interp._setup.axis_wrapped[4] = value; } static inline int get_b_indexer (Interp &interp) { - return interp._setup.b_indexer_jnum; + return interp._setup.axis_indexer_jnum[4]; } static inline void set_b_indexer(Interp &interp, int value) { - interp._setup.b_indexer_jnum = value; + interp._setup.axis_indexer_jnum[4] = value; } static inline int get_c_axis_wrapped (Interp &interp) { - return interp._setup.c_axis_wrapped; + return interp._setup.axis_wrapped[5]; } static inline void set_c_axis_wrapped(Interp &interp, int value) { - interp._setup.c_axis_wrapped = value; + interp._setup.axis_wrapped[5] = value; } static inline int get_c_indexer (Interp &interp) { - return interp._setup.c_indexer_jnum; + return interp._setup.axis_indexer_jnum[5]; } static inline void set_c_indexer(Interp &interp, int value) { - interp._setup.c_indexer_jnum = value; + interp._setup.axis_indexer_jnum[5] = value; } static inline int get_call_level (Interp &interp) { return interp._setup.call_level; diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index df0f7081a7e..87ba8d12148 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -854,13 +854,12 @@ int Interp::init() _setup.parameter_g73_peck_clearance = 1; _setup.parameter_g83_peck_clearance = 1; } - _setup.a_axis_wrapped = 0; - _setup.b_axis_wrapped = 0; - _setup.c_axis_wrapped = 0; + _setup.axis_kinds = axisKindsDefault(); + for (int n = 0; n < 9; n++) { + _setup.axis_wrapped[n] = 0; + _setup.axis_indexer_jnum[n] = -1; // -1 means not used + } _setup.random_toolchanger = 0; - _setup.a_indexer_jnum = -1; // -1 means not used - _setup.b_indexer_jnum = -1; // -1 means not used - _setup.c_indexer_jnum = -1; // -1 means not used _setup.return_value = 0; _setup.value_returned = 0; _setup.remap_level = 0; // remapped blocks stack index @@ -883,9 +882,20 @@ int Interp::init() _setup.tool_change_at_g30 = inifile.findBoolV("TOOL_CHANGE_AT_G30", "EMCIO", false); _setup.tool_change_quill_up = inifile.findBoolV("TOOL_CHANGE_QUILL_UP", "EMCIO", false); _setup.tool_change_with_spindle_on = inifile.findBoolV("TOOL_CHANGE_WITH_SPINDLE_ON", "EMCIO", false); - _setup.a_axis_wrapped = inifile.findBoolV("WRAPPED_ROTARY", "AXIS_A", false); - _setup.b_axis_wrapped = inifile.findBoolV("WRAPPED_ROTARY", "AXIS_B", false); - _setup.c_axis_wrapped = inifile.findBoolV("WRAPPED_ROTARY", "AXIS_C", false); + std::string kinds_err; + if (axisKindsRead(inifile, &_setup.axis_kinds, &kinds_err)) { + ERS("%s", kinds_err.c_str()); + } + // a wrapped rotary or a locking indexer is an angular axis + for (int n = 0; n < 9; n++) { + char section[] = "AXIS_X"; + section[5] = axis_kinds_letters[n]; + if (!axisKindsAngular(_setup.axis_kinds, n)) { continue; } + _setup.axis_wrapped[n] = inifile.findBoolV("WRAPPED_ROTARY", section, false); + if (auto inival = inifile.findInt("LOCKING_INDEXER_JOINT", section)) { + _setup.axis_indexer_jnum[n] = *inival; + } + } _setup.random_toolchanger = inifile.findBoolV("RANDOM_TOOLCHANGER", "EMCIO", false); _setup.num_spindles = inifile.findIntV("SPINDLES", "TRAJ", 1); @@ -908,15 +918,6 @@ int Interp::init() if (inifile.findBoolV("OWORD_WARNONLY", "RS274NGC", false)) _setup.feature_set |= FEATURE_OWORD_WARNONLY; - if (auto inival = inifile.findInt("LOCKING_INDEXER_JOINT", "AXIS_A")) { - _setup.a_indexer_jnum = *inival; - } - if (auto inival = inifile.findInt("LOCKING_INDEXER_JOINT", "AXIS_B")) { - _setup.b_indexer_jnum = *inival; - } - if (auto inival = inifile.findInt("LOCKING_INDEXER_JOINT", "AXIS_C")) { - _setup.c_indexer_jnum = *inival; - } _setup.orient_offset = inifile.findRealV("ORIENT_OFFSET", "RS274NGC", 0.0); double clr = _setup.length_units == CANON_UNITS_INCHES ? 0.050 : 1.0; _setup.parameter_g73_peck_clearance = inifile.findRealV("G73_PECK_CLEARANCE", "RS274NGC", clr); @@ -1096,12 +1097,12 @@ int Interp::init() _setup.origin_offset_x = USER_TO_PROGRAM_LEN(pars[k + 1]); _setup.origin_offset_y = USER_TO_PROGRAM_LEN(pars[k + 2]); _setup.origin_offset_z = USER_TO_PROGRAM_LEN(pars[k + 3]); - _setup.AA_origin_offset = USER_TO_PROGRAM_ANG(pars[k + 4]); - _setup.BB_origin_offset = USER_TO_PROGRAM_ANG(pars[k + 5]); - _setup.CC_origin_offset = USER_TO_PROGRAM_ANG(pars[k + 6]); - _setup.u_origin_offset = USER_TO_PROGRAM_LEN(pars[k + 7]); - _setup.v_origin_offset = USER_TO_PROGRAM_LEN(pars[k + 8]); - _setup.w_origin_offset = USER_TO_PROGRAM_LEN(pars[k + 9]); + _setup.AA_origin_offset = USER_TO_PROGRAM_AX(3, pars[k + 4]); + _setup.BB_origin_offset = USER_TO_PROGRAM_AX(4, pars[k + 5]); + _setup.CC_origin_offset = USER_TO_PROGRAM_AX(5, pars[k + 6]); + _setup.u_origin_offset = USER_TO_PROGRAM_AX(6, pars[k + 7]); + _setup.v_origin_offset = USER_TO_PROGRAM_AX(7, pars[k + 8]); + _setup.w_origin_offset = USER_TO_PROGRAM_AX(8, pars[k + 9]); SET_G5X_OFFSET(_setup.origin_index, _setup.origin_offset_x , @@ -1127,12 +1128,12 @@ int Interp::init() _setup.axis_offset_x = USER_TO_PROGRAM_LEN(pars[5211]); _setup.axis_offset_y = USER_TO_PROGRAM_LEN(pars[5212]); _setup.axis_offset_z = USER_TO_PROGRAM_LEN(pars[5213]); - _setup.AA_axis_offset = USER_TO_PROGRAM_ANG(pars[5214]); - _setup.BB_axis_offset = USER_TO_PROGRAM_ANG(pars[5215]); - _setup.CC_axis_offset = USER_TO_PROGRAM_ANG(pars[5216]); - _setup.u_axis_offset = USER_TO_PROGRAM_LEN(pars[5217]); - _setup.v_axis_offset = USER_TO_PROGRAM_LEN(pars[5218]); - _setup.w_axis_offset = USER_TO_PROGRAM_LEN(pars[5219]); + _setup.AA_axis_offset = USER_TO_PROGRAM_AX(3, pars[5214]); + _setup.BB_axis_offset = USER_TO_PROGRAM_AX(4, pars[5215]); + _setup.CC_axis_offset = USER_TO_PROGRAM_AX(5, pars[5216]); + _setup.u_axis_offset = USER_TO_PROGRAM_AX(6, pars[5217]); + _setup.v_axis_offset = USER_TO_PROGRAM_AX(7, pars[5218]); + _setup.w_axis_offset = USER_TO_PROGRAM_AX(8, pars[5219]); } else { _setup.axis_offset_x = 0.0; _setup.axis_offset_y = 0.0; diff --git a/src/emc/rs274ngc/units.h b/src/emc/rs274ngc/units.h index 5256c1a186c..2b1a3de8209 100644 --- a/src/emc/rs274ngc/units.h +++ b/src/emc/rs274ngc/units.h @@ -35,3 +35,7 @@ #define PROGRAM_TO_USER_ANG(p) (TO_EXT_ANG(FROM_PROG_ANG(p))) +/* the same for axis n, 0 X to 8 W, a length or an angle as its + [AXIS_] TYPE says */ +#define USER_TO_PROGRAM_AX(n, u) (axisKindsAngular(_setup.axis_kinds, (n)) ? USER_TO_PROGRAM_ANG(u) : USER_TO_PROGRAM_LEN(u)) +#define PROGRAM_TO_USER_AX(n, p) (axisKindsAngular(_setup.axis_kinds, (n)) ? PROGRAM_TO_USER_ANG(p) : PROGRAM_TO_USER_LEN(p)) diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 0461f233b6d..b101e86f398 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -63,6 +63,7 @@ #include "nml_intf/emcglb.h" // TRAJ_MAX_VELOCITY #include "nml_intf/modal_state.hh" #include "tooldata/tooldata.hh" +#include #include //#define EMCCANON_DEBUG @@ -114,6 +115,17 @@ void UPDATE_TAG(const StateTag& tag) { #define FROM_PROG_LEN(prog) ((prog) * (canon.lengthUnits == CANON_UNITS_INCHES ? 25.4 : canon.lengthUnits == CANON_UNITS_CM ? 10.0 : 1.0)) #define FROM_PROG_ANG(prog) (prog) +/* Axis n (0 X to 8 W) is a length or an angle as its [AXIS_] TYPE + says, and the move length is measured along the axes axisKindsMeasured() + picks. The defaults are the letters' own: A B C angles, XYZ the feed. */ +static AxisKinds kinds = axisKindsDefault(); + +#define AXIS_ANG(n) axisKindsAngular(kinds, (n)) +#define TO_EXT_AX(n, v) (AXIS_ANG(n) ? TO_EXT_ANG(v) : TO_EXT_LEN(v)) +#define FROM_EXT_AX(n, v) (AXIS_ANG(n) ? FROM_EXT_ANG(v) : FROM_EXT_LEN(v)) +#define TO_PROG_AX(n, v) (AXIS_ANG(n) ? TO_PROG_ANG(v) : TO_PROG_LEN(v)) +#define FROM_PROG_AX(n, v) (AXIS_ANG(n) ? FROM_PROG_ANG(v) : FROM_PROG_LEN(v)) + /* Certain axes are periodic. Hardcode this for now */ #define IS_PERIODIC(axisnum) \ ((axisnum) == 3 || (axisnum) == 4 || (axisnum) == 5) @@ -279,29 +291,24 @@ static void from_prog(double &x, double &y, double &z, double &a, double &b, dou x = FROM_PROG_LEN(x); y = FROM_PROG_LEN(y); z = FROM_PROG_LEN(z); - // Compiler will optimize: a=FROM_PROG_ANG(a) ==> a=a. - // 2.10 cannot handle suppress-macro - // cppcheck-suppress selfAssignment - a = FROM_PROG_ANG(a); - // cppcheck-suppress selfAssignment - b = FROM_PROG_ANG(b); - // cppcheck-suppress selfAssignment - c = FROM_PROG_ANG(c); - u = FROM_PROG_LEN(u); - v = FROM_PROG_LEN(v); - w = FROM_PROG_LEN(w); + a = FROM_PROG_AX(3, a); + b = FROM_PROG_AX(4, b); + c = FROM_PROG_AX(5, c); + u = FROM_PROG_AX(6, u); + v = FROM_PROG_AX(7, v); + w = FROM_PROG_AX(8, w); } static void from_prog(CANON_POSITION &pos) { pos.x = FROM_PROG_LEN(pos.x); pos.y = FROM_PROG_LEN(pos.y); pos.z = FROM_PROG_LEN(pos.z); - pos.a = FROM_PROG_ANG(pos.a); - pos.b = FROM_PROG_ANG(pos.b); - pos.c = FROM_PROG_ANG(pos.c); - pos.u = FROM_PROG_LEN(pos.u); - pos.v = FROM_PROG_LEN(pos.v); - pos.w = FROM_PROG_LEN(pos.w); + pos.a = FROM_PROG_AX(3, pos.a); + pos.b = FROM_PROG_AX(4, pos.b); + pos.c = FROM_PROG_AX(5, pos.c); + pos.u = FROM_PROG_AX(6, pos.u); + pos.v = FROM_PROG_AX(7, pos.v); + pos.w = FROM_PROG_AX(8, pos.w); } static void from_prog_len(PM_CARTESIAN &vec) { @@ -314,24 +321,24 @@ static void to_ext(double &x, double &y, double &z, double &a, double &b, double x = TO_EXT_LEN(x); y = TO_EXT_LEN(y); z = TO_EXT_LEN(z); - a = TO_EXT_ANG(a); - b = TO_EXT_ANG(b); - c = TO_EXT_ANG(c); - u = TO_EXT_LEN(u); - v = TO_EXT_LEN(v); - w = TO_EXT_LEN(w); + a = TO_EXT_AX(3, a); + b = TO_EXT_AX(4, b); + c = TO_EXT_AX(5, c); + u = TO_EXT_AX(6, u); + v = TO_EXT_AX(7, v); + w = TO_EXT_AX(8, w); } static void to_ext(CANON_POSITION & pos) { pos.x=TO_EXT_LEN(pos.x); pos.y=TO_EXT_LEN(pos.y); pos.z=TO_EXT_LEN(pos.z); - pos.a=TO_EXT_ANG(pos.a); - pos.b=TO_EXT_ANG(pos.b); - pos.c=TO_EXT_ANG(pos.c); - pos.u=TO_EXT_LEN(pos.u); - pos.v=TO_EXT_LEN(pos.v); - pos.w=TO_EXT_LEN(pos.w); + pos.a=TO_EXT_AX(3, pos.a); + pos.b=TO_EXT_AX(4, pos.b); + pos.c=TO_EXT_AX(5, pos.c); + pos.u=TO_EXT_AX(6, pos.u); + pos.v=TO_EXT_AX(7, pos.v); + pos.w=TO_EXT_AX(8, pos.w); } #endif @@ -348,12 +355,12 @@ static EmcPose to_ext_pose(double x, double y, double z, double a, double b, dou result.tran.x = TO_EXT_LEN(x); result.tran.y = TO_EXT_LEN(y); result.tran.z = TO_EXT_LEN(z); - result.a = TO_EXT_ANG(a); - result.b = TO_EXT_ANG(b); - result.c = TO_EXT_ANG(c); - result.u = TO_EXT_LEN(u); - result.v = TO_EXT_LEN(v); - result.w = TO_EXT_LEN(w); + result.a = TO_EXT_AX(3, a); + result.b = TO_EXT_AX(4, b); + result.c = TO_EXT_AX(5, c); + result.u = TO_EXT_AX(6, u); + result.v = TO_EXT_AX(7, v); + result.w = TO_EXT_AX(8, w); return result; } @@ -362,12 +369,12 @@ static EmcPose to_ext_pose(const CANON_POSITION & pos) { result.tran.x = TO_EXT_LEN(pos.x); result.tran.y = TO_EXT_LEN(pos.y); result.tran.z = TO_EXT_LEN(pos.z); - result.a = TO_EXT_ANG(pos.a); - result.b = TO_EXT_ANG(pos.b); - result.c = TO_EXT_ANG(pos.c); - result.u = TO_EXT_LEN(pos.u); - result.v = TO_EXT_LEN(pos.v); - result.w = TO_EXT_LEN(pos.w); + result.a = TO_EXT_AX(3, pos.a); + result.b = TO_EXT_AX(4, pos.b); + result.c = TO_EXT_AX(5, pos.c); + result.u = TO_EXT_AX(6, pos.u); + result.v = TO_EXT_AX(7, pos.v); + result.w = TO_EXT_AX(8, pos.w); return result; } @@ -375,12 +382,12 @@ static void to_prog(CANON_POSITION &e) { e.x = TO_PROG_LEN(e.x); e.y = TO_PROG_LEN(e.y); e.z = TO_PROG_LEN(e.z); - e.a = TO_PROG_ANG(e.a); - e.b = TO_PROG_ANG(e.b); - e.c = TO_PROG_ANG(e.c); - e.u = TO_PROG_LEN(e.u); - e.v = TO_PROG_LEN(e.v); - e.w = TO_PROG_LEN(e.w); + e.a = TO_PROG_AX(3, e.a); + e.b = TO_PROG_AX(4, e.b); + e.c = TO_PROG_AX(5, e.c); + e.u = TO_PROG_AX(6, e.u); + e.v = TO_PROG_AX(7, e.v); + e.w = TO_PROG_AX(8, e.w); } static int axis_valid(int n) { @@ -416,8 +423,8 @@ void CANON_UPDATE_END_POINT(double x, double y, double z, double u, double v, double w) { canonUpdateEndPoint(FROM_PROG_LEN(x),FROM_PROG_LEN(y),FROM_PROG_LEN(z), - FROM_PROG_ANG(a),FROM_PROG_ANG(b),FROM_PROG_ANG(c), - FROM_PROG_LEN(u),FROM_PROG_LEN(v),FROM_PROG_LEN(w)); + FROM_PROG_AX(3, a),FROM_PROG_AX(4, b),FROM_PROG_AX(5, c), + FROM_PROG_AX(6, u),FROM_PROG_AX(7, v),FROM_PROG_AX(8, w)); } static double toExtVel(double vel) { @@ -600,36 +607,6 @@ static double getMinAngularDisplacement() return FROM_EXT_ANG(CART_FUZZ); } -/** - * Apply the minimum displacement check to each axis delta. - * - * Checks that the axis is valid / active, and looks up the appropriate minimum - * displacement for the axis type and user units. - */ -static void applyMinDisplacement(double &dx, - double &dy, - double &dz, - double &da, - double &db, - double &dc, - double &du, - double &dv, - double &dw - ) -{ - const double tiny_linear = getMinLinearDisplacement(); - const double tiny_angular = getMinAngularDisplacement(); - if(!axis_valid(0) || dx < tiny_linear) dx = 0.0; - if(!axis_valid(1) || dy < tiny_linear) dy = 0.0; - if(!axis_valid(2) || dz < tiny_linear) dz = 0.0; - if(!axis_valid(3) || da < tiny_angular) da = 0.0; - if(!axis_valid(4) || db < tiny_angular) db = 0.0; - if(!axis_valid(5) || dc < tiny_angular) dc = 0.0; - if(!axis_valid(6) || du < tiny_linear) du = 0.0; - if(!axis_valid(7) || dv < tiny_linear) dv = 0.0; - if(!axis_valid(8) || dw < tiny_linear) dw = 0.0; -} - #ifndef MIN #define MIN(a,b) ((a)<(b)?(a):(b)) #endif @@ -682,124 +659,133 @@ static int __attribute__((unused)) findMinMoveJoint(double &dx, return saxis; } /** - * Get the limiting acceleration for a displacement from the current position to the given position. - * returns a single acceleration that is the minimum of all axis accelerations. + * A straight move from canon.endPoint to the given position: how far each + * axis goes, a distance below the smallest move motion can take, or on an + * axis the machine lacks, counting as none; the axes that move; the axes + * the move is measured along (axisKindsMeasured()) and its length along + * them. Sets canon.cartesian_move and canon.angular_move, whether a linear + * and whether an angular axis moves. */ -static double getStraightJerk(double x, double y, double z, - double a, double b, double c, - double u, double v, double w){ +struct StraightSpan { + double d[9]; + unsigned moving; + unsigned measured; + double length; +}; - double dx, dy, dz, du, dv, dw, da, db, dc; - double tx, ty, tz, tu, tv, tw, ta, tb, tc; - JerkData out; +static StraightSpan getStraightSpan(double x, double y, double z, + double a, double b, double c, + double u, double v, double w) +{ + const double end[9] = {x, y, z, a, b, c, u, v, w}; + const double start[9] = {canon.endPoint.x, canon.endPoint.y, canon.endPoint.z, + canon.endPoint.a, canon.endPoint.b, canon.endPoint.c, + canon.endPoint.u, canon.endPoint.v, canon.endPoint.w}; + const double tiny_linear = getMinLinearDisplacement(); + const double tiny_angular = getMinAngularDisplacement(); + StraightSpan span; - out.jerk = 0.0; // if a move to nowhere - out.tmax = 0.0; - out.dtot = 0.0; + span.moving = 0; + for (int n = 0; n < 9; n++) { + span.d[n] = fabs(end[n] - start[n]); + if (!axis_valid(n) || span.d[n] < (AXIS_ANG(n) ? tiny_angular : tiny_linear)) { + span.d[n] = 0.0; + } + if (span.d[n] > 0.0) { + span.moving |= 1u << n; + } + } + canon.cartesian_move = (span.moving & ~kinds.angular) != 0; + canon.angular_move = (span.moving & kinds.angular) != 0; + span.measured = axisKindsMeasured(kinds, span.moving); + span.length = axisKindsLength(span.measured, span.d); - // Compute absolute travel distance for each axis: - dx = fabs(x - canon.endPoint.x); - dy = fabs(y - canon.endPoint.y); - dz = fabs(z - canon.endPoint.z); - da = fabs(a - canon.endPoint.a); - db = fabs(b - canon.endPoint.b); - dc = fabs(c - canon.endPoint.c); - du = fabs(u - canon.endPoint.u); - dv = fabs(v - canon.endPoint.v); - dw = fabs(w - canon.endPoint.w); + if(debug_velacc) + printf("getStraightSpan dx %g dy %g dz %g da %g db %g dc %g du %g dv %g dw %g length %g\n", + span.d[0], span.d[1], span.d[2], span.d[3], span.d[4], span.d[5], + span.d[6], span.d[7], span.d[8], span.length); + return span; +} - applyMinDisplacement(dx, dy, dz, da, db, dc, du, dv, dw); +/** + * Motion measures a line along X Y Z, else U V W, else A B C, in external + * units, whatever the axes are. The rates canon sends are along its own + * length, span.length; this factor turns them into rates along motion's + * length, so the move takes the time canon planned. It is exactly 1 where + * both measure along the same axes of one kind, which the default kinds + * always do. + */ +static double motionLengthRatio(const StraightSpan &span) +{ + unsigned tier; - if(debug_velacc) - printf("getStraightJerk dx %g dy %g dz %g da %g db %g dc %g du %g dv %g dw %g ", - dx, dy, dz, da, db, dc, du, dv, dw); - - // Figure out what kind of move we're making. This is used to determine - // the units of vel/acc. - if (dx <= 0.0 && dy <= 0.0 && dz <= 0.0 && - du <= 0.0 && dv <= 0.0 && dw <= 0.0) { - canon.cartesian_move = 0; + if (span.moving & 0x007u) { + tier = 0x007u; + } else if (span.moving & 0x1c0u) { + tier = 0x1c0u; + } else if (span.moving & 0x038u) { + tier = 0x038u; } else { - canon.cartesian_move = 1; + return 1.0; } - if (da <= 0.0 && db <= 0.0 && dc <= 0.0) { - canon.angular_move = 0; - } else { - canon.angular_move = 1; + unsigned tier_angular = tier & kinds.angular; + if (tier == span.measured && (tier_angular == 0 || tier_angular == tier)) { + return 1.0; } + double ext[9]; + for (int n = 0; n < 9; n++) { + ext[n] = TO_EXT_AX(n, span.d[n]); + } + double own = axisKindsMeasuredAngular(kinds, span.measured) ? + TO_EXT_ANG(span.length) : TO_EXT_LEN(span.length); + if (own <= 0.0) { + return 1.0; + } + return axisKindsLength(tier, ext) / own; +} + +// Rates along canon's length, made rates along motion's: see +// motionLengthRatio(). +template static void toMotionLength(M &msg, double ratio) +{ + msg.vel *= ratio; + msg.ini_maxvel *= ratio; + msg.acc *= ratio; + msg.ini_maxjerk *= ratio; +} + +static double getMotionLengthRatio(double x, double y, double z, + double a, double b, double c, + double u, double v, double w) +{ + return motionLengthRatio(getStraightSpan(x, y, z, a, b, c, u, v, w)); +} + +/** + * Get the limiting jerk for a displacement from the current position to the + * given position: the path jerk at which the first axis reaches its own. + */ +static double getStraightJerk(double x, double y, double z, + double a, double b, double c, + double u, double v, double w){ + + StraightSpan span = getStraightSpan(x, y, z, a, b, c, u, v, w); + double tmax = 0.0; - // Pure linear move: // For jerk-limited motion: d = (1/6)*j*t³, so t = cbrt(6*d/j) // We use t = cbrt(d/j) as a characteristic time (omitting the constant factor, // which cancels out when we compute path jerk = dtot / tmax³) - if (canon.cartesian_move && !canon.angular_move) { - tx = dx? cbrt(dx / FROM_EXT_LEN(emcAxisGetMaxJerk(0))): 0.0; - ty = dy? cbrt(dy / FROM_EXT_LEN(emcAxisGetMaxJerk(1))): 0.0; - tz = dz? cbrt(dz / FROM_EXT_LEN(emcAxisGetMaxJerk(2))): 0.0; - tu = du? cbrt(du / FROM_EXT_LEN(emcAxisGetMaxJerk(6))): 0.0; - tv = dv? cbrt(dv / FROM_EXT_LEN(emcAxisGetMaxJerk(7))): 0.0; - tw = dw? cbrt(dw / FROM_EXT_LEN(emcAxisGetMaxJerk(8))): 0.0; - out.tmax = MAX3(tx, ty ,tz); - out.tmax = MAX4(tu, tv, tw, out.tmax); - - if(dx || dy || dz) - out.dtot = sqrt(dx * dx + dy * dy + dz * dz); - else - out.dtot = sqrt(du * du + dv * dv + dw * dw); - - if (out.tmax > 0.0) { - out.jerk = out.dtot / (out.tmax * out.tmax * out.tmax); + for (int n = 0; n < 9; n++) { + if (span.d[n]) { + tmax = std::max(tmax, cbrt(span.d[n] / FROM_EXT_AX(n, emcAxisGetMaxJerk(n)))); } } - // Pure angular move: - else if (!canon.cartesian_move && canon.angular_move) { - ta = da? cbrt(da / FROM_EXT_ANG(emcAxisGetMaxJerk(3))): 0.0; - tb = db? cbrt(db / FROM_EXT_ANG(emcAxisGetMaxJerk(4))): 0.0; - tc = dc? cbrt(dc / FROM_EXT_ANG(emcAxisGetMaxJerk(5))): 0.0; - out.tmax = MAX3(ta, tb, tc); - - out.dtot = sqrt(da * da + db * db + dc * dc); - if (out.tmax > 0.0) { - out.jerk = out.dtot / (out.tmax * out.tmax * out.tmax); - } + if (tmax > 0.0) { + return span.length / (tmax * tmax * tmax); } - // Combination angular and linear move: - else if (canon.cartesian_move && canon.angular_move) { - tx = dx? cbrt(dx / FROM_EXT_LEN(emcAxisGetMaxJerk(0))): 0.0; - ty = dy? cbrt(dy / FROM_EXT_LEN(emcAxisGetMaxJerk(1))): 0.0; - tz = dz? cbrt(dz / FROM_EXT_LEN(emcAxisGetMaxJerk(2))): 0.0; - ta = da? cbrt(da / FROM_EXT_ANG(emcAxisGetMaxJerk(3))): 0.0; - tb = db? cbrt(db / FROM_EXT_ANG(emcAxisGetMaxJerk(4))): 0.0; - tc = dc? cbrt(dc / FROM_EXT_ANG(emcAxisGetMaxJerk(5))): 0.0; - tu = du? cbrt(du / FROM_EXT_LEN(emcAxisGetMaxJerk(6))): 0.0; - tv = dv? cbrt(dv / FROM_EXT_LEN(emcAxisGetMaxJerk(7))): 0.0; - tw = dw? cbrt(dw / FROM_EXT_LEN(emcAxisGetMaxJerk(8))): 0.0; - out.tmax = MAX9(tx, ty, tz, - ta, tb, tc, - tu, tv, tw); - - if(debug_velacc) - printf("getStraightJerk t tx %g ty %g tz %g ta %g tb %g tc %g tu %g tv %g tw %g\n", - tx, ty, tz, ta, tb, tc, tu, tv, tw); - - if(dx || dy || dz) - out.dtot = sqrt(dx * dx + dy * dy + dz * dz); - else - out.dtot = sqrt(du * du + dv * dv + dw * dw); - - if (out.tmax > 0.0) { - out.jerk = out.dtot / (out.tmax * out.tmax * out.tmax); - } - } - //if(debug_velacc) - //printf("#### CALC THE JERK #### cartesian %d ang %d jerk %g\n", canon.cartesian_move, canon.angular_move, out.jerk); - return out.jerk; + return 0.0; // a move to nowhere } -static double __attribute__((unused)) getStraightJerk(CANON_POSITION pos) -{ - return getStraightJerk(pos.x, pos.y, pos.z, pos.a, pos.b, pos.c, pos.u, pos.v, pos.w); -} /** * Get the limiting acceleration for a displacement from the current position to the given position. * returns a single acceleration that is the minimum of all axis accelerations. @@ -808,111 +794,28 @@ static AccelData getStraightAcceleration(double x, double y, double z, double a, double b, double c, double u, double v, double w) { - double dx, dy, dz, du, dv, dw, da, db, dc; - double tx, ty, tz, tu, tv, tw, ta, tb, tc; + StraightSpan span = getStraightSpan(x, y, z, a, b, c, u, v, w); AccelData out; out.acc = 0.0; // if a move to nowhere out.tmax = 0.0; - out.dtot = 0.0; - - // Compute absolute travel distance for each axis: - dx = fabs(x - canon.endPoint.x); - dy = fabs(y - canon.endPoint.y); - dz = fabs(z - canon.endPoint.z); - da = fabs(a - canon.endPoint.a); - db = fabs(b - canon.endPoint.b); - dc = fabs(c - canon.endPoint.c); - du = fabs(u - canon.endPoint.u); - dv = fabs(v - canon.endPoint.v); - dw = fabs(w - canon.endPoint.w); - - applyMinDisplacement(dx, dy, dz, da, db, dc, du, dv, dw); - - if(debug_velacc) - printf("getStraightAcceleration dx %g dy %g dz %g da %g db %g dc %g du %g dv %g dw %g ", - dx, dy, dz, da, db, dc, du, dv, dw); - - // Figure out what kind of move we're making. This is used to determine - // the units of vel/acc. - if (dx <= 0.0 && dy <= 0.0 && dz <= 0.0 && - du <= 0.0 && dv <= 0.0 && dw <= 0.0) { - canon.cartesian_move = 0; - } else { - canon.cartesian_move = 1; - } - if (da <= 0.0 && db <= 0.0 && dc <= 0.0) { - canon.angular_move = 0; - } else { - canon.angular_move = 1; - } + out.dtot = span.length; - // Pure linear move: - if (canon.cartesian_move && !canon.angular_move) { - tx = dx? (dx / FROM_EXT_LEN(emcAxisGetMaxAcceleration(0))): 0.0; - ty = dy? (dy / FROM_EXT_LEN(emcAxisGetMaxAcceleration(1))): 0.0; - tz = dz? (dz / FROM_EXT_LEN(emcAxisGetMaxAcceleration(2))): 0.0; - tu = du? (du / FROM_EXT_LEN(emcAxisGetMaxAcceleration(6))): 0.0; - tv = dv? (dv / FROM_EXT_LEN(emcAxisGetMaxAcceleration(7))): 0.0; - tw = dw? (dw / FROM_EXT_LEN(emcAxisGetMaxAcceleration(8))): 0.0; - out.tmax = std::max({tx, ty ,tz}); - out.tmax = std::max({tu, tv, tw, out.tmax}); - - if(dx || dy || dz) - out.dtot = sqrt(dx * dx + dy * dy + dz * dz); - else - out.dtot = sqrt(du * du + dv * dv + dw * dw); - - if (out.tmax > 0.0) { - out.acc = out.dtot / out.tmax; - } - } - // Pure angular move: - else if (!canon.cartesian_move && canon.angular_move) { - ta = da? (da / FROM_EXT_ANG(emcAxisGetMaxAcceleration(3))): 0.0; - tb = db? (db / FROM_EXT_ANG(emcAxisGetMaxAcceleration(4))): 0.0; - tc = dc? (dc / FROM_EXT_ANG(emcAxisGetMaxAcceleration(5))): 0.0; - out.tmax = std::max({ta, tb, tc}); - - out.dtot = sqrt(da * da + db * db + dc * dc); - if (out.tmax > 0.0) { - out.acc = out.dtot / out.tmax; - } - } - // Combination angular and linear move: - else if (canon.cartesian_move && canon.angular_move) { - tx = dx? (dx / FROM_EXT_LEN(emcAxisGetMaxAcceleration(0))): 0.0; - ty = dy? (dy / FROM_EXT_LEN(emcAxisGetMaxAcceleration(1))): 0.0; - tz = dz? (dz / FROM_EXT_LEN(emcAxisGetMaxAcceleration(2))): 0.0; - ta = da? (da / FROM_EXT_ANG(emcAxisGetMaxAcceleration(3))): 0.0; - tb = db? (db / FROM_EXT_ANG(emcAxisGetMaxAcceleration(4))): 0.0; - tc = dc? (dc / FROM_EXT_ANG(emcAxisGetMaxAcceleration(5))): 0.0; - tu = du? (du / FROM_EXT_LEN(emcAxisGetMaxAcceleration(6))): 0.0; - tv = dv? (dv / FROM_EXT_LEN(emcAxisGetMaxAcceleration(7))): 0.0; - tw = dw? (dw / FROM_EXT_LEN(emcAxisGetMaxAcceleration(8))): 0.0; - out.tmax = std::max({tx, ty, tz, - ta, tb, tc, - tu, tv, tw}); - - if(debug_velacc) - printf("getStraightAcceleration t^2 tx %g ty %g tz %g ta %g tb %g tc %g tu %g tv %g tw %g\n", - tx, ty, tz, ta, tb, tc, tu, tv, tw); /* According to NIST IR6556 Section 2.1.2.5 Paragraph A a combnation move is handled like a linear move, except that the angular axes are allowed sufficient time to complete their motion coordinated with the motion of the linear axes. */ - if(dx || dy || dz) - out.dtot = sqrt(dx * dx + dy * dy + dz * dz); - else - out.dtot = sqrt(du * du + dv * dv + dw * dw); - - if (out.tmax > 0.0) { - out.acc = out.dtot / out.tmax; - } + for (int n = 0; n < 9; n++) { + if (span.d[n]) { + out.tmax = std::max(out.tmax, span.d[n] / FROM_EXT_AX(n, emcAxisGetMaxAcceleration(n))); + } } - if(debug_velacc) + if (out.tmax > 0.0) { + out.acc = out.dtot / out.tmax; + } + if(debug_velacc) printf("cartesian %d ang %d acc %g\n", canon.cartesian_move, canon.angular_move, out.acc); return out; } @@ -935,120 +838,28 @@ static VelData getStraightVelocity(double x, double y, double z, double a, double b, double c, double u, double v, double w) { - double dx, dy, dz, da, db, dc, du, dv, dw; - double tx, ty, tz, ta, tb, tc, tu, tv, tw; + StraightSpan span = getStraightSpan(x, y, z, a, b, c, u, v, w); VelData out; -/* If we get a move to nowhere (!canon.cartesian_move && !canon.angular_move) - we might as well go there at the canon.linearFeedRate... -*/ - out.vel = canon.linearFeedRate; out.tmax = 0; - out.dtot = 0; - - // Compute absolute travel distance for each axis: - dx = fabs(x - canon.endPoint.x); - dy = fabs(y - canon.endPoint.y); - dz = fabs(z - canon.endPoint.z); - da = fabs(a - canon.endPoint.a); - db = fabs(b - canon.endPoint.b); - dc = fabs(c - canon.endPoint.c); - du = fabs(u - canon.endPoint.u); - dv = fabs(v - canon.endPoint.v); - dw = fabs(w - canon.endPoint.w); - - applyMinDisplacement(dx, dy, dz, da, db, dc, du, dv, dw); - - if(debug_velacc) - printf("getStraightVelocity dx %g dy %g dz %g da %g db %g dc %g du %g dv %g dw %g\n", - dx, dy, dz, da, db, dc, du, dv, dw); - - // Figure out what kind of move we're making: - if (dx <= 0.0 && dy <= 0.0 && dz <= 0.0 && - du <= 0.0 && dv <= 0.0 && dw <= 0.0) { - canon.cartesian_move = 0; - } else { - canon.cartesian_move = 1; - } - if (da <= 0.0 && db <= 0.0 && dc <= 0.0) { - canon.angular_move = 0; - } else { - canon.angular_move = 1; - } - - // Pure linear move: - if (canon.cartesian_move && !canon.angular_move) { - tx = dx? fabs(dx / FROM_EXT_LEN(emcAxisGetMaxVelocity(0))): 0.0; - ty = dy? fabs(dy / FROM_EXT_LEN(emcAxisGetMaxVelocity(1))): 0.0; - tz = dz? fabs(dz / FROM_EXT_LEN(emcAxisGetMaxVelocity(2))): 0.0; - tu = du? fabs(du / FROM_EXT_LEN(emcAxisGetMaxVelocity(6))): 0.0; - tv = dv? fabs(dv / FROM_EXT_LEN(emcAxisGetMaxVelocity(7))): 0.0; - tw = dw? fabs(dw / FROM_EXT_LEN(emcAxisGetMaxVelocity(8))): 0.0; - out.tmax = std::max({tx, ty ,tz}); - out.tmax = std::max({tu, tv, tw, out.tmax}); - - if(dx || dy || dz) - out.dtot = sqrt(dx * dx + dy * dy + dz * dz); - else - out.dtot = sqrt(du * du + dv * dv + dw * dw); - - if (out.tmax <= 0.0) { - out.vel = canon.linearFeedRate; - } else { - out.vel = out.dtot / out.tmax; + out.dtot = span.length; + for (int n = 0; n < 9; n++) { + if (span.d[n]) { + out.tmax = std::max(out.tmax, fabs(span.d[n] / FROM_EXT_AX(n, emcAxisGetMaxVelocity(n)))); } } - // Pure angular move: - else if (!canon.cartesian_move && canon.angular_move) { - ta = da? fabs(da / FROM_EXT_ANG(emcAxisGetMaxVelocity(3))): 0.0; - tb = db? fabs(db / FROM_EXT_ANG(emcAxisGetMaxVelocity(4))): 0.0; - tc = dc? fabs(dc / FROM_EXT_ANG(emcAxisGetMaxVelocity(5))): 0.0; - out.tmax = std::max({ta, tb, tc}); - - out.dtot = sqrt(da * da + db * db + dc * dc); - if (out.tmax <= 0.0) { - out.vel = canon.angularFeedRate; - } else { - out.vel = out.dtot / out.tmax; - } - } - // Combination angular and linear move: - else if (canon.cartesian_move && canon.angular_move) { - tx = dx? fabs(dx / FROM_EXT_LEN(emcAxisGetMaxVelocity(0))): 0.0; - ty = dy? fabs(dy / FROM_EXT_LEN(emcAxisGetMaxVelocity(1))): 0.0; - tz = dz? fabs(dz / FROM_EXT_LEN(emcAxisGetMaxVelocity(2))): 0.0; - ta = da? fabs(da / FROM_EXT_ANG(emcAxisGetMaxVelocity(3))): 0.0; - tb = db? fabs(db / FROM_EXT_ANG(emcAxisGetMaxVelocity(4))): 0.0; - tc = dc? fabs(dc / FROM_EXT_ANG(emcAxisGetMaxVelocity(5))): 0.0; - tu = du? fabs(du / FROM_EXT_LEN(emcAxisGetMaxVelocity(6))): 0.0; - tv = dv? fabs(dv / FROM_EXT_LEN(emcAxisGetMaxVelocity(7))): 0.0; - tw = dw? fabs(dw / FROM_EXT_LEN(emcAxisGetMaxVelocity(8))): 0.0; - out.tmax = std::max({tx, ty, tz, - ta, tb, tc, - tu, tv, tw}); - - if(debug_velacc) - printf("getStraightVelocity times tx %g ty %g tz %g ta %g tb %g tc %g tu %g tv %g tw %g\n", - tx, ty, tz, ta, tb, tc, tu, tv, tw); -/* According to NIST IR6556 Section 2.1.2.5 Paragraph A - a combnation move is handled like a linear move, except - that the angular axes are allowed sufficient time to - complete their motion coordinated with the motion of - the linear axes. +/* If we get a move to nowhere (!canon.cartesian_move && !canon.angular_move) + we might as well go there at the canon.linearFeedRate... */ - if(dx || dy || dz) - out.dtot = sqrt(dx * dx + dy * dy + dz * dz); - else - out.dtot = sqrt(du * du + dv * dv + dw * dw); - - if (out.tmax <= 0.0) { - out.vel = canon.linearFeedRate; - } else { - out.vel = out.dtot / out.tmax; - } + if (out.tmax > 0.0) { + out.vel = out.dtot / out.tmax; + } else if (!canon.cartesian_move && canon.angular_move) { + out.vel = canon.angularFeedRate; + } else { + out.vel = canon.linearFeedRate; } - if(debug_velacc) + if(debug_velacc) printf("cartesian %d ang %d vel %g\n", canon.cartesian_move, canon.angular_move, out.vel); return out; } @@ -1123,14 +934,14 @@ static void flush_segments(void) { linearMoveMsg->end.tran.y = TO_EXT_LEN(y); linearMoveMsg->end.tran.z = TO_EXT_LEN(z); - linearMoveMsg->end.u = TO_EXT_LEN(u); - linearMoveMsg->end.v = TO_EXT_LEN(v); - linearMoveMsg->end.w = TO_EXT_LEN(w); + linearMoveMsg->end.u = TO_EXT_AX(6, u); + linearMoveMsg->end.v = TO_EXT_AX(7, v); + linearMoveMsg->end.w = TO_EXT_AX(8, w); // fill in the orientation - linearMoveMsg->end.a = TO_EXT_ANG(a); - linearMoveMsg->end.b = TO_EXT_ANG(b); - linearMoveMsg->end.c = TO_EXT_ANG(c); + linearMoveMsg->end.a = TO_EXT_AX(3, a); + linearMoveMsg->end.b = TO_EXT_AX(4, b); + linearMoveMsg->end.c = TO_EXT_AX(5, c); linearMoveMsg->vel = toExtVel(vel); linearMoveMsg->ini_maxvel = toExtVel(linedata.vel); @@ -1138,6 +949,7 @@ static void flush_segments(void) { double acc = lineaccdata.acc; linearMoveMsg->ini_maxjerk = toExtVel(jerk); linearMoveMsg->acc = toExtAcc(acc); + toMotionLength(*linearMoveMsg, getMotionLengthRatio(x, y, z, a, b, c, u, v, w)); linearMoveMsg->type = EMC_MOTION_TYPE_FEED; linearMoveMsg->indexer_jnum = -1; @@ -1266,6 +1078,7 @@ void generate_fast_move(double x, double y, double z, linearMoveMsg->vel = linearMoveMsg->ini_maxvel = toExtVel(vel); linearMoveMsg->acc = toExtAcc(acc); linearMoveMsg->ini_maxjerk = toExtVel(jerk); + toMotionLength(*linearMoveMsg, getMotionLengthRatio(x, y, z, a, b, c, u, v, w)); linearMoveMsg->type = EMC_MOTION_TYPE_FEED; linearMoveMsg->feed_mode = 0; @@ -1300,6 +1113,7 @@ void generate_move(double vel,double x, double y, double z, linearMoveMsg->vel = linearMoveMsg->ini_maxvel = toExtVel(vel); linearMoveMsg->acc = toExtAcc(acc); linearMoveMsg->ini_maxjerk = toExtVel(jerk); + toMotionLength(*linearMoveMsg, getMotionLengthRatio(x, y, z, a, b, c, u, v, w)); linearMoveMsg->type = EMC_MOTION_TYPE_FEED; linearMoveMsg->feed_mode = 0; linearMoveMsg->indexer_jnum = -1; @@ -1349,6 +1163,7 @@ void STRAIGHT_TRAVERSE(int line_number, linearMoveMsg->vel = linearMoveMsg->ini_maxvel = toExtVel(vel); linearMoveMsg->acc = toExtAcc(acc); linearMoveMsg->ini_maxjerk = toExtVel(jerk); + toMotionLength(*linearMoveMsg, getMotionLengthRatio(x, y, z, a, b, c, u, v, w)); linearMoveMsg->indexer_jnum = canon.rotary_unlock_for_traverse; int old_feed_mode = canon.feed_mode; @@ -1464,6 +1279,7 @@ void STRAIGHT_PROBE(int line_number, probeMsg->ini_maxvel = toExtVel(ini_maxvel); probeMsg->acc = toExtAcc(acc); probeMsg->ini_maxjerk = toExtVel(jerk); + toMotionLength(*probeMsg, getMotionLengthRatio(x, y, z, a, b, c, u, v, w)); probeMsg->type = EMC_MOTION_TYPE_PROBING; probeMsg->probe_type = probe_type; @@ -2636,17 +2452,12 @@ void ARC_FEED(int line_number, rotate_and_offset_pos(fe, se, ae, unused, unused, unused, unused, unused, unused); rotate_and_offset_pos(fa, sa, unused, unused, unused, unused, unused, unused, unused); if (chord_deviation(lx, ly, fe, se, fa, sa, rotation, mx, my) < canon.naivecamTolerance) { - // Compiler will optimize: a=FROM_PROG_ANG(a) ==> a=a. - // 2.10 cannot handle suppress-macro - // cppcheck-suppress selfAssignment - a = FROM_PROG_ANG(a); - // cppcheck-suppress selfAssignment - b = FROM_PROG_ANG(b); - // cppcheck-suppress selfAssignment - c = FROM_PROG_ANG(c); - u = FROM_PROG_LEN(u); - v = FROM_PROG_LEN(v); - w = FROM_PROG_LEN(w); + a = FROM_PROG_AX(3, a); + b = FROM_PROG_AX(4, b); + c = FROM_PROG_AX(5, c); + u = FROM_PROG_AX(6, u); + v = FROM_PROG_AX(7, v); + w = FROM_PROG_AX(8, w); rotate_and_offset_pos(unused, unused, unused, a, b, c, u, v, w); see_segment(line_number, _tag, mx, my, @@ -3144,24 +2955,24 @@ void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) canon.toolOffset.tran.x = FROM_PROG_LEN(offset.tran.x); canon.toolOffset.tran.y = FROM_PROG_LEN(offset.tran.y); canon.toolOffset.tran.z = FROM_PROG_LEN(offset.tran.z); - canon.toolOffset.a = FROM_PROG_ANG(offset.a); - canon.toolOffset.b = FROM_PROG_ANG(offset.b); - canon.toolOffset.c = FROM_PROG_ANG(offset.c); - canon.toolOffset.u = FROM_PROG_LEN(offset.u); - canon.toolOffset.v = FROM_PROG_LEN(offset.v); - canon.toolOffset.w = FROM_PROG_LEN(offset.w); + canon.toolOffset.a = FROM_PROG_AX(3, offset.a); + canon.toolOffset.b = FROM_PROG_AX(4, offset.b); + canon.toolOffset.c = FROM_PROG_AX(5, offset.c); + canon.toolOffset.u = FROM_PROG_AX(6, offset.u); + canon.toolOffset.v = FROM_PROG_AX(7, offset.v); + canon.toolOffset.w = FROM_PROG_AX(8, offset.w); /* append it to interp list so it gets updated at the right time, not at read-ahead time */ set_offset_msg->offset.tran.x = TO_EXT_LEN(canon.toolOffset.tran.x); set_offset_msg->offset.tran.y = TO_EXT_LEN(canon.toolOffset.tran.y); set_offset_msg->offset.tran.z = TO_EXT_LEN(canon.toolOffset.tran.z); - set_offset_msg->offset.a = TO_EXT_ANG(canon.toolOffset.a); - set_offset_msg->offset.b = TO_EXT_ANG(canon.toolOffset.b); - set_offset_msg->offset.c = TO_EXT_ANG(canon.toolOffset.c); - set_offset_msg->offset.u = TO_EXT_LEN(canon.toolOffset.u); - 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->offset.a = TO_EXT_AX(3, canon.toolOffset.a); + set_offset_msg->offset.b = TO_EXT_AX(4, canon.toolOffset.b); + set_offset_msg->offset.c = TO_EXT_AX(5, canon.toolOffset.c); + set_offset_msg->offset.u = TO_EXT_AX(6, canon.toolOffset.u); + set_offset_msg->offset.v = TO_EXT_AX(7, canon.toolOffset.v); + set_offset_msg->offset.w = TO_EXT_AX(8, canon.toolOffset.w); for (int s = 0; s < emcStatus->motion.traj.spindles; s++){ if(canon.spindle[s].css_maximum) { @@ -3200,15 +3011,15 @@ void CHANGE_TOOL() w = canon.endPoint.w; if (have_tool_change_position > 3) { - a = FROM_EXT_ANG(tool_change_position.a); - b = FROM_EXT_ANG(tool_change_position.b); - c = FROM_EXT_ANG(tool_change_position.c); + a = FROM_EXT_AX(3, tool_change_position.a); + b = FROM_EXT_AX(4, tool_change_position.b); + c = FROM_EXT_AX(5, tool_change_position.c); } if (have_tool_change_position > 6) { - u = FROM_EXT_LEN(tool_change_position.u); - v = FROM_EXT_LEN(tool_change_position.v); - w = FROM_EXT_LEN(tool_change_position.w); + u = FROM_EXT_AX(6, tool_change_position.u); + v = FROM_EXT_AX(7, tool_change_position.v); + w = FROM_EXT_AX(8, tool_change_position.w); } VelData veldata = getStraightVelocity(x, y, z, a, b, c, u, v, w); @@ -3225,6 +3036,7 @@ void CHANGE_TOOL() linearMoveMsg->vel = linearMoveMsg->ini_maxvel = toExtVel(vel); linearMoveMsg->acc = toExtAcc(acc); linearMoveMsg->ini_maxjerk = toExtVel(jerk); + toMotionLength(*linearMoveMsg, getMotionLengthRatio(x, y, z, a, b, c, u, v, w)); linearMoveMsg->type = EMC_MOTION_TYPE_TOOLCHANGE; linearMoveMsg->feed_mode = 0; linearMoveMsg->indexer_jnum = -1; @@ -3603,32 +3415,32 @@ double GET_EXTERNAL_TOOL_LENGTH_ZOFFSET() double GET_EXTERNAL_TOOL_LENGTH_AOFFSET() { - return TO_PROG_ANG(canon.toolOffset.a); + return TO_PROG_AX(3, canon.toolOffset.a); } double GET_EXTERNAL_TOOL_LENGTH_BOFFSET() { - return TO_PROG_ANG(canon.toolOffset.b); + return TO_PROG_AX(4, canon.toolOffset.b); } double GET_EXTERNAL_TOOL_LENGTH_COFFSET() { - return TO_PROG_ANG(canon.toolOffset.c); + return TO_PROG_AX(5, canon.toolOffset.c); } double GET_EXTERNAL_TOOL_LENGTH_UOFFSET() { - return TO_PROG_LEN(canon.toolOffset.u); + return TO_PROG_AX(6, canon.toolOffset.u); } double GET_EXTERNAL_TOOL_LENGTH_VOFFSET() { - return TO_PROG_LEN(canon.toolOffset.v); + return TO_PROG_AX(7, canon.toolOffset.v); } double GET_EXTERNAL_TOOL_LENGTH_WOFFSET() { - return TO_PROG_LEN(canon.toolOffset.w); + return TO_PROG_AX(8, canon.toolOffset.w); } /* @@ -3678,6 +3490,14 @@ void INIT_CANON() canon.angularFeedRate = 0.0; ZERO_EMC_POSE(canon.toolOffset); + { + std::string err; + linuxcnc::IniFile ini(emc_inifile); + if (axisKindsRead(ini, &kinds, &err)) { + rcs_print_error("%s\n", err.c_str()); + } + } + /* to set the units, note that GET_EXTERNAL_LENGTH_UNITS() returns traj->linearUnits, which is already set from the INI file in @@ -3774,8 +3594,8 @@ CANON_POSITION GET_EXTERNAL_POSITION() // first update internal record of last position canonUpdateEndPoint(FROM_EXT_LEN(pos.tran.x), FROM_EXT_LEN(pos.tran.y), FROM_EXT_LEN(pos.tran.z), - FROM_EXT_ANG(pos.a), FROM_EXT_ANG(pos.b), FROM_EXT_ANG(pos.c), - FROM_EXT_LEN(pos.u), FROM_EXT_LEN(pos.v), FROM_EXT_LEN(pos.w)); + FROM_EXT_AX(3, pos.a), FROM_EXT_AX(4, pos.b), FROM_EXT_AX(5, pos.c), + FROM_EXT_AX(6, pos.u), FROM_EXT_AX(7, pos.v), FROM_EXT_AX(8, pos.w)); // now calculate position in program units, for interpreter position = unoffset_and_unrotate_pos(canon.endPoint); @@ -3799,13 +3619,13 @@ CANON_POSITION GET_EXTERNAL_PROBE_POSITION() pos.tran.y = FROM_EXT_LEN(pos.tran.y); pos.tran.z = FROM_EXT_LEN(pos.tran.z); - pos.a = FROM_EXT_ANG(pos.a); - pos.b = FROM_EXT_ANG(pos.b); - pos.c = FROM_EXT_ANG(pos.c); + pos.a = FROM_EXT_AX(3, pos.a); + pos.b = FROM_EXT_AX(4, pos.b); + pos.c = FROM_EXT_AX(5, pos.c); - pos.u = FROM_EXT_LEN(pos.u); - pos.v = FROM_EXT_LEN(pos.v); - pos.w = FROM_EXT_LEN(pos.w); + pos.u = FROM_EXT_AX(6, pos.u); + pos.v = FROM_EXT_AX(7, pos.v); + pos.w = FROM_EXT_AX(8, pos.w); // now calculate position in program units, for interpreter position = unoffset_and_unrotate_pos(pos); @@ -3864,10 +3684,7 @@ double GET_EXTERNAL_AXIS_MAX_VELOCITY(int axis) double vel = emcAxisGetMaxVelocity(axis); - if (axis >= 3 && axis <= 5) { - return TO_PROG_ANG(FROM_EXT_ANG(vel)) * 60.0; - } - return TO_PROG_LEN(FROM_EXT_LEN(vel)) * 60.0; + return TO_PROG_AX(axis, FROM_EXT_AX(axis, vel)) * 60.0; } double GET_EXTERNAL_SPINDLE_MAX_VELOCITY(int spindle) From 3ea91ab52517ad8ad084ce278ce61ece7051e17e Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 23 Sep 2026 21:24:05 +1000 Subject: [PATCH 16/77] sai: take the axes from [TRAJ] COORDINATES rs274 answered GET_EXTERNAL_AXIS_MASK() with XYZABC whatever the INI said, so a program with U V W words could not be run through it. Given an INI with [TRAJ] COORDINATES, it now reports those axes; without one it stays XYZABC. --- src/emc/sai/driver.cc | 8 ++++++++ src/emc/sai/saicanon.cc | 3 ++- src/emc/sai/saicanon.hh | 1 + 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/emc/sai/driver.cc b/src/emc/sai/driver.cc index b0275236c3a..177aa2176b9 100644 --- a/src/emc/sai/driver.cc +++ b/src/emc/sai/driver.cc @@ -27,6 +27,7 @@ #include /* gets, etc. */ #include /* exit */ #include /* strcpy */ +#include /* toupper */ #include #include #include @@ -711,6 +712,13 @@ int main (int argc, char ** argv) _sai._external_length_units = 1.0; } } + if (auto coordinates = ini.findString("COORDINATES", "TRAJ")) { + _sai._axis_mask = 0; + for (char ch : *coordinates) { + const char *at = strchr("XYZABCUVW", toupper((unsigned char)ch)); + if (ch && at) { _sai._axis_mask |= 1 << (at - "XYZABCUVW"); } + } + } setenv("INI_FILE_NAME",inifile,1); } else unsetenv("INI_FILE_NAME"); diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index fd946d5c3af..d60d661d4f7 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -739,7 +739,7 @@ double GET_EXTERNAL_MOTION_CONTROL_NAIVECAM_TOLERANCE() { return _sai.naivecam_tolerance; } double GET_EXTERNAL_LENGTH_UNITS() {return _sai._external_length_units;} int GET_EXTERNAL_FEED_HOLD_ENABLE() {return 1;} -int GET_EXTERNAL_AXIS_MASK() {return 0x3f;} // XYZABC machine +int GET_EXTERNAL_AXIS_MASK() {return _sai._axis_mask;} double GET_EXTERNAL_ANGLE_UNITS() {return 1.0;} int GET_EXTERNAL_SELECTED_TOOL_SLOT() { return 0; } int GET_EXTERNAL_SPINDLE_OVERRIDE_ENABLE(int /*spindle*/) {return so_enable;} @@ -1169,6 +1169,7 @@ StandaloneInterpInternals::StandaloneInterpInternals() : _feed_rate(0.0), _flood(0), _external_length_units(1.0), + _axis_mask(0x3f), /* XYZABC unless the INI names the axes */ _length_unit_factor(1), /* 1 for MM 25.4 for inch */ _length_unit_type(CANON_UNITS_MM), _line_number(1), diff --git a/src/emc/sai/saicanon.hh b/src/emc/sai/saicanon.hh index d00233c02fd..f5946ef5039 100644 --- a/src/emc/sai/saicanon.hh +++ b/src/emc/sai/saicanon.hh @@ -26,6 +26,7 @@ struct StandaloneInterpInternals double _feed_rate; int _flood; double _external_length_units; + int _axis_mask; double _length_unit_factor; CANON_UNITS _length_unit_type; int _line_number; From 0d93892a67a922ad2280d3a23fb21ca9b4601f89 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 23 Sep 2026 21:38:48 +1000 Subject: [PATCH 17/77] tests: axis type in the interpreter and through motion interp/axis-type runs rs274 on a mm INI with A LINEAR and V ANGULAR and wrapped: under G20 A scales and V does not, G93 feeds along A, V wraps from 350 to 370, and G10 L2 stores A in mm and V in degrees. axis-type runs the same machine through task, canon and motion: G20 A1 V10 reaches A 25.4 mm and V 10 degrees, V alone at F3600 turns 60 degrees a second under G20, X with A feeds along X, A with V feeds along A while motion runs its V measure 8 times faster so the move takes its 1 s, and V wraps. On master it fails at the first check, A at 1. --- tests/axis-type/README | 4 ++ tests/axis-type/axis-type.hal | 17 +++++ tests/axis-type/axis-type.ini | 124 ++++++++++++++++++++++++++++++++ tests/axis-type/checkresult | 3 + tests/axis-type/test-ui.py | 111 ++++++++++++++++++++++++++++ tests/axis-type/test.sh | 3 + tests/interp/axis-type/expected | 48 +++++++++++++ tests/interp/axis-type/test.ini | 11 +++ tests/interp/axis-type/test.ngc | 22 ++++++ tests/interp/axis-type/test.sh | 3 + 10 files changed, 346 insertions(+) create mode 100644 tests/axis-type/README create mode 100644 tests/axis-type/axis-type.hal create mode 100644 tests/axis-type/axis-type.ini create mode 100755 tests/axis-type/checkresult create mode 100755 tests/axis-type/test-ui.py create mode 100755 tests/axis-type/test.sh create mode 100644 tests/interp/axis-type/expected create mode 100644 tests/interp/axis-type/test.ini create mode 100644 tests/interp/axis-type/test.ngc create mode 100755 tests/interp/axis-type/test.sh diff --git a/tests/axis-type/README b/tests/axis-type/README new file mode 100644 index 00000000000..e83aa12afec --- /dev/null +++ b/tests/axis-type/README @@ -0,0 +1,4 @@ +Axis type through task, canon and motion: a mm machine with A configured +LINEAR and V ANGULAR (and wrapped). Under G20 an A word is inches and a V +word degrees; F is measured along A when A is the linear axis that moves, +and along V in degrees per minute when V moves alone; V wraps. diff --git a/tests/axis-type/axis-type.hal b/tests/axis-type/axis-type.hal new file mode 100644 index 00000000000..0406391056f --- /dev/null +++ b/tests/axis-type/axis-type.hal @@ -0,0 +1,17 @@ +# HAL file for the axis type test + +loadrt [KINS]KINEMATICS coordinates=xyzav +loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net j0pos joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net j1pos joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net j2pos joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net j3pos joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net j4pos 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/axis-type/axis-type.ini b/tests/axis-type/axis-type.ini new file mode 100644 index 00000000000..4851966fd61 --- /dev/null +++ b/tests/axis-type/axis-type.ini @@ -0,0 +1,124 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0x0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[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] +HALFILE = axis-type.hal + +[TRAJ] +NO_FORCE_HOMING = 1 +COORDINATES = X Y Z A V +LINEAR_UNITS = mm +ANGULAR_UNITS = degree +DEFAULT_LINEAR_VELOCITY = 10 +MAX_LINEAR_VELOCITY = 400 +MAX_LINEAR_ACCELERATION = 50000 + +[EMCIO] +CYCLE_TIME = 0.100 + +[KINS] +KINEMATICS = trivkins +JOINTS = 5 + +[AXIS_X] +TYPE = LINEAR +MAX_VELOCITY = 100 +MAX_ACCELERATION = 50000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 + +[JOINT_0] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 50000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +FERROR = 1 +MIN_FERROR = 1 + +[AXIS_Y] +TYPE = LINEAR +MAX_VELOCITY = 100 +MAX_ACCELERATION = 50000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 + +[JOINT_1] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 50000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +FERROR = 1 +MIN_FERROR = 1 + +[AXIS_Z] +TYPE = LINEAR +MAX_VELOCITY = 100 +MAX_ACCELERATION = 50000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 + +[JOINT_2] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 50000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +FERROR = 1 +MIN_FERROR = 1 + +[AXIS_A] +TYPE = LINEAR +MAX_VELOCITY = 100 +MAX_ACCELERATION = 50000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 + +[JOINT_3] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 50000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +FERROR = 1 +MIN_FERROR = 1 + +[AXIS_V] +TYPE = ANGULAR +MAX_VELOCITY = 360 +MAX_ACCELERATION = 50000 +MIN_LIMIT = -1e+09 +MAX_LIMIT = 1e+09 +WRAPPED_ROTARY = 1 + +[JOINT_4] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 360 +MAX_ACCELERATION = 50000 +MIN_LIMIT = -1e+09 +MAX_LIMIT = 1e+09 +FERROR = 1 +MIN_FERROR = 1 diff --git a/tests/axis-type/checkresult b/tests/axis-type/checkresult new file mode 100755 index 00000000000..acfd40f1b32 --- /dev/null +++ b/tests/axis-type/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# Test passes if test-ui.py returns successfully +exit 0 diff --git a/tests/axis-type/test-ui.py b/tests/axis-type/test-ui.py new file mode 100755 index 00000000000..94e1fbe774d --- /dev/null +++ b/tests/axis-type/test-ui.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 +# A LINEAR and V ANGULAR on a mm machine: units, feed and wrap through +# task, canon and motion. + +import linuxcnc +import sys +import time + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +A, V = 3, 7 + + +def fail(msg): + print("FAIL: " + msg) + c.state(linuxcnc.STATE_ESTOP) + sys.exit(1) + + +def wait_ready(timeout=20.0): + end = time.time() + timeout + while time.time() < end: + s.poll() + if s.task_state == linuxcnc.STATE_ESTOP and s.interp_state == linuxcnc.INTERP_IDLE \ + and s.axis_mask != 0 and s.linear_units != 0.0: + return + time.sleep(0.1) + fail("linuxcnc did not come up") + + +def errors(): + err = e.poll() + if err: + fail("error: %s" % err[1]) + + +def mdi(cmd, timeout=20.0): + """Run one MDI line to the end; return the largest current_vel seen + and how long the machine moved.""" + c.mdi(cmd) + peak = 0.0 + first = last = None + end = time.time() + timeout + time.sleep(0.05) + while time.time() < end: + s.poll() + errors() + if s.current_vel > 1e-6: + now = time.time() + first = first or now + last = now + peak = max(peak, s.current_vel) + if s.interp_state == linuxcnc.INTERP_IDLE and s.queue == 0 and s.inpos \ + and s.state == linuxcnc.RCS_DONE: + break + time.sleep(0.001) + else: + fail("%s did not finish" % cmd) + errors() + return peak, (last - first) if first else 0.0 + + +def near(what, got, want, tol): + print("%s: %.6g (want %.6g)" % (what, got, want)) + if abs(got - want) > tol: + fail("%s is %.6g, not %.6g" % (what, got, want)) + + +wait_ready() +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete() + +# G20: an A word is inches, a V word degrees +mdi("G20 G90 G94 G0 A1 V10") +s.poll() +near("A after G20 A1 (mm)", s.position[A], 25.4, 1e-6) +near("V after G20 V10 (deg)", s.position[V], 10.0, 1e-6) + +# V alone: F is degrees per minute, G20 or not +peak, _ = mdi("G20 G1 V90 F3600") +near("V alone at F3600, deg/s", peak, 60.0, 0.6) + +# X with A: F along X, the feed axis (F comes before G21 in a block, so +# G21 goes on its own line) +mdi("G21") +peak, _ = mdi("G1 X10 A35.4 F600") +near("X with A at F600, mm/s", peak, 10.0, 0.1) + +# A with V: F along A, the linear axis that moves; motion measures the +# line along V, 8 times longer, so it runs at 8 times the rate and the +# move still takes the time of 10 mm at 10 mm/s +peak, secs = mdi("G1 A45.4 V170 F600") +near("A with V at F600, motion rate", peak, 80.0, 0.8) +near("A with V at F600, seconds", secs, 1.0, 0.1) +s.poll() +near("A after the move", s.position[A], 45.4, 1e-6) +near("V after the move", s.position[V], 170.0, 1e-6) + +# V wraps: 350 then 10 goes on to 370 +mdi("G0 V350") +mdi("G0 V10") +s.poll() +near("V wrapped", s.position[V], 370.0, 1e-6) + +c.state(linuxcnc.STATE_ESTOP) +print("PASS") +sys.exit(0) diff --git a/tests/axis-type/test.sh b/tests/axis-type/test.sh new file mode 100755 index 00000000000..42663324ad4 --- /dev/null +++ b/tests/axis-type/test.sh @@ -0,0 +1,3 @@ +#!/bin/bash -e + +linuxcnc -r axis-type.ini diff --git a/tests/interp/axis-type/expected b/tests/interp/axis-type/expected new file mode 100644 index 00000000000..0e165af8a3c --- /dev/null +++ b/tests/interp/axis-type/expected @@ -0,0 +1,48 @@ + N..... USE_LENGTH_UNITS(CANON_UNITS_MM) + N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + N..... SET_XY_ROTATION(0.0000) + N..... SET_FEED_REFERENCE(CANON_XYZ) + N..... ON_RESET() + N..... COMMENT("A is a length, V an angle, on a mm machine") + N..... COMMENT("interpreter: feed mode set to units per minute") + N..... SET_FEED_MODE(0, 0) + N..... SET_FEED_RATE(0.0000) + N..... SELECT_PLANE(CANON_PLANE_XY) + N..... USE_LENGTH_UNITS(CANON_UNITS_MM) + N..... SET_FEED_RATE(100.0000) + N..... STRAIGHT_FEED(0.0000, 0.0000, 0.0000, 10.0000, 0.0000, 0.0000) + N..... COMMENT("G20: A is 10 mm, 0.3937 inch; V stays 10 degrees") + N..... USE_LENGTH_UNITS(CANON_UNITS_INCHES) + N..... STRAIGHT_FEED(1.0000, 0.0000, 0.0000, 0.3937, 0.0000, 0.0000) + N..... MESSAGE(" G20 A=0.393701 V=10.000000") + N..... USE_LENGTH_UNITS(CANON_UNITS_MM) + N..... MESSAGE(" G21 A=10.000000 V=10.000000") + N..... COMMENT("G93 feeds along A, the linear axis that moves, else along V") + N..... COMMENT("interpreter: feed mode set to inverse time") + N..... SET_FEED_MODE(0, 0) + N..... SET_FEED_RATE(20.0000) + N..... STRAIGHT_FEED(25.4000, 0.0000, 0.0000, 10.0000, 0.0000, 0.0000) + N..... SET_FEED_RATE(20.0000) + N..... STRAIGHT_FEED(25.4000, 0.0000, 0.0000, 20.0000, 0.0000, 0.0000) + N..... COMMENT("interpreter: feed mode set to units per minute") + N..... SET_FEED_MODE(0, 0) + N..... SET_FEED_RATE(0.0000) + N..... COMMENT("V wraps: 350 then 10 goes on to 370") + N..... STRAIGHT_TRAVERSE(25.4000, 0.0000, 0.0000, 20.0000, 0.0000, 0.0000) + N..... STRAIGHT_TRAVERSE(25.4000, 0.0000, 0.0000, 20.0000, 0.0000, 0.0000) + N..... MESSAGE(" wrapped V=370.000000") + N..... COMMENT("offsets: A in program units, V in degrees") + N..... USE_LENGTH_UNITS(CANON_UNITS_INCHES) + N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 1.0000, 0.0000, 0.0000) + N..... SET_XY_ROTATION(0.0000) + N..... USE_LENGTH_UNITS(CANON_UNITS_MM) + N..... MESSAGE(" G54 A=25.400000 V=5.000000") + N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 25.4000, 0.0000, 0.0000) + N..... SET_XY_ROTATION(0.0000) + N..... SET_FEED_MODE(0, 0) + N..... SET_FEED_RATE(0.0000) + N..... STOP_SPINDLE_TURNING(0) + N..... SET_SPINDLE_MODE(0 0.0000) + N..... PROGRAM_END() + N..... ON_RESET() diff --git a/tests/interp/axis-type/test.ini b/tests/interp/axis-type/test.ini new file mode 100644 index 00000000000..aaf56c41ae4 --- /dev/null +++ b/tests/interp/axis-type/test.ini @@ -0,0 +1,11 @@ +[TRAJ] +COORDINATES = X Y Z A B C U V W +LINEAR_UNITS = mm +ANGULAR_UNITS = degree + +[AXIS_A] +TYPE = LINEAR + +[AXIS_V] +TYPE = ANGULAR +WRAPPED_ROTARY = 1 diff --git a/tests/interp/axis-type/test.ngc b/tests/interp/axis-type/test.ngc new file mode 100644 index 00000000000..397b2dbf492 --- /dev/null +++ b/tests/interp/axis-type/test.ngc @@ -0,0 +1,22 @@ +(A is a length, V an angle, on a mm machine) +G21 G90 G94 G17 +G1 F100 A10 V10 +(G20: A is 10 mm, 0.3937 inch; V stays 10 degrees) +G20 +G1 X1 +(debug, G20 A=#5423 V=#5427) +G21 +(debug, G21 A=#5423 V=#5427) +(G93 feeds along A, the linear axis that moves, else along V) +G93 G1 V20 F2 +G1 A20 V40 F2 +G94 +(V wraps: 350 then 10 goes on to 370) +G0 V350 +G0 V10 +(debug, wrapped V=#5427) +(offsets: A in program units, V in degrees) +G20 G10 L2 P1 A1 V5 +G21 +(debug, G54 A=#5224 V=#5228) +M2 diff --git a/tests/interp/axis-type/test.sh b/tests/interp/axis-type/test.sh new file mode 100755 index 00000000000..65d2a707e9c --- /dev/null +++ b/tests/interp/axis-type/test.sh @@ -0,0 +1,3 @@ +#!/bin/bash +rs274 -g -i test.ini test.ngc | awk '{$1=""; print}' +exit "${PIPESTATUS[0]}" From 433c00c175efdb1a16bbec1c7210a1200b795d44 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 5 Sep 2026 00:06:27 +1000 Subject: [PATCH 18/77] interpreter: add the tilted work plane, G68.2, G68.4 and G69 A frame composed inside the offset chain, so the blocks between a definition and its cancel are programmed in the tilted plane while G54 itself is untouched: world = TLO + G5x + Rz(rotation_xy) * (G92 + O + R * program) O and R are the plane's origin and rotation in the coordinate system active when it was defined; rotary and UVW words do not pass through it. G68.2 takes the Fanuc forms: three angles about the axes Q names (P0 Euler, P1 fixed axes), three points over four blocks (P2), two vectors over two blocks (P3), each with an R about the plane's own Z. G68.4 composes any of those onto the active plane. G69 cancels; so do init, M2 and M30. An abort cancels it and tells canon even when the read ahead had already cancelled: status carries the plane the executed canon stream last set, and an abort throws the queued G69 away, so the two disagree until canon is told. An explicit G69 tells canon either way. Canon gains SET_G68_FRAME and the frame stage in rotate_and_offset_pos() and its inverse, so positions, probe results and arcs follow the plane. Task status carries g68_offset, g68_rotation and g68_active; the python canons, AXIS and halui apply it. While a plane is active G92, G52, G10 L2/L20/L10/L11 and a coordinate system change are refused, since each defines the system the plane sits on. --- docs/src/config/python-interface.adoc | 9 + docs/src/gcode/g-code.adoc | 107 +++++ docs/src/gcode/overview.adoc | 1 + lib/python/rs274/glcanon.py | 9 + lib/python/rs274/interpret.py | 14 + src/emc/nml_intf/canon.hh | 14 + src/emc/nml_intf/emc.cc | 17 + src/emc/nml_intf/emc.hh | 1 + src/emc/nml_intf/emc_nml.hh | 23 + src/emc/nml_intf/emcops.cc | 3 + src/emc/rs274ngc/Submakefile | 1 + src/emc/rs274ngc/gcode_renderer.cc | 12 +- src/emc/rs274ngc/gcode_renderer.hh | 8 +- src/emc/rs274ngc/gcodemodule.cc | 31 +- src/emc/rs274ngc/gcodemodule.hh | 35 ++ src/emc/rs274ngc/interp_array.cc | 2 +- src/emc/rs274ngc/interp_check.cc | 22 +- src/emc/rs274ngc/interp_convert.cc | 54 ++- src/emc/rs274ngc/interp_find.cc | 40 +- src/emc/rs274ngc/interp_internal.cc | 5 + src/emc/rs274ngc/interp_internal.hh | 18 +- src/emc/rs274ngc/interp_namedparams.cc | 21 +- src/emc/rs274ngc/interp_setup.cc | 8 + src/emc/rs274ngc/interp_workplane.cc | 437 ++++++++++++++++++ src/emc/rs274ngc/interp_write.cc | 2 +- src/emc/rs274ngc/rs274ngc_interp.hh | 14 + src/emc/rs274ngc/rs274ngc_pre.cc | 9 + src/emc/sai/saicanon.cc | 10 + src/emc/task/emccanon.cc | 76 ++- src/emc/task/emctaskmain.cc | 11 + src/emc/usr_intf/axis/extensions/emcmodule.cc | 17 + src/emc/usr_intf/axis/scripts/axis.py | 1 + src/emc/usr_intf/halui.cc | 35 +- tests/gcode-renderer/programs.py | 21 + tests/gcode-renderer/test_transform.py | 28 ++ tests/interp/g68-frame/expected | 55 +++ tests/interp/g68-frame/g68.ngc | 44 ++ tests/interp/g68-frame/test.sh | 3 + 38 files changed, 1133 insertions(+), 85 deletions(-) create mode 100644 src/emc/rs274ngc/interp_workplane.cc create mode 100644 tests/interp/g68-frame/expected create mode 100644 tests/interp/g68-frame/g68.ngc create mode 100755 tests/interp/g68-frame/test.sh diff --git a/docs/src/config/python-interface.adoc b/docs/src/config/python-interface.adoc index 25d5762445b..0fce1c760ac 100644 --- a/docs/src/config/python-interface.adoc +++ b/docs/src/config/python-interface.adoc @@ -179,6 +179,15 @@ see <> for an example. *g5x_offset*:: '(returns tuple of floats)' - offset of the currently active coordinate system. +*g68_active*:: '(returns integer)' - + a tilted work plane (G68.2) is in effect. + +*g68_offset*:: '(returns tuple of floats)' - + origin of the tilted work plane, in the coordinate system it was defined in. + +*g68_rotation*:: '(returns tuple of floats)' - + rotation of the tilted work plane, nine values row by row; its columns are the plane's X, Y and Z. + *g92_offset*:: '(returns tuple of floats)' - pose of the current g92 offset. diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 8162e52c993..f9572fd4e1d 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -93,6 +93,7 @@ as the 'L number', and so on for any other letter. |<> |Exact Path Mode |<> |Exact Stop Mode |<> |Path Control Mode with Optional Tolerance +|<> |Tilted Work Plane |<> |Lathe finishing cycle |<> |Lathe roughing cycle |<> |Drilling Cycle with Chip Breaking @@ -1990,6 +1991,112 @@ G64 P0.015 Q2 .G64 Heart image::images/G64_Heart_Q2.png["G64 Heart",align="center"] +[[gcode:g68.2]] +== G68.2, G68.4, G69 Tilted Work Plane(((G68.2 Tilted Work Plane))) + +[source,ngc] +---- +G68.2 X- Y- Z- I- J- K- (three angles, Euler) +G68.2 P1 X- Y- Z- I- J- K- (three angles about fixed axes) +G68.2 P2 Q0 X- Y- Z- (three points: 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.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 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.4' takes any 'G68.2' form and composes it onto the active plane: the +words are in the plane, and the result is a new plane relative to the old +one. It needs a plane to build on. + +'G69' cancels the plane. So does the end of the program, 'M2' or 'M30', and +an abort: the plane is not persistent and nothing about it is written to the +parameter file. + +Defining the plane does not move anything. + +While a plane is active the codes that define the coordinate system the +plane sits on are refused: 'G92', 'G92.1', 'G92.2', 'G92.3', 'G52', 'G10 L2', +'G10 L20', 'G10 L10', 'G10 L11', and a change of coordinate system +('G54' to 'G59.3'). Cancel the plane first. + +The active plane is reported in the modal G-code display as the code that +defined it. Status carries it as `g68_offset`, `g68_rotation` and +`g68_active`, next to the other offsets, for displays that want to show +plane coordinates or draw the plane. + +.G68.2 Example +[source,ngc] +---- +G54 +G68.2 X50 Y50 Z0 I30 J20 K0 (Euler: 30 about Z, 20 about the new X) +G0 X0 Y0 Z5 (5 above the plane's origin, along its normal) +G1 Z-3 F150 (a hole 3 deep, straight into the plane) +G0 Z5 +G68.4 X20 P1 I0 J0 K90 (a plane 20 along X in the old one, turned 90 about Z) +G0 X0 Y0 Z5 +G69 (back to G54 as it was) +---- + +It is an error if: + +* 'P' is not 0, 1, 2 or 3, or 'Q' with 'P0' or 'P1' is not three axis digits + with no two adjacent alike. +* A 'P2' or 'P3' definition is interrupted, or its 'Q' words come out of + order. +* The points of a 'P2' definition coincide or lie on one line, or a vector of + a 'P3' definition is zero or the X direction lies along the normal. +* 'G68.4' is used with no plane active. +* Cutter compensation is on. +* Polar coordinates or a motion code are used on the same line. + [[gcode:g70]] == G70 Lathe finishing cycle(((G70 Lathe finishing cycle))) diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index 2b8a6829a91..bf1feabbd0e 100644 --- a/docs/src/gcode/overview.adoc +++ b/docs/src/gcode/overview.adoc @@ -974,6 +974,7 @@ The modal groups are shown in the following Table. |Units (Group 6) | G20, G21 |Cutter Diameter Compensation (Group 7) | G40, G41, G42, G41.1, G42.1 |Tool Length Offset (Group 8) | G43, G43.1, G43.2, G43.4, G49 +|Tilted Work Plane (Group 9) | G68.2, G68.4, G69 |Canned Cycles Return Mode (Group 10) | G98, G99 |Coordinate System (Group 12) | G54, G55, G56, G57, G58, G59, G59.1, G59.2, G59.3 |Control Mode (Group 13) | G61, G61.1, G64 diff --git a/lib/python/rs274/glcanon.py b/lib/python/rs274/glcanon.py index 0d9c794bb85..5c4c0b5eba6 100644 --- a/lib/python/rs274/glcanon.py +++ b/lib/python/rs274/glcanon.py @@ -1213,6 +1213,15 @@ def posstrs(self): positions[X] = _x * math.cos(t) - _y * math.sin(t) positions[Y] = _x * math.sin(t) + _y * math.cos(t) positions = [(i-j) for i, j in zip(positions, s.g92_offset)] + if s.g68_active: + # the tilted work plane sits inside G92 + r = s.g68_rotation + _x = positions[X] - s.g68_offset[X] + _y = positions[Y] - s.g68_offset[Y] + _z = positions[Z] - s.g68_offset[Z] + positions[X] = r[0]*_x + r[3]*_y + r[6]*_z + positions[Y] = r[1]*_x + r[4]*_y + r[7]*_z + positions[Z] = r[2]*_x + r[5]*_y + r[8]*_z else: positions = list(positions) diff --git a/lib/python/rs274/interpret.py b/lib/python/rs274/interpret.py index 8815f7f8696..3c83b5502a8 100644 --- a/lib/python/rs274/interpret.py +++ b/lib/python/rs274/interpret.py @@ -24,8 +24,18 @@ class Translated: g5x_offset_a = g5x_offset_b = g5x_offset_c = 0 g5x_offset_u = g5x_offset_v = g5x_offset_w = 0 rotation_xy = 0 + g68_active = 0 + g68_offset = (0.0, 0.0, 0.0) + g68_rotation = (1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0) def rotate_and_translate(self, x,y,z,a,b,c,u,v,w): + if self.g68_active: + r = self.g68_rotation + o = self.g68_offset + x, y, z = (r[0]*x + r[1]*y + r[2]*z + o[0], + r[3]*x + r[4]*y + r[5]*z + o[1], + r[6]*x + r[7]*y + r[8]*z + o[2]) + x += self.g92_offset_x y += self.g92_offset_y z += self.g92_offset_z @@ -83,6 +93,10 @@ def set_xy_rotation(self, theta): t = math.radians(theta) self.rotation_sin = math.sin(t) self.rotation_cos = math.cos(t) + def set_g68_frame(self, x, y, z, r0, r1, r2, r3, r4, r5, r6, r7, r8, active): + self.g68_active = active + self.g68_offset = (x, y, z) + self.g68_rotation = (r0, r1, r2, r3, r4, r5, r6, r7, r8) class ArcsToSegmentsMixin: plane = 1 diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index 96e558dbf23..0e6f6a0c25f 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -155,6 +155,9 @@ typedef struct CanonConfig_t { rotary_unlock_for_traverse(-1), g5xOffset{}, g92Offset{}, + g68Offset{}, + g68Rotation{1, 0, 0, 0, 1, 0, 0, 0, 1}, + g68Active(0), endPoint{}, lengthUnits(CANON_UNITS_INCHES), activePlane(CANON_PLANE::XY), @@ -178,6 +181,11 @@ typedef struct CanonConfig_t { CANON_POSITION g5xOffset; CANON_POSITION g92Offset; +/* The tilted work plane (G68.2): a frame inside the G92 stage of the chain, + in mm. Program X Y Z go through R * xyz + O before anything else. */ + double g68Offset[3]; + double g68Rotation[9]; // row major + int g68Active; /* canonEndPoint is the last programmed end point, stored in case it's needed for subsequent calculations. It's in absolute frame, mm units. @@ -250,6 +258,12 @@ extern void HOME_CYCLE(void); * [JOINT_n] INI section numbering). Maps to EMC_JOINT_HOME(joint). */ extern void HOME_CYCLE_JOINT(int joint); +/* The tilted work plane. Origin in program units and a row major rotation + matrix, both in the coordinate system active when the plane was defined; + active 0 cancels it. */ +extern void SET_G68_FRAME(double x, double y, double z, + const double rotation[9], int active); + /* Offset the origin to the point with absolute coordinates x, y, z, a, b, c, u, v, and w. Values of x, y, z, a, b, c, u, v, and w are real numbers. The units are whatever length units are being used at the time diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index fefc92b602f..99e62837172 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -308,6 +308,9 @@ int emcFormat(NMLTYPE type, void *buffer, CMS * cms) case EMC_TRAJ_SET_ROTATION_TYPE: ((EMC_TRAJ_SET_ROTATION *) buffer)->update(cms); break; + case EMC_TRAJ_SET_G68_TYPE: + ((EMC_TRAJ_SET_G68 *) buffer)->update(cms); + break; case EMC_TRAJ_SET_SCALE_TYPE: ((EMC_TRAJ_SET_SCALE *) buffer)->update(cms); break; @@ -531,6 +534,8 @@ const char *emc_symbol_lookup(uint32_t type) return "EMC_TRAJ_SET_G92"; case EMC_TRAJ_SET_ROTATION_TYPE: return "EMC_TRAJ_SET_ROTATION"; + case EMC_TRAJ_SET_G68_TYPE: + return "EMC_TRAJ_SET_G68"; case EMC_TRAJ_SET_SCALE_TYPE: return "EMC_TRAJ_SET_SCALE"; case EMC_TRAJ_SET_RAPID_SCALE_TYPE: @@ -1399,6 +1404,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); @@ -1682,6 +1690,15 @@ void EMC_TRAJ_SET_ROTATION::update(CMS * cms) cms->update(rotation); } +// cppcheck-suppress duplInheritedMember +void EMC_TRAJ_SET_G68::update(CMS * cms) +{ + EMC_TRAJ_CMD_MSG::update(cms); + EmcPose_update(cms, &origin); + cms->update(rotation, 9); + cms->update(active); +} + /* * NML/CMS Update function for EMC_SPINDLE_BRAKE_ENGAGE * Automatically generated by NML CodeGen Java Applet. diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index dbe30abe555..0628cfd3e78 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -111,6 +111,7 @@ struct PM_CARTESIAN; #define EMC_TRAJ_SET_SO_ENABLE_TYPE ((NMLTYPE) 235) #define EMC_TRAJ_SET_FH_ENABLE_TYPE ((NMLTYPE) 236) #define EMC_TRAJ_RIGID_TAP_TYPE ((NMLTYPE) 237) +#define EMC_TRAJ_SET_G68_TYPE ((NMLTYPE) 239) #define EMC_TRAJ_SELECT_KINS_TYPE ((NMLTYPE) 289) #define EMC_TRAJ_STAT_TYPE ((NMLTYPE) 299) diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index 2636d9c9302..06e96999de6 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -889,6 +889,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() @@ -1499,6 +1519,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 98e1eb26786..a39eae83da8 100644 --- a/src/emc/nml_intf/emcops.cc +++ b/src/emc/nml_intf/emcops.cc @@ -142,6 +142,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 41ca7ad1ba6..e1b246520a5 100644 --- a/src/emc/rs274ngc/Submakefile +++ b/src/emc/rs274ngc/Submakefile @@ -15,6 +15,7 @@ LIBRS274SRCS := $(addprefix emc/rs274ngc/, \ interp_inverse.cc \ interp_read.cc \ interp_write.cc \ + interp_workplane.cc \ interp_o_word.cc \ interp_g7x.cc \ nurbs_additional_functions.cc \ diff --git a/src/emc/rs274ngc/gcode_renderer.cc b/src/emc/rs274ngc/gcode_renderer.cc index 046d27e5eaf..9e8e18fcfdb 100644 --- a/src/emc/rs274ngc/gcode_renderer.cc +++ b/src/emc/rs274ngc/gcode_renderer.cc @@ -622,7 +622,9 @@ void GCodeRenderer::publish_line() { } void GCodeRenderer::transform(const Point9 &in, Point9 &out) const { - out = in + g92_; + out = in; + frame_.apply(out); + out += g92_; if(rotation_xy_ != 0.0) { double rotx = out[P9_X] * rotation_cos_ - out[P9_Y] * rotation_sin_; out[P9_Y] = out[P9_X] * rotation_sin_ + out[P9_Y] * rotation_cos_; @@ -948,7 +950,7 @@ void GCodeRenderer::render_arc(int line_number, double first_end, double second_ consumed_ = true; if(suppress_ > 0) return; arc_segments(lo_, plane_, rotation_cos_, rotation_sin_, - g5x_, g92_, first_end, second_end, + g5x_, g92_, frame_, first_end, second_end, first_axis, second_axis, rotation, axis_end_point, a, b, c, u, v, w, arcdivision_, segs_); @@ -984,6 +986,7 @@ static void rotate(double &x, double &y, double c, double s) { int arc_segments(const Point9 &lo, int plane, double rotation_cos, double rotation_sin, const Point9 &g5xoffset, const Point9 &g92offset, + const WorkFrame &frame, double x1, double y1, double cx, double cy, int rot, double z1, double a, double b, double c, double u, double v, double w, @@ -1010,6 +1013,9 @@ int arc_segments(const Point9 &lo, int plane, o -= g5xoffset; unrotate(o[P9_X], o[P9_Y], rotation_cos, rotation_sin); o -= g92offset; + // the tilted work plane sits inside G92: off the last point on the way + // in, back on every point on the way out + frame.remove(o); double theta1 = atan2(o[Y]-cy, o[X]-cx); double theta2 = atan2(n[Y]-cy, n[X]-cx); @@ -1048,10 +1054,12 @@ int arc_segments(const Point9 &lo, int plane, p[Y] = ty + cy; p[Z] = o[Z] + d[Z] * f; for(int j = P9_A; j < P9_COUNT; j++) p[j] = o[j] + d[j] * f; + frame.apply(p); p += g92offset; rotate(p[P9_X], p[P9_Y], rotation_cos, rotation_sin); p += g5xoffset; } + frame.apply(n); n += g92offset; rotate(n[P9_X], n[P9_Y], rotation_cos, rotation_sin); n += g5xoffset; diff --git a/src/emc/rs274ngc/gcode_renderer.hh b/src/emc/rs274ngc/gcode_renderer.hh index 7c4227d7b8d..222f92da48a 100644 --- a/src/emc/rs274ngc/gcode_renderer.hh +++ b/src/emc/rs274ngc/gcode_renderer.hh @@ -199,6 +199,7 @@ void renderer_canon_register(pybind11::module_ &m); int arc_segments(const Point9 &lo, int plane, double rotation_cos, double rotation_sin, const Point9 &g5xoffset, const Point9 &g92offset, + const WorkFrame &frame, double x1, double y1, double cx, double cy, int rot, double z1, double a, double b, double c, double u, double v, double w, @@ -359,6 +360,10 @@ public: g92_ = offsets; } void set_xy_rotation(double degrees) override; + void set_g68_frame(const WorkFrame &frame) override { + if(parse_state.interp_error) return; + frame_ = frame; + } // The plane reaches the record and the arc segmenter from here; nothing // on a rendered parse reads the canon's own copy. void set_plane(int plane) override { plane_ = plane; } @@ -457,7 +462,7 @@ private: // positions to go with them. bool read_axes(); void unrotate_xy(const Point9 &p, Point3 &out) const; - // g92 -> XY rotation -> g5x, the operations and the order + // work plane -> g92 -> XY rotation -> g5x, the operations and the order // `rs274.interpret.Translated.rotate_and_translate` applies - which is // where this came from, though that method no longer runs on a rendered // parse. Not bit-identical to it by construction: the compiler is free to @@ -478,6 +483,7 @@ private: double rotation_sin_ = 0.0; double unrot_cos_ = 1.0; // the same rotation, negated, for the double unrot_sin_ = 0.0; // rotation-removed extents + WorkFrame frame_; // the tilted work plane, inside g92 Point9 lo_ = {}; // chain point Point9 tool_ = {}; // xo..wo diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index cb51aee8611..9c660152496 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -359,6 +359,13 @@ class CallbackCanon final : public Canon { maybe_new_line(); forward("set_xy_rotation", degrees); } + void set_g68_frame(const WorkFrame &f) override { + maybe_new_line(); + const std::array &r = f.rotation; + forward("set_g68_frame", f.origin[P9_X], f.origin[P9_Y], f.origin[P9_Z], + r[0], r[1], r[2], r[3], r[4], r[5], r[6], r[7], r[8], + (int)f.active); + } void set_plane(int plane) override { maybe_new_line(); forward("set_plane", plane); @@ -538,6 +545,15 @@ void SET_XY_ROTATION(double t) { parse_state.canon->set_xy_rotation(t); }; +void SET_G68_FRAME(double x, double y, double z, + const double rotation[9], int active) { + WorkFrame frame; + frame.active = active != 0; + frame.origin = ensure_inch({x, y, z}); + std::copy(rotation, rotation + 9, frame.rotation.begin()); + parse_state.canon->set_g68_frame(frame); +}; + void USE_LENGTH_UNITS(CANON_UNITS u) { parse_state.metric = u == CANON_UNITS_MM; } void SELECT_PLANE(CANON_PLANE pl) { @@ -1194,10 +1210,23 @@ static py::list rs274_arc_to_segments(py::handle canon, g5xoffset[i] = attr_double(canon, G5X[i]); g92offset[i] = attr_double(canon, G92[i]); } + // The tilted work plane, from a canon that keeps one (rs274.interpret + // .Translated does); one without the attributes has no plane. + WorkFrame frame; + if(py::hasattr(canon, "g68_active") && attr_int(canon, "g68_active")) { + frame.active = true; + py::sequence origin = canon.attr("g68_offset").cast(); + py::sequence rotation = canon.attr("g68_rotation").cast(); + if(py::len(origin) != 3 || py::len(rotation) != 9) + throw py::value_error("arc_to_segments: canon.g68_offset is three " + "numbers and canon.g68_rotation nine"); + for(size_t i=0; i<3; i++) frame.origin[i] = origin[i].cast(); + for(size_t i=0; i<9; i++) frame.rotation[i] = rotation[i].cast(); + } std::vector pts; int steps = arc_segments(o, plane, rotation_cos, rotation_sin, - g5xoffset, g92offset, + g5xoffset, g92offset, frame, x1, y1, cx, cy, rot, z1, a, b, c, u, v, w, max_segments, pts); py::list segs(steps); diff --git a/src/emc/rs274ngc/gcodemodule.hh b/src/emc/rs274ngc/gcodemodule.hh index 3cf83f59ef2..bac3ece80fe 100644 --- a/src/emc/rs274ngc/gcodemodule.hh +++ b/src/emc/rs274ngc/gcodemodule.hh @@ -91,6 +91,38 @@ inline Point9 operator*(Point9 a, double s) { return a *= s; } inline Point9 operator/(Point9 a, double s) { return a /= s; } inline Point9 operator*(double s, Point9 a) { return a *= s; } +// The tilted work plane, G68.2: a frame the program's points go through +// before anything else, so it sits inside G92 and the transform chain is +// g5x + Rz(rotation_xy) * (g92 + origin + rotation * program). The origin +// is in inches like the offsets; the rotation is row major, its rows the +// plane's X, Y and Z in the coordinates outside it. +struct WorkFrame { + bool active = false; + Point9 origin = {}; + std::array rotation = {1, 0, 0, 0, 1, 0, 0, 0, 1}; + + // A point of the plane to the coordinates outside it. Only xyz turn: + // the rotary and UVW axes are not in the plane. + void apply(Point9 &p) const { + if(!active) return; + const std::array &r = rotation; + double x = p[P9_X], y = p[P9_Y], z = p[P9_Z]; + p[P9_X] = r[0] * x + r[1] * y + r[2] * z + origin[P9_X]; + p[P9_Y] = r[3] * x + r[4] * y + r[5] * z + origin[P9_Y]; + p[P9_Z] = r[6] * x + r[7] * y + r[8] * z + origin[P9_Z]; + } + // The way back, by the transpose. + void remove(Point9 &p) const { + if(!active) return; + const std::array &r = rotation; + double x = p[P9_X] - origin[P9_X], y = p[P9_Y] - origin[P9_Y], + z = p[P9_Z] - origin[P9_Z]; + p[P9_X] = r[0] * x + r[3] * y + r[6] * z; + p[P9_Y] = r[1] * x + r[4] * y + r[7] * z; + p[P9_Z] = r[2] * x + r[5] * y + r[8] * z; + } +}; + // --------------------------------------------------------------------------- // The canon protocol // --------------------------------------------------------------------------- @@ -141,6 +173,9 @@ public: virtual void set_g5x_offset(int index, const Point9 &offsets) = 0; virtual void set_g92_offset(const Point9 &offsets) = 0; virtual void set_xy_rotation(double degrees) = 0; + // The tilted work plane, or its cancellation; the origin already in + // inches. + virtual void set_g68_frame(const WorkFrame &frame) = 0; virtual void set_plane(int plane) = 0; virtual void set_feed_rate(double rate) = 0; virtual void set_traverse_rate(double rate) = 0; diff --git a/src/emc/rs274ngc/interp_array.cc b/src/emc/rs274ngc/interp_array.cc index 829c1777337..2a4fb2c469e 100644 --- a/src/emc/rs274ngc/interp_array.cc +++ b/src/emc/rs274ngc/interp_array.cc @@ -104,7 +104,7 @@ const int Interp::gees[] = { /* 620 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 640 */ 13,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 660 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, -/* 680 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, +/* 680 */ -1,-1, 9,-1, 9,-1,-1,-1,-1,-1, 9,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 700 */ 1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 1, 1, 1,-1,-1,-1,-1,-1,-1,-1, /* 720 */ 1, 1, 1,-1,-1,-1,-1,-1,-1,-1, 1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 740 */ 1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, diff --git a/src/emc/rs274ngc/interp_check.cc b/src/emc/rs274ngc/interp_check.cc index fc5ae57586e..a0a2fe3f069 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -295,20 +295,23 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block (motion != G_6) && (motion != G_6_1) && (motion != G_71) && (motion != G_71_1) && (motion != G_71_2) && (motion != G_72) && (motion != G_72_1) && (motion != G_72_2) && - (motion != G_76) && (motion != G_87) && (motion != G_33_1) && (block->g_modes[GM_MODAL_0] != G_10)), - _("I word with no G2, G3, G5, G5.1, G6, G6.1, G10, G33.1, G76, or G87 to use it")); + (motion != G_76) && (motion != G_87) && (motion != G_33_1) && (block->g_modes[GM_MODAL_0] != G_10) && + (block->g_modes[GM_WORK_PLANE] == -1)), + _("I word with no G2, G3, G5, G5.1, G6, G6.1, G10, G33.1, G68.2, G76, or G87 to use it")); } if (block->j_flag) { /* could still be useless if xz_plane arc */ CHKS(((motion != G_2) && (motion != G_3) && (motion != G_5) && (motion != G_5_1) && (motion != G_6) && (motion != G_6_1) && - (motion != G_76) && (motion != G_87) && (block->g_modes[GM_MODAL_0] != G_10)), - _("J word with no G2, G3, G5, G5.1, G6, G6.1, G10, G76 or G87 to use it")); + (motion != G_76) && (motion != G_87) && (block->g_modes[GM_MODAL_0] != G_10) && + (block->g_modes[GM_WORK_PLANE] == -1)), + _("J word with no G2, G3, G5, G5.1, G6, G6.1, G10, G68.2, G76 or G87 to use it")); } if (block->k_flag) { /* could still be useless if xy_plane arc */ - CHKS(((motion != G_2) && (motion != G_3) && (motion != G_6_2) && (motion != G_33) && (motion != G_33_1) && (motion != G_76) && (motion != G_87)), - _("K word with no G2, G3, G6.2, G33, G33.1, G76, or G87 to use it")); + CHKS(((motion != G_2) && (motion != G_3) && (motion != G_6_2) && (motion != G_33) && (motion != G_33_1) && (motion != G_76) && (motion != G_87) && + (block->g_modes[GM_WORK_PLANE] == -1)), + _("K word with no G2, G3, G6.2, G33, G33.1, G68.2, G76, or G87 to use it")); } if (block->l_number != -1) { @@ -326,6 +329,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block if (block->p_flag) { CHKS(((block->g_modes[GM_MODAL_0] != G_10) && (block->g_modes[GM_MODAL_0] != G_4) && (block->g_modes[GM_CONTROL_MODE] != G_64 && (block->g_modes[GM_MODAL_0] != G_12_1)) && + (block->g_modes[GM_WORK_PLANE] == -1) && (motion != G_76) && (motion != G_82) && (motion != G_86) && (motion != G_88) && (motion != G_89) && (motion != G_5) && (motion != G_5_2) && (motion != G_70) && @@ -338,7 +342,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block (block->m_modes[5] != 64) && (block->m_modes[5] != 65) && (block->m_modes[5] != 66) && (block->m_modes[7] != 19) && (block->user_m != 1) && (block->o_type != M_98)), - _("P word with no G2 G3 G4 G10 G12.1 G64 G5 G5.2 G6, G6.2, G76 G82 G86 G88 G89" + _("P word with no G2 G3 G4 G10 G12.1 G64 G68.2 G5 G5.2 G6, G6.2, G76 G82 G86 G88 G89" " G28.2" " or M50 M51 M52 M53 M62 M63 M64 M65 M66 M98 " "or user M code to use it")); @@ -356,11 +360,12 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block CHKS((motion != G_83) && (motion != G_73) && (motion != G_5) && (motion != G_6) && (motion != G_6_2) && (block->user_m != 1) && (motion != G_76) && (block->m_modes[5] != 66) && (block->m_modes[5] != 67) && (block->m_modes[5] != 68) && (block->g_modes[GM_MODAL_0] != G_10) && (block->m_modes[6] != 61) && (block->g_modes[GM_CONTROL_MODE] != G_64) && + (block->g_modes[GM_WORK_PLANE] == -1) && (motion != G_70) && (motion != G_71) && (motion != G_71_1) && (motion != G_71_2) && (motion != G_72) && (motion != G_72_1) && (motion != G_72_2) && (block->m_modes[7] != 19), - _("Q word with no G5, G6, G10, G64, G73, G76, G83, M19, M66, M67, M68 or user M code that uses it")); + _("Q word with no G5, G6, G10, G64, G68.2, G73, G76, G83, M19, M66, M67, M68 or user M code that uses it")); } if (block->r_flag) { @@ -371,6 +376,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block (motion != G_74) && (block->g_modes[GM_CUTTER_COMP] != G_41_1) && (block->g_modes[GM_CUTTER_COMP] != G_42_1) && (block->g_modes[GM_MODAL_0] != G_10) && (block->m_modes[7] != 19) && + (block->g_modes[GM_WORK_PLANE] == -1) && (block->g_modes[GM_CONTROL_MODE] != G_64) ), /* G64_R_PLANNER: R selects planner on G64 */ NCE_R_WORD_WITH_NO_G_CODE_THAT_USES_IT); /* G64_R_PLANNER: a block has one shared R word; with G64 it is the planner diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 15fdb41a7f8..82a69a240e1 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -1641,6 +1641,8 @@ int Interp::convert_axis_offsets(int g_code, //!< g_code being executed (mus CHKS((settings->cutter_comp_side != CUTTER_COMP::OFF), /* not "== true" */ NCE_CANNOT_CHANGE_AXIS_OFFSETS_WITH_CUTTER_RADIUS_COMP); + CHKS((settings->g68_active), + _("Cannot change G92 offsets while a tilted work plane (G68.2) is active")); CHKS((block->a_flag && settings->axis_wrapped[3] && (block->a_number <= -360.0 || block->a_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), @@ -2376,6 +2378,12 @@ int Interp::convert_coordinate_system(int g_code, //!< g_code called (mus CHKS((settings->cutter_comp_side != CUTTER_COMP::OFF), (_("Cannot change coordinate systems with cutter radius compensation on"))); + { + // the plane sits on the active system; reselecting that one is harmless + int target = (g_code < G_59_1) ? (g_code - G_54) / 10 + 1 : g_code - G_59_1 + 7; + CHKS((settings->g68_active && target != settings->origin_index), + _("Cannot change coordinate systems while a tilted work plane (G68.2) is active")); + } parameters = settings->parameters; switch (g_code) { case G_54: @@ -2972,6 +2980,7 @@ int Interp::convert_g(block_pointer block, //!< pointer to a block of RS27 { int status; + CHP(work_plane_check_sequence(block, settings)); if ((block->g_modes[GM_MODAL_0] == G_4) && ONCE(STEP_DWELL)) { status = convert_dwell(settings, block->p_number); CHP(status); @@ -3000,6 +3009,10 @@ int Interp::convert_g(block_pointer block, //!< pointer to a block of RS27 status = convert_coordinate_system(block->g_modes[GM_COORD_SYSTEM], settings); CHP(status); } + if ((block->g_modes[GM_WORK_PLANE] != -1) && ONCE(STEP_WORK_PLANE)){ + status = convert_work_plane(block->g_modes[GM_WORK_PLANE], block, settings); + CHP(status); + } if ((block->g_modes[GM_CONTROL_MODE] != -1) && ONCE(STEP_CONTROL_MODE)) { status = convert_control_mode(block->g_modes[GM_CONTROL_MODE], block->p_number, block->q_number, @@ -3052,12 +3065,8 @@ offsetless machine coordinate. void Interp::get_abs_position(setup_pointer s, double abs_pos[9]) { - double x = s->current_x + s->axis_offset_x; - double y = s->current_y + s->axis_offset_y; - rotate(&x, &y, s->rotation_xy); - abs_pos[0] = x + s->origin_offset_x + s->tool_offset.tran.x; - abs_pos[1] = y + s->origin_offset_y + s->tool_offset.tran.y; - abs_pos[2] = s->current_z + s->axis_offset_z + s->origin_offset_z + s->tool_offset.tran.z; + program_to_world_xyz(s, s->current_x, s->current_y, s->current_z, + &abs_pos[0], &abs_pos[1], &abs_pos[2]); abs_pos[3] = s->AA_current + s->AA_axis_offset + s->AA_origin_offset + s->tool_offset.a; abs_pos[4] = s->BB_current + s->BB_axis_offset + s->BB_origin_offset + s->tool_offset.b; abs_pos[5] = s->CC_current + s->CC_axis_offset + s->CC_origin_offset + s->tool_offset.c; @@ -3089,12 +3098,11 @@ int Interp::convert_savehome(int code, block_pointer /*block*/, setup_pointer s) ERS(_("Cannot set reference point with cutter compensation in effect")); } - double x = s->current_x + s->axis_offset_x; - double y = s->current_y + s->axis_offset_y; - rotate(&x, &y, s->rotation_xy); - x = PROGRAM_TO_USER_LEN(x + s->tool_offset.tran.x + s->origin_offset_x); - y = PROGRAM_TO_USER_LEN(y + s->tool_offset.tran.y + s->origin_offset_y); - double z = PROGRAM_TO_USER_LEN(s->current_z + s->tool_offset.tran.z + s->origin_offset_z + s->axis_offset_z); + double x, y, z; + program_to_world_xyz(s, s->current_x, s->current_y, s->current_z, &x, &y, &z); + x = PROGRAM_TO_USER_LEN(x); + y = PROGRAM_TO_USER_LEN(y); + z = PROGRAM_TO_USER_LEN(z); double a = PROGRAM_TO_USER_AX(3, s->AA_current + s->tool_offset.a + s->AA_origin_offset + s->AA_axis_offset); double b = PROGRAM_TO_USER_AX(4, s->BB_current + s->tool_offset.b + s->BB_origin_offset + s->BB_axis_offset); double c = PROGRAM_TO_USER_AX(5, s->CC_current + s->tool_offset.c + s->CC_origin_offset + s->CC_axis_offset); @@ -3513,6 +3521,9 @@ int Interp::convert_length_units(int g_code, //!< g_code being executed (mus settings->origin_offset_x = (settings->origin_offset_x * INCH_PER_MM); settings->origin_offset_y = (settings->origin_offset_y * INCH_PER_MM); settings->origin_offset_z = (settings->origin_offset_z * INCH_PER_MM); + settings->g68_offset[0] = (settings->g68_offset[0] * INCH_PER_MM); + settings->g68_offset[1] = (settings->g68_offset[1] * INCH_PER_MM); + settings->g68_offset[2] = (settings->g68_offset[2] * INCH_PER_MM); scale_linear_axes(settings, INCH_PER_MM); @@ -3548,6 +3559,9 @@ int Interp::convert_length_units(int g_code, //!< g_code being executed (mus settings->origin_offset_x = (settings->origin_offset_x * MM_PER_INCH); settings->origin_offset_y = (settings->origin_offset_y * MM_PER_INCH); settings->origin_offset_z = (settings->origin_offset_z * MM_PER_INCH); + settings->g68_offset[0] = (settings->g68_offset[0] * MM_PER_INCH); + settings->g68_offset[1] = (settings->g68_offset[1] * MM_PER_INCH); + settings->g68_offset[2] = (settings->g68_offset[2] * MM_PER_INCH); scale_linear_axes(settings, MM_PER_INCH); @@ -4735,6 +4749,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))); @@ -4974,6 +4990,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; @@ -5441,6 +5460,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); @@ -6697,16 +6718,21 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu } USE_TOOL_LENGTH_OFFSET(tool_offset); - double dx, dy; + double dx, dy, dz; + // the tool does not move, so its program coordinates change by the + // offset difference seen from the program: the XY rotation and the + // tilted work plane taken off it dx = settings->tool_offset.tran.x - tool_offset.tran.x; dy = settings->tool_offset.tran.y - tool_offset.tran.y; + dz = settings->tool_offset.tran.z - tool_offset.tran.z; rotate(&dx, &dy, -settings->rotation_xy); + g68_unrotate(settings, &dx, &dy, &dz); settings->current_x += dx; settings->current_y += dy; - settings->current_z += settings->tool_offset.tran.z - tool_offset.tran.z; + settings->current_z += dz; settings->AA_current += settings->tool_offset.a - tool_offset.a; settings->BB_current += settings->tool_offset.b - tool_offset.b; settings->CC_current += settings->tool_offset.c - tool_offset.c; diff --git a/src/emc/rs274ngc/interp_find.cc b/src/emc/rs274ngc/interp_find.cc index 5748f8ccd92..0ad0afdf11f 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->axis_wrapped[3]) { @@ -466,12 +449,7 @@ int Interp::find_relative(double x1, //!< absolute x position double *w_2, setup_pointer settings) //!< pointer to machine settings { - *x2 = x1 - settings->origin_offset_x - settings->tool_offset.tran.x; - *y2 = y1 - settings->origin_offset_y - settings->tool_offset.tran.y; - rotate(x2, y2, -settings->rotation_xy); - *x2 -= settings->axis_offset_x; - *y2 -= settings->axis_offset_y; - *z2 = z1 - settings->origin_offset_z - settings->axis_offset_z - settings->tool_offset.tran.z; + world_to_program_xyz(settings, x1, y1, z1, x2, y2, z2); if(settings->axis_wrapped[3]) { CHP(unwrap_rotary(AA_2, AA_1, diff --git a/src/emc/rs274ngc/interp_internal.cc b/src/emc/rs274ngc/interp_internal.cc index 468827192a3..e7f3f32bdf9 100644 --- a/src/emc/rs274ngc/interp_internal.cc +++ b/src/emc/rs274ngc/interp_internal.cc @@ -175,6 +175,11 @@ int Interp::enhance_block(block_pointer block, //!< pointer to a block to be c mode_zero_covets_axes = ((mode0 == G_10) || (mode0 == G_28) || (mode0 == G_30) || (mode0 == G_52) || (mode0 == G_92)); + // a tilted work plane definition takes the axis words the same way + if (block->g_modes[GM_WORK_PLANE] == G_68_2 || block->g_modes[GM_WORK_PLANE] == G_68_4) { + CHKS(polar_flag, _("Polar coordinates cannot define a tilted work plane")); + mode_zero_covets_axes = 1; + } if (mode1 != -1) { if (mode1 == G_80) { diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index 1fc8700848d..acd4eeb39ae 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -264,6 +264,9 @@ enum GCodes G_59_1 = 591, G_59_2 = 592, G_59_3 = 593, + G_68_2 = 682, + G_68_4 = 684, + G_69 = 690, G_61 = 610, G_61_1 = 611, G_64 = 640, @@ -367,6 +370,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, @@ -381,7 +385,7 @@ enum phases { // Modal groups // also indices into g_modes -// unused: 9,11 +// unused: 11 enum ModalGroups { GM_MODAL_0 = 0, @@ -393,7 +397,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, @@ -749,6 +753,16 @@ 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 double parameters[interp_param_global::RS274NGC_MAX_PARAMETERS]; // system parameters int parameter_occurrence; // parameter buffer index int parameter_numbers[MAX_NAMED_PARAMETERS]; // parameter number buffer diff --git a/src/emc/rs274ngc/interp_namedparams.cc b/src/emc/rs274ngc/interp_namedparams.cc index fb12f1e8d26..9ab4bae38b5 100644 --- a/src/emc/rs274ngc/interp_namedparams.cc +++ b/src/emc/rs274ngc/interp_namedparams.cc @@ -759,26 +759,27 @@ int Interp::lookup_named_param(const char *nameBuf, case NP_ABS_X: // abs position { - double x = _setup.current_x + _setup.axis_offset_x; - double y = _setup.current_y + _setup.axis_offset_y; - rotate(&x, &y, _setup.rotation_xy); - *value = x + _setup.origin_offset_x + _setup.tool_offset.tran.x; + double abs_pos[9]; + get_abs_position(&_setup, abs_pos); + *value = abs_pos[0]; } break; case NP_ABS_Y: // abs position { - double x = _setup.current_x + _setup.axis_offset_x; - double y = _setup.current_y + _setup.axis_offset_y; - rotate(&x, &y, _setup.rotation_xy); - *value = y + _setup.origin_offset_y + _setup.tool_offset.tran.y; + double abs_pos[9]; + get_abs_position(&_setup, abs_pos); + *value = abs_pos[1]; } break; case NP_ABS_Z: // abs position - *value = _setup.current_z + _setup.axis_offset_z + - _setup.origin_offset_z + _setup.tool_offset.tran.z; + { + double abs_pos[9]; + get_abs_position(&_setup, abs_pos); + *value = abs_pos[2]; + } break; case NP_ABS_A: // abs position diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index 384baee3cdf..566188109ee 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -104,6 +104,14 @@ 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{}, parameters{0}, parameter_occurrence(0), diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc new file mode 100644 index 00000000000..5f14ceea40e --- /dev/null +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -0,0 +1,437 @@ +/******************************************************************** +* 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); + } + + CHKS((g_code != G_68_2 && g_code != G_68_4), "BUG: code not G68.2, G68.4 or G69"); + CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot define a tilted work plane with cutter radius compensation on")); + CHKS((g_code == G_68_4 && !s->g68_active), + _("G68.4 needs an active tilted work plane to build on")); + + CHP(work_plane_build(block, s, origin, rotation, &complete)); + if (!complete) { return INTERP_OK; } + + if (g_code == G_68_4) { + // composed onto the active plane: the new origin is a point of the + // old plane and the new rotation follows the old one + double ox = origin[0], oy = origin[1], oz = origin[2]; + + g68_apply(s, &ox, &oy, &oz); + origin[0] = ox; + origin[1] = oy; + origin[2] = oz; + mat_mul(s->g68_rotation, rotation, rotation); + } + return work_plane_set(s, g_code, origin, rotation); +} diff --git a/src/emc/rs274ngc/interp_write.cc b/src/emc/rs274ngc/interp_write.cc index b61982ea4f8..54dfe128f98 100644 --- a/src/emc/rs274ngc/interp_write.cc +++ b/src/emc/rs274ngc/interp_write.cc @@ -126,7 +126,7 @@ int Interp::write_g_codes(block_pointer block, //!< pointer to a block of RS27 settings->active_g_codes[11] = (settings->control_mode == CANON_CONTINUOUS) ? G_64 : (settings->control_mode == CANON_EXACT_PATH) ? G_61 : G_61_1; - settings->active_g_codes[12] = -1; + settings->active_g_codes[12] = settings->g68_active ? settings->g68_code : -1; settings->active_g_codes[13] = //I don't even know how to display the mode of an arbitrary number of spindles (andypugh 17/6/16) (settings->spindle_mode[0] == SPINDLE_MODE::CONSTANT_RPM) ? G_97 : G_96; settings->active_g_codes[14] = (settings->ijk_distance_mode == DISTANCE_MODE::ABSOLUTE) ? G_90_1 : G_91_1; diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index 6ee52ae4e44..23fd0f0d3c2 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -364,6 +364,20 @@ public: setup_pointer settings); int convert_tool_select(block_pointer block, setup_pointer settings); int convert_kins_switch(int code, block_pointer block, setup_pointer settings); + int convert_work_plane(int g_code, block_pointer block, setup_pointer settings); + int work_plane_build(block_pointer block, setup_pointer settings, + double origin[3], double rotation[3][3], int *complete); + int work_plane_set(setup_pointer settings, int code, + const double origin[3], const double rotation[3][3]); + int work_plane_cancel(setup_pointer settings, bool tell_canon_anyway = false); + int work_plane_check_sequence(block_pointer block, setup_pointer settings); + void g68_apply(setup_pointer settings, double *x, double *y, double *z); + void g68_remove(setup_pointer settings, double *x, double *y, double *z); + void g68_unrotate(setup_pointer settings, double *x, double *y, double *z); + void program_to_world_xyz(setup_pointer settings, double px, double py, double pz, + double *wx, double *wy, double *wz); + void world_to_program_xyz(setup_pointer settings, double wx, double wy, double wz, + double *px, double *py, double *pz); int update_tag(StateTag &tag); int cycle_feed(block_pointer block, CANON_PLANE plane, double end1, double end2, double end3); diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index 87ba8d12148..899d507eb4f 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -1197,6 +1197,9 @@ int Interp::init() _setup.home_flag = false; _setup.input_flag = false; _setup.kinsSwitch_flag = false; + // the tilted work plane does not survive an abort or a program start; + // canon hears about it only if there was one + work_plane_cancel(&_setup); _setup.input_index = -1; _setup.input_digital = false; _setup.program_x = 0.; /* for cutter comp */ @@ -2700,6 +2703,12 @@ int Interp::on_abort(int reason, const char *message) reset(); _setup.mdi_interrupt = false; + // the tilted work plane goes before the abort routine runs, so that + // routine can change coordinate systems as it likes. Canon is told + // even when the read ahead had already cancelled it, since the message + // that would have said so died with the queue. + work_plane_cancel(&_setup, true); + /* A thread's queued override restore is lost when abort clears the interpreter list, so re-assert the modal state here. */ if (_setup.speed_override[_setup.active_spindle]) { diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index d60d661d4f7..6ee6ba4d4e9 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -115,6 +115,16 @@ void SET_XY_ROTATION(double t) { void HOME_CYCLE(void) { ECHO_WITH_ARGS(""); } void HOME_CYCLE_JOINT(int joint) { ECHO_WITH_ARGS("%d", joint); } +void SET_G68_FRAME(double x, double y, double z, + const double rotation[9], int active) { + ECHO_WITH_ARGS("%.4f, %.4f, %.4f, " + "[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f], %d", + x, y, z, + rotation[0], rotation[1], rotation[2], + rotation[3], rotation[4], rotation[5], + rotation[6], rotation[7], rotation[8], active); +} + void SET_G5X_OFFSET(int index, double x, double y, double z, double a, double b, double c, diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index b101e86f398..9f2d6d691d1 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -193,13 +193,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 @@ -209,6 +242,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); @@ -220,6 +255,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); @@ -244,10 +281,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; @@ -510,6 +551,26 @@ void HOME_CYCLE_JOINT(int joint) interp_list.append(std::move(msg)); } +void SET_G68_FRAME(double x, double y, double z, + const double rotation[9], int active) +{ + flush_segments(); + + canon.g68Offset[0] = FROM_PROG_LEN(x); + canon.g68Offset[1] = FROM_PROG_LEN(y); + canon.g68Offset[2] = FROM_PROG_LEN(z); + for (int i = 0; i < 9; i++) { canon.g68Rotation[i] = rotation[i]; } + canon.g68Active = active; + + auto msg = std::make_unique(); + msg->origin.tran.x = TO_EXT_LEN(canon.g68Offset[0]); + msg->origin.tran.y = TO_EXT_LEN(canon.g68Offset[1]); + msg->origin.tran.z = TO_EXT_LEN(canon.g68Offset[2]); + for (int i = 0; i < 9; i++) { msg->rotation[i] = rotation[i]; } + msg->active = active; + interp_list.append(std::move(msg)); +} + void SET_G5X_OFFSET(int index, double x, double y, double z, double a, double b, double c, @@ -2440,7 +2501,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; @@ -2674,7 +2737,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. @@ -3455,6 +3518,9 @@ void INIT_CANON() // initialize locals to original values canon.xy_rotation = 0.0; + canon.g68Offset[0] = canon.g68Offset[1] = canon.g68Offset[2] = 0.0; + for (int i = 0; i < 9; i++) { canon.g68Rotation[i] = (i % 4 == 0) ? 1.0 : 0.0; } + canon.g68Active = 0; canon.rotary_unlock_for_traverse = -1; canon.feed_mode = 0; canon.g5xOffset.x = 0.0; diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index 9c3d432315e..d26c65de714 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -1572,6 +1572,7 @@ static EMC_TASK_EXEC emcTaskCheckPreconditions(NMLmsg * cmd) case EMC_TRAJ_SET_G5X_TYPE: case EMC_TRAJ_SET_G92_TYPE: case EMC_TRAJ_SET_ROTATION_TYPE: + case EMC_TRAJ_SET_G68_TYPE: // this applies the program origin after previous motions return EMC_TASK_EXEC::WAITING_FOR_MOTION; break; @@ -1991,6 +1992,15 @@ static int emcTaskIssueCommand(NMLmsg * cmd) retval = 0; break; + case EMC_TRAJ_SET_G68_TYPE: { + EMC_TRAJ_SET_G68 *g68 = reinterpret_cast(cmd); + emcStatus->task.g68_offset = g68->origin; + for (int i = 0; i < 9; i++) { emcStatus->task.g68_rotation[i] = g68->rotation[i]; } + emcStatus->task.g68_active = g68->active; + retval = 0; + break; + } + case EMC_TRAJ_SET_G5X_TYPE: // struct-copy program origin emcStatus->task.g5x_offset = (reinterpret_cast(cmd))->origin; @@ -2581,6 +2591,7 @@ static EMC_TASK_EXEC emcTaskCheckPostconditions(NMLmsg * cmd) case EMC_TRAJ_SET_G5X_TYPE: case EMC_TRAJ_SET_G92_TYPE: case EMC_TRAJ_SET_ROTATION_TYPE: + case EMC_TRAJ_SET_G68_TYPE: case EMC_TRAJ_PROBE_TYPE: case EMC_TRAJ_RIGID_TAP_TYPE: case EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG_TYPE: diff --git a/src/emc/usr_intf/axis/extensions/emcmodule.cc b/src/emc/usr_intf/axis/extensions/emcmodule.cc index 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 c8760294036..ef3f6bdd977 100755 --- a/src/emc/usr_intf/axis/scripts/axis.py +++ b/src/emc/usr_intf/axis/scripts/axis.py @@ -1617,6 +1617,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/tests/gcode-renderer/programs.py b/tests/gcode-renderer/programs.py index 636ddff66d1..8ffe665a707 100644 --- a/tests/gcode-renderer/programs.py +++ b/tests/gcode-renderer/programs.py @@ -822,6 +822,27 @@ def rotated_xy(): """ +def tilted_work_plane(): + """A ``G68.2`` plane inside a ``G54`` offset, cut in with a line and an arc. + + The plane is turned 90 degrees about Z at (0.5, 0, 0) of G54, which is + (1, 2, 3) of the machine: a plane point (x, y) lands at machine + (1.5 - y, 2 + x, 3). The line ends at plane (1, 0), the quarter arc + runs from there to (0, 1) about the plane origin, and both stay in the + plane's Z, which is machine Z 3 throughout. + """ + return """G20 G90 G94 G17 +G10 L2 P1 X1 Y2 Z3 +G54 +G68.2 X0.5 Y0 Z0 I90 J0 K0 +G0 X0 Y0 Z0 +G1 F20 X1 Y0 +G3 X0 Y1 I-1 J0 +G69 +M2 +""" + + def blank_m2(): """A program that emits no motion at all. diff --git a/tests/gcode-renderer/test_transform.py b/tests/gcode-renderer/test_transform.py index 420d47c8634..4c01c1d27f2 100755 --- a/tests/gcode-renderer/test_transform.py +++ b/tests/gcode-renderer/test_transform.py @@ -270,5 +270,33 @@ def test_the_machine_frame_extents_are_not_the_drawn_ones(self): self.assertAlmostEqual(canon.min_extents[2], -1.5, 9) +class TiltedWorkPlane(unittest.TestCase): + """``G68.2`` turns the program's points before the offsets do.""" + + def test_the_moves_land_in_the_plane(self): + canon = parse(programs.tilted_work_plane(), "XYZ") + geometry = canon.program_geometry + drawn = geometry.positions() + kinds = geometry.kinds + # the line: plane (1, 0) is machine (1.5, 3, 3) + feed = drawn[kinds == bake.KIND_FEED] + self.assertEqual(len(feed), 1) + np.testing.assert_allclose(feed[0], (1.5, 3.0, 3.0), atol=1e-6) + # the arc: every point one unit from the plane origin, machine + # (1.5, 2, 3), at the plane's Z, ending at plane (0, 1) + arc = drawn[kinds == bake.KIND_ARC] + self.assertGreater(len(arc), 3) + radius = np.hypot(arc[:, 0] - 1.5, arc[:, 1] - 2.0) + np.testing.assert_allclose(radius, 1.0, atol=1e-6) + np.testing.assert_allclose(arc[:, 2], 3.0, atol=1e-6) + np.testing.assert_allclose(arc[-1], (0.5, 2.0, 3.0), atol=1e-6) + + def test_the_plane_is_gone_after_g69(self): + program = programs.tilted_work_plane().replace("M2", "G1 X1 Y0\nM2") + drawn = parse(program, "XYZ").program_geometry.positions() + # a plain G54 point again: (1, 0) of G54 is machine (2, 2, 3) + np.testing.assert_allclose(drawn[-1], (2.0, 2.0, 3.0), atol=1e-6) + + if __name__ == "__main__": unittest.main() diff --git a/tests/interp/g68-frame/expected b/tests/interp/g68-frame/expected new file mode 100644 index 00000000000..98c0321fffe --- /dev/null +++ b/tests/interp/g68-frame/expected @@ -0,0 +1,55 @@ + 1 N..... USE_LENGTH_UNITS(CANON_UNITS_MM) + 2 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 3 N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 4 N..... SET_XY_ROTATION(0.0000) + 5 N..... SET_FEED_REFERENCE(CANON_XYZ) + 6 N..... ON_RESET() + 7 N..... COMMENT("the tilted work plane through the stand alone canon") + 8 N..... USE_LENGTH_UNITS(CANON_UNITS_MM) + 9 N..... COMMENT("interpreter: setting coordinate system origin") + 10 N..... SET_G5X_OFFSET(2, 100.0000, 200.0000, 300.0000, 0.0000, 0.0000, 0.0000) + 11 N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 12 N..... SET_XY_ROTATION(0.0000) + 13 N..... COMMENT("a plane rotated 90 about X: plane Y is world Z, plane Z is world -Y") + 14 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) + 15 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 16 N..... COMMENT("the same plane by fixed axis angles about X") + 17 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) + 18 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 19 N..... COMMENT("the same plane by three points") + 20 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) + 21 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 22 N..... COMMENT("the same plane by two vectors, X nudged off square") + 23 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) + 24 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 25 N..... COMMENT("R turns the plane about its own Z") + 26 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.0000, -1.0000, 0.0000, 0.0000, 0.0000, -1.0000, 1.0000, 0.0000, 0.0000], 1) + 27 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 28 N..... COMMENT("an arc in the plane") + 29 N..... SET_FEED_RATE(100.0000) + 30 N..... STRAIGHT_FEED(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 31 N..... ARC_FEED(2.0000, 0.0000, 1.0000, 0.0000, -1, 0.0000, 0.0000, 0.0000, 0.0000) + 32 N..... COMMENT("G53 inside the plane goes to absolute coordinates") + 33 N..... STRAIGHT_TRAVERSE(-30.0000, 10.0000, 20.0000, 0.0000, 0.0000, 0.0000) + 34 N..... COMMENT("and #5021 reports them") + 35 N..... MESSAGE(" abs 100.000000 200.000000 300.000000 prog -30.000000 10.000000 20.000000") + 36 N..... COMMENT("a probe result comes back in plane coordinates: nothing to run here") + 37 N..... COMMENT("a tool length change moves the program coordinates along the plane axis that is world Z") + 38 N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 7.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000) + 39 N..... MESSAGE(" prog -37.000000 10.000000 20.000000") + 40 N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000) + 41 N..... COMMENT("G68.4 composes: a further 90 about the plane's X") + 42 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.0000, 0.0000, 1.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000, 0.0000], 1) + 43 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 44 N..... COMMENT("G69 cancels") + 45 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) + 46 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 47 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 48 N..... SET_XY_ROTATION(0.0000) + 49 N..... SET_FEED_MODE(0, 0) + 50 N..... SET_FEED_RATE(0.0000) + 51 N..... STOP_SPINDLE_TURNING(0) + 52 N..... SET_SPINDLE_MODE(0 0.0000) + 53 N..... PROGRAM_END() + 54 N..... ON_RESET() + 55 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..656cce9b906 --- /dev/null +++ b/tests/interp/g68-frame/g68.ngc @@ -0,0 +1,44 @@ +% +(the tilted work plane through the stand alone canon) +g21 g90 +g10 l2 p2 x100 y200 z300 r0 +g55 +(a plane rotated 90 about X: plane Y is world Z, plane Z is world -Y) +g68.2 x10 y20 z30 i0 j90 k0 +g0 x1 y2 z3 +(the same plane by fixed axis angles about X) +g68.2 p1 q123 x10 y20 z30 i90 j0 k0 +g0 x1 y2 z3 +(the same plane by three points) +g68.2 p2 q0 x10 y20 z30 +g68.2 p2 q1 x0 y0 z0 +g68.2 p2 q2 x5 y0 z0 +g68.2 p2 q3 x0 y0 z5 +g0 x1 y2 z3 +(the same plane by two vectors, X nudged off square) +g68.2 p3 q1 x10 y20 z30 i1 j0 k0.000001 +g68.2 p3 q2 i0 j-1 k0 +g0 x1 y2 z3 +(R turns the plane about its own Z) +g68.2 p1 q123 x10 y20 z30 i90 j0 k0 r90 +g0 x1 y2 z3 +(an arc in the plane) +g1 f100 x0 y0 z0 +g2 x2 y0 i1 j0 +(G53 inside the plane goes to absolute coordinates) +g53 g0 x100 y200 z300 +(and #5021 reports them) +(debug, abs #5021 #5022 #5023 prog #5420 #5421 #5422) +(a probe result comes back in plane coordinates: nothing to run here) +(a tool length change moves the program coordinates along the plane axis that is world Z) +g43.1 z7 +(debug, prog #5420 #5421 #5422) +g49 +(G68.4 composes: a further 90 about the plane's X) +g68.4 p1 q123 i90 j0 k0 +g0 x1 y2 z3 +(G69 cancels) +g69 +g0 x1 y2 z3 +m2 +% diff --git a/tests/interp/g68-frame/test.sh b/tests/interp/g68-frame/test.sh new file mode 100755 index 00000000000..c11ecd785a9 --- /dev/null +++ b/tests/interp/g68-frame/test.sh @@ -0,0 +1,3 @@ +#!/bin/bash +rs274 -g g68.ngc | sed 's/-0\.0000/0.0000/g' +exit "${PIPESTATUS[0]}" From 4115d57d4b7c0eefcf5fe7740f0bbd81e3da3e19 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 17 Sep 2026 23:13:58 +1000 Subject: [PATCH 19/77] glcanon: draw the tilted work planes a program defines Where a program's G68.2 planes sit is the hardest thing to check by reading it, so the preview draws each: a rectangle lying in the plane over the moves made under it, with a margin, the plane's X, Y and Z at the centre of the rectangle in the machine axis colours, and a cross at the plane's origin, which need not lie in the rectangle: a program drilling a sphere puts every origin at the centre and works out on the surface. The rectangle sits at the plane's Z where the program reaches it and otherwise at the nearest end of the Z range worked in, so a drilling cycle shows its holes; a plane nothing moved under gets a square a tenth of the program. A plane restated in a loop is one plane. The plane in effect on the machine, what status reports the last executed G68.2 set, is drawn over the others in its own colour, so the plane the program is in stands out; an MDI plane shows the same way. The renderer already receives every plane through set_g68_frame(); it records each as a WorkPlaneRecord, origin and axes through its transform like a move endpoint, every move under it extends the extents, and the records reach the canon with the rest of the program, as glcanon_scene.WorkPlane. WorkPlanePart draws them in the program's frame through gcode.display_points(), the GEOMETRY transform for points that are not a parse's, gated by show_workplane, colours 'workplane' and 'workplane_active'. Suggested by Sigma1912. tests/glcanon/test_workplane.py, on parsed programs; docs in the G68.2 section. --- docs/src/gcode/g-code.adoc | 20 +++ lib/python/rs274/glcanon.py | 16 ++ lib/python/rs274/glcanon_scene.py | 245 ++++++++++++++++++++++++++++- src/emc/rs274ngc/gcode_renderer.cc | 130 ++++++++++++--- src/emc/rs274ngc/gcode_renderer.hh | 46 +++++- tests/gcode-renderer/canon.py | 7 +- tests/glcanon/test.sh | 3 +- tests/glcanon/test_workplane.py | 186 ++++++++++++++++++++++ 8 files changed, 627 insertions(+), 26 deletions(-) create mode 100755 tests/glcanon/test_workplane.py diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index f9572fd4e1d..e5e642a40c5 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -2056,6 +2056,26 @@ old plane. words are in the plane, and the result is a new plane relative to the old one. It needs a plane to build on. +The G-code preview draws every plane a program defines, so a program can +be checked for where its planes sit before it runs: a rectangle lying in +the plane, over the moves made under it with a margin around them, the +plane's own X, Y and Z at the centre of the rectangle in the colours of +the machine axes, and a cross at the plane's origin, which need not lie +in the rectangle: a program drilling a sphere puts every plane's origin +at the centre and works out on the surface. The rectangle is drawn at the plane's Z where the program comes +down to it, and otherwise at the end of the Z range the program worked +in that is nearest to it, so a drilling cycle that stays above the +origin shows its holes where they are cut; a plane nothing moved under +is drawn as a square sized from the program. A plane restated with the +same words is one plane, not one per restatement. The plane in effect +on the machine, the one the last executed 'G68.2' set, is drawn over +the others in its own colour, so as the program runs the plane it is +in stands out from the ones it has been in and will be in; a plane set +from MDI shows the same way, as a square with nothing under it. The plane +in effect is drawn no smaller than the coordinate system axes, with its +own axes as long as those, so its orientation can be checked against them +before a program runs, however small the program. + 'G69' cancels the plane. So does the end of the program, 'M2' or 'M30', and an abort: the plane is not persistent and nothing about it is written to the parameter file. diff --git a/lib/python/rs274/glcanon.py b/lib/python/rs274/glcanon.py index 5c4c0b5eba6..31f62d5d394 100644 --- a/lib/python/rs274/glcanon.py +++ b/lib/python/rs274/glcanon.py @@ -173,6 +173,9 @@ def __init__(self, colors, geometry, is_foam=0, foam_w=1.5, foam_z=0.0): # fixture may hold several pieces. A canon is built per file load, so # nothing else ever clears this. self.workpieces = [] + # The tilted work planes the program defined, in order, from the + # renderer's records at the end of the parse. + self.workplanes = [] def comment(self, arg): """``(WORKPIECE,...)``, ``stop``, ``notify`` and the foam Z levels. @@ -247,6 +250,8 @@ def adopt_geometry(self, pg): # hands over, so a partial load yields the partial list it always did. self.tool_list = [tool for _lineno, tool, _points in self.program_geometry.toolchanges] + self.workplanes = [glcanon_scene.WorkPlane(*record) + for record in pg.workplanes()] # -- the program record ------------------------------------------------ @@ -519,6 +524,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 @@ -990,6 +999,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 @@ -1129,6 +1144,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(), diff --git a/lib/python/rs274/glcanon_scene.py b/lib/python/rs274/glcanon_scene.py index 38f0b0bff43..979a14edee6 100644 --- a/lib/python/rs274/glcanon_scene.py +++ b/lib/python/rs274/glcanon_scene.py @@ -53,6 +53,7 @@ glDepthFunc, glDepthMask, glDisable, glEnable, glLineWidth, glUseProgram) +import gcode import glnav import linuxcnc from rs274 import glcanon_bake, glcanon_gl @@ -98,6 +99,26 @@ #: 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 + +#: The plane in effect is what an operator who set it from MDI looks at to +#: check the orientation before running a program, so it is drawn no smaller +#: than the coordinate system axes, an inch each way, with its own axes the +#: same length as those. +WORKPLANE_ACTIVE_SIZE = 1.0 + def minmax(*args: float) -> tuple[float, float]: return min(*args), max(*args) @@ -211,7 +232,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 +310,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. @@ -2402,6 +2424,224 @@ def circle_edges(cls, axis: int, c1: float, c2: float, a: float, return edges +class WorkPlane: + """One tilted work plane the program defined, with G68.2, G68.3 or + G68.4: where it sits on the machine, and what the program did in it. + + A record of the renderer's: it places the plane and accumulates the + moves under it while it parses, and hands the records over with the + rest of the program. :attr:`origin` and :attr:`axes` are in absolute + machine coordinates, in the canon's units, with the g92 offset, the g5x + XY rotation and the g5x offset that were active at the definition + applied the way a move endpoint on the same line gets them. The extents + are in the plane's own coordinates, the ones the program writes under + it, accumulated from every move made while it was in effect, so a + reader can tell a plane the program only oriented to from one it cut + in; they are empty (``min > max``) while nothing moved. + + Read them off the widget's canon, as the workpieces:: + + for plane in gremlin_widget.canon.workplanes: + print(plane.lineno, plane.origin, plane.extents) + """ + + __slots__ = ('lineno', 'origin', 'axes', 'min_extents', 'max_extents') + + def __init__(self, lineno: int, origin: Sequence[float], + axes: Sequence[Sequence[float]], + min_extents: Sequence[float] = (9e99, 9e99, 9e99), + max_extents: Sequence[float] = (-9e99, -9e99, -9e99)) -> None: + #: Source line of the definition, or ``-1`` without one. + self.lineno = lineno + #: The plane's origin, machine coordinates. + self.origin = tuple(origin) + #: The plane's X, Y and Z as unit vectors in machine coordinates. + self.axes = tuple(tuple(a) for a in axes) + self.min_extents = tuple(min_extents) + self.max_extents = tuple(max_extents) + + def __repr__(self) -> str: + return "" % (self.lineno, self.origin) + + def same_as(self, origin: Sequence[float], + axes: Sequence[Sequence[float]], tol: float = 1e-9) -> bool: + """Whether a definition names this plane again.""" + for a, b in zip(self.origin, origin): + if abs(a - b) > tol: + return False + for u, v in zip(self.axes, axes): + for a, b in zip(u, v): + if abs(a - b) > tol: + return False + return True + + @property + def has_moves(self) -> bool: + return self.max_extents[X] >= self.min_extents[X] + + @property + def extents(self) -> tuple[tuple[float, ...], tuple[float, ...]]: + """``(min_xyz, max_xyz)`` of the moves made under the plane, in the + plane's coordinates.""" + return self.min_extents, self.max_extents + + def machine_point(self, x: float, y: float, z: float) -> tuple[float, float, float]: + """A point of the plane in machine coordinates.""" + o, (ax, ay, az) = self.origin, self.axes + return (o[0] + x*ax[0] + y*ay[0] + z*az[0], + o[1] + x*ax[1] + y*ay[1] + z*az[1], + o[2] + x*ax[2] + y*ay[2] + z*az[2]) + + +def active_workplane(ctx: FrameContext) -> "WorkPlane | None": + """The plane in effect on the machine, as status reports it: the one the + last executed G68.2 set, whether from the program or from MDI. + + Status carries it as the interpreter gave it, the origin in the + coordinate system the plane was defined in and in machine units, so it + goes through the g92 offset, the XY rotation and the g5x offset the way + the canon takes a definition, in the canon's units. Where it is one the + loaded program defined, that record is returned, extents and all; a + plane from MDI, or from a program no longer loaded, comes back as a + record of its own with nothing under it. + """ + s = ctx.stat + if s is None or not getattr(s, 'g68_active', 0): + return None + try: + o = ctx.to_internal_units(s.g68_offset) + r = s.g68_rotation + g92 = ctx.to_internal_units(s.g92_offset) + g5x = ctx.to_internal_units(s.g5x_offset) + t = math.radians(s.rotation_xy) + except (AttributeError, TypeError): + return None + c, sn = math.cos(t), math.sin(t) + + def through(x: float, y: float, z: float) -> tuple[float, float, float]: + x, y, z = (r[0]*x + r[1]*y + r[2]*z + o[X] + g92[X], + r[3]*x + r[4]*y + r[5]*z + o[Y] + g92[Y], + r[6]*x + r[7]*y + r[8]*z + o[Z] + g92[Z]) + return (x*c - y*sn + g5x[X], x*sn + y*c + g5x[Y], z + g5x[Z]) + + origin = through(0, 0, 0) + axes = [] + for unit in ((1, 0, 0), (0, 1, 0), (0, 0, 1)): + p = through(*unit) + axes.append((p[0] - origin[0], p[1] - origin[1], p[2] - origin[2])) + for plane in getattr(ctx.canon, 'workplanes', ()): + # status has been through machine units and back, so not to the bit + if plane.same_as(origin, axes, tol=1e-6): + return plane + return WorkPlane(-1, origin, axes) + + +class WorkPlanePart(Part): + """The tilted work planes the program defined, one drawing each, and + the one in effect on the machine over them in its own colour. + + A plane is drawn where the program worked in it: a rectangle lying in + the plane, over the moves made under it with a margin around them, at + the plane's own Z where the program reached that and otherwise at the + end of the Z range it worked in nearest to it, so a drilling cycle that + never comes down to the plane's origin still shows its holes; a plane + nothing moved under gets a square sized from the program. The plane's + axes stand at the centre of the rectangle in the machine-axis colours, + so the direction the program's X and Y took can be read where the work + is, and the plane's origin is marked with a cross, since the two need + not coincide: a program that drills a sphere puts every plane's origin + at the centre and works out on the surface. + + The active plane is whatever status says the last executed G68.2 set, + so it walks through the program's planes as the program runs, and an + MDI plane shows too, as a square with nothing under it. It is drawn no + smaller than the coordinate system axes, whatever the program's size, + since a plane set from MDI is checked by eye against them. + """ + + #: Inner lines each way, to read as a surface rather than an outline. + SUBDIVISIONS = 4 + + def draw(self, ctx: FrameContext) -> None: + canon = ctx.canon + planes = list(getattr(canon, 'workplanes', ())) + active = active_workplane(ctx) + if not planes and active is None: + return + # the size a plane with nothing, or only a point, under it is drawn + # at: a tenth of the program, as the extents part spaces its labels + size = max(canon.max_extents[X] - canon.min_extents[X], + canon.max_extents[Y] - canon.min_extents[Y], + canon.max_extents[Z] - canon.min_extents[Z], 2) * .1 + others = [p for p in planes if p is not active] + if others: + self.draw_planes(ctx, others, size, + ctx.colors.get('workplane', WORKPLANE_COLOR), + ctx.colors.get('workplane_alpha', WORKPLANE_ALPHA)) + if active is not None: + self.draw_planes(ctx, [active], size, + ctx.colors.get('workplane_active', WORKPLANE_ACTIVE_COLOR), + ctx.colors.get('workplane_active_alpha', WORKPLANE_ACTIVE_ALPHA), + least=WORKPLANE_ACTIVE_SIZE) + + def draw_planes(self, ctx: FrameContext, planes: Sequence[WorkPlane], + size: float, color: Color, alpha: float, + least: float = 0.0) -> None: + """``size`` is the side of the square a plane with nothing under it + gets; ``least`` a floor under every plane's half-width and axis + length, in the canon's units.""" + canon = ctx.canon + outline, inner, cross = [], [], [] + axes = {'axis_x': [], 'axis_y': [], 'axis_z': []} + n = self.SUBDIVISIONS + for plane in planes: + if plane.has_moves: + lo, hi = plane.min_extents, plane.max_extents + cx, cy = (lo[X] + hi[X]) / 2, (lo[Y] + hi[Y]) / 2 + hw = max((hi[X] - lo[X]) * .6, size / 2, least) + hh = max((hi[Y] - lo[Y]) * .6, size / 2, least) + z = min(max(lo[Z], 0.0), hi[Z]) + else: + cx = cy = z = 0.0 + hw = hh = max(size / 2, least) + x0, x1, y0, y1 = cx - hw, cx + hw, cy - hh, cy + hh + corner = [plane.machine_point(x0, y0, z), plane.machine_point(x1, y0, z), + plane.machine_point(x1, y1, z), plane.machine_point(x0, y1, z)] + for i in range(4): + outline += [corner[i], corner[(i + 1) % 4]] + for i in range(1, n): + f = i / n + inner += [plane.machine_point(x0 + (x1 - x0) * f, y0, z), + plane.machine_point(x0 + (x1 - x0) * f, y1, z), + plane.machine_point(x0, y0 + (y1 - y0) * f, z), + plane.machine_point(x1, y0 + (y1 - y0) * f, z)] + length = max(hw * .5, hh * .5, least) + centre = plane.machine_point(cx, cy, z) + axes['axis_x'] += [centre, plane.machine_point(cx + length, cy, z)] + axes['axis_y'] += [centre, plane.machine_point(cx, cy + length, z)] + axes['axis_z'] += [centre, plane.machine_point(cx, cy, z + length)] + arm = length * .4 + cross += [plane.machine_point(-arm, 0, 0), plane.machine_point(arm, 0, 0), + plane.machine_point(0, -arm, 0), plane.machine_point(0, arm, 0), + plane.machine_point(0, 0, -arm), plane.machine_point(0, 0, arm)] + def to_display(points: list[tuple[float, float, float]]) -> Any: + # machine to drawn coordinates by the program's GEOMETRY, as the + # renderer places a move; nothing is parsing, so through the + # module function rather than the canon + return np.array(gcode.display_points(canon.program_geometry, points)) + ctx.prim.draw_lines(ctx, to_display(outline), color, alpha) + ctx.prim.draw_lines(ctx, to_display(inner), color, alpha * .4) + ctx.prim.draw_lines(ctx, to_display(cross), color, alpha) + # the axes wide, as the backplot is: at one pixel in the axis colours + # they are lost against the rectangle and the program under it + set_line_width(3.0) + try: + for name, verts in axes.items(): + ctx.prim.draw_lines(ctx, to_display(verts), ctx.colors[name], alpha) + finally: + set_line_width(1.0) + + class WorkpiecePart(Part): """Wireframe stock outlines declared by ``(WORKPIECE,...)`` comments. @@ -2468,6 +2708,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__([ @@ -2484,6 +2725,8 @@ def __init__(self) -> None: (self.backplot, lambda ctx: ctx.show_live_plot), (self.workpiece, lambda ctx: ctx.show_workpiece and ctx.canon is not None), + (self.workplane, lambda ctx: ctx.show_workplane + and ctx.canon is not None), (self.tool, lambda ctx: ctx.show_tool), (self.overlay, lambda ctx: ctx.enable_dro), ]) diff --git a/src/emc/rs274ngc/gcode_renderer.cc b/src/emc/rs274ngc/gcode_renderer.cc index 9e8e18fcfdb..fa9abcee66c 100644 --- a/src/emc/rs274ngc/gcode_renderer.cc +++ b/src/emc/rs274ngc/gcode_renderer.cc @@ -48,6 +48,42 @@ void GCodeRenderer::set_xy_rotation(double degrees) { unrot_sin_ = -rotation_sin_; // sin(back); } +// The plane's origin and axes on the machine, through the transform the way +// a move endpoint on this line goes, so the drawn plane and the moves in it +// agree. Recorded here rather than at hand over because the offsets in +// force at the definition are what place it, and they move on. +void GCodeRenderer::set_g68_frame(const WorkFrame &frame) { + if(parse_state.interp_error) return; + frame_ = frame; + workplane_ = -1; + if(!frame.active || !data_) return; + Point9 at; + transform({}, at); + Point3 origin = {at[P9_X], at[P9_Y], at[P9_Z]}; + std::array axes; + for(int i = 0; i < P3_COUNT; i++) { + Point9 unit = {}; + unit[i] = 1.0; + transform(unit, at); + for(int j = 0; j < P3_COUNT; j++) axes[i][j] = at[j] - origin[j]; + } + std::vector &planes = data_->workplanes; + if(!planes.empty() && planes.back().same_as(origin, axes)) { + workplane_ = (long)planes.size() - 1; + return; + } + WorkPlaneRecord record; + record.lineno = parse_state.current_line(); + record.origin = origin; + record.axes = axes; + for(int i = 0; i < P3_COUNT; i++) { + record.extents[BOX_MIN][i] = 9e99; + record.extents[BOX_MAX][i] = -9e99; + } + planes.push_back(record); + workplane_ = (long)planes.size() - 1; +} + void GCodeRenderer::arc_feed(int line_number, double first_end, double second_end, double first_axis, double second_axis, int rotation, double axis_end_point, double a, double b, double c, @@ -285,6 +321,18 @@ void preview_geometry_register(py::module_ &m) { points_tuple(r.pts, d.nplanes))); return out; }, "(lineno, tool number, points per plane) per tool change") + .def("workplanes", [](const PreviewData &d) { + py::list out; + for(const WorkPlaneRecord &r : d.workplanes) + out.append(py::make_tuple(r.lineno, triple(r.origin), + py::make_tuple(triple(r.axes[0]), triple(r.axes[1]), + triple(r.axes[2])), + triple(r.extents[BOX_MIN]), + triple(r.extents[BOX_MAX]))); + return out; + }, "(lineno, origin, (x, y, z) axes, min, max) per tilted work " + "plane; origin and axes in the machine frame, the extents " + "of the moves made under it in the plane's own coordinates") .def("tool_offsets", [](const PreviewData &d) { py::list out; for(const ToolOffsetRecord &r : d.tool_offsets) @@ -418,28 +466,43 @@ static py::tuple point_rows(const std::vector &xyz) { return out; } -static py::tuple renderer_transform(py::handle self, py::handle points) { - if(!parse_state.in_parse || !parse_state.canon) - throw py::value_error("transform: no parse in progress"); - // The parse transforms *its* canon's points. A canon left over from an - // earlier parse would otherwise read this one's offsets and be believed. - if(self.ptr() != parse_state.callback) - throw py::value_error("transform: not the canon of the parse in " - "flight"); +// The GEOMETRY-string transform (the C vertex9), for one point through one +// compiled plane. +static void plane_point(const std::vector &ops, + const Point9 &pts9, Point3 &out) { + out = {}; + for(const GeomOp &op : ops) op.apply(pts9, out); +} + +// Rows of 3 or 9 numbers as 9-DOF points, the short ones zero-filled. +static std::vector read_points(const char *what, py::handle points) { std::vector in; for(py::handle row : points) { Point9 p = {}; size_t n = 0; for(py::handle value : py::reinterpret_borrow(row)) { if(n == P9_COUNT) - throw py::value_error("transform: a point takes 3 or 9 " - "numbers"); + throw py::value_error(std::string(what) + + ": a point takes 3 or 9 numbers"); p[n++] = value.cast(); } if(n != P3_COUNT && n != P9_COUNT) - throw py::value_error("transform: a point takes 3 or 9 numbers"); + throw py::value_error(std::string(what) + + ": a point takes 3 or 9 numbers"); in.push_back(p); } + return in; +} + +static py::tuple renderer_transform(py::handle self, py::handle points) { + if(!parse_state.in_parse || !parse_state.canon) + throw py::value_error("transform: no parse in progress"); + // The parse transforms *its* canon's points. A canon left over from an + // earlier parse would otherwise read this one's offsets and be believed. + if(self.ptr() != parse_state.callback) + throw py::value_error("transform: not the canon of the parse in " + "flight"); + std::vector in = read_points("transform", points); std::vector machine(in.size() * P3_COUNT); std::vector display(in.size() * P3_COUNT); if(!parse_state.canon->transform_points(in.data(), in.size(), @@ -451,6 +514,35 @@ static py::tuple renderer_transform(py::handle self, py::handle points) { // `gcode.RendererCanon`. This holds a reference of its own: the module // attribute is deletable, and a type freed under us would leave every // later PyObject_TypeCheck reading freed memory. +// The GEOMETRY transform of a canon's first drawn plane, for points that +// are not a parse's: a work plane an operator set from MDI is drawn in +// the same frame the program is, and there is no parse in flight to ask. +static py::tuple display_points(py::handle program_geometry, py::handle points) { + py::object ro = program_geometry.attr("ro"); + long mask = ro.attr("axis_mask").cast(); + bool respect = PyObject_IsTrue(ro.attr("respect_offsets").ptr()); + double rox = 0.0, roy = 0.0, roz = 0.0; + if(respect) { + rox = ro.attr("x").cast(); + roy = ro.attr("y").cast(); + roz = ro.attr("z").cast(); + } + py::sequence names = program_geometry.attr("planes").cast(); + if(py::len(names) < 1) + throw py::value_error("display_points: no drawn plane"); + std::vector ops = GeomOp::compile(names[0].cast(), + mask, rox, roy, roz); + std::vector in = read_points("display_points", points); + std::vector display(in.size() * P3_COUNT); + for(size_t i = 0; i < in.size(); i++) { + Point3 drawn; + plane_point(ops, in[i], drawn); + for(int c = 0; c < P3_COUNT; c++) + display[i * P3_COUNT + c] = drawn[c]; + } + return point_rows(display); +} + static PyTypeObject *renderer_canon_type; void renderer_canon_register(py::module_ &m) { @@ -502,6 +594,13 @@ void renderer_canon_register(py::module_ &m) { "Program points to (machine, display) coordinates, as the parse " "in flight has them at this moment."); m.attr("RendererCanon") = cls; + m.def("display_points", &display_points, + py::arg("program_geometry"), py::arg("points"), + "Machine points to display coordinates through a canon's " + "program_geometry: the GEOMETRY string of its first drawn plane " + "and its rotation offsets, as the renderer applies them. Points " + "are 3 or 9 numbers each. For geometry that is not a program's, " + "outside any parse."); renderer_canon_type = (PyTypeObject *)cls.release().ptr(); } @@ -633,14 +732,6 @@ void GCodeRenderer::transform(const Point9 &in, Point9 &out) const { out += g5x_; } -// The GEOMETRY-string transform (the C vertex9), for one point through one -// compiled plane. -static void plane_point(const std::vector &ops, - const Point9 &pts9, Point3 &out) { - out = {}; - for(const GeomOp &op : ops) op.apply(pts9, out); -} - // Points through the live transform, by the two steps a move endpoint takes: // the g92 -> XY rotation -> g5x chain, and then the GEOMETRY string of the // plane drawn first. Nothing is sampled, linearised or rebuilt, so a point @@ -751,6 +842,7 @@ void GCodeRenderer::fill(int line_number, const Point9 &p1, const Point9 &p2, double feedrate, unsigned char cat) { data_->moves ++; accumulate_extents(p1, p2); + reach(p2); double dx = p2[P9_X] - p1[P9_X], dy = p2[P9_Y] - p1[P9_Y], dz = p2[P9_Z] - p1[P9_Z]; diff --git a/src/emc/rs274ngc/gcode_renderer.hh b/src/emc/rs274ngc/gcode_renderer.hh index 222f92da48a..985656fb440 100644 --- a/src/emc/rs274ngc/gcode_renderer.hh +++ b/src/emc/rs274ngc/gcode_renderer.hh @@ -133,6 +133,40 @@ struct ToolOffsetRecord { Point9 offsets; // xo..wo, as the canon was given them }; +// A tilted work plane the program defined, G68.2, G68.3 or G68.4: where it +// sits on the machine, and what the program did in it. The origin and the +// axes are in the machine frame, through the offsets in force at the +// definition the way a move endpoint on the same line gets them; the +// extents are in the plane's own coordinates, over every move made while +// it was in effect, empty (min > max) while nothing moved. A definition +// that names the plane in effect again is the same record, so a program +// that restates its plane in a loop has one plane, not one per pass. +struct WorkPlaneRecord { + int lineno; + Point3 origin; + std::array axes; // the plane's X, Y and Z, unit + Box3 extents; + + bool same_as(const Point3 &o, const std::array &a) const { + for(int i = 0; i < P3_COUNT; i++) { + if(fabs(origin[i] - o[i]) > 1e-9) return false; + for(int j = 0; j < P3_COUNT; j++) + if(fabs(axes[i][j] - a[i][j]) > 1e-9) return false; + } + return true; + } + // Take in a machine point the program reached under the plane. + void extend(const Point9 &p) { + Point3 d = {p[P9_X] - origin[P3_X], p[P9_Y] - origin[P3_Y], + p[P9_Z] - origin[P3_Z]}; + for(int i = 0; i < P3_COUNT; i++) { + double v = d[0] * axes[i][0] + d[1] * axes[i][1] + d[2] * axes[i][2]; + if(v < extents[BOX_MIN][i]) extents[BOX_MIN][i] = v; + if(v > extents[BOX_MAX][i]) extents[BOX_MAX][i] = v; + } + } +}; + struct PreviewData { ~PreviewData(); // False when the arrays could not grow: the caller must stop writing, as @@ -178,6 +212,7 @@ struct PreviewData { std::vector dwells; std::vector toolchanges; std::vector tool_offsets; + std::vector workplanes; // Record an offset at the current row; drop records governing no row. void set_tool_offset(const Point9 &offsets); void drop_trailing_tool_offset(); @@ -360,10 +395,7 @@ public: g92_ = offsets; } void set_xy_rotation(double degrees) override; - void set_g68_frame(const WorkFrame &frame) override { - if(parse_state.interp_error) return; - frame_ = frame; - } + void set_g68_frame(const WorkFrame &frame) override; // The plane reaches the record and the arc segmenter from here; nothing // on a rendered parse reads the canon's own copy. void set_plane(int plane) override { plane_ = plane; } @@ -451,6 +483,11 @@ private: // One move into the geometry: extents, length, then its vertices. void fill(int line_number, const Point9 &p1, const Point9 &p2, double feedrate, unsigned char cat); + // The plane in effect takes in the end of every move made under it. + void reach(const Point9 &p) { + if(workplane_ >= 0 && data_) + data_->workplanes[(size_t)workplane_].extend(p); + } // One record vertex at `at`, writing its per-plane position to `points`. void mark(int line_number, const Point9 &at, unsigned char kind, PlanePoints *points); @@ -484,6 +521,7 @@ private: double unrot_cos_ = 1.0; // the same rotation, negated, for the double unrot_sin_ = 0.0; // rotation-removed extents WorkFrame frame_; // the tilted work plane, inside g92 + long workplane_ = -1; // its record, or none in effect Point9 lo_ = {}; // chain point Point9 tool_ = {}; // xo..wo diff --git a/tests/gcode-renderer/canon.py b/tests/gcode-renderer/canon.py index 6869f02ce4e..d32d35f8741 100644 --- a/tests/gcode-renderer/canon.py +++ b/tests/gcode-renderer/canon.py @@ -196,7 +196,8 @@ class FakePreview: def __init__(self, planes, lines, kinds, tools=None, moves=None, rapid_length=0.0, cut_lengths=None, tool_numbers=None, dwells=(), toolchanges=(), dwell_time=0.0, extents=None, - axes="", axis_positions=None, tool_offsets=()): + axes="", axis_positions=None, tool_offsets=(), + workplanes=()): self._planes = [np.ascontiguousarray(p, dtype=np.float32) for p in planes] lines = np.asarray(lines, dtype=np.uint32) @@ -216,6 +217,7 @@ def __init__(self, planes, lines, kinds, tools=None, moves=None, self._dwells = list(dwells) self._toolchanges = list(toolchanges) self._tool_offsets = list(tool_offsets) + self._workplanes = list(workplanes) #: The machine's letters come back from every parse; the positions are #: (N, 0) unless asked for, which is what C hands over. self.axes = axes @@ -245,6 +247,9 @@ def axis_positions(self): def tool_offsets(self): return self._tool_offsets + def workplanes(self): + return list(self._workplanes) + def extents(self): return self._extents diff --git a/tests/glcanon/test.sh b/tests/glcanon/test.sh index 0268ec90d1f..cf53bc3c761 100755 --- a/tests/glcanon/test.sh +++ b/tests/glcanon/test.sh @@ -15,8 +15,9 @@ set -e if python3 -c 'import OpenGL' 2>/dev/null; then ./test_workpiece.py >&2 + ./test_workplane.py >&2 else - echo "skip: test_workpiece.py needs PyOpenGL (headless build)" >&2 + echo "skip: test_workpiece.py and test_workplane.py need PyOpenGL (headless build)" >&2 fi echo ok diff --git a/tests/glcanon/test_workplane.py b/tests/glcanon/test_workplane.py new file mode 100755 index 00000000000..5f40f76f6a3 --- /dev/null +++ b/tests/glcanon/test_workplane.py @@ -0,0 +1,186 @@ +#!/usr/bin/env python3 +"""The tilted work planes the preview draws: what the renderer records from +G68.2 and the moves under it, and what the part draws from that. + +Needs the RIP environment (rs274 pulls the compiled gcode extension) but no +display and no GL context: nothing below calls into OpenGL. + + . scripts/rip-environment && runtests tests/glcanon +""" +import math +import unittest + +import rs274.glcanon as glcanon +from rs274 import glcanon_bake, glcanon_scene +from test_workpiece import run + + +def program(*lines, offset="G10 L2 P1 X0 Y0 Z0"): + """A G20 program under G54: the plane definitions and moves given.""" + return "\n".join(("G20 G90 G94 G17", offset, "G54", "F10") + lines + + ("M2", "")) + + +#: A plane at (0, 40, 20) of G54, tilted 30 degrees about X; G68.2 takes +#: Euler angles about Z, X and Z, so the middle one alone tilts about X. +TILTED = "G68.2 X0 Y40 Z20 I0 J30 K0" + + +class WorkPlaneRecordTest(unittest.TestCase): + def test_where_the_plane_sits(self): + canon = run(program(TILTED, offset="G10 L2 P1 X100 Y0 Z0")) + plane, = canon.workplanes + # the origin goes through the g5x offset like a move endpoint + self.assertAlmostEqual(plane.origin[0], 100) + self.assertAlmostEqual(plane.origin[1], 40) + self.assertAlmostEqual(plane.origin[2], 20) + # X is untouched by a tilt about X; Z leans back towards -Y + x, y, z = plane.axes + self.assertAlmostEqual(x[0], 1) + self.assertAlmostEqual(z[1], -math.sin(math.radians(30))) + self.assertAlmostEqual(z[2], math.cos(math.radians(30))) + self.assertFalse(plane.has_moves) + self.assertEqual(plane.lineno, 5) + + def test_moves_extend_the_plane_in_its_own_coordinates(self): + canon = run(program(TILTED, "G1 X-30 Y-20 Z5", "G1 X30 Y20 Z-4")) + plane, = canon.workplanes + self.assertTrue(plane.has_moves) + lo, hi = plane.extents + for got, want in zip(lo + hi, (-30, -20, -4, 30, 20, 5)): + self.assertAlmostEqual(got, want) + # and a point of the plane comes back where the move went + p = plane.machine_point(30, 20, -4) + for a, b in zip(p, canon.program_geometry.positions()[-1]): + self.assertAlmostEqual(a, b, 5) + + def test_cancel_stops_the_recording(self): + canon = run(program("G68.2 X0 Y0 Z0 I0 J0 K0", "G1 X1 Y1 Z0", "G69", + "G1 X50 Y50 Z50")) + plane, = canon.workplanes + self.assertAlmostEqual(plane.max_extents[0], 1) + + def test_restating_the_plane_is_one_plane(self): + lines = [] + for i in range(3): + lines += ["G68.2 X0 Y0 Z0 I0 J30 K0", "G1 X%d Y0 Z0" % i] + lines.append("G68.2 X0 Y0 Z0 I0 J45 K0") + canon = run(program(*lines)) + self.assertEqual(len(canon.workplanes), 2) + self.assertAlmostEqual(canon.workplanes[0].max_extents[0], 2) + + def test_an_arc_under_the_plane_stays_in_it(self): + # a quarter circle in the tilted plane: every drawn point is in the + # plane, one unit from its origin + canon = run(program(TILTED, "G1 X1 Y0 Z0", "G3 X0 Y1 I-1 J0")) + plane, = canon.workplanes + o, (ax, ay, az) = plane.origin, plane.axes + geometry = canon.program_geometry + arc = geometry.positions()[geometry.kinds == glcanon_bake.KIND_ARC] + self.assertGreater(len(arc), 3) + for p in arc: + d = [p[i] - o[i] for i in range(3)] + self.assertAlmostEqual(sum(d[i] * az[i] for i in range(3)), 0, 5) + self.assertAlmostEqual(math.sqrt(sum(v * v for v in d)), 1, 5) + lo, hi = plane.extents + self.assertAlmostEqual(hi[0], 1, 5) + self.assertAlmostEqual(hi[1], 1, 5) + + +class StatStub: + """The plane in effect as status reports it, in machine units (mm).""" + + def __init__(self, active=0, origin=(0, 0, 0), tilt_about_x=0.0): + c, s = math.cos(math.radians(tilt_about_x)), math.sin(math.radians(tilt_about_x)) + self.g68_active = active + self.g68_offset = tuple(origin) + (0,) * 6 + self.g68_rotation = (1, 0, 0, 0, c, -s, 0, s, c) + self.g92_offset = (0,) * 9 + self.g5x_offset = (0,) * 9 + self.rotation_xy = 0.0 + + +class CtxStub: + """What WorkPlanePart is allowed to read, and a record of what it drew.""" + + class Prim: + def __init__(self): + self.calls = [] + self.points = [] + + def draw_lines(self, ctx, points, color, alpha=1.0): + self.calls.append((len(points), tuple(color), alpha)) + self.points.append(points) + + def __init__(self, canon, stat=None): + self.canon = canon + self.stat = stat + self.colors = glcanon.GlCanonDraw.colors + self.prim = self.Prim() + + @staticmethod + def to_internal_units(pos): + return [v / 25.4 for v in pos[:3]] + list(pos[3:]) + + +def drawn(*lines, stat=None, **kw): + """Parse, then draw the planes through the stub, and hand back the stub.""" + canon = run(program(*lines, **kw)) + canon.calc_extents() + ctx = CtxStub(canon, stat) + glcanon_scene.WorkPlanePart().draw(ctx) + return ctx + + +class WorkPlanePartTest(unittest.TestCase): + def test_draws_the_outline_the_grid_and_the_axes(self): + ctx = drawn(TILTED, "G1 X-30 Y-20 Z5", "G1 X30 Y20 Z-4") + n = glcanon_scene.WorkPlanePart.SUBDIVISIONS + # four edges, the inner lines each way, the origin cross, then one + # line per axis + self.assertEqual([c[0] for c in ctx.prim.calls], [8, 4 * (n - 1), 6, 2, 2, 2]) + self.assertEqual(ctx.prim.calls[0][1], tuple(glcanon_scene.WORKPLANE_COLOR)) + self.assertEqual([c[1] for c in ctx.prim.calls[3:]], + [tuple(glcanon.GlCanonDraw.colors[k]) for k in ('axis_x', 'axis_y', 'axis_z')]) + + def test_draws_nothing_without_planes(self): + for canon in (None, object(), run(program("G1 X1"))): + ctx = CtxStub(canon) + glcanon_scene.WorkPlanePart().draw(ctx) + self.assertEqual(ctx.prim.calls, []) + + def test_the_active_plane_is_the_program_record(self): + # the canon counts in inches, status in this machine's mm + stat = StatStub(active=1, origin=(0, 40 * 25.4, 20 * 25.4), tilt_about_x=30) + canon = run(program(TILTED, "G1 X-30 Y-20 Z5", + "G68.2 X0 Y0 Z0 I0 J0 K0", "G1 X1 Y1 Z1")) + canon.calc_extents() + ctx = CtxStub(canon, stat) + self.assertIs(glcanon_scene.active_workplane(ctx), canon.workplanes[0]) + glcanon_scene.WorkPlanePart().draw(ctx) + # the other plane in the plane colour, then the active one in its own + colors = [c[1] for c in ctx.prim.calls] + self.assertEqual(colors[0], tuple(glcanon_scene.WORKPLANE_COLOR)) + self.assertEqual(colors[6], tuple(glcanon_scene.WORKPLANE_ACTIVE_COLOR)) + self.assertEqual(len(ctx.prim.calls), 12) + + def test_an_mdi_plane_is_drawn_on_its_own(self): + canon = run(program("G1 X1")) + canon.calc_extents() + ctx = CtxStub(canon, StatStub(active=1, origin=(10, 0, 0))) + plane = glcanon_scene.active_workplane(ctx) + self.assertEqual(plane.lineno, -1) + self.assertAlmostEqual(plane.origin[0], 10 / 25.4) + self.assertFalse(plane.has_moves) + glcanon_scene.WorkPlanePart().draw(ctx) + self.assertEqual(len(ctx.prim.calls), 6) + self.assertEqual(ctx.prim.calls[0][1], tuple(glcanon_scene.WORKPLANE_ACTIVE_COLOR)) + + def test_no_plane_in_effect(self): + ctx = drawn("G68.2 X0 Y0 Z0 I0 J0 K0", stat=StatStub(active=0)) + self.assertIsNone(glcanon_scene.active_workplane(ctx)) + self.assertEqual(len(ctx.prim.calls), 6) + + +if __name__ == '__main__': + unittest.main() From cfc2cc305d9c19f80eb15e58a0a9de11b23a3c4b Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 11 Aug 2026 22:59:37 +1000 Subject: [PATCH 20/77] switchkins: separate the dispatch from rtapi_app_main() switchkins.c owned rtapi_app_main(), so a module could only use it by having no main of its own, which ruled out halcompile components: the switchable kinematics in hal/components each carry a private copy of the dispatch. Move rtapi_app_main(), rtapi_app_exit() and the coordinates= and sparm= parameters to switchkins_main.c and give switchkins.c one entry point, switchkinsInit(comp_id, kp, coordinates), which counts and validates the registered types, creates the pins and starts on type 0. The caller owns the component. The types switchkinsSetup() supplies now go through switchkinsRegister() like any others, so one registration path checks every type and a double registration is refused. The eight existing modules add switchkins_main.o to their objects and are otherwise untouched. --- src/Makefile | 8 +++ src/emc/kinematics/switchkins.c | 61 ++++++------------ src/emc/kinematics/switchkins.h | 11 +++- src/emc/kinematics/switchkins_main.c | 94 ++++++++++++++++++++++++++++ 4 files changed, 130 insertions(+), 44 deletions(-) create mode 100644 src/emc/kinematics/switchkins_main.c diff --git a/src/Makefile b/src/Makefile index 3fab1c5dae2..25cfb823090 100644 --- a/src/Makefile +++ b/src/Makefile @@ -1180,6 +1180,7 @@ genhexkins-objs += libposemath/_posemath.o genhexkins-objs += $(MATHSTUB) genhexkins-objs += emc/kinematics/kins_util.o genhexkins-objs += emc/kinematics/switchkins.o +genhexkins-objs += emc/kinematics/switchkins_main.o genhexkins-objs += $(USERKFUNCS) obj-m += genserkins.o @@ -1189,6 +1190,7 @@ genserkins-objs += libposemath/gomath.o genserkins-objs += $(MATHSTUB) genserkins-objs += emc/kinematics/kins_util.o genserkins-objs += emc/kinematics/switchkins.o +genserkins-objs += emc/kinematics/switchkins_main.o genserkins-objs += $(USERKFUNCS) obj-m += xyzac-trt-kins.o @@ -1196,6 +1198,7 @@ xyzac-trt-kins-objs := emc/kinematics/xyzac-trt-kins.o xyzac-trt-kins-objs += emc/kinematics/trtfuncs.o xyzac-trt-kins-objs += emc/kinematics/kins_util.o xyzac-trt-kins-objs += emc/kinematics/switchkins.o +xyzac-trt-kins-objs += emc/kinematics/switchkins_main.o xyzac-trt-kins-objs += $(USERKFUNCS) obj-m += xyzbc-trt-kins.o @@ -1203,6 +1206,7 @@ xyzbc-trt-kins-objs := emc/kinematics/xyzbc-trt-kins.o xyzbc-trt-kins-objs += emc/kinematics/trtfuncs.o xyzbc-trt-kins-objs += emc/kinematics/kins_util.o xyzbc-trt-kins-objs += emc/kinematics/switchkins.o +xyzbc-trt-kins-objs += emc/kinematics/switchkins_main.o xyzbc-trt-kins-objs += $(USERKFUNCS) obj-m += scarakins.o @@ -1211,6 +1215,7 @@ scarakins-objs += libposemath/_posemath.o scarakins-objs += $(MATHSTUB) scarakins-objs += emc/kinematics/kins_util.o scarakins-objs += emc/kinematics/switchkins.o +scarakins-objs += emc/kinematics/switchkins_main.o scarakins-objs += $(USERKFUNCS) obj-m += pumakins.o @@ -1219,6 +1224,7 @@ pumakins-objs += libposemath/_posemath.o pumakins-objs += $(MATHSTUB) pumakins-objs += emc/kinematics/kins_util.o pumakins-objs += emc/kinematics/switchkins.o +pumakins-objs += emc/kinematics/switchkins_main.o pumakins-objs += $(USERKFUNCS) obj-m += three21kins.o @@ -1227,6 +1233,7 @@ three21kins-objs += libposemath/_posemath.o three21kins-objs += $(MATHSTUB) three21kins-objs += emc/kinematics/kins_util.o three21kins-objs += emc/kinematics/switchkins.o +three21kins-objs += emc/kinematics/switchkins_main.o three21kins-objs += $(USERKFUNCS) obj-m += 5axiskins.o @@ -1235,6 +1242,7 @@ obj-m += 5axiskins.o 5axiskins-objs += $(MATHSTUB) 5axiskins-objs += emc/kinematics/kins_util.o 5axiskins-objs += emc/kinematics/switchkins.o +5axiskins-objs += emc/kinematics/switchkins_main.o 5axiskins-objs += $(USERKFUNCS) #---------------------------------------------------------------- diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index f832246a926..a9fa9027cd5 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -27,7 +27,6 @@ * Using modules must supply function: switchkinsSetup() */ #include -#include #include #include #include @@ -422,12 +421,6 @@ int kinematicsTypeFlags(int ktype) 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"); - EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsType); @@ -443,33 +436,24 @@ EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); EXPORT_SYMBOL(switchkinsDeclare); EXPORT_SYMBOL(kinematicsTypeFlags); EXPORT_SYMBOL(switchkinsRegisterJacobian); -MODULE_LICENSE("GPL"); +EXPORT_SYMBOL(switchkinsInit); -static int comp_id; //********************************************************************* -int rtapi_app_main(void) +// The caller owns the hal component: it does hal_init() before this and +// hal_ready() after it. Every switchkins-type must be registered by +// now. +int switchkinsInit(const int comp_id, + kparms* ksetup_parms, + const char* coordinates) { - int i,res,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; + int identities; + 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() @@ -485,7 +469,7 @@ int rtapi_app_main(void) } } - // 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; } } @@ -544,11 +528,8 @@ int rtapi_app_main(void) emsg = "incomplete switchkins-type"; goto error; } - comp_id = hal_init(kp.kinsname); - if(comp_id < 0) goto error; - swdata = hal_malloc(sizeof(struct swdata)); - if (!swdata) goto error; + if (!swdata) {emsg = "hal_malloc fail"; goto error;} for (i=0; i < kins_count; i++) { res += hal_pin_new_bool(comp_id, HAL_OUT, &(swdata->kinstype_is[i]), @@ -562,8 +543,8 @@ int rtapi_app_main(void) res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_a, 0.0, "skgui.a"); res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_b, 0.0, "skgui.b"); res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_c, 0.0, "skgui.c"); - if (res) {emsg = "hal pin create fail";goto error;} } + if (res) {emsg = "hal pin create fail"; goto error;} switchkins_type = 0; // startup with default type kinematicsSwitch(switchkins_type); @@ -574,14 +555,10 @@ int rtapi_app_main(void) ksetups[i](comp_id,coordinates,&kp); } - hal_ready(comp_id); return 0; error: rtapi_print_msg(RTAPI_MSG_ERR, "\nSwitchkins FAIL %s:<%s>\n",kp.kinsname,emsg); - hal_exit(comp_id); return -1; -} // rtapi_app_main() - -void rtapi_app_exit(void) { hal_exit(comp_id); } +} // switchkinsInit() diff --git a/src/emc/kinematics/switchkins.h b/src/emc/kinematics/switchkins.h index 77caca90629..f6b8aa06e4f 100644 --- a/src/emc/kinematics/switchkins.h +++ b/src/emc/kinematics/switchkins.h @@ -34,14 +34,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 @@ -84,4 +84,11 @@ typedef int (*KJ)(const double *joint, // 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); + +// create the hal pins and start on type 0; the caller owns the hal +// component and does hal_init() before and hal_ready() after +extern int switchkinsInit(const int comp_id, + kparms* ksetup_parms, + const char* coordinates + ); #endif // } diff --git a/src/emc/kinematics/switchkins_main.c b/src/emc/kinematics/switchkins_main.c new file mode 100644 index 00000000000..4a4cc05153c --- /dev/null +++ b/src/emc/kinematics/switchkins_main.c @@ -0,0 +1,94 @@ +/* + Copyright 2019 Dewey Garrett + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. +*/ + +/* switchkins_main.c provides rtapi_app_main() for kinematics modules +* built around switchkins.c. A module that gets its rtapi_app_main() +* from somewhere else (a halcompile component, for instance) links +* switchkins.c alone and calls switchkinsInit() itself. +* +* Using modules must supply function: switchkinsSetup() +*/ +#include +#include +#include + +#include "switchkins.h" + +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); +static char *sparm; +RTAPI_MP_STRING(sparm, "switchkins module-specific parameter"); + +MODULE_LICENSE("GPL"); + +static int comp_id = -1; + +int rtapi_app_main(void) +{ + kparms kp; + KS ksetup[3] = {NULL}; + KF kfwd[3] = {NULL}; + KI kinv[3] = {NULL}; + int i; + + // defaults prior to switchkinsSetup() call + kp.kinsname = NULL; + kp.halprefix = NULL; + kp.required_coordinates = ""; + kp.max_joints = 0; // Setup must supply + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; + kp.gui_kinstype = -1; // negative means: not used + + kp.sparm = sparm; // module parm passed to kins + + // switchkinsSetup() provides types 0,1,2 and may also call + // switchkinsRegister() for any others + if (switchkinsSetup(&kp, + &ksetup[0], &ksetup[1], &ksetup[2], + &kfwd[0], &kfwd[1], &kfwd[2], + &kinv[0], &kinv[1], &kinv[2])) { + rtapi_print_msg(RTAPI_MSG_ERR,"\nSwitchkins FAIL:\n"); + return -1; + } + + // the types switchkinsSetup() supplied go in by the same route as + // any other, so that providing one twice is caught + for (i=0; i < 3; i++) { + if (!ksetup[i] && !kfwd[i] && !kinv[i]) { continue; } + if (switchkinsRegister(i, ksetup[i], kfwd[i], kinv[i])) { return -1; } + } + + if (!kp.kinsname) { + rtapi_print_msg(RTAPI_MSG_ERR,"\nSwitchkins FAIL:\n"); + return -1; + } + + comp_id = hal_init(kp.kinsname); + if (comp_id < 0) return comp_id; + + if (switchkinsInit(comp_id, &kp, coordinates)) { + hal_exit(comp_id); + return -1; + } + + hal_ready(comp_id); + return 0; +} // rtapi_app_main() + +void rtapi_app_exit(void) { hal_exit(comp_id); } From 4b6eed4248f932a7e4b7e3776f32a0e76f8d8af5 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 12 Aug 2026 00:17:36 +1000 Subject: [PATCH 21/77] switchkins: let halcompile components use the switchkins core millturn, xyzab_tdr_kins, xyzacb_trsrn and xyzbca_trsrn each carried a copy of the switchkins dispatch, a private switchkins_type, a hand-written kinematicsSwitch() and a setup that had to hal_set_unready() the component again. Now that the dispatch is separate from the main program a component links it and calls switchkinsInit() from EXTRA_SETUP(). Two build changes: the generated per-comp .mak takes a -extra-objs list, and switchkins.h is copied to ../include and installed so resolves from a generated source. Each of the four registers its types and calls switchkinsInit(); their identity type comes from kins_util.c, which gets them the coordinates= parameter, and a bad motion.switchkins-type is refused instead of stranding the module. Pin names are unchanged except millturn's unused in/out template pins. The four sim configs give the same positions through the same MDI sequence as before, in every kinematics type. --- docs/src/motion/switchkins.adoc | 78 +++- src/Makefile | 1 + src/emc/kinematics/switchkins.h | 8 +- src/hal/components/Submakefile | 13 +- src/hal/components/millturn.comp | 269 ++++------- src/hal/components/xyzab_tdr_kins.comp | 384 ++++++--------- src/hal/components/xyzacb_trsrn.comp | 621 +++++++++++------------- src/hal/components/xyzbca_trsrn.comp | 623 +++++++++++-------------- 8 files changed, 869 insertions(+), 1128 deletions(-) diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index fe1fd05eed9..cfa230f5f43 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 @@ -419,6 +423,10 @@ 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 @@ -466,19 +474,14 @@ protocols. == Code Notes Kinematic modules providing switchkins functionality are linked to -the switchkins.o object (switchkins.c) that provides the module -'main' program (rtapi_app_main()) and related functions. This -'main' program reads (optional) module command-line parameters -(coordinates, sparm) and passes them to the module-provided -function switchkinsSetup(). - -The switchkinsSetup() function identifies kinstype-specific setup -routines and the functions for forward an inverse calculation for -each kinstype (0,1,2) and sets a number of configuration -settings. - -A module can provide further kinstypes by calling -switchkinsRegister() from within switchkinsSetup(), once per +the switchkins.o object (switchkins.c). It provides +kinematicsForward(), kinematicsInverse(), kinematicsSwitch() and +the rest of the kinematics interface, dispatching each call to the +kinstype currently selected, and it creates the HAL pins common to +all switchkins modules. It does not provide the module 'main' +program, so a module can get that from wherever suits it. + +A kinstype is supplied by calling switchkinsRegister(), once per kinstype: ---- @@ -521,16 +524,53 @@ 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. -After calling switchkinsSetup(), rtapi_app_main() checks the -supplied parameters, creates a HAL component, and then invokes -the setup routine identified for each kinstype. +When every kinstype is registered, the module calls: + +---- +int switchkinsInit(const int comp_id, kparms* kp, const char* coordinates); +---- + +which checks the supplied parameters, creates the HAL pins, selects +kinstype 0, and then invokes the setup routine registered for each +kinstype. The caller owns the HAL component: it does hal_init() +before switchkinsInit() and hal_ready() after it. Each kinstype setup routine can (optionally) create HAL pins and set them to default values. A setup routine is called once per kinstype it is registered for, so a routine used for two -kinstypes must not create the same pin twice. When all setup -routines finish, rtapi_app_main() issues hal_ready() for the -component to complete creation of the module. +kinstypes must not create the same pin twice. + +=== Module main program + +A module written as a plain C file links switchkins_main.o +(switchkins_main.c) for its rtapi_app_main(). That 'main' program +reads the (optional) module command-line parameters (coordinates, +sparm) and passes them to the module-provided function +switchkinsSetup(): + +---- +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2); +---- + +which identifies the setup, forward and inverse routines for +kinstypes 0,1,2 and sets a number of configuration settings. Those +three are registered for the module, so it can supply further +kinstypes by calling switchkinsRegister() itself, and registering +one that switchkinsSetup() has already filled in is the same error +as any other duplicate. + +A module written as a halcompile component gets rtapi_app_main() +from halcompile instead. It registers its kinstypes and calls +switchkinsInit() from its EXTRA_SETUP() routine, which halcompile +runs after hal_init() and before hal_ready(). The component names +the objects it needs in hal/components/Submakefile: + +---- +millturn-extra-objs := emc/kinematics/switchkins.o emc/kinematics/kins_util.o +---- === Outline diff --git a/src/Makefile b/src/Makefile index 25cfb823090..ae7fe8df6d3 100644 --- a/src/Makefile +++ b/src/Makefile @@ -402,6 +402,7 @@ SRCHEADERS := \ hal/drivers/mesa-hostmot2/hostmot2-serial.h \ emc/linuxcnc.h \ emc/kinematics/kinematics.h \ + emc/kinematics/switchkins.h \ emc/nml_intf/emcmotcfg.h \ emc/ini/axis_kinds.hh \ emc/ini/inifile.hh \ diff --git a/src/emc/kinematics/switchkins.h b/src/emc/kinematics/switchkins.h index f6b8aa06e4f..c7114262403 100644 --- a/src/emc/kinematics/switchkins.h +++ b/src/emc/kinematics/switchkins.h @@ -1,10 +1,10 @@ /* ** License GPL Version 2 */ -#ifndef SWITCHKINS_H // { -#define SWITCHKINS_H +#ifndef __LINUXCNC_SWITCHKINS_H +#define __LINUXCNC_SWITCHKINS_H -#include +#include "kinematics.h" //SWITCHKINS_MAX_TYPES (max number of types a module may provide) //is in kinematics.h: motion and the NML status channel need it too @@ -91,4 +91,4 @@ extern int switchkinsInit(const int comp_id, kparms* ksetup_parms, const char* coordinates ); -#endif // } +#endif diff --git a/src/hal/components/Submakefile b/src/hal/components/Submakefile index 62c9940cfbb..d97a0baf2f1 100644 --- a/src/hal/components/Submakefile +++ b/src/hal/components/Submakefile @@ -94,11 +94,20 @@ endif obj-m += $(patsubst hal/drivers/%.comp, %.o, $(patsubst hal/components/%.comp, %.o, $(COMPS) $(COMP_DRIVERS))) +# A component that links objects besides its own names them here as +# -extra-objs. The list is expanded when the .mak is written, +# so it has to be defined in this file (which the .mak depends on). +SWITCHKINS_OBJS := emc/kinematics/switchkins.o emc/kinematics/kins_util.o +millturn-extra-objs := $(SWITCHKINS_OBJS) +xyzab_tdr_kins-extra-objs := $(SWITCHKINS_OBJS) +xyzacb_trsrn-extra-objs := $(SWITCHKINS_OBJS) +xyzbca_trsrn-extra-objs := $(SWITCHKINS_OBJS) + objects/%.mak: %.comp hal/components/Submakefile $(ECHO) "Creating $(notdir $@)" @mkdir -p $(dir $@) - $(Q)echo $(notdir $*)-objs := objects/$*.o > $@.tmp - $(Q)echo ../rtlib/$(notdir $*)$(MODULE_EXT): objects/rtobjects/$*.o >> $@.tmp + $(Q)echo $(notdir $*)-objs := objects/$*.o $($(notdir $*)-extra-objs) > $@.tmp + $(Q)echo ../rtlib/$(notdir $*)$(MODULE_EXT): objects/rtobjects/$*.o $(addprefix objects/rt,$($(notdir $*)-extra-objs)) >> $@.tmp $(Q)mv -f $@.tmp $@ objects/%.c: %.comp ../bin/halcompile diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index abb217f7a3a..161e8abebba 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,7 +26,7 @@ chapter (docs/src/motion/switchkins.txt) """; // The fpin pin is not accessible in kinematics functions. -// Use EXTRA_SETUP() for pins and params used by kinematics. +// 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; @@ -36,20 +35,10 @@ 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 @@ -60,121 +49,30 @@ FUNCTION(fdemo) { fpin_set(fpin + 1); } -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; -#define HAL_PREFIX "millturn" - int res=0; - - // inherit comp_id from rtapi_main() - if (comp_id < 0) 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; - 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(kinematicsJacobian); -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) +// the turn kinematics need no hal pins of their own +static int turnKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { - switchkins_type = new_switchkins_type; - rtapi_print("kinematicsSwitch(): type=%d\n",switchkins_type); - // create case structure for switchable kinematics - switch (switchkins_type) { - case 0: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE0\n"); - hal_set_bool(haldata->kinstype_is_0, 1); - hal_set_bool(haldata->kinstype_is_1, 0); - break; - case 1: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 1); - break; - default: rtapi_print_msg(RTAPI_MSG_ERR, - "kinematicsSwitch:BAD VALUE <%d>\n", - switchkins_type); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_0, 0); - return -1; // FAIL - } - return 0; // ok -} - -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() + (void)comp_id; + (void)coords; + (void)kp; + return 0; +} // turnKinematicsSetup() -static bool is_ready=0; -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int turnKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; (void)iflags; - static bool gave_msg; - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - case 0: - pos->tran.x = j[0]; - pos->tran.y = j[1]; - pos->tran.z = j[2]; - pos->a = j[3]; - break; - case 1: - pos->tran.x = j[2]; - pos->tran.y = -j[1]; - pos->tran.z = j[0]; - pos->a = j[3]; - break; - } + + pos->tran.x = j[2]; + pos->tran.y = -j[1]; + pos->tran.z = j[0]; + pos->a = j[3]; + // unused coordinates: pos->b = 0; pos->c = 0; @@ -182,77 +80,70 @@ int kinematicsForward(const double *j, pos->v = 0; pos->w = 0; - if (hal_get_ui32(haldata->in) && !is_ready && !gave_msg) { - rtapi_print_msg(RTAPI_MSG_ERR, - "%s the 'in' pin not echoed until Inverse called\n", - __FILE__); - gave_msg=1; - } return 0; -} // kinematicsForward() +} // turnKinematicsForward() -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int turnKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { (void)iflags; (void)fflags; - is_ready = 1; // Inverse is not called until homed for KINEMATICS_BOTH - - // Update the kinematic joints specified by the - // [KINS]JOINTS setting (4 required for this template). - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - case 0: - j[0] = pos->tran.x; - j[1] = pos->tran.y; - j[2] = pos->tran.z; - j[3] = pos->a; - break; - case 1: - j[2] = pos->tran.x; - j[1] = -pos->tran.y; - j[0] = pos->tran.z; - j[3] = pos->a; - break; - } - //example hal pin update (homing reqd before kinematicsInverse) - hal_set_ui32(haldata->out, hal_get_ui32(haldata->in)); //dereference - //read from param example: *haldata->out = hal_get_real(haldata->param_rw); + j[0] = pos->tran.z; + j[1] = -pos->tran.y; + j[2] = pos->tran.x; + j[3] = pos->a; return 0; -} // kinematicsInverse() +} // turnKinematicsInverse() -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int turnKinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { - int r, c; + 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 derivative of kinematicsInverse() for each type: which joint - // follows which pose coordinate, and in which sense - switch (switchkins_type) { - case 0: - jac[0][0] = 1; - jac[1][1] = 1; - jac[2][2] = 1; - jac[3][3] = 1; - break; - case 1: - jac[2][0] = 1; - jac[1][1] = -1; - jac[0][2] = 1; - jac[3][3] = 1; - break; + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } } + // the derivative of turnKinematicsInverse(): 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; -} // kinematicsJacobian() +} // turnKinematicsJacobian() + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what switchkinsInit() expects +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + kp.kinsname = "millturn"; + kp.halprefix = "millturn"; + kp.required_coordinates = "xyza"; + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; + kp.gui_kinstype = -1; + kp.sparm = NULL; + kp.max_joints = strlen(kp.required_coordinates); + + if (switchkinsRegister(0, identityKinematicsSetup, + identityKinematicsForward, + identityKinematicsInverse)) { return -1; } + if (switchkinsRegister(1, turnKinematicsSetup, + turnKinematicsForward, + turnKinematicsInverse)) { return -1; } + if (switchkinsRegisterJacobian(1, turnKinematicsJacobian)) { return -1; } + + if (switchkinsDeclare(0, KINSTYPE_IDENTITY)) { return -1; } + + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index 2ee61e6a9b8..0387d73c85a 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 @@ -31,6 +30,7 @@ chapter (docs/src/motion/switchkins.txt) """; pin out si32 dummy=0"one pin needed to satisfy halcompile requirement"; + option extra_setup; license "GPL"; @@ -38,127 +38,61 @@ author "David Mueller"; ;; #include -#include -static struct haldata { +#include - // Declare hal pin pointers used for xyzab_tdr kinematics: +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); + +static struct haldata { hal_real_t tool_offset_z; hal_real_t x_offset; hal_real_t z_offset; hal_real_t x_rot_point; hal_real_t y_rot_point; hal_real_t z_rot_point; +} *tdrdata; - //Declare hal pin pointers used for switchable kinematics - hal_bool_t kinstype_is_0; - hal_bool_t kinstype_is_1; -} *haldata; - -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; -#define HAL_PREFIX "xyzab_tdr_kins" - int res=0; - // inherit comp_id from rtapi_main() - if (comp_id < 0) 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; - 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(kinematicsJacobian); -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 int tdrKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { - 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 res = 0; + (void)coords; + + tdrdata = hal_malloc(sizeof(*tdrdata)); + if (!tdrdata) return -1; + + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->tool_offset_z, 0.0, + "%s.tool-offset-z", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->x_offset, 0.0, + "%s.x-offset", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->z_offset, 0.0, + "%s.z-offset", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->x_rot_point, 0.0, + "%s.x-rot-point", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->y_rot_point, 0.0, + "%s.y-rot-point", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->z_rot_point, 0.0, + "%s.z-rot-point", kp->halprefix); + if (res) return -1; -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) + return 0; +} // tdrKinematicsSetup() +static int tdrKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; (void)iflags; - double x_rot_point = hal_get_real(haldata->x_rot_point); - double y_rot_point = hal_get_real(haldata->y_rot_point); - double z_rot_point = hal_get_real(haldata->z_rot_point); + double x_rot_point = hal_get_real(tdrdata->x_rot_point); + double y_rot_point = hal_get_real(tdrdata->y_rot_point); + double z_rot_point = hal_get_real(tdrdata->z_rot_point); - double dz = hal_get_real(haldata->z_offset); - double dt = hal_get_real(haldata->tool_offset_z); + double dz = hal_get_real(tdrdata->z_offset); + double dt = hal_get_real(tdrdata->tool_offset_z); // substitutions as used in mathematical documentation // including degree -> radians angle conversion @@ -168,39 +102,22 @@ int kinematicsForward(const double *j, double cb = cos(j[4]*TO_RAD); // used to be consistent with math in the documentation - double px = 0; - double py = 0; - double pz = 0; - - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - case 0: // ====================== IDENTITY kinematics FORWARD ==================== - pos->tran.x = j[0]; - pos->tran.y = j[1]; - pos->tran.z = j[2]; - pos->a = j[3]; - pos->b = j[4]; - break; - case 1: // ========================= TCP kinematics FORWARD ====================== - px = j[0] - x_rot_point; - py = j[1] - y_rot_point; - pz = j[2] - z_rot_point - dt; - - pos->tran.x = cb*px + sb*pz - + x_rot_point; - - pos->tran.y = sa*sb*px + ca*py - cb*sa*pz + sa*dz - + y_rot_point; - - pos->tran.z = - ca*sb*px + sa*py + ca*cb*pz - ca*dz - + z_rot_point + dz + dt; - - pos->a = j[3]; - pos->b = j[4]; - pos->c = j[5]; - break; - } + double px = j[0] - x_rot_point; + double py = j[1] - y_rot_point; + double pz = j[2] - z_rot_point - dt; + + pos->tran.x = cb*px + sb*pz + + x_rot_point; + + pos->tran.y = sa*sb*px + ca*py - cb*sa*pz + sa*dz + + y_rot_point; + + pos->tran.z = - ca*sb*px + sa*py + ca*cb*pz - ca*dz + + z_rot_point + dz + dt; + + pos->a = j[3]; + pos->b = j[4]; + // unused coordinates: pos->c = 0; pos->u = 0; @@ -208,22 +125,22 @@ int kinematicsForward(const double *j, pos->w = 0; return 0; -} // kinematicsForward() +} // tdrKinematicsForward() -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int tdrKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { (void)iflags; (void)fflags; - double x_rot_point = hal_get_real(haldata->x_rot_point); - double y_rot_point = hal_get_real(haldata->y_rot_point); - double z_rot_point = hal_get_real(haldata->z_rot_point); + double x_rot_point = hal_get_real(tdrdata->x_rot_point); + double y_rot_point = hal_get_real(tdrdata->y_rot_point); + double z_rot_point = hal_get_real(tdrdata->z_rot_point); - double dx = hal_get_real(haldata->x_offset); - double dz = hal_get_real(haldata->z_offset); - double dt = hal_get_real(haldata->tool_offset_z); + double dx = hal_get_real(tdrdata->x_offset); + double dz = hal_get_real(tdrdata->z_offset); + double dt = hal_get_real(tdrdata->tool_offset_z); // substitutions as used in mathematical documentation // including degree -> radians angle conversion @@ -233,53 +150,38 @@ int kinematicsInverse(const EmcPose * pos, double cb = cos(pos->b*TO_RAD); // used to be consistent with math in the documentation - double qx = 0; - double qy = 0; - double qz = 0; - - switch (switchkins_type) { - case 0:// ====================== IDENTITY kinematics INVERSE ===================== - j[0] = pos->tran.x; - j[1] = pos->tran.y; - j[2] = pos->tran.z; - j[3] = pos->a; - j[4] = pos->b; - break; - case 1: // ========================= TCP kinematics INVERSE ====================== - qx = pos->tran.x - x_rot_point - dx; - qy = pos->tran.y - y_rot_point; - qz = pos->tran.z - z_rot_point - dz - dt; - - j[0] = cb*qx + sa*sb*qy - ca*sb*qz + cb*dx - sb*dz - + x_rot_point; - - j[1] = ca*qy + sa*qz - + y_rot_point; - - j[2] = sb*qx - sa*cb*qy + ca*cb*qz + sb*dx + cb*dz - + z_rot_point + dt; - - j[3] = pos->a; - j[4] = pos->b; - break; - } + double qx = pos->tran.x - x_rot_point - dx; + double qy = pos->tran.y - y_rot_point; + double qz = pos->tran.z - z_rot_point - dz - dt; + + j[0] = cb*qx + sa*sb*qy - ca*sb*qz + cb*dx - sb*dz + + x_rot_point; + + j[1] = ca*qy + sa*qz + + y_rot_point; + + j[2] = sb*qx - sa*cb*qy + ca*cb*qz + sb*dx + cb*dz + + z_rot_point + dt; + + j[3] = pos->a; + j[4] = pos->b; return 0; -} // kinematicsInverse() +} // tdrKinematicsInverse() -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int tdrKinematicsJacobian(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 = 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 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 x_rot_point = hal_get_real(tdrdata->x_rot_point); + double y_rot_point = hal_get_real(tdrdata->y_rot_point); + double z_rot_point = hal_get_real(tdrdata->z_rot_point); + double dx = hal_get_real(tdrdata->x_offset); + double dz = hal_get_real(tdrdata->z_offset); + double dt = hal_get_real(tdrdata->tool_offset_z); double sa = sin(pos->a*TO_RAD); double ca = cos(pos->a*TO_RAD); double sb = sin(pos->b*TO_RAD); @@ -287,43 +189,61 @@ int kinematicsJacobian(const double *j, 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; + int R, C; - for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { - for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } } - switch (switchkins_type) { - case 0: // ====================== IDENTITY kinematics JACOBIAN ==================== - jac[0][0] = 1; - jac[1][1] = 1; - jac[2][2] = 1; - jac[3][3] = 1; - jac[4][4] = 1; - break; - case 1: // ========================= TCP kinematics JACOBIAN ====================== - // the TCP inverse above 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; - break; - } + // tdrKinematicsInverse() 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; -} // kinematicsJacobian() +} // tdrKinematicsJacobian() + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what switchkinsInit() expects +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + kp.kinsname = "xyzab_tdr_kins"; + kp.halprefix = "xyzab_tdr_kins"; + kp.required_coordinates = "xyzab"; + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; + kp.gui_kinstype = -1; + kp.sparm = NULL; + kp.max_joints = strlen(kp.required_coordinates); + + if (switchkinsRegister(0, identityKinematicsSetup, + identityKinematicsForward, + identityKinematicsInverse)) { return -1; } + if (switchkinsRegister(1, tdrKinematicsSetup, + tdrKinematicsForward, + tdrKinematicsInverse)) { return -1; } + if (switchkinsRegisterJacobian(1, tdrKinematicsJacobian)) { return -1; } + + if (switchkinsDeclare(0, KINSTYPE_IDENTITY)) { return -1; } + if (switchkinsDeclare(1, KINSTYPE_PRIMARY)) { 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 67fd97715a8..45a8a9af4f9 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,8 +19,11 @@ author "David Mueller"; ;; #include -#include +#include + +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); static struct haldata { // these should be parameters really but we want to be able to @@ -36,133 +44,50 @@ static struct haldata { // Declare hal pin pointers used for xyzacb_trsrn kinematics: hal_real_t tool_offset_z; - - //Declare hal pin pointers used for switchable kinematics - hal_bool_t kinstype_is_0; - hal_bool_t kinstype_is_1; - hal_bool_t kinstype_is_2; } *haldata; - -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(kinematicsJacobian); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsToolFrame); -EXPORT_SYMBOL(kinematicsWorkFrame); - -static rtapi_u32 switchkins_type; - -int kinematicsSwitchable() {return 1;} - -int kinematicsTypeFlags(int ktype) +// the pins are shared by the TCP and TOOL kinematics; the TOOL type has +// no setup routine of its own +static int trsrnKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { - switch (ktype) { - case 0: return KINSTYPE_IDENTITY; - case 1: return KINSTYPE_PRIMARY; - default: return -1; - } -} - + int res = 0; + (void)coords; + haldata = hal_malloc(sizeof(struct haldata)); + if (!haldata) return -1; + + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_pivot, 0.0, "%s.y-pivot" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_rot_axis, 0.0, "%s.y-rot-axis" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle" ,kp->halprefix); + if (res) return -1; -int kinematicsSwitch(int new_switchkins_type) -{ - switchkins_type = new_switchkins_type; - rtapi_print("kinematicsSwitch(): type=%d\n",switchkins_type); - // create case structure for switchable kinematics - switch (switchkins_type) { - case 0: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE0\n"); - hal_set_bool(haldata->kinstype_is_0, 1); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_2, 0); - break; - case 1: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 1); - hal_set_bool(haldata->kinstype_is_2, 0); - break; - case 2: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_2, 1); - break; - default: rtapi_print_msg(RTAPI_MSG_ERR, - "kinematicsSwitch:BAD VALUE <%d>\n", - switchkins_type); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_2, 0); - return -1; // FAIL - } - return 0; // ok -} + return 0; +} // trsrnKinematicsSetup() -KINEMATICS_TYPE kinematicsType() +static int toolKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { - return KINEMATICS_BOTH; // set as required - // Note: If kinematics are identity, using KINEMATICS_BOTH - // may be used in order to allow a gui to display - // joint values in preview prior to homing -} // kinematicsType() - - -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) + (void)comp_id; + (void)coords; + (void)kp; + return 0; // pins created by trsrnKinematicsSetup() +} // toolKinematicsSetup() + +// tool_kins==0: TCP kinematics, using the current spindle joint positions +// tool_kins==1: TOOL kinematics, using the angles calculated in remap.py +static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) { - (void)fflags; - (void)iflags; - // START of custom variable declaration for Forward kinematics // geometric offsets of the universal spindle head as defined in the ini file @@ -207,20 +132,7 @@ int kinematicsForward(const double *j, // END of custom variable declaration for Forward kinematics - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - case 0: // ========================= IDENTITY kinematics FORWARD ====================== - pos->tran.x = j[0]; - pos->tran.y = j[1]; - pos->tran.z = j[2]; - pos->a = j[3]; - pos->b = j[4]; - pos->c = j[5]; - - break; - - case 1: // ========================= TCP kinematics FORWARD + if (!tool_kins) { // ========================= TCP kinematics FORWARD // in TCP we use the current positions of the spindle joints Ss = sin(j[4]*TO_RAD); Cs = cos(j[4]*TO_RAD); @@ -263,9 +175,7 @@ int kinematicsForward(const double *j, pos->b = j[4]; pos->c = j[5]; - break; - - case 2: // ========================= TOOL kinematics FORWARD + } else { // ========================= TOOL kinematics FORWARD // in TOOL kinematics we use the articulated joint positions from the TWP Ss = sin(theta_2*TO_RAD); Cs = cos(theta_2*TO_RAD); @@ -303,10 +213,6 @@ int kinematicsForward(const double *j, pos->a = j[3]; pos->b = j[4]; pos->c = j[5]; - - break; - - } // unused coordinates: pos->u = 0; @@ -314,98 +220,30 @@ 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 double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (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[4]*TO_RAD); - double Cs = cos(j[4]*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] = {{Cs, -Cv*Ss, Sv*Ss}, - {Cv*Ss, r, t}, - {-Sv*Ss, t, 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() + (void)iflags; + return trsrnForward(j, pos, 0); +} // tcpKinematicsForward() -int kinematicsWorkFrame(const double *j, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int toolKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (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}}; - - 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; + return trsrnForward(j, pos, 1); +} // toolKinematicsForward() +static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) +{ // START of custom variable declaration for Forward kinematics // geometric offsets of the universal spindle head as defined in the ini file @@ -453,23 +291,7 @@ int kinematicsInverse(const EmcPose * pos, // END of custom variable declaration for Forward kinematics - // Update the kinematic joints specified by the - // [KINS]JOINTS setting (4 required for this template). - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - - case 0: // ========================= IDENTITY kinematics INVERSE ====================== - j[0] = pos->tran.x; - j[1] = pos->tran.y; - j[2] = pos->tran.z; - j[3] = pos->a; - j[4] = pos->b; - j[5] = pos->c; - - break; - - case 1: // ========================= TCP kinematics INVERSE + if (!tool_kins) { // ========================= TCP kinematics INVERSE // in TCP we use the current positions of the spindle joints Ss = sin(j[4]*TO_RAD); Cs = cos(j[4]*TO_RAD); @@ -506,9 +328,7 @@ int kinematicsInverse(const EmcPose * pos, j[4] = pos->b; j[5] = pos->c; - break; - - case 2: // ========================= TOOL kinematics INVERSE + } else { // ========================= TOOL kinematics INVERSE // in TOOL kinematics we use the articulated joint positions from the TWP Ss = sin(theta_2*TO_RAD); Cs = cos(theta_2*TO_RAD); @@ -550,38 +370,112 @@ int kinematicsInverse(const EmcPose * pos, j[3] = pos->a; j[4] = pos->b; j[5] = pos->c; + } + + return 0; +} // trsrnInverse() - break; +static int tcpKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + return trsrnInverse(pos, j, 0); +} // tcpKinematicsInverse() + +static int toolKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + return trsrnInverse(pos, j, 1); +} // toolKinematicsInverse() + +// The head answers in the convention already, so the native rotation +// registered with these frames is TOOL_FRAME_SPINDLE. +static int tcpKinematicsToolFrame(const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (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[4]*TO_RAD); + double Cs = cos(j[4]*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] = {{Cs, -Cv*Ss, Sv*Ss}, + {Cv*Ss, r, t}, + {-Sv*Ss, t, 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() -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int tcpKinematicsWorkFrame(const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + double Sw = sin(j[3]*TO_RAD); + double Cw = cos(j[3]*TO_RAD); + + // 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}}; + + 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 double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)j; (void)iflags; - // the same geometry as kinematicsInverse(), read the same way + // the same geometry as trsrnInverse(), read the same way 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 double Dt = hal_get_real(haldata->tool_offset_z); double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); - double Stc = sin(tc); - double Ctc = cos(tc); // The TCP inverse reads the rotary angles from its joint argument, // where the machine is, and its own pose words for the same angles @@ -589,105 +483,152 @@ int kinematicsJacobian(const double *j, // 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 = 0, Cs = 0, Sp = 0, Cp = 0; - double CvSs = 0, SvSs = 0, r = 0, s = 0, t = 0; + 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 = 0, dr = 0, ds = 0, dt_ = 0, dCvSs = 0, dSvSs = 0; - double dSp = 0, dCp = 0; + 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; - double Ay, Az; // the two lever arms the table turns about + // 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; } } - switch (switchkins_type) { + // 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() - case 0: // ========================= IDENTITY kinematics JACOBIAN ==================== - for (R = 0; R < 6; R++) { jac[R][R] = 1; } - break; +static int toolKinematicsJacobian(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; - case 1: // ========================= TCP kinematics JACOBIAN - Ss = sin(pos->b*TO_RAD); - Cs = cos(pos->b*TO_RAD); - Sp = sin(pos->c*TO_RAD); - Cp = cos(pos->c*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); + // the head angles come from pins, so the inverse is linear in the pose + // and the rows are its coefficients + 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 - dSs = Cs*TO_RAD; - dr = -Ss*Cv*Cv*TO_RAD; - ds = -Ss*Sv*Sv*TO_RAD; - dt_ = Sv*Cv*Ss*TO_RAD; - dCvSs = Cv*dSs; - dSvSs = Sv*dSs; - dSp = Cp*TO_RAD; - dCp = -Sp*TO_RAD; - - Ay = Dray + Dy + Ly - Qy; - Az = Draz + Dt + Lz - Qz; - - // 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; - break; - - case 2: // ========================= TOOL kinematics JACOBIAN - // the head angles come from pins, so the inverse is linear in - // the pose and the rows are its coefficients - 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); + 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; - 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); + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } + } - 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); + 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); - jac[2][0] = -(Ctc*SvSs - Stc*t); - jac[2][1] = (Stc*SvSs + Ctc*t); - jac[2][2] = s; + 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); - jac[3][3] = 1; - jac[4][4] = 1; - jac[5][5] = 1; - break; - } + jac[2][0] = -(Ctc*SvSs - Stc*t); + jac[2][1] = (Stc*SvSs + Ctc*t); + jac[2][2] = s; + + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; return 0; -} // kinematicsJacobian() +} // toolKinematicsJacobian() + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what switchkinsInit() expects +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + kp.kinsname = "xyzacb_trsrn"; + kp.halprefix = "xyzacb_trsrn_kins"; + kp.required_coordinates = "xyzabc"; + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; + kp.gui_kinstype = -1; + kp.sparm = NULL; + kp.max_joints = strlen(kp.required_coordinates); + + if (switchkinsRegister(0, identityKinematicsSetup, + identityKinematicsForward, + identityKinematicsInverse)) { return -1; } + if (switchkinsRegister(1, trsrnKinematicsSetup, + tcpKinematicsForward, + tcpKinematicsInverse)) { return -1; } + if (switchkinsRegister(2, toolKinematicsSetup, + toolKinematicsForward, + toolKinematicsInverse)) { return -1; } + if (switchkinsRegisterFrames(1, tcpKinematicsWorkFrame, + tcpKinematicsToolFrame, + &TOOL_FRAME_SPINDLE)) { return -1; } + if (switchkinsRegisterJacobian(1, tcpKinematicsJacobian)) { return -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 + if (switchkinsRegisterFrames(2, identityKinematicsWorkFrame, + identityKinematicsToolFrame, + &TOOL_FRAME_SPINDLE)) { return -1; } + if (switchkinsRegisterJacobian(2, toolKinematicsJacobian)) { return -1; } + + if (switchkinsDeclare(0, KINSTYPE_IDENTITY)) { return -1; } + if (switchkinsDeclare(1, KINSTYPE_PRIMARY)) { 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 10126165eb1..75e6f7ea0f4 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,8 +19,11 @@ author "David Mueller"; ;; #include -#include +#include + +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); static struct haldata { // these should be parameters really but we want to be able to @@ -36,135 +44,50 @@ static struct haldata { // Declare hal pin pointers used for xyzbca_trsrn kinematics: hal_real_t tool_offset_z; - - //Declare hal pin pointers used for switchable kinematics - hal_bool_t kinstype_is_0; - hal_bool_t kinstype_is_1; - hal_bool_t kinstype_is_2; } *haldata; - -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(kinematicsJacobian); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsToolFrame); -EXPORT_SYMBOL(kinematicsWorkFrame); - -static rtapi_u32 switchkins_type; - -int kinematicsSwitchable() {return 1;} - -int kinematicsTypeFlags(int ktype) +// the pins are shared by the TCP and TOOL kinematics; the TOOL type has +// no setup routine of its own +static int trsrnKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { - switch (ktype) { - case 0: return KINSTYPE_IDENTITY; - case 1: return KINSTYPE_PRIMARY; - default: return -1; - } -} - + int res = 0; + (void)coords; + haldata = hal_malloc(sizeof(struct haldata)); + if (!haldata) return -1; + + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_pivot, 0.0, "%s.x-pivot", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_rot_axis, 0.0, "%s.x-rot-axis", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle", kp->halprefix); + if (res) return -1; -int kinematicsSwitch(int new_switchkins_type) -{ - switchkins_type = new_switchkins_type; - rtapi_print("kinematicsSwitch(): type=%d\n",switchkins_type); - // create case structure for switchable kinematics - switch (switchkins_type) { - case 0: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE0\n"); - hal_set_bool(haldata->kinstype_is_0, 1); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_2, 0); - break; - case 1: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 1); - hal_set_bool(haldata->kinstype_is_2, 0); - break; - case 2: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_2, 1); - break; - default: rtapi_print_msg(RTAPI_MSG_ERR, - "kinematicsSwitch:BAD VALUE <%d>\n", - switchkins_type); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_2, 0); - return -1; // FAIL - } - return 0; // ok -} + return 0; +} // trsrnKinematicsSetup() -KINEMATICS_TYPE kinematicsType() +static int toolKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { - return KINEMATICS_BOTH; // set as required - // Note: If kinematics are identity, using KINEMATICS_BOTH - // may be used in order to allow a gui to display - // joint values in preview prior to homing -} // kinematicsType() - - -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) + (void)comp_id; + (void)coords; + (void)kp; + return 0; // pins created by trsrnKinematicsSetup() +} // toolKinematicsSetup() + +// tool_kins==0: TCP kinematics, using the current spindle joint positions +// tool_kins==1: TOOL kinematics, using the angles calculated in remap.py +static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) { - (void)fflags; - (void)iflags; - // START of custom variable declaration for Forward kinematics // geometric offsets of the universal spindle head as defined in the ini file @@ -210,20 +133,7 @@ int kinematicsForward(const double *j, // END of custom variable declaration for Forward kinematics - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - case 0: // ========================= IDENTITY kinematics FORWARD ====================== - pos->tran.x = j[0]; - pos->tran.y = j[1]; - pos->tran.z = j[2]; - pos->a = j[3]; - pos->b = j[4]; - pos->c = j[5]; - - break; - - case 1: // ========================= TCP kinematics FORWARD + if (!tool_kins) { // ========================= TCP kinematics FORWARD // in TCP we use the current positions of the spindle joints Ss = sin(j[3]*TO_RAD); Cs = cos(j[3]*TO_RAD); @@ -270,9 +180,7 @@ int kinematicsForward(const double *j, pos->b = j[4]; pos->c = j[5]; - break; - - case 2: // ========================= TOOL kinematics FORWARD + } else { // ========================= TOOL kinematics FORWARD // in TOOL kinematics we use the articulated joint positions from the TWP Ss = sin(theta_2*TO_RAD); Cs = cos(theta_2*TO_RAD); @@ -310,10 +218,6 @@ int kinematicsForward(const double *j, pos->a = j[3]; pos->b = j[4]; pos->c = j[5]; - - break; - - } // unused coordinates: pos->u = 0; @@ -321,98 +225,30 @@ 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 double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (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() + (void)iflags; + return trsrnForward(j, pos, 0); +} // tcpKinematicsForward() -int kinematicsWorkFrame(const double *j, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int toolKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (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; + return trsrnForward(j, pos, 1); +} // toolKinematicsForward() +static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) +{ // START of custom variable declaration for Forward kinematics // geometric offsets of the universal spindle head as defined in the ini file @@ -458,23 +294,7 @@ int kinematicsInverse(const EmcPose * pos, // END of custom variable declaration for Forward kinematics - // Update the kinematic joints specified by the - // [KINS]JOINTS setting (4 required for this template). - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - - case 0: // ========================= IDENTITY kinematics INVERSE ====================== - j[0] = pos->tran.x; - j[1] = pos->tran.y; - j[2] = pos->tran.z; - j[3] = pos->a; - j[4] = pos->b; - j[5] = pos->c; - - break; - - case 1: // ========================= TCP kinematics INVERSE + if (!tool_kins) { // ========================= TCP kinematics INVERSE // in TCP we use the current positions of the spindle joints Ss = sin(j[3]*TO_RAD); Cs = cos(j[3]*TO_RAD); @@ -511,9 +331,7 @@ int kinematicsInverse(const EmcPose * pos, j[4] = pos->b; j[5] = pos->c; - break; - - case 2: // ========================= TOOL kinematics INVERSE + } else { // ========================= TOOL kinematics INVERSE // in TOOL kinematics we use the articulated joint positions from the TWP Ss = sin(theta_2*TO_RAD); Cs = cos(theta_2*TO_RAD); @@ -555,38 +373,112 @@ int kinematicsInverse(const EmcPose * pos, j[3] = pos->a; j[4] = pos->b; j[5] = pos->c; + } + + return 0; +} // trsrnInverse() - break; +static int tcpKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + return trsrnInverse(pos, j, 0); +} // tcpKinematicsInverse() + +static int toolKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + return trsrnInverse(pos, j, 1); +} // toolKinematicsInverse() + +// The head answers in the convention already, so the native rotation +// registered with these frames is TOOL_FRAME_SPINDLE. +static int tcpKinematicsToolFrame(const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (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; + + // 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() -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int tcpKinematicsWorkFrame(const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (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 double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)j; (void)iflags; - // the same geometry as kinematicsInverse(), read the same way + // the same geometry as trsrnInverse(), read the same way 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 double Dt = hal_get_real(haldata->tool_offset_z); double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); - double Stc = sin(tc); - double Ctc = cos(tc); // The TCP inverse reads the rotary angles from its joint argument, // where the machine is, and its own pose words for the same angles @@ -594,105 +486,152 @@ int kinematicsJacobian(const double *j, // 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 = 0, Cs = 0, Sp = 0, Cp = 0; - double CvSs = 0, SvSs = 0, r = 0, s = 0, t = 0; + 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 = 0, dr = 0, ds = 0, dt_ = 0, dCvSs = 0, dSvSs = 0; - double dSp = 0, dCp = 0; + 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; - double Ax, Az; // the two lever arms the table turns about + // 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; } } - switch (switchkins_type) { + // 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() - case 0: // ========================= IDENTITY kinematics JACOBIAN ==================== - for (R = 0; R < 6; R++) { jac[R][R] = 1; } - break; +static int toolKinematicsJacobian(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; - case 1: // ========================= TCP kinematics JACOBIAN - Ss = sin(pos->a*TO_RAD); - Cs = cos(pos->a*TO_RAD); - Sp = sin(pos->c*TO_RAD); - Cp = cos(pos->c*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); + // the head angles come from pins, so the inverse is linear in the pose + // and the rows are its coefficients + 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 - dSs = Cs*TO_RAD; - dr = -Ss*Cv*Cv*TO_RAD; - ds = -Ss*Sv*Sv*TO_RAD; - dt_ = Sv*Cv*Ss*TO_RAD; - dCvSs = Cv*dSs; - dSvSs = Sv*dSs; - dSp = Cp*TO_RAD; - dCp = -Sp*TO_RAD; - - Ax = Drax + Dx + Lx - Qx; - Az = Draz + Dt + Lz - Qz; - - // 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; - break; - - case 2: // ========================= TOOL kinematics JACOBIAN - // the head angles come from pins, so the inverse is linear in - // the pose and the rows are its coefficients - 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); + 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; - 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); + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } + } - 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[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[2][0] = (Stc*SvSs + Ctc*t); - jac[2][1] = (Ctc*SvSs - Stc*t); - jac[2][2] = s; + 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[3][3] = 1; - jac[4][4] = 1; - jac[5][5] = 1; - break; - } + 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; -} // kinematicsJacobian() +} // toolKinematicsJacobian() + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what switchkinsInit() expects +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + kp.kinsname = "xyzbca_trsrn"; + kp.halprefix = "xyzbca_trsrn_kins"; + kp.required_coordinates = "xyzabc"; + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; + kp.gui_kinstype = -1; + kp.sparm = NULL; + kp.max_joints = strlen(kp.required_coordinates); + + if (switchkinsRegister(0, identityKinematicsSetup, + identityKinematicsForward, + identityKinematicsInverse)) { return -1; } + if (switchkinsRegister(1, trsrnKinematicsSetup, + tcpKinematicsForward, + tcpKinematicsInverse)) { return -1; } + if (switchkinsRegister(2, toolKinematicsSetup, + toolKinematicsForward, + toolKinematicsInverse)) { return -1; } + if (switchkinsRegisterFrames(1, tcpKinematicsWorkFrame, + tcpKinematicsToolFrame, + &TOOL_FRAME_SPINDLE)) { return -1; } + if (switchkinsRegisterJacobian(1, tcpKinematicsJacobian)) { return -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 + if (switchkinsRegisterFrames(2, identityKinematicsWorkFrame, + identityKinematicsToolFrame, + &TOOL_FRAME_SPINDLE)) { return -1; } + if (switchkinsRegisterJacobian(2, toolKinematicsJacobian)) { return -1; } + + if (switchkinsDeclare(0, KINSTYPE_IDENTITY)) { return -1; } + if (switchkinsDeclare(1, KINSTYPE_PRIMARY)) { return -1; } + + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() From cce6a4512e4b07cf5f9ebc0908e3de6e842903a0 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 12 Aug 2026 11:05:02 +1000 Subject: [PATCH 22/77] switchkins: add an out-of-tree module template An out-of-tree module could not reach the switchkins implementation, so it reimplemented kinematicsSwitch() and the kinstype.is-N pins or did without. switchkinscomp.comp is the template: it sets TOPDIR to a source tree and includes switchkins.c and kins_util.c, as tpcomp.comp and homecomp.comp reach their sources, then registers its kinstypes and calls switchkinsInit() from EXTRA_SETUP(). The sources compile into the module, so no ABI is involved. Like tpcomp it is not built in tree, since it has no kinematics until TOPDIR is set. Renamed and loaded as [KINS]KINEMATICS it homes, switches to its example type and back, and rejects a type it does not have. --- docs/src/hal/components.adoc | 1 + docs/src/motion/switchkins.adoc | 48 +++++++ src/hal/components/Submakefile | 8 +- src/hal/components/switchkinscomp.comp | 167 +++++++++++++++++++++++++ 4 files changed, 220 insertions(+), 4 deletions(-) create mode 100644 src/hal/components/switchkinscomp.comp diff --git a/docs/src/hal/components.adoc b/docs/src/hal/components.adoc index ab9155e8d17..6a45f3547cb 100644 --- a/docs/src/hal/components.adoc +++ b/docs/src/hal/components.adoc @@ -335,6 +335,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/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index cfa230f5f43..44b67ef6e88 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -430,6 +430,12 @@ configs/sim/axis/vismach/ . == 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 @@ -447,6 +453,47 @@ 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), point its TOPDIR +at a LinuxCNC source tree, and replace the example kinstype with the +real kinematics: + +[source,c] +---- +#define TOPDIR /home/myname/linuxcnc-dev +// ... +#include USE_TOPDIR(src/emc/kinematics/switchkins.c) +#include USE_TOPDIR(src/emc/kinematics/kins_util.c) +---- + +The module registers each of its kinstypes and calls switchkinsInit() +from EXTRA_SETUP(), which halcompile runs after hal_init() and before +hal_ready(). See <> for both +calls. + +---- +$ halcompile --install user_switchkins.comp +---- + +[source,ini] +---- +[KINS] +KINEMATICS = user_switchkins +JOINTS = 3 +---- + +[NOTE] +The switchkins sources are compiled into the module, so it is built +against one source tree and has to be rebuilt when that tree changes. + == Warnings Unexpected behavior can result if a G-code program is inadvertently @@ -471,6 +518,7 @@ 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 diff --git a/src/hal/components/Submakefile b/src/hal/components/Submakefile index d97a0baf2f1..8ad4ee1740e 100644 --- a/src/hal/components/Submakefile +++ b/src/hal/components/Submakefile @@ -1,5 +1,5 @@ ifneq ($(KERNELRELEASE),) -COMPS := $(filter-out %/tpcomp.comp, $(patsubst $(BASEPWD)/%,%,$(wildcard $(BASEPWD)/hal/components/*.comp $(BASEPWD)/hal/drivers/*.comp))) +COMPS := $(filter-out %/tpcomp.comp %/switchkinscomp.comp, $(patsubst $(BASEPWD)/%,%,$(wildcard $(BASEPWD)/hal/components/*.comp $(BASEPWD)/hal/drivers/*.comp))) include $(patsubst %.comp, $(BASEPWD)/objects/%.mak, $(COMPS)) else CONVERTERS := \ @@ -32,8 +32,8 @@ CONVERTERS := \ conv_u64_s32.comp \ conv_u64_u32.comp \ conv_u64_s64.comp -COMPS := $(filter-out hal/components/tpcomp.comp, $(sort $(wildcard hal/components/*.comp) $(addprefix hal/components/, $(CONVERTERS)))) -COMP_MANPAGES := $(patsubst hal/components/%.comp, ../docs/build/man/man9/%.9, $(COMPS)) ../docs/build/man/man9/tpcomp.9 +COMPS := $(filter-out hal/components/tpcomp.comp hal/components/switchkinscomp.comp, $(sort $(wildcard hal/components/*.comp) $(addprefix hal/components/, $(CONVERTERS)))) +COMP_MANPAGES := $(patsubst hal/components/%.comp, ../docs/build/man/man9/%.9, $(COMPS)) ../docs/build/man/man9/tpcomp.9 ../docs/build/man/man9/switchkinscomp.9 ifeq ($(BUILD_SYS),uspace) COMP_DRIVERS += hal/drivers/serport.comp COMP_DRIVERS += hal/drivers/mesa_7i65.comp @@ -58,7 +58,7 @@ endif # wildcard that mixes hal/components and hal/drivers, so deriving the adoc # targets from it there yields hal/drivers/*.comp entries that fail the # hal/components/%.comp static pattern rule. -COMP_MANPAGE_ADOCS := $(patsubst hal/components/%.comp, objects/man/man9/%.9.adoc, $(COMPS)) objects/man/man9/tpcomp.9.adoc +COMP_MANPAGE_ADOCS := $(patsubst hal/components/%.comp, objects/man/man9/%.9.adoc, $(COMPS)) objects/man/man9/tpcomp.9.adoc objects/man/man9/switchkinscomp.9.adoc COMP_DRIVER_MANPAGE_ADOCS := $(patsubst hal/drivers/%.comp, objects/man/man9/%.9.adoc, $(COMP_DRIVERS)) # Extract adoc from .comp via halcompile --adoc. Only needs Python + diff --git a/src/hal/components/switchkinscomp.comp b/src/hal/components/switchkinscomp.comp new file mode 100644 index 00000000000..1d9fbdbe6d7 --- /dev/null +++ b/src/hal/components/switchkinscomp.comp @@ -0,0 +1,167 @@ +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 switchkinscomp.comp is not usable until modified for the +user environment. To create a runnable switchkinscomp module, the +file must be edited to supply a valid '#define TOPDIR' pointing at a +LinuxCNC source tree. + +To avoid updates that overwrite switchkinscomp.comp, best practice is +to rename the file and its component name (example: +*user_switchkins.comp* creates module: *user_switchkins*). + +The (renamed) component can be built and installed with halcompile +and then used as the kinematics module by inifile setting: + +[source,ini] +---- +[KINS] +KINEMATICS = user_switchkins +JOINTS = 3 +---- + +*Note:* If using a deb install: + +1. halcompile is 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; +;; + +//===================================================================== +/* To use the switchkins implementation from a local git src tree: +** set TOPDIR to the git tree top directory +** (Edit 'myname' as required) +*/ + +//#define TOPDIR /home/myname/linuxcnc-dev + +#ifdef TOPDIR // { + +#define STR(s) #s +#define XSTR(s) STR(s) +#define USE_TOPDIR(b) XSTR(TOPDIR/b) + +// switchkins.c provides kinematicsForward(), kinematicsInverse(), +// kinematicsSwitch() and the rest of the kinematics interface, and +// dispatches each call to the currently selected switchkins-type. +// kins_util.c provides the identity kinematics and the coordinates +// letters-to-joints mapping they use. +#include USE_TOPDIR(src/emc/kinematics/switchkins.c) +#include USE_TOPDIR(src/emc/kinematics/kins_util.c) + +#else +#error No TOPDIR defined, skeleton component provides no kinematics functions. +#endif // } +//===================================================================== + +// module parameter naming the joint order for the identity type +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); + +//--------------------------------------------------------------------- +// Example switchkins-type. A setup routine creating whatever hal pins +// the kinematics need, plus a forward and an inverse routine. Replace +// the arithmetic with the real kinematics. + +static struct { + hal_real_t x_offset; +} *mydata; + +static int myKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) +{ + (void)coords; // this type does not use the coordinates mapping + + mydata = hal_malloc(sizeof(*mydata)); + if (!mydata) return -1; + + return hal_pin_new_real(comp_id, HAL_IN, &mydata->x_offset, 0.0, + "%s.x-offset", kp->halprefix); +} // myKinematicsSetup() + +static int myKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)fflags; + (void)iflags; + + pos->tran.x = j[0] + hal_get_real(mydata->x_offset); + pos->tran.y = j[1]; + pos->tran.z = j[2]; + + // unused coordinates: + pos->a = pos->b = pos->c = 0; + pos->u = pos->v = pos->w = 0; + + return 0; +} // myKinematicsForward() + +static int myKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + + j[0] = pos->tran.x - hal_get_real(mydata->x_offset); + j[1] = pos->tran.y; + j[2] = pos->tran.z; + + return 0; +} // myKinematicsInverse() + +//--------------------------------------------------------------------- +// rtapi_app_main() is supplied by halcompile, which calls hal_init() +// before EXTRA_SETUP() and hal_ready() after it. That is what +// switchkinsInit() expects, so the switchkins-types are registered and +// the implementation started from here. + +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + kp.kinsname = "switchkinscomp"; // must agree with the module name + kp.halprefix = "switchkinscomp"; // hal pin names + kp.required_coordinates = "xyz"; + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; // set bit N if type N iterates + kp.gui_kinstype = -1; // negative means: not used + kp.sparm = NULL; + kp.max_joints = strlen(kp.required_coordinates); + + // switchkins-type 0 is the startup default. Types run from 0 to + // SWITCHKINS_MAX_TYPES-1 with no gaps. + if (switchkinsRegister(0, identityKinematicsSetup, + identityKinematicsForward, + identityKinematicsInverse)) { return -1; } + if (switchkinsRegister(1, myKinematicsSetup, + myKinematicsForward, + myKinematicsInverse)) { return -1; } + + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() From 032ef12ca96c8c9b9c85d6ca136db96c04fa1fae Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 12 Aug 2026 21:02:46 +1000 Subject: [PATCH 23/77] kins: include switchkins.h as an exported header The kinematics modules are users of switchkins, not part of it, so they take the header the way any other user would. switchkins.c and switchkins_main.c keep the quoted form, being the source itself. --- src/emc/kinematics/5axiskins.c | 3 +-- src/emc/kinematics/genhexkins.c | 3 +-- src/emc/kinematics/genserkins.c | 2 +- src/emc/kinematics/pumakins.c | 3 +-- src/emc/kinematics/scarakins.c | 3 +-- src/emc/kinematics/switchkins.c | 1 - src/emc/kinematics/three21kins.c | 3 +-- src/emc/kinematics/xyzac-trt-kins.c | 2 +- src/emc/kinematics/xyzbc-trt-kins.c | 2 +- 9 files changed, 8 insertions(+), 14 deletions(-) diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index 6e7923d58f4..b2510e094ef 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -59,9 +59,8 @@ #include #include #include -#include -#include "switchkins.h" +#include static struct haldata { hal_real_t pivot_length; diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index c634611ee62..0964fa7d088 100644 --- a/src/emc/kinematics/genhexkins.c +++ b/src/emc/kinematics/genhexkins.c @@ -110,10 +110,9 @@ #include #include #include -#include /* these decls, KINEMATICS_FORWARD_FLAGS */ #include "genhexkins.h" -#include "switchkins.h" +#include static struct haldata { hal_real_t basex[NUM_STRUTS]; diff --git a/src/emc/kinematics/genserkins.c b/src/emc/kinematics/genserkins.c index 94a325cbcf6..be71f33ffc5 100644 --- a/src/emc/kinematics/genserkins.c +++ b/src/emc/kinematics/genserkins.c @@ -42,7 +42,7 @@ frame-larger-than: #include #include "genserkins.h" -#include "switchkins.h" +#include //-7 is system defined -3 ok, -4 ok, -5 ok,-6 ok (mm system) #undef GO_REAL_EPSILON diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index aace42d7b37..b63cbb64c5a 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -20,10 +20,9 @@ #include #include #include -#include #include "pumakins.h" -#include "switchkins.h" +#include struct haldata { hal_real_t a2, a3, d3, d4, d6; diff --git a/src/emc/kinematics/scarakins.c b/src/emc/kinematics/scarakins.c index db3ad15b737..e2dedb241b6 100644 --- a/src/emc/kinematics/scarakins.c +++ b/src/emc/kinematics/scarakins.c @@ -19,9 +19,8 @@ #include #include #include -#include -#include "switchkins.h" +#include static struct scara_data { hal_real_t d1, d2, d3, d4, d5, d6; diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index a9fa9027cd5..472394cefdd 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -29,7 +29,6 @@ #include #include #include -#include #include "switchkins.h" diff --git a/src/emc/kinematics/three21kins.c b/src/emc/kinematics/three21kins.c index 30c7f938e15..5cce5796a30 100644 --- a/src/emc/kinematics/three21kins.c +++ b/src/emc/kinematics/three21kins.c @@ -2,9 +2,8 @@ #include #include #include -#include -#include "switchkins.h" +#include /* default values for ar2 robot */ #define DEFAULT_THREE21_A1 64.2 diff --git a/src/emc/kinematics/xyzac-trt-kins.c b/src/emc/kinematics/xyzac-trt-kins.c index b8bb47bbc1f..b6b35538f25 100644 --- a/src/emc/kinematics/xyzac-trt-kins.c +++ b/src/emc/kinematics/xyzac-trt-kins.c @@ -15,7 +15,7 @@ #include #include -#include "switchkins.h" +#include int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, diff --git a/src/emc/kinematics/xyzbc-trt-kins.c b/src/emc/kinematics/xyzbc-trt-kins.c index 7b61a69e301..401311e4398 100644 --- a/src/emc/kinematics/xyzbc-trt-kins.c +++ b/src/emc/kinematics/xyzbc-trt-kins.c @@ -15,7 +15,7 @@ #include #include -#include "switchkins.h" +#include int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, From 8d6a4e858b3c137db6e600e327036aa2f7a27c03 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 12 Aug 2026 23:51:35 +1000 Subject: [PATCH 24/77] switchkins: install the implementation as source for out-of-tree modules A realtime module cannot link a library, so an out-of-tree kinematics module has to compile the switchkins implementation itself. Asking it for the path to a source tree, as the template did, leaves anybody on a deb install with nothing to point at. Install switchkins.c and kins_util.c into share/linuxcnc, the way mesa_modbus.c.tmpl already is, and put that directory on the realtime include path. The template then reads #include #include and builds as it stands. --- .gitignore | 2 ++ debian/linuxcnc-uspace-dev.install | 2 ++ docs/src/motion/switchkins.adoc | 17 +++++++------ src/Makefile | 2 ++ src/Makefile.modinc.in | 4 +-- src/emc/kinematics/Submakefile | 13 ++++++++++ src/hal/components/switchkinscomp.comp | 34 ++++++++------------------ 7 files changed, 41 insertions(+), 33 deletions(-) diff --git a/.gitignore b/.gitignore index ae679ab6090..82df1b00474 100644 --- a/.gitignore +++ b/.gitignore @@ -20,6 +20,8 @@ share/menus/CNC.menu share/desktop-directories/linuxcnc-cnc.directory share/desktop-directories/linuxcnc-ref.directory share/desktop-directories/linuxcnc-doc.directory +share/linuxcnc/switchkins.c +share/linuxcnc/kins_util.c src/modules.order /configs/*/emc.nml !/configs/common/emc.nml diff --git a/debian/linuxcnc-uspace-dev.install b/debian/linuxcnc-uspace-dev.install index f7e58d261ea..f55d2a4e230 100644 --- a/debian/linuxcnc-uspace-dev.install +++ b/debian/linuxcnc-uspace-dev.install @@ -3,3 +3,5 @@ usr/include/linuxcnc usr/lib/liblinuxcnc.a usr/lib/*.so usr/share/linuxcnc/Makefile.modinc +usr/share/linuxcnc/switchkins.c +usr/share/linuxcnc/kins_util.c diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 44b67ef6e88..14fb1b7aabb 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -462,18 +462,21 @@ it gets the kinematics switching, the 'kinstype.is-N' pins, the without reimplementing any of them. The template is src/hal/components/switchkinscomp.comp. Copy and -rename it (both the file and the component name), point its TOPDIR -at a LinuxCNC source tree, and replace the example kinstype with the -real kinematics: +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] ---- -#define TOPDIR /home/myname/linuxcnc-dev -// ... -#include USE_TOPDIR(src/emc/kinematics/switchkins.c) -#include USE_TOPDIR(src/emc/kinematics/kins_util.c) +#include +#include ---- +A realtime module cannot link a library, so the implementation arrives +as source: switchkins.c and kins_util.c are installed beside the +headers, in share/linuxcnc, and halcompile already looks there. With +a deb install they come from the linuxcnc-dev package. + The module registers each of its kinstypes and calls switchkinsInit() from EXTRA_SETUP(), which halcompile runs after hal_init() and before hal_ready(). See <> for both diff --git a/src/Makefile b/src/Makefile index ae7fe8df6d3..f70576335a7 100644 --- a/src/Makefile +++ b/src/Makefile @@ -785,6 +785,8 @@ ifeq ($(BUILD_GUI),yes) $(FILE) ../share/gtksourceview-4/language-specs/*.lang $(DESTDIR)$(datadir)/gtksourceview-4/language-specs/ endif + $(FILE) ../src/emc/kinematics/switchkins.c ../src/emc/kinematics/kins_util.c $(DESTDIR)$(prefix)/share/linuxcnc/ + install-kernel-indep: install-python install-python: install-dirs $(DIR) $(DESTDIR)$(SITEPY) $(DESTDIR)$(SITEPY)/rs274 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/Submakefile b/src/emc/kinematics/Submakefile index 77085c21c2e..7e2f2d84b4b 100644 --- a/src/emc/kinematics/Submakefile +++ b/src/emc/kinematics/Submakefile @@ -33,3 +33,16 @@ $(RDELTAMODULE): $(call TOOBJS, $(RDELTAMODULESRCS)) $(ECHO) Linking python module $(notdir $@) $(CXX) $(LDFLAGS) -shared -o $@ $^ $(BOOST_PYTHON_LIB) PYTARGETS += $(RDELTAMODULE) + +# The switchkins implementation is shipped as source, since a realtime module +# cannot link a library, so a module built out of tree includes it the way the +# in-tree ones link it. +EMCKINEMATICSSRCS = \ + ../share/linuxcnc/switchkins.c \ + ../share/linuxcnc/kins_util.c + +$(EMCKINEMATICSSRCS): ../share/linuxcnc/%.c: ./emc/kinematics/%.c + $(ECHO) Copying switchkins source $(notdir $@) + $(Q)cp -f $< $@ + +TARGETS += $(EMCKINEMATICSSRCS) diff --git a/src/hal/components/switchkinscomp.comp b/src/hal/components/switchkinscomp.comp index 1d9fbdbe6d7..7e90edc380f 100644 --- a/src/hal/components/switchkinscomp.comp +++ b/src/hal/components/switchkinscomp.comp @@ -12,10 +12,10 @@ 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 switchkinscomp.comp is not usable until modified for the -user environment. To create a runnable switchkinscomp module, the -file must be edited to supply a valid '#define TOPDIR' pointing at a -LinuxCNC source tree. +The example builds as it stands, its type 1 being an X offset to +replace with the kinematics wanted. The switchkins implementation is +installed as source alongside the headers, so nothing needs a path to +a LinuxCNC source tree. To avoid updates that overwrite switchkinscomp.comp, best practice is to rename the file and its component name (example: @@ -33,7 +33,8 @@ JOINTS = 3 *Note:* If using a deb install: -1. halcompile is provided by the deb package linuxcnc-dev +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 @@ -49,30 +50,15 @@ option extra_setup; ;; //===================================================================== -/* To use the switchkins implementation from a local git src tree: -** set TOPDIR to the git tree top directory -** (Edit 'myname' as required) -*/ - -//#define TOPDIR /home/myname/linuxcnc-dev - -#ifdef TOPDIR // { - -#define STR(s) #s -#define XSTR(s) STR(s) -#define USE_TOPDIR(b) XSTR(TOPDIR/b) - // switchkins.c provides kinematicsForward(), kinematicsInverse(), // kinematicsSwitch() and the rest of the kinematics interface, and // dispatches each call to the currently selected switchkins-type. // kins_util.c provides the identity kinematics and the coordinates -// letters-to-joints mapping they use. -#include USE_TOPDIR(src/emc/kinematics/switchkins.c) -#include USE_TOPDIR(src/emc/kinematics/kins_util.c) +// letters-to-joints mapping they use. Both are installed with the +// headers, so halcompile finds them with no path of your own. -#else -#error No TOPDIR defined, skeleton component provides no kinematics functions. -#endif // } +#include +#include //===================================================================== // module parameter naming the joint order for the identity type From e60ef3e9eabb16d01a860a7766c9b1c407777937 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 25 Aug 2026 01:03:33 +1000 Subject: [PATCH 25/77] canon: stop printing on every kinematics switch gcodemodule.cc got a raw printf when SELECT_KINS_TYPE was added, so the preview printed a line for every G12.1 and G13.1 in the program. It is the only live printf in the file, every other one having been commented out, and the neighbouring canon stubs are empty. Make this one empty too. saicanon.cc had the same printf. There it should report, since saicanon exists to echo the canonical commands, but through the same macro as the rest of the file so it lands in the canon output with a line number and the argument rather than beside it on stdout. --- src/emc/rs274ngc/gcodemodule.cc | 8 +------- src/emc/sai/saicanon.cc | 5 +---- 2 files changed, 2 insertions(+), 11 deletions(-) diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index 9c660152496..caa4103db1f 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -652,13 +652,7 @@ void ON_RESET() {} void PALLET_SHUTTLE() {} void SELECT_TOOL(int tool) {parse_state.selected_tool = tool;} void UPDATE_TAG(const StateTag& /*tag*/) {} -void SELECT_KINS_TYPE(int switchkins_type) -{ - (void)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;} diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index 6ee6ba4d4e9..61fa7637a9b 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -1233,8 +1233,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); } From 2d45cbd83f6a35cf940a26906770e2a34a649c76 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 10 Aug 2026 18:05:21 +1000 Subject: [PATCH 26/77] kinematics: evaluate a module outside RT by binding to its live pins A module's haldata is a struct of pin handles, and a handle is a pointer to the value cell hal_get_real() reads, so a second copy of the module outside realtime can point its haldata at cells carrying the values the RT instance reads and its forward and inverse work unmodified, on live values, at any pose. Per module: export nonrt_attach(), which asks a caller-supplied resolver for each input pin by name, runs the same coordinate parse setup runs and returns the forward and inverse; the maths is untouched and no header grows per module. The cells bound are pins of the caller's own component, connected to the signal the RT pin reads or to one made for the purpose, since a reference into another component's pin has that component's lifetime. Name lookup lives in the loader, userspace code linked against liblinuxcnchal; walking the HAL name space from an RT object is what the HAL isolation work removes. Input pins only, or the two copies write each other's state. Verified against the shared-memory snapshot design this replaces: identical caps from kinslimits, and a pin set by setp is seen without a motion thread running. --- debian/linuxcnc.install.in | 1 + src/Makefile | 3 + src/emc/kinematics/5axiskins.c | 63 ++- src/emc/kinematics/nonrt_kins.h | 95 +++++ src/emc/kinematics/trivkins.c | 15 + src/emc/kinematics_userspace/Submakefile | 1 + .../kinematics_userspace/kinematics_user.c | 381 ++++++++++++++++++ .../kinematics_userspace/kinematics_user.h | 197 +++++++++ src/emc/motion_planning/Submakefile | 46 +++ src/emc/motion_planning/jacobian.cc | 197 +++++++++ src/emc/motion_planning/jacobian.hh | 99 +++++ src/emc/motion_planning/joint_limits.cc | 358 ++++++++++++++++ src/emc/motion_planning/joint_limits.hh | 234 +++++++++++ src/emc/motion_planning/kinslimits.cc | 269 +++++++++++++ 14 files changed, 1950 insertions(+), 9 deletions(-) create mode 100644 src/emc/kinematics/nonrt_kins.h create mode 100644 src/emc/kinematics_userspace/Submakefile create mode 100644 src/emc/kinematics_userspace/kinematics_user.c create mode 100644 src/emc/kinematics_userspace/kinematics_user.h create mode 100644 src/emc/motion_planning/Submakefile create mode 100644 src/emc/motion_planning/jacobian.cc create mode 100644 src/emc/motion_planning/jacobian.hh create mode 100644 src/emc/motion_planning/joint_limits.cc create mode 100644 src/emc/motion_planning/joint_limits.hh create mode 100644 src/emc/motion_planning/kinslimits.cc 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/src/Makefile b/src/Makefile index f70576335a7..4a63f092109 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 \ @@ -403,6 +404,8 @@ SRCHEADERS := \ emc/linuxcnc.h \ emc/kinematics/kinematics.h \ emc/kinematics/switchkins.h \ + emc/kinematics/nonrt_kins.h \ + emc/kinematics_userspace/kinematics_user.h \ emc/nml_intf/emcmotcfg.h \ emc/ini/axis_kinds.hh \ emc/ini/inifile.hh \ diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index b2510e094ef..d4798261ff8 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -61,6 +61,7 @@ #include #include +#include static struct haldata { hal_real_t pivot_length; @@ -201,11 +202,20 @@ static int fiveaxis_KinematicsJacobian(const double *joints, jac); } // fiveaxis_KinematicsJacobian() -int fiveaxis_KinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) +// module constants, shared by switchkinsSetup() and nonrt_attach() +static void fiveaxis_kparms(kparms* kp) +{ + kp->kinsname = "5axiskins"; // !!! must agree with filename + kp->halprefix = "5axiskins"; // hal pin names + kp->required_coordinates = REQUIRED_COORDINATES; + kp->allow_duplicates = 1; + kp->max_joints = EMCMOT_MAX_JOINTS; +} + +// assign principal joint numbers from the coordinates string. +// No HAL involvement, so the non-RT path can use it too. +static int fiveaxis_map_joints(const char* coordinates, kparms* kp) { - int result=0; int i,jno; int axis_idx_for_jno[EMCMOT_MAX_JOINTS]; int minjoints = strlen(kp->required_coordinates); @@ -254,6 +264,20 @@ int fiveaxis_KinematicsSetup(const int comp_id, if (axis_idx_for_jno[jno] == 8) {if (JW == -1) JW=jno;} } + return 0; + +error: + return -1; +} // fiveaxis_map_joints() + +int fiveaxis_KinematicsSetup(const int comp_id, + const char* coordinates, + kparms* kp) +{ + int result=0; + + if (fiveaxis_map_joints(coordinates, kp)) goto error; + haldata = hal_malloc(sizeof(*haldata)); if(!haldata) goto error; @@ -286,11 +310,7 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { - kp->kinsname = "5axiskins"; // !!! must agree with filename - kp->halprefix = "5axiskins"; // hal pin names - kp->required_coordinates = REQUIRED_COORDINATES; - kp->allow_duplicates = 1; - kp->max_joints = EMCMOT_MAX_JOINTS; + fiveaxis_kparms(kp); if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); @@ -323,3 +343,28 @@ int switchkinsSetup(kparms* kp, return 0; } // switchkinsSetup() + +// Non-RT entry point: bind this copy of the module to the pins the +// running RT instance owns, then hand back the unmodified kinematics. +int nonrt_attach(const char* coordinates, nonrt_ops_t* ops, + nonrt_resolve_fn resolve, void* arg) +{ + static struct haldata nonrt_haldata; // private to this copy of the module + kparms kp = {0}; + + fiveaxis_kparms(&kp); + + haldata = &nonrt_haldata; + + if (nonrt_resolve_real(resolve, arg, &haldata->pivot_length, + "%s.pivot-length", kp.halprefix)) return -1; + + if (fiveaxis_map_joints(coordinates, &kp)) return -1; + + ops->forward = fiveaxis_KinematicsForward; + ops->inverse = fiveaxis_KinematicsInverse; + ops->is_identity = 0; + return 0; +} // nonrt_attach() + +EXPORT_SYMBOL(nonrt_attach); diff --git a/src/emc/kinematics/nonrt_kins.h b/src/emc/kinematics/nonrt_kins.h new file mode 100644 index 00000000000..ed564f21b6f --- /dev/null +++ b/src/emc/kinematics/nonrt_kins.h @@ -0,0 +1,95 @@ +/******************************************************************** + * Description: nonrt_kins.h + * Interface a kinematics module exports so that a non-RT caller can + * evaluate it. + * + * A trajectory planner needs forward and inverse kinematics at poses + * the machine has not reached yet, which means calling them outside + * the servo thread. A module opts in by exporting nonrt_attach(). + * + * The caller dlopens the module and calls nonrt_attach() once with + * the coordinates string and a resolver callback. The module names + * each of the pins it reads, keeps the references the resolver + * returns in its own haldata, and hands back its existing forward + * and inverse. The kinematics code itself does not change. + * + * A reference does not point into the RT instance's pin. The + * resolver creates an input pin on the caller's own component and + * connects it to the signal the RT pin reads, so the reference + * belongs to the caller and rewiring cannot strand it. + * + * Name lookup belongs to the caller, userspace code linked against + * liblinuxcnchal. This file is compiled into an RT module, which + * has no business walking the HAL name space and would risk binding + * against rtlib's copy of the same symbols. + * + * Resolve input pins only. Output pins and scratch storage stay + * private to the non-RT copy, or the two copies write to each + * other's state. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#ifndef NONRT_KINS_H +#define NONRT_KINS_H + +#include + +#include +#include +#include +#include + +/* Supplied by the caller. Finds 'pin_name' in HAL, checks that it has + type 'type', and writes to 'out' a reference carrying that pin's + value. The reference is to storage the caller owns, not to the named + pin itself. Returns 0 on success. */ +typedef int (*nonrt_resolve_fn)(const char *pin_name, + hal_type_t type, + hal_refs_u *out, + void *arg); + +/* Filled in by nonrt_attach(). A module that reports is_identity has + joints equal to axes and the caller needs no module code at all, so + forward and inverse may be left NULL. */ +typedef struct { + int (*forward)(const double *joints, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags); + int (*inverse)(const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); + int is_identity; +} nonrt_ops_t; + +/* Exported by a participating module: + int nonrt_attach(const char *coordinates, nonrt_ops_t *ops, + nonrt_resolve_fn resolve, void *arg); + Returns 0 on success. */ + +/* Convenience for the common case: resolve one float pin, by printf + style name, into a haldata field. */ +static inline int nonrt_resolve_real(nonrt_resolve_fn resolve, void *arg, + hal_real_t *dst, const char *fmt, ...) +{ + char name[HAL_NAME_LEN + 1]; + hal_refs_u ref; + va_list ap; + + if (!resolve || !dst) return -1; + + va_start(ap, fmt); + rtapi_vsnprintf(name, sizeof(name), fmt, ap); + va_end(ap); + + if (resolve(name, HAL_FLOAT, &ref, arg) != 0) return -1; + + *dst = ref.r; + return 0; +} + +#endif /* NONRT_KINS_H */ diff --git a/src/emc/kinematics/trivkins.c b/src/emc/kinematics/trivkins.c index f04d9642622..0690aa9ee39 100644 --- a/src/emc/kinematics/trivkins.c +++ b/src/emc/kinematics/trivkins.c @@ -18,6 +18,7 @@ #include #include #include +#include "nonrt_kins.h" #define SET(f) pos->f = joints[i] @@ -110,3 +111,17 @@ int rtapi_app_main(void) { } void rtapi_app_exit(void) { hal_exit(comp_id); } + +// Non-RT entry point: joints are axes, so a non-RT caller needs no +// module code at all and reads nothing from HAL. +int nonrt_attach(const char* coordinates, nonrt_ops_t* ops, + nonrt_resolve_fn resolve, void* arg) +{ + (void)coordinates; (void)resolve; (void)arg; + ops->forward = NULL; + ops->inverse = NULL; + ops->is_identity = 1; + return 0; +} + +EXPORT_SYMBOL(nonrt_attach); diff --git a/src/emc/kinematics_userspace/Submakefile b/src/emc/kinematics_userspace/Submakefile new file mode 100644 index 00000000000..f92b17d356e --- /dev/null +++ b/src/emc/kinematics_userspace/Submakefile @@ -0,0 +1 @@ +INCLUDES += emc/kinematics_userspace diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c new file mode 100644 index 00000000000..69abd527dac --- /dev/null +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -0,0 +1,381 @@ +/******************************************************************** + * Description: kinematics_user.c + * Non-RT loader for kinematics modules + * + * Loads a kinematics .so with dlopen and calls the nonrt_attach() it + * exports, so this process evaluates the kinematics the machine is + * running, at whatever poses it likes. See nonrt_kins.h. + * + * Identity kinematics needs no module code: the module says so through + * nonrt_ops_t and this file maps joints to axes directly. A module + * exporting no nonrt_attach() is not an error either; the context comes + * back flagged rt_only. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include "kinematics_user.h" +#include +#include +#include +#include +#include +#include + +#include "config.h" /* EMC2_HOME */ + +typedef int (*nonrt_attach_fn)(const char *coordinates, nonrt_ops_t *ops, + nonrt_resolve_fn resolve, void *arg); + +/* One per value a kinematics module reads is a generous bound. */ +#define MAX_MADE_SIGNALS 16 +#define MAX_BOUND_PINS 16 + +struct KinematicsUserContext { + int initialized; + int rt_only; /* 1 if the module exports no nonrt_attach() */ + int is_identity; /* 1 for identity kinematics: no module code needed */ + KINEMATICS_TYPE kins_type; + void *rt_handle; /* dlopen handle */ + nonrt_ops_t ops; + int num_joints; + int joint_to_axis[KINEMATICS_USER_MAX_JOINTS]; /* identity path only */ + char module_name[64]; + int comp_id; /* the caller's component, owns the pins made here */ + const char *prefix; /* its name, which those pin names start with */ + char made_signal[MAX_MADE_SIGNALS][HAL_NAME_LEN + 1]; + int num_made_signals; + hal_refs_u *cell; /* HAL storage those pins are made against */ + int num_cells; +}; + +/* ======================================================================== + * Pin binding + * ======================================================================== */ + +/* + * Give a kinematics module a reference to a value it asked for. + * + * The reference is to a pin of ours rather than into the RT instance's, + * so that its lifetime is ours: see nonrt_kins.h. Ours is connected to + * the signal the RT pin reads, or, when the RT pin has no signal, to one + * made here and removed again in kinematicsUserFree(). + * + * The reference has to live in HAL shared memory, since that is where + * HAL rewrites it on connect and disconnect, so the pins are made + * against hal_malloc() cells and the module gets what a cell holds once + * the connection is in place. + */ +static int make_signal(KinematicsUserContext *ctx, const char *pin_name, + hal_type_t type, char *out, size_t outlen) +{ + if (ctx->num_made_signals >= MAX_MADE_SIGNALS) { + fprintf(stderr, "kinematicsUserInit: too many signals to create\n"); + return -1; + } + if ((size_t)snprintf(out, outlen, "%s-nonrt", pin_name) >= outlen) { + fprintf(stderr, "kinematicsUserInit: signal name for '%s' too long\n", + pin_name); + return -1; + } + if (hal_signal_new(out, type) != 0) return -1; + if (hal_link(pin_name, out) != 0) { + hal_signal_delete(out); + return -1; + } + snprintf(ctx->made_signal[ctx->num_made_signals++], + sizeof(ctx->made_signal[0]), "%s", out); + return 0; +} + +static int new_pin(int comp_id, hal_type_t type, hal_refs_u *out, + const char *name) +{ + switch (type) { + case HAL_BIT: return hal_pin_new_bool(comp_id, HAL_IN, &out->b, 0, "%s", name); + case HAL_FLOAT: return hal_pin_new_real(comp_id, HAL_IN, &out->r, 0.0, "%s", name); + case HAL_S32: return hal_pin_new_si32(comp_id, HAL_IN, &out->s, 0, "%s", name); + case HAL_U32: return hal_pin_new_ui32(comp_id, HAL_IN, &out->u, 0, "%s", name); + case HAL_S64: return hal_pin_new_sint(comp_id, HAL_IN, &out->s, 0, "%s", name); + case HAL_U64: return hal_pin_new_uint(comp_id, HAL_IN, &out->u, 0, "%s", name); + default: break; + } + return -1; +} + +static int bind_pin(const char *pin_name, hal_type_t type, + hal_refs_u *out, void *arg) +{ + KinematicsUserContext *ctx = (KinematicsUserContext *)arg; + char signal[HAL_NAME_LEN + 1]; + char mine[HAL_NAME_LEN + 1]; + hal_refs_u *cell; + hal_query_t q; + + if (!ctx || !pin_name || !out) return -1; + + memset(&q, 0, sizeof(q)); + q.name = pin_name; + q.qtype = HAL_QTYPE_PIN; + + if (hal_getref_p(&q) != 0) { + fprintf(stderr, "kinematicsUserInit: no such pin '%s'\n", pin_name); + return -1; + } + if (q.pp.type != type) { + fprintf(stderr, "kinematicsUserInit: pin '%s' has the wrong type\n", + pin_name); + return -1; + } + + if (q.pp.signal) { + snprintf(signal, sizeof(signal), "%s", q.pp.signal); + } else if (make_signal(ctx, pin_name, type, signal, sizeof(signal))) { + fprintf(stderr, "kinematicsUserInit: cannot reach '%s'\n", pin_name); + return -1; + } + + if ((size_t)snprintf(mine, sizeof(mine), "%s.%s", ctx->prefix, pin_name) + >= sizeof(mine)) { + fprintf(stderr, "kinematicsUserInit: pin name for '%s' too long\n", + pin_name); + return -1; + } + if (ctx->num_cells >= MAX_BOUND_PINS) { + fprintf(stderr, "kinematicsUserInit: too many pins to bind\n"); + return -1; + } + cell = &ctx->cell[ctx->num_cells++]; + + if (new_pin(ctx->comp_id, type, cell, mine) != 0) { + fprintf(stderr, "kinematicsUserInit: cannot create pin '%s'\n", mine); + return -1; + } + if (hal_link(mine, signal) != 0) { + fprintf(stderr, "kinematicsUserInit: cannot link '%s' to '%s'\n", + mine, signal); + return -1; + } + + *out = *cell; + return 0; +} + +/* ======================================================================== + * Identity joint mapping + * ======================================================================== */ + +static void fill_identity_joint_map(KinematicsUserContext *ctx, const char *coords) +{ + int i, j = 0; + for (i = 0; i < KINEMATICS_USER_MAX_JOINTS; i++) ctx->joint_to_axis[i] = -1; + if (!coords) return; + for (; *coords && j < ctx->num_joints; coords++) { + int axis; + switch (tolower((unsigned char)*coords)) { + case 'x': axis = 0; break; case 'y': axis = 1; break; + case 'z': axis = 2; break; case 'a': axis = 3; break; + case 'b': axis = 4; break; case 'c': axis = 5; break; + case 'u': axis = 6; break; case 'v': axis = 7; break; + case 'w': axis = 8; break; default: continue; + } + ctx->joint_to_axis[j++] = axis; + } +} + +/* ======================================================================== + * Module loading + * ======================================================================== */ + +static int load_module(KinematicsUserContext *ctx, + const char *module_name, + const char *coordinates) +{ + char module_path[512]; + void *handle; + nonrt_attach_fn attach; + + snprintf(module_path, sizeof(module_path), + "%s/rtlib/%s.so", EMC2_HOME, module_name); + + handle = dlopen(module_path, RTLD_NOW | RTLD_LOCAL); + if (!handle) { + fprintf(stderr, "kinematicsUserInit: dlopen '%s': %s\n", + module_path, dlerror()); + return -1; + } + ctx->rt_handle = handle; + + attach = (nonrt_attach_fn)dlsym(handle, "nonrt_attach"); + if (!attach) { + fprintf(stderr, "kinematicsUserInit: '%s' exports no nonrt_attach\n", + module_name); + dlclose(handle); + ctx->rt_handle = NULL; + ctx->rt_only = 1; + return -1; + } + + if (attach(coordinates, &ctx->ops, bind_pin, ctx) != 0) { + fprintf(stderr, "kinematicsUserInit: nonrt_attach failed for '%s'\n", + module_name); + dlclose(handle); + ctx->rt_handle = NULL; + ctx->rt_only = 1; + return -1; + } + + if (ctx->ops.is_identity) { + ctx->is_identity = 1; + ctx->kins_type = KINEMATICS_IDENTITY; + return 0; + } + + if (!ctx->ops.forward || !ctx->ops.inverse) { + fprintf(stderr, "kinematicsUserInit: '%s' set no fwd/inv\n", module_name); + dlclose(handle); + ctx->rt_handle = NULL; + ctx->rt_only = 1; + return -1; + } + + ctx->kins_type = KINEMATICS_BOTH; + return 0; +} + +/* ======================================================================== + * Public API + * ======================================================================== */ + +KinematicsUserContext* kinematicsUserInit(const char* kins_type, + int num_joints, + const char* coordinates, + int comp_id, + const char* prefix) +{ + KinematicsUserContext *ctx; + + if (!kins_type || num_joints < 1 || num_joints > KINEMATICS_USER_MAX_JOINTS + || comp_id < 0 || !prefix) { + fprintf(stderr, "kinematicsUserInit: invalid arguments\n"); + return NULL; + } + + ctx = (KinematicsUserContext *)calloc(1, sizeof(KinematicsUserContext)); + if (!ctx) return NULL; + + ctx->num_joints = num_joints; + ctx->comp_id = comp_id; + ctx->prefix = prefix; + + ctx->cell = (hal_refs_u *)hal_malloc(MAX_BOUND_PINS * sizeof(hal_refs_u)); + if (!ctx->cell) { + fprintf(stderr, "kinematicsUserInit: out of HAL memory\n"); + free(ctx); + return NULL; + } + strncpy(ctx->module_name, kins_type, sizeof(ctx->module_name) - 1); + + load_module(ctx, kins_type, coordinates); + + if (ctx->is_identity) { + fill_identity_joint_map(ctx, coordinates); + } + + ctx->initialized = 1; + return ctx; +} + +int kinematicsUserInverse(KinematicsUserContext* ctx, + const EmcPose* world, + double* joints) +{ + if (!ctx || !ctx->initialized || !world || !joints) return -1; + + if (ctx->is_identity) { + int i; + for (i = 0; i < ctx->num_joints; i++) { + int ax = ctx->joint_to_axis[i]; + joints[i] = (ax >= 0) ? emcPoseGetAxis(world, ax) : 0.0; + } + return 0; + } + + if (ctx->rt_only) return -1; + return ctx->ops.inverse(world, joints, NULL, NULL); +} + +int kinematicsUserForward(KinematicsUserContext* ctx, + const double* joints, + EmcPose* world) +{ + if (!ctx || !ctx->initialized || !joints || !world) return -1; + + if (ctx->is_identity) { + int i; + memset(world, 0, sizeof(*world)); + for (i = 0; i < ctx->num_joints; i++) { + int ax = ctx->joint_to_axis[i]; + if (ax >= 0) emcPoseSetAxis(world, ax, joints[i]); + } + return 0; + } + + if (ctx->rt_only) return -1; + return ctx->ops.forward(joints, world, NULL, NULL); +} + +int kinematicsUserIsIdentity(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return 0; + return ctx->is_identity; +} + +int kinematicsUserGetNumJoints(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return 0; + return ctx->num_joints; +} + +KINEMATICS_TYPE kinematicsUserGetType(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return KINEMATICS_IDENTITY; + return ctx->kins_type; +} + +const char* kinematicsUserGetModuleName(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return "unknown"; + return ctx->module_name; +} + +int kinematicsUserRefreshParams(KinematicsUserContext* ctx) +{ + (void)ctx; + return 0; /* nothing to refresh: the bound pins are the live values */ +} + +int kinematicsUserIsRtOnly(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return 1; + return ctx->rt_only; +} + +void kinematicsUserFree(KinematicsUserContext* ctx) +{ + int i; + + if (!ctx) return; + + /* Removing one hands its value back to the RT pin, leaving the + machine as it was found. */ + for (i = 0; i < ctx->num_made_signals; i++) { + hal_signal_delete(ctx->made_signal[i]); + } + if (ctx->rt_handle) dlclose(ctx->rt_handle); + free(ctx); +} diff --git a/src/emc/kinematics_userspace/kinematics_user.h b/src/emc/kinematics_userspace/kinematics_user.h new file mode 100644 index 00000000000..d01d8a8d277 --- /dev/null +++ b/src/emc/kinematics_userspace/kinematics_user.h @@ -0,0 +1,197 @@ +/******************************************************************** + * Description: kinematics_user.h + * Userspace kinematics interface for trajectory planning + * + * This provides a userspace-compatible kinematics interface that mirrors + * the RT kinematics interface. Used by the 9D planner to compute joint + * positions from world coordinates without requiring RT kernel calls. + * + * The kinematics module is loaded into this process and given input pins + * belonging to the caller's HAL component, connected to the same signals + * the running RT instance reads. Its own forward and inverse then work on + * live values, unmodified. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ +#ifndef KINEMATICS_USER_H +#define KINEMATICS_USER_H + +#include /* EmcPose */ +#include /* KINEMATICS_TYPE, flags */ +#include /* hal_type_t, HAL_NAME_LEN */ + +#ifdef __cplusplus +extern "C" { +#endif + +/* Maximum number of joints supported */ +#define KINEMATICS_USER_MAX_JOINTS 9 + +/* Axis coordinate indices for EmcPose */ +typedef enum { + AXIS_X = 0, AXIS_Y = 1, AXIS_Z = 2, + AXIS_A = 3, AXIS_B = 4, AXIS_C = 5, + AXIS_U = 6, AXIS_V = 7, AXIS_W = 8, + AXIS_COUNT = 9 +} AxisIndex; + +/* Opaque context for userspace kinematics */ +typedef struct KinematicsUserContext KinematicsUserContext; + +/** + * Initialize userspace kinematics context + * + * The pins this creates belong to the caller's component, so call this + * after hal_init() and before hal_ready(): HAL refuses new pins once a + * component is ready. + * + * @param kins_type Kinematics module name (e.g., "trivkins", "5axiskins", "maxkins") + * @param num_joints Number of joints in the machine + * @param coordinates Coordinate string (e.g., "XYZABC", "XYZBCW") + * @param comp_id Caller's HAL component, from hal_init() + * @param prefix Its name, which the created pin names start with + * @return Allocated context, or NULL if kinematics type not supported + */ +KinematicsUserContext* kinematicsUserInit(const char* kins_type, + int num_joints, + const char* coordinates, + int comp_id, + const char* prefix); + +/** + * Perform inverse kinematics (world coords -> joint positions) + * + * @param ctx Kinematics context from kinematicsUserInit + * @param world World coordinates (X, Y, Z, A, B, C, U, V, W) + * @param joints Output array of joint positions [KINEMATICS_USER_MAX_JOINTS] + * @return 0 on success, -1 on failure + */ +int kinematicsUserInverse(KinematicsUserContext* ctx, + const EmcPose* world, + double* joints); + +/** + * Perform forward kinematics (joint positions -> world coords) + * + * @param ctx Kinematics context from kinematicsUserInit + * @param joints Array of joint positions [KINEMATICS_USER_MAX_JOINTS] + * @param world Output world coordinates + * @return 0 on success, -1 on failure + */ +int kinematicsUserForward(KinematicsUserContext* ctx, + const double* joints, + EmcPose* world); + +/** + * Check if kinematics type is identity (world coords = joint coords) + * + * @param ctx Kinematics context + * @return 1 if identity, 0 if not + */ +int kinematicsUserIsIdentity(KinematicsUserContext* ctx); + +/** + * Get number of joints + * + * @param ctx Kinematics context + * @return Number of joints + */ +int kinematicsUserGetNumJoints(KinematicsUserContext* ctx); + +/** + * Get KINEMATICS_TYPE (IDENTITY, BOTH, FORWARD_ONLY, INVERSE_ONLY) + * + * @param ctx Kinematics context + * @return KINEMATICS_TYPE enum value + */ +KINEMATICS_TYPE kinematicsUserGetType(KinematicsUserContext* ctx); + +/** + * Get kinematics module name + * + * @param ctx Kinematics context + * @return Module name string (e.g., "5axiskins") + */ +const char* kinematicsUserGetModuleName(KinematicsUserContext* ctx); + +/** + * Refresh kinematics parameters (no-op) + * + * The bound pins read the live values, so there is nothing to fetch. + * This function is kept for API compatibility but does nothing. + * + * @param ctx Kinematics context + * @return 0 always + */ +int kinematicsUserRefreshParams(KinematicsUserContext* ctx); + +/** + * Check if this context is RT-only + * + * An RT-only module exports no nonrt_attach() and so cannot be evaluated + * outside RT. Planner 2 is unavailable for such modules. + * + * @param ctx Kinematics context + * @return 1 if RT-only (planner 2 unavailable), 0 if the module is bound + */ +int kinematicsUserIsRtOnly(KinematicsUserContext* ctx); + +/** + * Free kinematics context + * + * @param ctx Context to free + */ +void kinematicsUserFree(KinematicsUserContext* ctx); + +/** + * Get axis value from EmcPose by index + * + * @param pose Pointer to EmcPose + * @param axis Axis index (AXIS_X through AXIS_W) + * @return Axis value + */ +static inline double emcPoseGetAxis(const EmcPose* pose, int axis) { + switch (axis) { + case AXIS_X: return pose->tran.x; + case AXIS_Y: return pose->tran.y; + case AXIS_Z: return pose->tran.z; + case AXIS_A: return pose->a; + case AXIS_B: return pose->b; + case AXIS_C: return pose->c; + case AXIS_U: return pose->u; + case AXIS_V: return pose->v; + case AXIS_W: return pose->w; + default: return 0.0; + } +} + +/** + * Set axis value in EmcPose by index + * + * @param pose Pointer to EmcPose + * @param axis Axis index (AXIS_X through AXIS_W) + * @param value Value to set + */ +static inline void emcPoseSetAxis(EmcPose* pose, int axis, double value) { + switch (axis) { + case AXIS_X: pose->tran.x = value; break; + case AXIS_Y: pose->tran.y = value; break; + case AXIS_Z: pose->tran.z = value; break; + case AXIS_A: pose->a = value; break; + case AXIS_B: pose->b = value; break; + case AXIS_C: pose->c = value; break; + case AXIS_U: pose->u = value; break; + case AXIS_V: pose->v = value; break; + case AXIS_W: pose->w = value; break; + } +} + +#ifdef __cplusplus +} +#endif + +#endif /* KINEMATICS_USER_H */ diff --git a/src/emc/motion_planning/Submakefile b/src/emc/motion_planning/Submakefile new file mode 100644 index 00000000000..553849e7ba5 --- /dev/null +++ b/src/emc/motion_planning/Submakefile @@ -0,0 +1,46 @@ +INCLUDES += emc/motion_planning +INCLUDES += emc/kinematics_userspace + +# Jacobian-based world-space limit calculation, plus the non-RT kinematics +# loader it sits on top of. +LIBKINSLIMITS_CXXSRCS := $(addprefix emc/motion_planning/, \ + jacobian.cc \ + joint_limits.cc \ + ) + +LIBKINSLIMITS_CSRCS := $(addprefix emc/kinematics_userspace/, \ + kinematics_user.c \ + ) + +USERSRCS += $(LIBKINSLIMITS_CXXSRCS) $(LIBKINSLIMITS_CSRCS) + +$(call TOOBJSDEPS, $(LIBKINSLIMITS_CXXSRCS)): EXTRAFLAGS = -fPIC +$(call TOOBJSDEPS, $(LIBKINSLIMITS_CSRCS)): EXTRAFLAGS = -fPIC -D_GNU_SOURCE + +../lib/libkinslimits.so.0: $(call TOOBJS, $(LIBKINSLIMITS_CXXSRCS) $(LIBKINSLIMITS_CSRCS)) \ + ../lib/libposemath.so.0 ../lib/liblinuxcnchal.so.0 + $(ECHO) Linking $(notdir $@) + @mkdir -p ../lib + $(Q)$(CXX) $(LDFLAGS) -Wl,-soname,$(notdir $@) -shared -o $@ $^ -ldl + +../lib/libkinslimits.so: ../lib/libkinslimits.so.0 + ln -sf $(notdir $<) $@ + +TARGETS += ../lib/libkinslimits.so ../lib/libkinslimits.so.0 + +# Diagnostic: print the Jacobian and the caps it implies for one move. +KINSLIMITS_SRCS := emc/motion_planning/kinslimits.cc +USERSRCS += $(KINSLIMITS_SRCS) + +../bin/kinslimits: $(call TOOBJS, $(KINSLIMITS_SRCS)) \ + ../lib/libkinslimits.so.0 ../lib/liblinuxcnchal.so.0 ../lib/libposemath.so.0 + $(ECHO) Linking $(notdir $@) + @mkdir -p ../bin + $(Q)$(CXX) $(LDFLAGS) -o $@ $^ + +TARGETS += ../bin/kinslimits + +MOTION_PLANNING_HH := emc/motion_planning/jacobian.hh emc/motion_planning/joint_limits.hh +$(patsubst emc/motion_planning/%,../include/%,$(MOTION_PLANNING_HH)): ../include/%.hh: emc/motion_planning/%.hh + cp $^ $@ +HEADERS += $(patsubst emc/motion_planning/%,../include/%,$(MOTION_PLANNING_HH)) diff --git a/src/emc/motion_planning/jacobian.cc b/src/emc/motion_planning/jacobian.cc new file mode 100644 index 00000000000..a7d5a7661e7 --- /dev/null +++ b/src/emc/motion_planning/jacobian.cc @@ -0,0 +1,197 @@ +/******************************************************************** + * Description: jacobian.cc + * Jacobian calculation implementation for userspace kinematics trajectory planning + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include "jacobian.hh" +#include +#include +#include + +namespace motion_planning { + +JacobianCalculator::JacobianCalculator() + : kins_ctx_(nullptr), + is_identity_(false), + num_joints_(0) { +} + +JacobianCalculator::~JacobianCalculator() { + // kins_ctx_ is owned externally +} + +bool JacobianCalculator::init(KinematicsUserContext* kins_ctx) { + if (!kins_ctx) { + return false; + } + + kins_ctx_ = kins_ctx; + is_identity_ = (kinematicsUserIsIdentity(kins_ctx) != 0); + num_joints_ = kinematicsUserGetNumJoints(kins_ctx); + + return true; +} + +void JacobianCalculator::computeTrivkins(double J[9][9]) { + // Zero the matrix + std::memset(J, 0, sizeof(double) * 9 * 9); + + // For trivkins, the Jacobian is identity (with axis mapping) + // Since trivkins maps: joint[i] = world_axis[mapped_axis[i]] + // The Jacobian is: J[joint][axis] = 1 if axis == mapped_axis[joint], else 0 + + // For a simple XYZ trivkins: + // J[0][AXIS_X] = 1 (joint 0 = X) + // J[1][AXIS_Y] = 1 (joint 1 = Y) + // J[2][AXIS_Z] = 1 (joint 2 = Z) + // etc. + + // We need to query the kinematics context for the mapping. + // Since the context is opaque, we use inverse kinematics to determine + // the mapping. + + // Test each axis: perturb it and see which joint changes + EmcPose zero_pose; + ZERO_EMC_POSE(zero_pose); + double zero_joints[9]; + kinematicsUserInverse(kins_ctx_, &zero_pose, zero_joints); + + for (int axis = 0; axis < AXIS_COUNT; axis++) { + EmcPose test_pose = zero_pose; + emcPoseSetAxis(&test_pose, axis, 1.0); + + double test_joints[9]; + kinematicsUserInverse(kins_ctx_, &test_pose, test_joints); + + for (int joint = 0; joint < num_joints_; joint++) { + double delta = test_joints[joint] - zero_joints[joint]; + if (std::fabs(delta) > 0.5) { + // This axis maps to this joint + J[joint][axis] = 1.0; + } + } + } +} + +bool JacobianCalculator::computeNumerical(const EmcPose& pose, double J[9][9]) { + // Zero the matrix + std::memset(J, 0, sizeof(double) * 9 * 9); + + // Compute joints at nominal pose + double joints_center[9]; + if (kinematicsUserInverse(kins_ctx_, &pose, joints_center) != 0) { + return false; + } + + // Perturb each axis and compute derivatives + for (int axis = 0; axis < AXIS_COUNT; axis++) { + // Choose perturbation size based on axis type + double delta = (axis < 3 || axis >= 6) ? DELTA_LINEAR : DELTA_ROTARY; + + // Positive perturbation + EmcPose pose_plus = pose; + double val_plus = emcPoseGetAxis(&pose_plus, axis) + delta; + emcPoseSetAxis(&pose_plus, axis, val_plus); + + double joints_plus[9]; + if (kinematicsUserInverse(kins_ctx_, &pose_plus, joints_plus) != 0) { + // Kinematics failed - use one-sided difference + for (int joint = 0; joint < num_joints_; joint++) { + J[joint][axis] = (joints_plus[joint] - joints_center[joint]) / delta; + } + continue; + } + + // Negative perturbation + EmcPose pose_minus = pose; + double val_minus = emcPoseGetAxis(&pose_minus, axis) - delta; + emcPoseSetAxis(&pose_minus, axis, val_minus); + + double joints_minus[9]; + if (kinematicsUserInverse(kins_ctx_, &pose_minus, joints_minus) != 0) { + // Use forward difference + for (int joint = 0; joint < num_joints_; joint++) { + J[joint][axis] = (joints_plus[joint] - joints_center[joint]) / delta; + } + continue; + } + + // Central difference (most accurate) + for (int joint = 0; joint < num_joints_; joint++) { + J[joint][axis] = (joints_plus[joint] - joints_minus[joint]) / (2.0 * delta); + } + } + + // Check for NaN/Inf values and replace with safe defaults + bool had_nan = false; + for (int joint = 0; joint < num_joints_; joint++) { + for (int axis = 0; axis < AXIS_COUNT; axis++) { + if (!std::isfinite(J[joint][axis])) { + // Replace NaN/Inf with 0 (assume no coupling) + J[joint][axis] = 0.0; + had_nan = true; + } + } + } + + // If we had NaN values, the Jacobian may be unreliable + // Return true anyway but the condition number check will catch issues + (void)had_nan; // Could log this in debug mode + + return true; +} + +bool JacobianCalculator::compute(const EmcPose& pose, double J[9][9]) { + if (!kins_ctx_) { + return false; + } + + if (is_identity_) { + // For trivkins, use the fast identity computation + computeTrivkins(J); + return true; + } else { + // For non-trivial kinematics, use numerical differentiation + return computeNumerical(pose, J); + } +} + +double JacobianCalculator::conditionNumber(const double J[9][9]) { + if (is_identity_) { + // Identity matrix has condition number 1 + return 1.0; + } + + // We use a simplified condition number estimate: + // Find the ratio of largest to smallest row norms + // This is not the true 2-norm condition number, but gives a rough indication + + double max_row_norm = 0.0; + double min_row_norm = 1e18; + + for (int joint = 0; joint < num_joints_; joint++) { + double row_norm = 0.0; + for (int axis = 0; axis < AXIS_COUNT; axis++) { + row_norm += J[joint][axis] * J[joint][axis]; + } + row_norm = std::sqrt(row_norm); + + if (row_norm > max_row_norm) max_row_norm = row_norm; + if (row_norm > 1e-15 && row_norm < min_row_norm) min_row_norm = row_norm; + } + + if (min_row_norm < 1e-15) { + // Near-singular: a row is almost zero + return 1e18; + } + + return max_row_norm / min_row_norm; +} + +} // namespace motion_planning diff --git a/src/emc/motion_planning/jacobian.hh b/src/emc/motion_planning/jacobian.hh new file mode 100644 index 00000000000..8713e89f180 --- /dev/null +++ b/src/emc/motion_planning/jacobian.hh @@ -0,0 +1,99 @@ +/******************************************************************** + * Description: jacobian.hh + * Jacobian calculation for userspace kinematics trajectory planning + * + * Computes the Jacobian matrix relating world velocities to joint + * velocities. For trivkins this is the identity matrix. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ +#ifndef JACOBIAN_HH +#define JACOBIAN_HH + +#include +#include + +namespace motion_planning { + +/** + * Jacobian calculator class + * + * Computes the Jacobian matrix J where: + * joint_velocities = J × world_velocities + * + * For trivkins, J is the identity matrix (with appropriate axis mapping). + * For non-trivial kinematics, J is computed via numerical differentiation. + */ +class JacobianCalculator { +public: + JacobianCalculator(); + ~JacobianCalculator(); + + /** + * Initialize with kinematics context + * + * @param kins_ctx Userspace kinematics context + * @return true on success + */ + bool init(KinematicsUserContext* kins_ctx); + + /** + * Compute Jacobian at a given pose + * + * The Jacobian J[joint][axis] relates: + * d(joint[j])/dt = sum over axis a of J[j][a] * d(axis[a])/dt + * + * @param pose World pose at which to compute Jacobian + * @param J Output 9×9 Jacobian matrix [joint][axis] + * @return true on success, false on failure + */ + bool compute(const EmcPose& pose, double J[9][9]); + + /** + * Compute condition number of Jacobian + * + * The condition number indicates how close to a singularity the pose is. + * High condition number = near singularity. + * + * For trivkins, always returns 1.0 (no singularities). + * + * @param J Jacobian matrix + * @return Condition number (≥ 1.0), or -1.0 on error + */ + double conditionNumber(const double J[9][9]); + + /** + * Check if current kinematics is identity (trivkins) + */ + bool isIdentity() const { return is_identity_; } + +private: + /** + * Compute Jacobian for trivkins (identity with axis mapping) + */ + void computeTrivkins(double J[9][9]); + + /** + * Compute Jacobian via numerical differentiation + * Uses central differences: J[j][a] = (f(x+h) - f(x-h)) / (2h) + */ + bool computeNumerical(const EmcPose& pose, double J[9][9]); + + KinematicsUserContext* kins_ctx_; + bool is_identity_; + int num_joints_; + + // Perturbation size for numerical differentiation (mm or degrees) + // Must be large enough for kinematics to produce stable results + // but small enough for accurate derivatives + static constexpr double DELTA_LINEAR = 0.1; // 0.1 mm + static constexpr double DELTA_ROTARY = 0.1; // 0.1 degrees +}; + +} // namespace motion_planning + +#endif // JACOBIAN_HH diff --git a/src/emc/motion_planning/joint_limits.cc b/src/emc/motion_planning/joint_limits.cc new file mode 100644 index 00000000000..ee4ee34d06f --- /dev/null +++ b/src/emc/motion_planning/joint_limits.cc @@ -0,0 +1,358 @@ +/******************************************************************** + * Description: joint_limits.cc + * Joint limit calculation implementation for userspace kinematics trajectory planning + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include "joint_limits.hh" +#include +#include +#include + +namespace motion_planning { + +JointLimitCalculator::JointLimitCalculator() + : num_joints_(0), + initialized_(false) { +} + +JointLimitCalculator::~JointLimitCalculator() { +} + +bool JointLimitCalculator::init(int num_joints) { + if (num_joints < 1 || num_joints > KINEMATICS_USER_MAX_JOINTS) { + return false; + } + + num_joints_ = num_joints; + + // Initialize with default (very permissive) limits + for (int i = 0; i < KINEMATICS_USER_MAX_JOINTS; i++) { + limits_[i] = JointLimitConfig(); + } + + initialized_ = true; + return true; +} + +bool JointLimitCalculator::setJointLimits(int joint, const JointLimitConfig& limits) { + if (joint < 0 || joint >= num_joints_) { + return false; + } + limits_[joint] = limits; + return true; +} + +const JointLimitConfig& JointLimitCalculator::getJointLimits(int joint) const { + static JointLimitConfig default_limits; + if (joint < 0 || joint >= num_joints_) { + return default_limits; + } + return limits_[joint]; +} + +double JointLimitCalculator::getJointVelLimit(int joint) const { + if (joint < 0 || joint >= num_joints_) return 1e9; + return limits_[joint].vel_limit; +} + +double JointLimitCalculator::getJointAccLimit(int joint) const { + if (joint < 0 || joint >= num_joints_) return 1e9; + return limits_[joint].acc_limit; +} + +double JointLimitCalculator::getJointJerkLimit(int joint) const { + if (joint < 0 || joint >= num_joints_) return 1e9; + return limits_[joint].jerk_limit; +} + +bool JointLimitCalculator::updateAllLimits(const double* vel_limits, + const double* acc_limits, + const double* min_pos, + const double* max_pos, + const double* jerk_limits) { + if (!initialized_) { + return false; + } + + // Update limits from arrays + // This is used to refresh limits from shared memory (motion status), + // which reflects any runtime changes via HAL pins (ini.N.max_limit, etc.) + for (int j = 0; j < num_joints_; j++) { + if (vel_limits) limits_[j].vel_limit = vel_limits[j]; + if (acc_limits) limits_[j].acc_limit = acc_limits[j]; + if (min_pos) limits_[j].min_pos_limit = min_pos[j]; + if (max_pos) limits_[j].max_pos_limit = max_pos[j]; + if (jerk_limits) limits_[j].jerk_limit = jerk_limits[j]; + } + + return true; +} + +bool JointLimitCalculator::checkPositionLimits(const double joint_pos[9]) { + for (int j = 0; j < num_joints_; j++) { + if (joint_pos[j] > limits_[j].max_pos_limit || + joint_pos[j] < limits_[j].min_pos_limit) { + return false; + } + } + return true; +} + +double JointLimitCalculator::computeMaxVelocity(const double J[9][9], int& limiting_joint) { + // Conservative estimate: assume worst-case direction + // For each joint j, find the maximum Jacobian element magnitude + // max_world_vel = min over j of: vel_limit[j] / max(|J[j][:]|) + + double max_world_vel = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + // Find maximum absolute value in this row of J + double max_abs_J = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + double abs_J = std::fabs(J[j][a]); + if (abs_J > max_abs_J) { + max_abs_J = abs_J; + } + } + + if (max_abs_J > 1e-15) { + // This joint contributes to motion + double vel_limit_world = limits_[j].vel_limit / max_abs_J; + if (vel_limit_world < max_world_vel) { + max_world_vel = vel_limit_world; + limiting_joint = j; + } + } + } + + // Apply sanity bounds + if (max_world_vel > 1e9) max_world_vel = 1e9; + if (max_world_vel < 1e-9) max_world_vel = 1e-9; + + return max_world_vel; +} + +double JointLimitCalculator::computeMaxAcceleration(const double J[9][9], int& limiting_joint) { + // Same approach as velocity + double max_world_acc = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double max_abs_J = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + double abs_J = std::fabs(J[j][a]); + if (abs_J > max_abs_J) { + max_abs_J = abs_J; + } + } + + if (max_abs_J > 1e-15) { + double acc_limit_world = limits_[j].acc_limit / max_abs_J; + if (acc_limit_world < max_world_acc) { + max_world_acc = acc_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_acc > 1e9) max_world_acc = 1e9; + if (max_world_acc < 1e-9) max_world_acc = 1e-9; + + return max_world_acc; +} + +double JointLimitCalculator::computeMaxJerk(const double J[9][9], int& limiting_joint) { + // Same approach as velocity and acceleration + double max_world_jerk = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double max_abs_J = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + double abs_J = std::fabs(J[j][a]); + if (abs_J > max_abs_J) { + max_abs_J = abs_J; + } + } + + if (max_abs_J > 1e-15) { + double jerk_limit_world = limits_[j].jerk_limit / max_abs_J; + if (jerk_limit_world < max_world_jerk) { + max_world_jerk = jerk_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_jerk > 1e9) max_world_jerk = 1e9; + if (max_world_jerk < 1e-9) max_world_jerk = 1e-9; + + return max_world_jerk; +} + +double JointLimitCalculator::computeMaxVelocityForTangent(const double J[9][9], const double tangent[9], int& limiting_joint) { + double max_world_vel = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + // Compute sum(|J[j][a]| * |tangent[a]|) — the actual amplification + // for this joint along the given path direction + double amplification = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + amplification += std::fabs(J[j][a]) * std::fabs(tangent[a]); + } + + if (amplification > 1e-15) { + double vel_limit_world = limits_[j].vel_limit / amplification; + if (vel_limit_world < max_world_vel) { + max_world_vel = vel_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_vel > 1e9) max_world_vel = 1e9; + if (max_world_vel < 1e-9) max_world_vel = 1e-9; + return max_world_vel; +} + +double JointLimitCalculator::computeMaxAccelerationForTangent(const double J[9][9], const double tangent[9], int& limiting_joint) { + double max_world_acc = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double amplification = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + amplification += std::fabs(J[j][a]) * std::fabs(tangent[a]); + } + + if (amplification > 1e-15) { + double acc_limit_world = limits_[j].acc_limit / amplification; + if (acc_limit_world < max_world_acc) { + max_world_acc = acc_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_acc > 1e9) max_world_acc = 1e9; + if (max_world_acc < 1e-9) max_world_acc = 1e-9; + return max_world_acc; +} + +double JointLimitCalculator::computeMaxJerkForTangent(const double J[9][9], const double tangent[9], int& limiting_joint) { + double max_world_jerk = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double amplification = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + amplification += std::fabs(J[j][a]) * std::fabs(tangent[a]); + } + + if (amplification > 1e-15) { + double jerk_limit_world = limits_[j].jerk_limit / amplification; + if (jerk_limit_world < max_world_jerk) { + max_world_jerk = jerk_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_jerk > 1e9) max_world_jerk = 1e9; + if (max_world_jerk < 1e-9) max_world_jerk = 1e-9; + return max_world_jerk; +} + +bool JointLimitCalculator::computeForTangent(const double J[9][9], + const double joint_pos[9], + const double tangent[9], + JointLimitResult& result, + double singularity_threshold) { + if (!initialized_) { + return false; + } + + result.position_ok = checkPositionLimits(joint_pos); + result.condition_number = computeConditionNumber(J); + + result.max_world_vel = computeMaxVelocityForTangent(J, tangent, result.limiting_joint_vel); + result.max_world_acc = computeMaxAccelerationForTangent(J, tangent, result.limiting_joint_acc); + result.max_world_jerk = computeMaxJerkForTangent(J, tangent, result.limiting_joint_jerk); + + if (result.condition_number > singularity_threshold) { + double slowdown_factor = singularity_threshold / result.condition_number; + result.max_world_vel *= slowdown_factor; + result.max_world_acc *= slowdown_factor; + result.max_world_jerk *= slowdown_factor; + } + + return true; +} + +double JointLimitCalculator::computeConditionNumber(const double J[9][9]) { + // Simplified condition number: ratio of max to min row norms + double max_row_norm = 0.0; + double min_row_norm = 1e18; + + for (int j = 0; j < num_joints_; j++) { + double row_norm = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + row_norm += J[j][a] * J[j][a]; + } + row_norm = std::sqrt(row_norm); + + if (row_norm > max_row_norm) max_row_norm = row_norm; + if (row_norm > 1e-15 && row_norm < min_row_norm) min_row_norm = row_norm; + } + + if (min_row_norm < 1e-15) { + return 1e18; // Near-singular + } + + return max_row_norm / min_row_norm; +} + +bool JointLimitCalculator::compute(const double J[9][9], + const double joint_pos[9], + JointLimitResult& result, + double singularity_threshold) { + if (!initialized_) { + return false; + } + + // Check position limits + result.position_ok = checkPositionLimits(joint_pos); + + // Compute condition number + result.condition_number = computeConditionNumber(J); + + // Compute max velocity + result.max_world_vel = computeMaxVelocity(J, result.limiting_joint_vel); + + // Compute max acceleration + result.max_world_acc = computeMaxAcceleration(J, result.limiting_joint_acc); + + // Compute max jerk + result.max_world_jerk = computeMaxJerk(J, result.limiting_joint_jerk); + + // Apply singularity slowdown + // If condition number exceeds threshold, reduce limits proportionally + if (result.condition_number > singularity_threshold) { + double slowdown_factor = singularity_threshold / result.condition_number; + result.max_world_vel *= slowdown_factor; + result.max_world_acc *= slowdown_factor; + result.max_world_jerk *= slowdown_factor; + } + + return true; +} + +} // namespace motion_planning diff --git a/src/emc/motion_planning/joint_limits.hh b/src/emc/motion_planning/joint_limits.hh new file mode 100644 index 00000000000..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; +} From cc1b81880d76d05c1f9c27d1eac29d9de0f344ca Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 12:44:16 +1000 Subject: [PATCH 27/77] kinematics: add the parameter block form of a module A module reads its geometry from pins it created and keeps its type and iteration scratch in statics, so it can only answer for the machine as it is now, from inside realtime; a planner, a load-time check or a preview had to carry a second copy of the maths. Add the form in which the caller supplies everything: a kins_params block with the type, the joint map, the tool and the geometry, a kins_scratch for what an iterative method carries between calls, and a kins_ops table with the forward, inverse, frames and Jacobian of one type as functions of the two. A module declares its geometry as a table of named entries; in RT the shared code makes one pin per entry under the names configs already use and copies pins in and outputs out around every call, so the maths never touches a pin. kins_single.c supplies the classic entry points for a module with one type, switchkins.c for several, dispatching a type registered with switchkinsRegisterOps() through the block and the older registrations as before, so modules convert one at a time. Every converted module exports kinsDescribe() for callers outside RT; one that does not reports itself RT-only. trivkins, 5axiskins and userkfuncs convert here. The non-RT loader moves onto kinsDescribe() and nonrt_attach() goes; jacobian.cc asks the module for its Jacobian. An iterating ops forward restarts from the pose it last solved and otherwise from the caller's estimate, keeping a result only when the solve succeeded, and the gui forward of a pure type is seeded the same way. kinslimits reports the same caps for 5axiskins to the digit. --- src/Makefile | 11 +- src/emc/kinematics/5axiskins.c | 280 +++------- src/emc/kinematics/kinematics.h | 255 +++++++++- src/emc/kinematics/kins_rt.h | 57 +++ src/emc/kinematics/kins_single.c | 155 ++++++ src/emc/kinematics/kins_util.c | 479 +++++++++++++++++- src/emc/kinematics/nonrt_kins.h | 95 ---- src/emc/kinematics/switchkins.c | 285 +++++++++-- src/emc/kinematics/switchkins.h | 27 +- src/emc/kinematics/switchkins_main.c | 42 +- src/emc/kinematics/switchkins_setup.c | 100 ++++ src/emc/kinematics/trivkins.c | 126 ++--- src/emc/kinematics/userkfuncs.c | 34 ++ .../kinematics_userspace/kinematics_user.c | 363 +++++++++---- .../kinematics_userspace/kinematics_user.h | 61 ++- src/emc/motion_planning/Submakefile | 4 +- src/emc/motion_planning/jacobian.cc | 121 +---- src/emc/motion_planning/jacobian.hh | 24 +- src/hal/components/millturn.comp | 2 +- src/hal/components/xyzab_tdr_kins.comp | 2 +- src/hal/components/xyzacb_trsrn.comp | 2 +- src/hal/components/xyzbca_trsrn.comp | 2 +- 22 files changed, 1761 insertions(+), 766 deletions(-) create mode 100644 src/emc/kinematics/kins_rt.h create mode 100644 src/emc/kinematics/kins_single.c delete mode 100644 src/emc/kinematics/nonrt_kins.h create mode 100644 src/emc/kinematics/switchkins_setup.c diff --git a/src/Makefile b/src/Makefile index 4a63f092109..b0026deb94f 100644 --- a/src/Makefile +++ b/src/Makefile @@ -404,7 +404,7 @@ SRCHEADERS := \ emc/linuxcnc.h \ emc/kinematics/kinematics.h \ emc/kinematics/switchkins.h \ - emc/kinematics/nonrt_kins.h \ + emc/kinematics/kins_rt.h \ emc/kinematics_userspace/kinematics_user.h \ emc/nml_intf/emcmotcfg.h \ emc/ini/axis_kinds.hh \ @@ -1132,6 +1132,7 @@ 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 @@ -1187,6 +1188,7 @@ 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 @@ -1197,6 +1199,7 @@ 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 @@ -1205,6 +1208,7 @@ 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 @@ -1213,6 +1217,7 @@ 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 @@ -1222,6 +1227,7 @@ 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 @@ -1231,6 +1237,7 @@ 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 @@ -1240,6 +1247,7 @@ 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 @@ -1249,6 +1257,7 @@ obj-m += 5axiskins.o 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/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index d4798261ff8..cf2e0254da4 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 comes +* in on the tool-length pin of its own * 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 -static struct haldata { - hal_real_t pivot_length; - hal_real_t tool_length; -} *haldata; -static int fiveaxis_max_joints; +// 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, 0, 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,21 +158,18 @@ 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() - -static int fiveaxis_KinematicsJacobian(const double *joints, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) + 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) { (void)joints; (void)iflags; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); - const double R = pivot_length + pos->w; + const double R = p->geometry[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]; @@ -197,112 +196,15 @@ static int fiveaxis_KinematicsJacobian(const double *joints, for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } - return kinsJacobianFromMappedAxes(fiveaxis_max_joints, - (const double (*)[EMCMOT_MAX_AXIS])dP, - jac); -} // fiveaxis_KinematicsJacobian() - -// module constants, shared by switchkinsSetup() and nonrt_attach() -static void fiveaxis_kparms(kparms* kp) -{ - kp->kinsname = "5axiskins"; // !!! must agree with filename - kp->halprefix = "5axiskins"; // hal pin names - kp->required_coordinates = REQUIRED_COORDINATES; - kp->allow_duplicates = 1; - kp->max_joints = EMCMOT_MAX_JOINTS; -} - -// assign principal joint numbers from the coordinates string. -// No HAL involvement, so the non-RT path can use it too. -static int fiveaxis_map_joints(const char* coordinates, kparms* kp) -{ - int 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; jnopivot_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; - - 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)); - - return 0; + return kinsJacobianFromMappedAxesP(p, (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // fiveaxis_jacobian() -error: - return -1; -} // fiveaxis_KinematicsSetup() +static const kins_ops fiveaxis_ops = { + .forward = fiveaxis_forward, + .inverse = fiveaxis_inverse, + .jacobian = fiveaxis_jacobian, +}; int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -310,61 +212,27 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { - fiveaxis_kparms(kp); + (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); - switchkinsRegisterJacobian(1, fiveaxis_KinematicsJacobian); + 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; - switchkinsRegisterJacobian(0, fiveaxis_KinematicsJacobian); - - *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() - -// Non-RT entry point: bind this copy of the module to the pins the -// running RT instance owns, then hand back the unmodified kinematics. -int nonrt_attach(const char* coordinates, nonrt_ops_t* ops, - nonrt_resolve_fn resolve, void* arg) -{ - static struct haldata nonrt_haldata; // private to this copy of the module - kparms kp = {0}; - - fiveaxis_kparms(&kp); - - haldata = &nonrt_haldata; - - if (nonrt_resolve_real(resolve, arg, &haldata->pivot_length, - "%s.pivot-length", kp.halprefix)) return -1; - - if (fiveaxis_map_joints(coordinates, &kp)) return -1; - - ops->forward = fiveaxis_KinematicsForward; - ops->inverse = fiveaxis_KinematicsInverse; - ops->is_identity = 0; - return 0; -} // nonrt_attach() - -EXPORT_SYMBOL(nonrt_attach); diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index 5597d7a14a6..8bd17137362 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -107,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 @@ -161,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 @@ -197,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 @@ -472,6 +474,217 @@ extern int identityKinematicsJacobian(const double *joint, 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 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. */ +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 */ +} 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); //NOTE: switchable kinematics may require Interp::Synch @@ -488,6 +701,8 @@ 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); diff --git a/src/emc/kinematics/kins_rt.h b/src/emc/kinematics/kins_rt.h new file mode 100644 index 00000000000..96d7309c739 --- /dev/null +++ b/src/emc/kinematics/kins_rt.h @@ -0,0 +1,57 @@ +/******************************************************************** +* 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); + +/* 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..58f914076d5 --- /dev/null +++ b/src/emc/kinematics/kins_single.c @@ -0,0 +1,155 @@ +/******************************************************************** +* 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 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 +static void read_pins(void) +{ + kinsParamsPinsRead(pins, 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); +} + +KINEMATICS_TYPE kinematicsType(void) +{ + return reported_type; +} + +int kinematicsSwitchable(void) { return 0; } + +int kinematicsSwitch(int switchkins_type) +{ + (void)switchkins_type; + return 0; +} + +// 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(kinematicsSwitchable); +EXPORT_SYMBOL(kinematicsSwitch); +EXPORT_SYMBOL(kinsDescribe); diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index 27773c9edb1..5187c2598ce 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"); } @@ -1281,3 +1306,405 @@ int identityKinematicsJacobian(const double *joint, (const double (*)[EMCMOT_MAX_AXIS])dP, jac); } // identityKinematicsJacobian() + +//---------------------------------------------------------------------- +// The parameter block. See kinematics.h for what it is for. +//---------------------------------------------------------------------- + +int kinsParamsMapCoordinates(kins_params *p, + const char *coordinates, + int max_joints, + int allow_duplicates, + const char *required_coordinates) +{ + int axis_idx_for_jno[EMCMOT_MAX_JOINTS]; + int jno, a; + + if (!p) { return -1; } + if (!coordinates) { coordinates = "XYZABCUVW"; } + + if (kins_scan_coordinates(coordinates, max_joints, allow_duplicates, + axis_idx_for_jno, + "kinsParamsMapCoordinates: ERROR:\n ")) { + return -1; + } + + // every letter the module cannot do without has to be there + for (a = 0; required_coordinates && required_coordinates[a]; a++) { + char want = required_coordinates[a]; + const char *c; + int seen = 0; + for (c = coordinates; *c; c++) { + if (*c == want || *c == want + ('a' - 'A') || *c == want - ('a' - 'A')) { + seen = 1; break; + } + } + if (!seen) { + rtapi_print_msg(RTAPI_MSG_ERR, + "kinsParamsMapCoordinates: ERROR:\n required coordinates:%s\n" + " specified coordinates:%s\n", + required_coordinates, coordinates); + return -1; + } + } + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + p->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() diff --git a/src/emc/kinematics/nonrt_kins.h b/src/emc/kinematics/nonrt_kins.h deleted file mode 100644 index ed564f21b6f..00000000000 --- a/src/emc/kinematics/nonrt_kins.h +++ /dev/null @@ -1,95 +0,0 @@ -/******************************************************************** - * Description: nonrt_kins.h - * Interface a kinematics module exports so that a non-RT caller can - * evaluate it. - * - * A trajectory planner needs forward and inverse kinematics at poses - * the machine has not reached yet, which means calling them outside - * the servo thread. A module opts in by exporting nonrt_attach(). - * - * The caller dlopens the module and calls nonrt_attach() once with - * the coordinates string and a resolver callback. The module names - * each of the pins it reads, keeps the references the resolver - * returns in its own haldata, and hands back its existing forward - * and inverse. The kinematics code itself does not change. - * - * A reference does not point into the RT instance's pin. The - * resolver creates an input pin on the caller's own component and - * connects it to the signal the RT pin reads, so the reference - * belongs to the caller and rewiring cannot strand it. - * - * Name lookup belongs to the caller, userspace code linked against - * liblinuxcnchal. This file is compiled into an RT module, which - * has no business walking the HAL name space and would risk binding - * against rtlib's copy of the same symbols. - * - * Resolve input pins only. Output pins and scratch storage stay - * private to the non-RT copy, or the two copies write to each - * other's state. - * - * Author: LinuxCNC - * License: GPL Version 2 - * System: Linux - * - * Copyright (c) 2024 All rights reserved. - ********************************************************************/ - -#ifndef NONRT_KINS_H -#define NONRT_KINS_H - -#include - -#include -#include -#include -#include - -/* Supplied by the caller. Finds 'pin_name' in HAL, checks that it has - type 'type', and writes to 'out' a reference carrying that pin's - value. The reference is to storage the caller owns, not to the named - pin itself. Returns 0 on success. */ -typedef int (*nonrt_resolve_fn)(const char *pin_name, - hal_type_t type, - hal_refs_u *out, - void *arg); - -/* Filled in by nonrt_attach(). A module that reports is_identity has - joints equal to axes and the caller needs no module code at all, so - forward and inverse may be left NULL. */ -typedef struct { - int (*forward)(const double *joints, EmcPose *pos, - const KINEMATICS_FORWARD_FLAGS *fflags, - KINEMATICS_INVERSE_FLAGS *iflags); - int (*inverse)(const EmcPose *pos, double *joints, - const KINEMATICS_INVERSE_FLAGS *iflags, - KINEMATICS_FORWARD_FLAGS *fflags); - int is_identity; -} nonrt_ops_t; - -/* Exported by a participating module: - int nonrt_attach(const char *coordinates, nonrt_ops_t *ops, - nonrt_resolve_fn resolve, void *arg); - Returns 0 on success. */ - -/* Convenience for the common case: resolve one float pin, by printf - style name, into a haldata field. */ -static inline int nonrt_resolve_real(nonrt_resolve_fn resolve, void *arg, - hal_real_t *dst, const char *fmt, ...) -{ - char name[HAL_NAME_LEN + 1]; - hal_refs_u ref; - va_list ap; - - if (!resolve || !dst) return -1; - - va_start(ap, fmt); - rtapi_vsnprintf(name, sizeof(name), fmt, ap); - va_end(ap); - - if (resolve(name, HAL_FLOAT, &ref, arg) != 0) return -1; - - *dst = ref.r; - return 0; -} - -#endif /* NONRT_KINS_H */ diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index 472394cefdd..24b432262fb 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -27,10 +27,12 @@ * Using modules must supply function: switchkinsSetup() */ #include +#include #include #include #include "switchkins.h" +#include //********************************************************************* // kinematic functions (default=0 for err detection): @@ -46,6 +48,15 @@ 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 int inited; + // types provided, counted in rtapi_app_main() once they are all in static int kins_count; static int register_error; @@ -102,6 +113,36 @@ static void get_lastpose(int ktype, EmcPose* pos) pos->w = lastpose[ktype].w; } // get_lastpose() +// the block sees the pins as they are now, and the type asked for +static void read_block(int ktype) +{ + rt_params.ktype = ktype; + kinsParamsPinsRead(pins, kp.params, kp.nparams, &rt_params); +} + +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 @@ -113,7 +154,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); @@ -123,8 +164,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); @@ -163,6 +204,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() @@ -174,26 +219,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; @@ -223,12 +279,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() @@ -239,9 +303,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); @@ -256,9 +324,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 @@ -275,10 +347,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 } @@ -307,6 +381,12 @@ int kinematicsJacobian(const double *joint, 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); @@ -333,7 +413,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); @@ -346,6 +426,65 @@ 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; } + return 0; +} // switchkinsRegisterOps() + int switchkinsRegisterFrames(int ktype, KT kwork, KT ktool, const PmRotationMatrix *native) { @@ -400,26 +539,6 @@ int switchkinsRegisterToolFrameInverse(int ktype, KTI kinv) return 0; } // switchkinsRegisterToolFrameInverse() -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]) { return -1; } - return ktype_flags[ktype]; -} // kinematicsTypeFlags() - EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsType); @@ -435,7 +554,46 @@ EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); EXPORT_SYMBOL(switchkinsDeclare); EXPORT_SYMBOL(kinematicsTypeFlags); EXPORT_SYMBOL(switchkinsRegisterJacobian); +EXPORT_SYMBOL(switchkinsRegisterOps); EXPORT_SYMBOL(switchkinsInit); +EXPORT_SYMBOL(switchkinsDescribe); +EXPORT_SYMBOL(switchkinsDescribeSetup); + +//********************************************************************* +// the module as registered so far, described for a caller outside RT +int switchkinsDescribeSetup(const kparms *k, kins_module_info *info) +{ + 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 @@ -470,7 +628,7 @@ int switchkinsInit(const int comp_id, // 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; } @@ -517,6 +675,7 @@ int switchkinsInit(const int comp_id, // a type left out below the highest one provided is a gap, not a count for (i=0; i < kins_count; i++) { + if (kops[i]) { continue; } if (ksetups[i] && kfwds[i] && kinvs[i]) { continue; } rtapi_print_msg(RTAPI_MSG_ERR, "switchkins: switchkins-type %d incomplete:%s%s%s\n", @@ -550,10 +709,36 @@ int switchkinsInit(const int comp_id, 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); } } + inited = 1; return 0; error: diff --git a/src/emc/kinematics/switchkins.h b/src/emc/kinematics/switchkins.h index c7114262403..c5f87b35117 100644 --- a/src/emc/kinematics/switchkins.h +++ b/src/emc/kinematics/switchkins.h @@ -7,7 +7,10 @@ #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, @@ -85,10 +88,32 @@ typedef int (*KJ)(const double *joint, // 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 index 4a4cc05153c..8ab98b54223 100644 --- a/src/emc/kinematics/switchkins_main.c +++ b/src/emc/kinematics/switchkins_main.c @@ -19,9 +19,10 @@ /* switchkins_main.c provides rtapi_app_main() for kinematics modules * built around switchkins.c. A module that gets its rtapi_app_main() * from somewhere else (a halcompile component, for instance) links -* switchkins.c alone and calls switchkinsInit() itself. +* switchkins.c without this file and calls switchkinsInit() itself. * -* Using modules must supply function: switchkinsSetup() +* Using modules must supply function: switchkinsSetup(), which +* switchkinsRunSetup() in switchkins_setup.c runs. */ #include #include @@ -41,43 +42,8 @@ static int comp_id = -1; int rtapi_app_main(void) { kparms kp; - KS ksetup[3] = {NULL}; - KF kfwd[3] = {NULL}; - KI kinv[3] = {NULL}; - int i; - // defaults prior to switchkinsSetup() call - kp.kinsname = NULL; - kp.halprefix = NULL; - kp.required_coordinates = ""; - kp.max_joints = 0; // Setup must supply - kp.allow_duplicates = 0; - kp.fwd_iterates_mask = 0; - kp.gui_kinstype = -1; // negative means: not used - - kp.sparm = sparm; // module parm passed to kins - - // switchkinsSetup() provides types 0,1,2 and may also call - // switchkinsRegister() for any others - if (switchkinsSetup(&kp, - &ksetup[0], &ksetup[1], &ksetup[2], - &kfwd[0], &kfwd[1], &kfwd[2], - &kinv[0], &kinv[1], &kinv[2])) { - rtapi_print_msg(RTAPI_MSG_ERR,"\nSwitchkins FAIL:\n"); - return -1; - } - - // the types switchkinsSetup() supplied go in by the same route as - // any other, so that providing one twice is caught - for (i=0; i < 3; i++) { - if (!ksetup[i] && !kfwd[i] && !kinv[i]) { continue; } - if (switchkinsRegister(i, ksetup[i], kfwd[i], kinv[i])) { return -1; } - } - - if (!kp.kinsname) { - rtapi_print_msg(RTAPI_MSG_ERR,"\nSwitchkins FAIL:\n"); - return -1; - } + if (switchkinsRunSetup(&kp, sparm)) { return -1; } comp_id = hal_init(kp.kinsname); if (comp_id < 0) return comp_id; diff --git a/src/emc/kinematics/switchkins_setup.c b/src/emc/kinematics/switchkins_setup.c new file mode 100644 index 00000000000..eab033e919a --- /dev/null +++ b/src/emc/kinematics/switchkins_setup.c @@ -0,0 +1,100 @@ +/* + 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 copy outside RT replays setup once, 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) +{ + // One copy of the module per process, however many callers load it: + // the replay registers the types in the module's statics, so it runs + // once and later calls describe what it left. + static kparms kp; + static int replayed; // 0 not yet, 1 done, -1 failed + static char sparm_seen[256]; + const char *s = sparm ? sparm : ""; + (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; } + + if (!replayed) { + rtapi_strlcpy(sparm_seen, s, sizeof(sparm_seen)); + // a copy outside RT: register the types the way the module would + replayed = switchkinsRunSetup(&kp, sparm ? sparm_seen : NULL) ? -1 : 1; + } else if (strncmp(sparm_seen, s, sizeof(sparm_seen) - 1)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "kinsDescribe: already set up with sparm '%s'," + " not '%s'\n", sparm_seen, s); + return -1; + } + if (replayed < 0) { return -1; } + if (switchkinsDescribeSetup(&kp, info)) { return -1; } + return 0; +} // kinsDescribe() + +EXPORT_SYMBOL(switchkinsRunSetup); +EXPORT_SYMBOL(kinsDescribe); diff --git a/src/emc/kinematics/trivkins.c b/src/emc/kinematics/trivkins.c index 0690aa9ee39..2de2368614a 100644 --- a/src/emc/kinematics/trivkins.c +++ b/src/emc/kinematics/trivkins.c @@ -10,63 +10,27 @@ * ********************************************************************/ -#include #include /* RTAPI realtime OS API */ #include /* RTAPI realtime module decls */ -#include #include #include #include #include -#include "nonrt_kins.h" - - -#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); -} - -int kinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) -{ - return identityKinematicsJacobian(joints, pos, jac, iflags); -} - -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; @@ -75,19 +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); -EXPORT_SYMBOL(kinematicsJacobian); 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; @@ -99,29 +84,14 @@ 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; } void rtapi_app_exit(void) { hal_exit(comp_id); } - -// Non-RT entry point: joints are axes, so a non-RT caller needs no -// module code at all and reads nothing from HAL. -int nonrt_attach(const char* coordinates, nonrt_ops_t* ops, - nonrt_resolve_fn resolve, void* arg) -{ - (void)coordinates; (void)resolve; (void)arg; - ops->forward = NULL; - ops->inverse = NULL; - ops->is_identity = 1; - return 0; -} - -EXPORT_SYMBOL(nonrt_attach); diff --git a/src/emc/kinematics/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_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index 69abd527dac..b1ccec60eca 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -2,14 +2,17 @@ * Description: kinematics_user.c * Non-RT loader for kinematics modules * - * Loads a kinematics .so with dlopen and calls the nonrt_attach() it - * exports, so this process evaluates the kinematics the machine is - * running, at whatever poses it likes. See nonrt_kins.h. + * 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: one input pin + * of the caller's component per table entry, connected to the signal the + * RT instance's pin reads, so the values are the live ones; and the tool + * 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. * - * Identity kinematics needs no module code: the module says so through - * nonrt_ops_t and this file maps joints to axes directly. A module - * exporting no nonrt_attach() is not an error either; the context comes - * back flagged rt_only. + * 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 @@ -19,31 +22,30 @@ ********************************************************************/ #include "kinematics_user.h" -#include #include #include #include #include -#include +#include #include "config.h" /* EMC2_HOME */ -typedef int (*nonrt_attach_fn)(const char *coordinates, nonrt_ops_t *ops, - nonrt_resolve_fn resolve, void *arg); +typedef int (*kins_describe_fn)(const char *coordinates, const char *sparm, + kins_module_info *info); -/* One per value a kinematics module reads is a generous bound. */ -#define MAX_MADE_SIGNALS 16 -#define MAX_BOUND_PINS 16 +#define MAX_BOUND_PINS (KINS_MAX_PARAMS + AXIS_COUNT) +#define MAX_MADE_SIGNALS MAX_BOUND_PINS struct KinematicsUserContext { int initialized; - int rt_only; /* 1 if the module exports no nonrt_attach() */ - int is_identity; /* 1 for identity kinematics: no module code needed */ + int rt_only; /* 1 if the module exports no kinsDescribe() */ KINEMATICS_TYPE kins_type; void *rt_handle; /* dlopen handle */ - nonrt_ops_t ops; + kins_module_info info; + kins_params params; + kins_scratch scratch; + int ktype; /* kinematics type being evaluated */ int num_joints; - int joint_to_axis[KINEMATICS_USER_MAX_JOINTS]; /* identity path only */ char module_name[64]; int comp_id; /* the caller's component, owns the pins made here */ const char *prefix; /* its name, which those pin names start with */ @@ -51,6 +53,10 @@ struct KinematicsUserContext { int num_made_signals; hal_refs_u *cell; /* HAL storage those pins are made against */ int num_cells; + int cell_of_param[KINS_MAX_PARAMS]; /* -1 if not bound */ + int cell_of_tool[AXIS_COUNT]; /* motion.tooloffset.*, -1 if absent */ + int tool_param; /* the table's tool entry, -1 if none */ + int warned_tool; }; /* ======================================================================== @@ -58,16 +64,16 @@ struct KinematicsUserContext { * ======================================================================== */ /* - * Give a kinematics module a reference to a value it asked for. + * Give the block a reference to a value it needs. * * The reference is to a pin of ours rather than into the RT instance's, - * so that its lifetime is ours: see nonrt_kins.h. Ours is connected to - * the signal the RT pin reads, or, when the RT pin has no signal, to one - * made here and removed again in kinematicsUserFree(). + * so that its lifetime is ours. Ours is connected to the signal the RT + * pin reads, or, when the RT pin has no signal, to one made here and + * removed again in kinematicsUserFree(). * * The reference has to live in HAL shared memory, since that is where * HAL rewrites it on connect and disconnect, so the pins are made - * against hal_malloc() cells and the module gets what a cell holds once + * against hal_malloc() cells and the block reads what a cell holds once * the connection is in place. */ static int make_signal(KinematicsUserContext *ctx, const char *pin_name, @@ -100,23 +106,30 @@ static int new_pin(int comp_id, hal_type_t type, hal_refs_u *out, case HAL_FLOAT: return hal_pin_new_real(comp_id, HAL_IN, &out->r, 0.0, "%s", name); case HAL_S32: return hal_pin_new_si32(comp_id, HAL_IN, &out->s, 0, "%s", name); case HAL_U32: return hal_pin_new_ui32(comp_id, HAL_IN, &out->u, 0, "%s", name); - case HAL_S64: return hal_pin_new_sint(comp_id, HAL_IN, &out->s, 0, "%s", name); - case HAL_U64: return hal_pin_new_uint(comp_id, HAL_IN, &out->u, 0, "%s", name); default: break; } return -1; } -static int bind_pin(const char *pin_name, hal_type_t type, - hal_refs_u *out, void *arg) +/* 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; +} + +/* Bind pin_name; returns the cell index, or -1. */ +static int bind_pin(KinematicsUserContext *ctx, const char *pin_name, + hal_type_t type) { - KinematicsUserContext *ctx = (KinematicsUserContext *)arg; char signal[HAL_NAME_LEN + 1]; char mine[HAL_NAME_LEN + 1]; hal_refs_u *cell; hal_query_t q; - - if (!ctx || !pin_name || !out) return -1; + int idx; memset(&q, 0, sizeof(q)); q.name = pin_name; @@ -149,7 +162,8 @@ static int bind_pin(const char *pin_name, hal_type_t type, fprintf(stderr, "kinematicsUserInit: too many pins to bind\n"); return -1; } - cell = &ctx->cell[ctx->num_cells++]; + idx = ctx->num_cells; + cell = &ctx->cell[idx]; if (new_pin(ctx->comp_id, type, cell, mine) != 0) { fprintf(stderr, "kinematicsUserInit: cannot create pin '%s'\n", mine); @@ -160,30 +174,103 @@ static int bind_pin(const char *pin_name, hal_type_t type, mine, signal); return -1; } + ctx->num_cells++; + return idx; +} - *out = *cell; - return 0; +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; + } } -/* ======================================================================== - * Identity joint mapping - * ======================================================================== */ +static double cell_value(const hal_refs_u *cell, kins_param_type t) +{ + switch (t) { + case KINS_PARAM_BIT: return hal_get_bool(cell->b) ? 1.0 : 0.0; + case KINS_PARAM_S32: return hal_get_si32(cell->s); + case KINS_PARAM_U32: return hal_get_ui32(cell->u); + default: return hal_get_real(cell->r); + } +} -static void fill_identity_joint_map(KinematicsUserContext *ctx, const char *coords) +/* Bind every input of the table, and motion's tool where motion is there. */ +static int bind_all(KinematicsUserContext *ctx) { - int i, j = 0; - for (i = 0; i < KINEMATICS_USER_MAX_JOINTS; i++) ctx->joint_to_axis[i] = -1; - if (!coords) return; - for (; *coords && j < ctx->num_joints; coords++) { - int axis; - switch (tolower((unsigned char)*coords)) { - case 'x': axis = 0; break; case 'y': axis = 1; break; - case 'z': axis = 2; break; case 'a': axis = 3; break; - case 'b': axis = 4; break; case 'c': axis = 5; break; - case 'u': axis = 6; break; case 'v': axis = 7; break; - case 'w': axis = 8; break; default: continue; - } - ctx->joint_to_axis[j++] = axis; + 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->cell_of_param[i] = -1; + for (i = 0; i < AXIS_COUNT; i++) ctx->cell_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->cell_of_param[i] = bind_pin(ctx, name, hal_type_of(d->type)); + if (ctx->cell_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->cell_of_tool[i] = bind_pin(ctx, name, HAL_FLOAT); + if (ctx->cell_of_tool[i] < 0) return -1; + } + return 0; +} + +/* The block sees the pins as they are now. */ +static void refresh(KinematicsUserContext *ctx) +{ + int i; + double tool[AXIS_COUNT]; + int have_motion_tool = 0; + + for (i = 0; i < ctx->info.nparams; i++) { + int c = ctx->cell_of_param[i]; + if (c < 0) continue; + ctx->params.geometry[i] = cell_value(&ctx->cell[c], ctx->info.params[i].type); + } + if (ctx->tool_param >= 0) { + ctx->params.tool.tran.z = ctx->params.geometry[ctx->tool_param]; + } + + for (i = 0; i < AXIS_COUNT; i++) { + int c = ctx->cell_of_tool[i]; + tool[i] = 0.0; + if (c < 0) continue; + tool[i] = hal_get_real(ctx->cell[c].r); + 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. */ + if (ctx->tool_param >= 0 && !ctx->warned_tool + && 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]; } } @@ -193,11 +280,12 @@ static void fill_identity_joint_map(KinematicsUserContext *ctx, const char *coor static int load_module(KinematicsUserContext *ctx, const char *module_name, - const char *coordinates) + const char *coordinates, + const char *sparm) { char module_path[512]; void *handle; - nonrt_attach_fn attach; + kins_describe_fn describe; snprintf(module_path, sizeof(module_path), "%s/rtlib/%s.so", EMC2_HOME, module_name); @@ -210,18 +298,18 @@ static int load_module(KinematicsUserContext *ctx, } ctx->rt_handle = handle; - attach = (nonrt_attach_fn)dlsym(handle, "nonrt_attach"); - if (!attach) { - fprintf(stderr, "kinematicsUserInit: '%s' exports no nonrt_attach\n", - module_name); + 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 (attach(coordinates, &ctx->ops, bind_pin, ctx) != 0) { - fprintf(stderr, "kinematicsUserInit: nonrt_attach failed for '%s'\n", + if (describe(coordinates, sparm, &ctx->info) != 0) { + fprintf(stderr, "kinematicsUserInit: kinsDescribe failed for '%s'\n", module_name); dlclose(handle); ctx->rt_handle = NULL; @@ -229,21 +317,28 @@ static int load_module(KinematicsUserContext *ctx, return -1; } - if (ctx->ops.is_identity) { - ctx->is_identity = 1; - ctx->kins_type = KINEMATICS_IDENTITY; - return 0; + 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 (!ctx->ops.forward || !ctx->ops.inverse) { - fprintf(stderr, "kinematicsUserInit: '%s' set no fwd/inv\n", module_name); + 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->kins_type = KINEMATICS_BOTH; + ctx->ktype = 0; + ctx->kins_type = ctx->info.ops[0]->identity ? KINEMATICS_IDENTITY + : KINEMATICS_BOTH; return 0; } @@ -251,11 +346,12 @@ static int load_module(KinematicsUserContext *ctx, * Public API * ======================================================================== */ -KinematicsUserContext* kinematicsUserInit(const char* kins_type, - int num_joints, - const char* coordinates, - int comp_id, - const char* prefix) +KinematicsUserContext* kinematicsUserInitSparm(const char* kins_type, + int num_joints, + const char* coordinates, + const char* sparm, + int comp_id, + const char* prefix) { KinematicsUserContext *ctx; @@ -280,59 +376,124 @@ KinematicsUserContext* kinematicsUserInit(const char* kins_type, } strncpy(ctx->module_name, kins_type, sizeof(ctx->module_name) - 1); - load_module(ctx, kins_type, coordinates); - - if (ctx->is_identity) { - fill_identity_joint_map(ctx, coordinates); + if (load_module(ctx, kins_type, coordinates, sparm) == 0) { + if (bind_all(ctx) != 0) { + fprintf(stderr, "kinematicsUserInit: cannot bind 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 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; - if (ctx->is_identity) { - int i; - for (i = 0; i < ctx->num_joints; i++) { - int ax = ctx->joint_to_axis[i]; - joints[i] = (ax >= 0) ? emcPoseGetAxis(world, ax) : 0.0; - } - return 0; + refresh(ctx); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) j[i] = 0.0; + if (kinsOpsInverse(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, + world, j, &iflags, &fflags) != 0) { + return -1; } - - if (ctx->rt_only) return -1; - return ctx->ops.inverse(world, joints, NULL, NULL); + for (i = 0; i < ctx->num_joints; i++) joints[i] = j[i]; + 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; - if (ctx->is_identity) { - int i; - memset(world, 0, sizeof(*world)); - for (i = 0; i < ctx->num_joints; i++) { - int ax = ctx->joint_to_axis[i]; - if (ax >= 0) emcPoseSetAxis(world, ax, joints[i]); - } - return 0; + refresh(ctx); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + j[i] = (i < ctx->num_joints) ? joints[i] : 0.0; } + 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; - return ctx->ops.forward(joints, world, NULL, NULL); + + refresh(ctx); + for (r = 0; r < EMCMOT_MAX_JOINTS; r++) j[r] = 0.0; + 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) return 0; - return ctx->is_identity; + if (!ctx || !ctx->initialized || ctx->rt_only) return 0; + return ctx->info.ops[ctx->ktype]->identity; } int kinematicsUserGetNumJoints(KinematicsUserContext* ctx) @@ -355,8 +516,16 @@ const char* kinematicsUserGetModuleName(KinematicsUserContext* ctx) int kinematicsUserRefreshParams(KinematicsUserContext* ctx) { - (void)ctx; - return 0; /* nothing to refresh: the bound pins are the live values */ + 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) diff --git a/src/emc/kinematics_userspace/kinematics_user.h b/src/emc/kinematics_userspace/kinematics_user.h index d01d8a8d277..0a7187537f9 100644 --- a/src/emc/kinematics_userspace/kinematics_user.h +++ b/src/emc/kinematics_userspace/kinematics_user.h @@ -6,10 +6,11 @@ * the RT kinematics interface. Used by the 9D planner to compute joint * positions from world coordinates without requiring RT kernel calls. * - * The kinematics module is loaded into this process and given input pins - * belonging to the caller's HAL component, connected to the same signals - * the running RT instance reads. Its own forward and inverse then work on - * live values, unmodified. + * 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, and from motion's tool + * offset pins where motion is loaded, so the maths runs on live values. * * Author: LinuxCNC * License: GPL Version 2 @@ -62,6 +63,30 @@ KinematicsUserContext* kinematicsUserInit(const char* kins_type, 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); + /** * Perform inverse kinematics (world coords -> joint positions) * @@ -86,6 +111,24 @@ 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, so the + * derivative is taken on the solution branch the module picks there. + * + * @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) * @@ -119,20 +162,18 @@ KINEMATICS_TYPE kinematicsUserGetType(KinematicsUserContext* ctx); const char* kinematicsUserGetModuleName(KinematicsUserContext* ctx); /** - * Refresh kinematics parameters (no-op) - * - * The bound pins read the live values, so there is nothing to fetch. - * This function is kept for API compatibility but does nothing. + * 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 always + * @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 nonrt_attach() and so cannot be evaluated + * 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 diff --git a/src/emc/motion_planning/Submakefile b/src/emc/motion_planning/Submakefile index 553849e7ba5..3a8ccdf737a 100644 --- a/src/emc/motion_planning/Submakefile +++ b/src/emc/motion_planning/Submakefile @@ -8,9 +8,11 @@ LIBKINSLIMITS_CXXSRCS := $(addprefix emc/motion_planning/, \ 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) diff --git a/src/emc/motion_planning/jacobian.cc b/src/emc/motion_planning/jacobian.cc index a7d5a7661e7..ba8c69fdd42 100644 --- a/src/emc/motion_planning/jacobian.cc +++ b/src/emc/motion_planning/jacobian.cc @@ -12,7 +12,6 @@ #include "jacobian.hh" #include #include -#include namespace motion_planning { @@ -38,128 +37,12 @@ bool JacobianCalculator::init(KinematicsUserContext* kins_ctx) { return true; } -void JacobianCalculator::computeTrivkins(double J[9][9]) { - // Zero the matrix - std::memset(J, 0, sizeof(double) * 9 * 9); - - // For trivkins, the Jacobian is identity (with axis mapping) - // Since trivkins maps: joint[i] = world_axis[mapped_axis[i]] - // The Jacobian is: J[joint][axis] = 1 if axis == mapped_axis[joint], else 0 - - // For a simple XYZ trivkins: - // J[0][AXIS_X] = 1 (joint 0 = X) - // J[1][AXIS_Y] = 1 (joint 1 = Y) - // J[2][AXIS_Z] = 1 (joint 2 = Z) - // etc. - - // We need to query the kinematics context for the mapping. - // Since the context is opaque, we use inverse kinematics to determine - // the mapping. - - // Test each axis: perturb it and see which joint changes - EmcPose zero_pose; - ZERO_EMC_POSE(zero_pose); - double zero_joints[9]; - kinematicsUserInverse(kins_ctx_, &zero_pose, zero_joints); - - for (int axis = 0; axis < AXIS_COUNT; axis++) { - EmcPose test_pose = zero_pose; - emcPoseSetAxis(&test_pose, axis, 1.0); - - double test_joints[9]; - kinematicsUserInverse(kins_ctx_, &test_pose, test_joints); - - for (int joint = 0; joint < num_joints_; joint++) { - double delta = test_joints[joint] - zero_joints[joint]; - if (std::fabs(delta) > 0.5) { - // This axis maps to this joint - J[joint][axis] = 1.0; - } - } - } -} - -bool JacobianCalculator::computeNumerical(const EmcPose& pose, double J[9][9]) { - // Zero the matrix - std::memset(J, 0, sizeof(double) * 9 * 9); - - // Compute joints at nominal pose - double joints_center[9]; - if (kinematicsUserInverse(kins_ctx_, &pose, joints_center) != 0) { - return false; - } - - // Perturb each axis and compute derivatives - for (int axis = 0; axis < AXIS_COUNT; axis++) { - // Choose perturbation size based on axis type - double delta = (axis < 3 || axis >= 6) ? DELTA_LINEAR : DELTA_ROTARY; - - // Positive perturbation - EmcPose pose_plus = pose; - double val_plus = emcPoseGetAxis(&pose_plus, axis) + delta; - emcPoseSetAxis(&pose_plus, axis, val_plus); - - double joints_plus[9]; - if (kinematicsUserInverse(kins_ctx_, &pose_plus, joints_plus) != 0) { - // Kinematics failed - use one-sided difference - for (int joint = 0; joint < num_joints_; joint++) { - J[joint][axis] = (joints_plus[joint] - joints_center[joint]) / delta; - } - continue; - } - - // Negative perturbation - EmcPose pose_minus = pose; - double val_minus = emcPoseGetAxis(&pose_minus, axis) - delta; - emcPoseSetAxis(&pose_minus, axis, val_minus); - - double joints_minus[9]; - if (kinematicsUserInverse(kins_ctx_, &pose_minus, joints_minus) != 0) { - // Use forward difference - for (int joint = 0; joint < num_joints_; joint++) { - J[joint][axis] = (joints_plus[joint] - joints_center[joint]) / delta; - } - continue; - } - - // Central difference (most accurate) - for (int joint = 0; joint < num_joints_; joint++) { - J[joint][axis] = (joints_plus[joint] - joints_minus[joint]) / (2.0 * delta); - } - } - - // Check for NaN/Inf values and replace with safe defaults - bool had_nan = false; - for (int joint = 0; joint < num_joints_; joint++) { - for (int axis = 0; axis < AXIS_COUNT; axis++) { - if (!std::isfinite(J[joint][axis])) { - // Replace NaN/Inf with 0 (assume no coupling) - J[joint][axis] = 0.0; - had_nan = true; - } - } - } - - // If we had NaN values, the Jacobian may be unreliable - // Return true anyway but the condition number check will catch issues - (void)had_nan; // Could log this in debug mode - - return true; -} - bool JacobianCalculator::compute(const EmcPose& pose, double J[9][9]) { if (!kins_ctx_) { return false; } - - if (is_identity_) { - // For trivkins, use the fast identity computation - computeTrivkins(J); - return true; - } else { - // For non-trivial kinematics, use numerical differentiation - return computeNumerical(pose, J); - } + std::memset(J, 0, sizeof(double) * 9 * 9); + return kinematicsUserJacobian(kins_ctx_, &pose, J) == 0; } double JacobianCalculator::conditionNumber(const double J[9][9]) { diff --git a/src/emc/motion_planning/jacobian.hh b/src/emc/motion_planning/jacobian.hh index 8713e89f180..2db3cf00d3e 100644 --- a/src/emc/motion_planning/jacobian.hh +++ b/src/emc/motion_planning/jacobian.hh @@ -3,7 +3,8 @@ * Jacobian calculation for userspace kinematics trajectory planning * * Computes the Jacobian matrix relating world velocities to joint - * velocities. For trivkins this is the identity matrix. + * velocities, from the module's own closed form through the non-RT + * kinematics loader. * * Author: LinuxCNC * License: GPL Version 2 @@ -25,8 +26,8 @@ namespace motion_planning { * Computes the Jacobian matrix J where: * joint_velocities = J × world_velocities * - * For trivkins, J is the identity matrix (with appropriate axis mapping). - * For non-trivial kinematics, J is computed via numerical differentiation. + * The module answers: a closed form where it has one, its inverse + * differenced where it does not. See kinematicsUserJacobian(). */ class JacobianCalculator { public: @@ -72,26 +73,9 @@ public: bool isIdentity() const { return is_identity_; } private: - /** - * Compute Jacobian for trivkins (identity with axis mapping) - */ - void computeTrivkins(double J[9][9]); - - /** - * Compute Jacobian via numerical differentiation - * Uses central differences: J[j][a] = (f(x+h) - f(x-h)) / (2h) - */ - bool computeNumerical(const EmcPose& pose, double J[9][9]); - KinematicsUserContext* kins_ctx_; bool is_identity_; int num_joints_; - - // Perturbation size for numerical differentiation (mm or degrees) - // Must be large enough for kinematics to produce stable results - // but small enough for accurate derivatives - static constexpr double DELTA_LINEAR = 0.1; // 0.1 mm - static constexpr double DELTA_ROTARY = 0.1; // 0.1 degrees }; } // namespace motion_planning diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index 161e8abebba..abbff00a375 100644 --- a/src/hal/components/millturn.comp +++ b/src/hal/components/millturn.comp @@ -123,7 +123,7 @@ static int turnKinematicsJacobian(const double *j, // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp; + kparms kp = {0}; (void)__comp_inst; (void)prefix; (void)extra_arg; kp.kinsname = "millturn"; diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index 0387d73c85a..c5d4db81f66 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -222,7 +222,7 @@ static int tdrKinematicsJacobian(const double *j, // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp; + kparms kp = {0}; (void)__comp_inst; (void)prefix; (void)extra_arg; kp.kinsname = "xyzab_tdr_kins"; diff --git a/src/hal/components/xyzacb_trsrn.comp b/src/hal/components/xyzacb_trsrn.comp index 45a8a9af4f9..3d26044943d 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -595,7 +595,7 @@ static int toolKinematicsJacobian(const double *j, // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp; + kparms kp = {0}; (void)__comp_inst; (void)prefix; (void)extra_arg; kp.kinsname = "xyzacb_trsrn"; diff --git a/src/hal/components/xyzbca_trsrn.comp b/src/hal/components/xyzbca_trsrn.comp index 75e6f7ea0f4..075e3cbfeb2 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -598,7 +598,7 @@ static int toolKinematicsJacobian(const double *j, // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp; + kparms kp = {0}; (void)__comp_inst; (void)prefix; (void)extra_arg; kp.kinsname = "xyzbca_trsrn"; From 5617cfc8a0175b9a9fbe4919de744bf66e2f2fb7 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 12:48:41 +1000 Subject: [PATCH 28/77] trtfuncs, xyzac-trt-kins, xyzbc-trt-kins, maxkins: move onto the parameter block The trt maths becomes two ops tables over one geometry table, TRT_PARAMS, with the joint map read from the block instead of the JX statics and the tool length from p->tool.tran.z, which the shared code fills from the tool-offset entry. The two modules register the tables with switchkinsRegisterOps() in the order sparm decides, and the identity and userk types come from the shared ops. The joint assignment print and the required-letter check that trtKinematicsSetup() did are now the shared code's, so the setup function goes with the haldata. maxkins keeps its fixed joint order and becomes a kins_single.c module: the table declares pivot-length as the HAL_IO pin it was and conventional-directions as before, and the three functions read the block. Pin names and defaults are unchanged throughout. --- src/Makefile | 2 + src/emc/kinematics/kinematics.h | 57 +--- src/emc/kinematics/maxkins.c | 131 ++++----- src/emc/kinematics/trtfuncs.c | 402 +++++++++++----------------- src/emc/kinematics/xyzac-trt-kins.c | 48 ++-- src/emc/kinematics/xyzbc-trt-kins.c | 48 ++-- 6 files changed, 265 insertions(+), 423 deletions(-) diff --git a/src/Makefile b/src/Makefile index b0026deb94f..7ead32fc293 100644 --- a/src/Makefile +++ b/src/Makefile @@ -1136,6 +1136,8 @@ 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 diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index 8bd17137362..97f2e7f2cb1 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -552,6 +552,7 @@ typedef struct kins_scratch { 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; @@ -717,57 +718,11 @@ extern int userkKinematicsInverse(const struct EmcPose * world, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags); //********************************************************************* -// 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 xyzacKinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags); - - -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); - -extern int xyzbcKinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags); +// 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 diff --git a/src/emc/kinematics/maxkins.c b/src/emc/kinematics/maxkins.c index a5725480094..cce1fbc1797 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 ********************************************************************/ @@ -22,6 +22,7 @@ #include #include #include /* these decls */ +#include #define d2r(d) ((d)*PM_PI/180.0) #define r2d(r) ((r)*180.0/PM_PI) @@ -30,27 +31,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, 0, 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])); + 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])); @@ -82,22 +89,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); @@ -122,27 +131,27 @@ int kinematicsInverse(const EmcPose * pos, return 0; } -int kinematicsJacobian(const double *joints, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +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) { - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); + 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 + pos->w; + const double R = pivot_length + p->geometry[P_TOOL_LENGTH] + pos->w; int j; (void)joints; (void)iflags; memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); - // kinematicsInverse() 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 + // 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; @@ -164,40 +173,40 @@ int kinematicsJacobian(const double *joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +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 }, +}; -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsJacobian); 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/trtfuncs.c b/src/emc/kinematics/trtfuncs.c index b445524b6e9..023f0a3d1fa 100644 --- a/src/emc/kinematics/trtfuncs.c +++ b/src/emc/kinematics/trtfuncs.c @@ -25,150 +25,68 @@ * 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 +116,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 +173,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 +187,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,32 +202,21 @@ 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) -{ - (void)joints; - (void)fflags; - // both rotaries carry the work, so the tool never turns in the machine - *rot = TOOL_FRAME_SPINDLE; - return 0; -} // xyzacKinematicsToolFrame() - -int xyzacKinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +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)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 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 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; @@ -320,12 +225,12 @@ int xyzacKinematicsJacobian(const double *joints, double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; int a; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); memset(dP, 0, sizeof(dP)); - // the computed position P of xyzacKinematicsInverse(), differentiated: - // its coefficients for x, y and z, and the same expressions with the + // 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; @@ -345,29 +250,41 @@ int xyzacKinematicsJacobian(const double *joints, for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } - return kinsJacobianFromMappedAxes(trtfuncs_max_joints, - (const double (*)[EMCMOT_MAX_AXIS])dP, - jac); -} // xyzacKinematicsJacobian() - -int xyzbcKinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) + 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, +}; + +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) @@ -396,25 +313,27 @@ 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; - 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 @@ -451,23 +370,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 kinsPoseToMappedJoints(p, &P, joints); +} // xyzbc_inverse() - return 0; -} // xyzbcKinematicsInverse() - -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); @@ -482,32 +397,21 @@ int xyzbcKinematicsWorkFrame(const double *joints, rot->z.z = cos(b_rad); return 0; -} // xyzbcKinematicsWorkFrame() - -int xyzbcKinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) -{ - (void)joints; - (void)fflags; - // both rotaries carry the work, so the tool never turns in the machine - *rot = TOOL_FRAME_SPINDLE; - return 0; -} // xyzbcKinematicsToolFrame() +} // xyzbc_work_frame() -int xyzbcKinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +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)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 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 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; @@ -516,12 +420,12 @@ int xyzbcKinematicsJacobian(const double *joints, double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; int a; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); memset(dP, 0, sizeof(dP)); - // see the comment in xyzacKinematicsJacobian(); dpx and dpz of the - // inverse depend on b as well + // 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; @@ -540,7 +444,15 @@ int xyzbcKinematicsJacobian(const double *joints, for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } - return kinsJacobianFromMappedAxes(trtfuncs_max_joints, - (const double (*)[EMCMOT_MAX_AXIS])dP, - jac); -} // xyzbcKinematicsJacobian() + 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, +}; diff --git a/src/emc/kinematics/xyzac-trt-kins.c b/src/emc/kinematics/xyzac-trt-kins.c index b6b35538f25..3fc14fc9de4 100644 --- a/src/emc/kinematics/xyzac-trt-kins.c +++ b/src/emc/kinematics/xyzac-trt-kins.c @@ -4,11 +4,12 @@ * * 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 @@ -23,47 +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); - switchkinsRegisterJacobian(1, xyzacKinematicsJacobian); + 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); - switchkinsRegisterJacobian(0, xyzacKinematicsJacobian); - - *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 401311e4398..45c41b448dd 100644 --- a/src/emc/kinematics/xyzbc-trt-kins.c +++ b/src/emc/kinematics/xyzbc-trt-kins.c @@ -4,11 +4,12 @@ * * 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 @@ -23,47 +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); - switchkinsRegisterJacobian(1, xyzbcKinematicsJacobian); + 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); - switchkinsRegisterJacobian(0, xyzbcKinematicsJacobian); - - *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; } From cf31bd94ed208c1f491300e611c54fe5e6ab5ffd Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 12:57:51 +1000 Subject: [PATCH 29/77] corexykins, rotatekins, rosekins, tripodkins, scorbot-kins, the deltas, matrixkins, userkins: move onto the parameter block Each becomes a kins_module over one ops table and links kins_single.c. The maths is unchanged: where it read a pin it reads the block, what it kept between calls it keeps in the scratch, so rosekins counts its turns per caller and reports them as declared outputs, and tripodkins keeps its HAL_IO pins. kinematicsHome() goes from corexykins and rotatekins; nothing called it. The two deltas share their maths with a python module through a header whose geometry was statics; it is a struct the caller passes now, python API unchanged. matrixkins's nine coefficients were HAL parameters and are pins of the same names. userkins includes kins_util.c and kins_single.c by name so halcompile builds it on its own, and kins_single.c joins the sources installed in share/linuxcnc. --- .gitignore | 1 + debian/linuxcnc-uspace-dev.install | 1 + src/Makefile | 19 +- src/emc/kinematics/Submakefile | 3 +- src/emc/kinematics/corexykins.c | 72 +++--- src/emc/kinematics/lineardeltakins-common.h | 46 ++-- src/emc/kinematics/lineardeltakins.c | 94 ++++---- src/emc/kinematics/lineardeltakins.cc | 14 +- src/emc/kinematics/rosekins.c | 110 +++++---- src/emc/kinematics/rotarydeltakins-common.h | 62 +++-- src/emc/kinematics/rotarydeltakins.c | 116 ++++----- src/emc/kinematics/rotarydeltakins.cc | 15 +- src/emc/kinematics/rotatekins.c | 84 ++++--- src/emc/kinematics/scorbot-kins.c | 127 +++------- src/emc/kinematics/tripodkins.c | 246 +++++--------------- src/hal/components/Submakefile | 1 + src/hal/components/matrixkins.comp | 196 ++++++++-------- src/hal/components/userkins.comp | 174 +++++++------- 18 files changed, 650 insertions(+), 731 deletions(-) diff --git a/.gitignore b/.gitignore index 82df1b00474..14b60adb42f 100644 --- a/.gitignore +++ b/.gitignore @@ -22,6 +22,7 @@ share/desktop-directories/linuxcnc-ref.directory share/desktop-directories/linuxcnc-doc.directory share/linuxcnc/switchkins.c share/linuxcnc/kins_util.c +share/linuxcnc/kins_single.c src/modules.order /configs/*/emc.nml !/configs/common/emc.nml diff --git a/debian/linuxcnc-uspace-dev.install b/debian/linuxcnc-uspace-dev.install index f55d2a4e230..1e845bd2fef 100644 --- a/debian/linuxcnc-uspace-dev.install +++ b/debian/linuxcnc-uspace-dev.install @@ -5,3 +5,4 @@ usr/lib/*.so usr/share/linuxcnc/Makefile.modinc usr/share/linuxcnc/switchkins.c usr/share/linuxcnc/kins_util.c +usr/share/linuxcnc/kins_single.c diff --git a/src/Makefile b/src/Makefile index 7ead32fc293..2ead045de1a 100644 --- a/src/Makefile +++ b/src/Makefile @@ -788,7 +788,7 @@ ifeq ($(BUILD_GUI),yes) $(FILE) ../share/gtksourceview-4/language-specs/*.lang $(DESTDIR)$(datadir)/gtksourceview-4/language-specs/ endif - $(FILE) ../src/emc/kinematics/switchkins.c ../src/emc/kinematics/kins_util.c $(DESTDIR)$(prefix)/share/linuxcnc/ + $(FILE) ../src/emc/kinematics/switchkins.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 @@ -918,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 \ @@ -1141,15 +1144,23 @@ 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 @@ -1158,14 +1169,20 @@ 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: diff --git a/src/emc/kinematics/Submakefile b/src/emc/kinematics/Submakefile index 7e2f2d84b4b..c71c18696e2 100644 --- a/src/emc/kinematics/Submakefile +++ b/src/emc/kinematics/Submakefile @@ -39,7 +39,8 @@ PYTARGETS += $(RDELTAMODULE) # in-tree ones link it. EMCKINEMATICSSRCS = \ ../share/linuxcnc/switchkins.c \ - ../share/linuxcnc/kins_util.c + ../share/linuxcnc/kins_util.c \ + ../share/linuxcnc/kins_single.c $(EMCKINEMATICSSRCS): ../share/linuxcnc/%.c: ./emc/kinematics/%.c $(ECHO) Copying switchkins source $(notdir $@) diff --git a/src/emc/kinematics/corexykins.c b/src/emc/kinematics/corexykins.c index 6d3ac4e75ab..ebc7a685d2a 100644 --- a/src/emc/kinematics/corexykins.c +++ b/src/emc/kinematics/corexykins.c @@ -9,12 +9,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]); @@ -30,11 +33,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; @@ -50,12 +55,13 @@ int kinematicsInverse(const EmcPose *pos return 0; } -int kinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *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; + (void)p; (void)joints; (void)pos; (void)iflags; @@ -67,23 +73,26 @@ int kinematicsJacobian(const double *joints, 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 const kins_ops corexy_ops = { + .forward = corexy_forward, + .inverse = corexy_inverse, + .jacobian = corexy_jacobian, +}; -KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; } +// 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); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; @@ -91,6 +100,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/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 d894bb46adb..5b780635578 100644 --- a/src/emc/kinematics/lineardeltakins.c +++ b/src/emc/kinematics/lineardeltakins.c @@ -19,50 +19,65 @@ #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); } -int kinematicsJacobian(const double *joints, +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; (void)iflags; - set_geometry(hal_get_real(haldata->r), hal_get_real(haldata->l)); + geometry_of(p, &g); memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][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) ? Ax : (i == 1) ? Bx : Cx; - double ty = (i == 0) ? Ay : (i == 1) ? By : Cy; + 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; @@ -73,32 +88,38 @@ int kinematicsJacobian(const double *joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +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) @@ -106,9 +127,4 @@ void rtapi_app_exit(void) hal_exit(comp_id); } -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); 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/rosekins.c b/src/emc/kinematics/rosekins.c index ac7a11159f5..4d088ffe88b 100644 --- a/src/emc/kinematics/rosekins.c +++ b/src/emc/kinematics/rosekins.c @@ -22,29 +22,36 @@ #include #include #include +#include -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsJacobian); 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; @@ -66,18 +73,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; @@ -96,9 +105,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; @@ -110,18 +119,20 @@ int kinematicsInverse(const EmcPose * pos, joints[7] = 0; joints[8] = 0; - oldquad = nowquad; + OLDQUAD(s) = nowquad; + REVOLUTIONS(s) = revolutions; return 0; } -int kinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +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) { double x = pos->tran.x, y = pos->tran.y; double r2 = x*x + y*y; double r = sqrt(r2); + (void)p; (void)joints; (void)iflags; // on the axis the angle is undefined and its rate unbounded @@ -134,34 +145,39 @@ int kinematicsJacobian(const double *joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +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 92e8a763a99..03c654cac1d 100644 --- a/src/emc/kinematics/rotarydeltakins.c +++ b/src/emc/kinematics/rotarydeltakins.c @@ -20,73 +20,88 @@ #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); } -int kinematicsJacobian(const double *joints, +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; (void)iflags; - set_geometry(hal_get_real(haldata->pfr), hal_get_real(haldata->tl), hal_get_real(haldata->sl), hal_get_real(haldata->fr)); + geometry_of(p, &g); memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][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 kinematics_forward() + // 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 = platformradius - footradius + thighlength * cos(q); + 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 = thighlength * sin(q); + dkx = 0; dky = g.thighlength * sin(q); break; case 1: kx = reach * 0.5 * sqrt(3); ky = reach * 0.5; - dkx = -thighlength * sin(q) * 0.5 * sqrt(3); - dky = -thighlength * sin(q) * 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 = thighlength * sin(q) * 0.5 * sqrt(3); - dky = -thighlength * sin(q) * 0.5; + dkx = g.thighlength * sin(q) * 0.5 * sqrt(3); + dky = -g.thighlength * sin(q) * 0.5; break; } - kz = -thighlength * sin(q); - dkz = -thighlength * cos(q); + 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; @@ -102,36 +117,38 @@ int kinematicsJacobian(const double *joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +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) @@ -139,9 +156,4 @@ void rtapi_app_exit(void) hal_exit(comp_id); } -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); 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 47f95bee2cd..708bc412a7b 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. * ********************************************************************/ @@ -18,12 +18,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; @@ -40,11 +44,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; @@ -61,14 +68,15 @@ int kinematicsInverse(const EmcPose * pos, return 0; } -int kinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const 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) { double c_rad = pos->c*M_PI/180; double cc = cos(c_rad), sc = sin(c_rad); int j; + (void)p; (void)joints; (void)iflags; memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); @@ -82,38 +90,40 @@ int kinematicsJacobian(const double *joints, return 0; } -/* implemented for these kinematics as giving joints preference */ -int kinematicsHome(EmcPose * world, - double *joint, - KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) -{ - *fflags = 0; - *iflags = 0; +static const kins_ops rotate_ops = { + .forward = rotate_forward, + .inverse = rotate_inverse, + .jacobian = rotate_jacobian, +}; - return kinematicsForward(joint, world, fflags, iflags); -} +// 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 }, +}; -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} - -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); 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/scorbot-kins.c b/src/emc/kinematics/scorbot-kins.c index 828f7b4ec43..b840fb38969 100644 --- a/src/emc/kinematics/scorbot-kins.c +++ b/src/emc/kinematics/scorbot-kins.c @@ -44,6 +44,7 @@ #include #include #include +#include // @@ -76,12 +77,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) @@ -90,16 +94,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. @@ -107,13 +108,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]; @@ -135,15 +134,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 @@ -153,16 +154,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. @@ -176,19 +171,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. @@ -207,86 +199,23 @@ 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); - -#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)); - - 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; - } - - 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)); - - 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)); - - 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. - - if (iz1 > iz2) { - j2_r = ir1; - j2_z = iz1; - } else { - j2_r = ir2; - j2_z = iz2; - } - // 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]); - } - - // 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]); - } -#endif - // A and B are wrist roll and pitch, handled in hal by external kinematics joints[3] = pose->a; joints[4] = pose->b; @@ -295,13 +224,14 @@ int kinematicsInverse( } -int kinematicsJacobian( +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 ) { - // kinematicsInverse() above, differentiated step by step in the same + // 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; @@ -311,6 +241,7 @@ int kinematicsJacobian( double q; int i; + (void)p; (void)joints; (void)iflags; if (rho2 <= 0) { return -1; } @@ -363,15 +294,26 @@ int kinematicsJacobian( return 0; } -KINEMATICS_TYPE kinematicsType(void) { - return KINEMATICS_BOTH; -} +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 }, +}; -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; @@ -381,6 +323,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; } @@ -388,4 +334,3 @@ int rtapi_app_main(void) { void rtapi_app_exit(void) { hal_exit(comp_id); } - diff --git a/src/emc/kinematics/tripodkins.c b/src/emc/kinematics/tripodkins.c index ab5c7081b19..47f4baf2b4a 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: @@ -68,19 +68,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)) @@ -124,11 +120,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]) @@ -138,9 +136,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); @@ -184,11 +182,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]) @@ -196,9 +196,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)); @@ -219,14 +219,14 @@ int kinematicsInverse(const EmcPose * pos, #undef Dz } -int kinematicsJacobian(const double * joints, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +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) { - 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]; /* the three strut base points, in the order of the joints */ const double base[3][2] = { {0, 0}, {Bx, 0}, {Cx, Cy} }; int i; @@ -248,170 +248,40 @@ int kinematicsJacobian(const double * joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - 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; - } - - 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); -EXPORT_SYMBOL(kinematicsJacobian); +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/hal/components/Submakefile b/src/hal/components/Submakefile index 8ad4ee1740e..865b70ece96 100644 --- a/src/hal/components/Submakefile +++ b/src/hal/components/Submakefile @@ -98,6 +98,7 @@ obj-m += $(patsubst hal/drivers/%.comp, %.o, $(patsubst hal/components/%.comp, % # -extra-objs. The list is expanded when the .mak is written, # so it has to be defined in this file (which the .mak depends on). SWITCHKINS_OBJS := emc/kinematics/switchkins.o emc/kinematics/kins_util.o +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) diff --git a/src/hal/components/matrixkins.comp b/src/hal/components/matrixkins.comp index 8bf76899c8e..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, @@ -180,68 +180,30 @@ 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; - -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; - 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; - - 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; - - 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(kinematicsJacobian); -EXPORT_SYMBOL(kinematicsForward); - -KINEMATICS_TYPE kinematicsType() -{ - 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 @@ -251,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] @@ -290,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; @@ -323,10 +287,10 @@ int kinematicsInverse(const EmcPose * pos, return 0; } -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +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; @@ -337,15 +301,41 @@ int kinematicsJacobian(const double *j, } // the inverse is the calibration matrix itself, so its derivative is // that matrix, and the pass-through axes are ones - jac[0][0] = hal_get_real(haldata->C_xx); - jac[0][1] = hal_get_real(haldata->C_xy); - jac[0][2] = hal_get_real(haldata->C_xz); - jac[1][0] = hal_get_real(haldata->C_yx); - jac[1][1] = hal_get_real(haldata->C_yy); - jac[1][2] = hal_get_real(haldata->C_yz); - jac[2][0] = hal_get_real(haldata->C_zx); - jac[2][1] = hal_get_real(haldata->C_zy); - jac[2][2] = hal_get_real(haldata->C_zz); + 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/userkins.comp b/src/hal/components/userkins.comp index ac0c003369d..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,16 +49,18 @@ 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 the *EXTRA_SETUP()* function, which - halcompile runs once when the module is loaded, before the component is - made ready. """; // The fpin pin is not accessible in kinematics functions. -// Use EXTRA_SETUP() 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; @@ -69,20 +70,22 @@ 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 @@ -93,61 +96,16 @@ FUNCTION(fdemo) { fpin_set(fpin + 1); } -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; -#define HAL_PREFIX "userkins" - 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 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; - 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(kinematicsJacobian); -EXPORT_SYMBOL(kinematicsForward); - -KINEMATICS_TYPE kinematicsType() -{ - 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 @@ -160,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). @@ -189,25 +141,26 @@ 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; -} // kinematicsInverse() +} // userkins_inverse() -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +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 - // kinematicsInverse(): for this template joint 0 follows x, joint 1 + // 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; } } @@ -215,4 +168,33 @@ int kinematicsJacobian(const double *j, jac[1][1] = 1; jac[2][2] = 1; return 0; -} // kinematicsJacobian() +} // 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); +} From 527da8a10be4edae174f1bbc0c77ed7d04240579 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:00:55 +1000 Subject: [PATCH 30/77] scarakins, pumakins, three21kins: move onto the parameter block Each arm declares its dimensions as a table, reads them from the block and registers one ops table for its own type, with the identity and userk types from the shared ops. pumakins keeps its flange frame and declares the half turn through the ops table's native rotation, where switchkinsRegisterFrames() carried it before. The setup functions and haldata go; pin names and defaults are unchanged. --- src/emc/kinematics/pumakins.c | 170 +++++++++++++------------------ src/emc/kinematics/scarakins.c | 155 ++++++++++++---------------- src/emc/kinematics/three21kins.c | 156 +++++++++++++--------------- 3 files changed, 205 insertions(+), 276 deletions(-) diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index b63cbb64c5a..d93ed5ed3b9 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -24,9 +24,15 @@ #include "pumakins.h" #include -struct haldata { - hal_real_t a2, a3, d3, d4, d6; -} *haldata = NULL; +// 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 */ @@ -107,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; @@ -135,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. */ @@ -191,7 +199,7 @@ static int pumaKinematicsForward(const double * joint, *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; @@ -216,45 +224,38 @@ static int pumaKinematicsForward(const double * joint, the base z, the upper arm and forearm about axes at right angles to it, the wrist about three axes meeting at its centre, and D6 carries the tool point out along the flange z. */ -static int pumaKinematicsJacobian(const double * joint, - const EmcPose * world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int puma_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; const double alpha[6] = { 0, -90, 0, -90, 90, -90 }; - const double a[6] = { 0, 0, hal_get_real(haldata->a2), hal_get_real(haldata->a3), 0, 0 }; - const double d[6] = { 0, 0, hal_get_real(haldata->d3), hal_get_real(haldata->d4), 0, 0 }; + const double a[6] = { 0, 0, p->geometry[P_A2], p->geometry[P_A3], 0, 0 }; + const double d[6] = { 0, 0, p->geometry[P_D3], p->geometry[P_D4], 0, 0 }; - return kinsJacobianFromDhArm(alpha, a, d, joint, hal_get_real(haldata->d6), world, jac); -} // pumaKinematicsJacobian() + return kinsJacobianFromDhArm(alpha, a, d, joint, p->geometry[P_D6], world, jac); +} // puma_jacobian() -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() +} // puma_tool_frame() -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() - -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; @@ -290,11 +291,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; @@ -407,29 +408,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, + .jacobian = puma_jacobian, + .work = kinsIdentityFrame, + .tool = puma_tool_frame, + .native = &TOOL_FRAME_FLANGE, +}; int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -437,51 +428,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); - switchkinsRegisterJacobian(1, pumaKinematicsJacobian); - 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); - switchkinsRegisterJacobian(0, pumaKinematicsJacobian); - - *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/scarakins.c b/src/emc/kinematics/scarakins.c index e2dedb241b6..34035b8adac 100644 --- a/src/emc/kinematics/scarakins.c +++ b/src/emc/kinematics/scarakins.c @@ -22,10 +22,6 @@ #include -static struct scara_data { - hal_real_t d1, d2, d3, d4, d5, d6; -} *haldata = NULL; - /* key dimensions joint[0] = Entire arm rotates around a vertical axis at its inner end @@ -62,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; @@ -83,12 +98,12 @@ 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); @@ -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,17 +193,17 @@ static int scaraKinematicsInverse(const EmcPose * world, *fflags = 0; return (0); -} // scaraKinematicsInverse() +} // scara_inverse() -static int scaraKinematicsJacobian(const double * joint, - const EmcPose * world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +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)iflags; - rtapi_real D2 = hal_get_real(haldata->d2); - rtapi_real D4 = hal_get_real(haldata->d4); - rtapi_real D6 = hal_get_real(haldata->d6); + 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); @@ -226,38 +243,13 @@ static int scaraKinematicsJacobian(const double * joint, jac[4][3] = 1; jac[5][4] = 1; return 0; -} // scaraKinematicsJacobian() - -#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) -{ - (void)coordinates; - int res=0; - - haldata = hal_malloc(sizeof(*haldata)); - if (!haldata) goto error; - - 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; } - - return 0; +} // scara_jacobian() -error: - return -1; -} // scaraKinematicsSetup() +static const kins_ops scara_ops = { + .forward = scara_forward, + .inverse = scara_inverse, + .jacobian = scara_jacobian, +}; int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -265,41 +257,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); - switchkinsRegisterJacobian(1, scaraKinematicsJacobian); + 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; - switchkinsRegisterJacobian(0, scaraKinematicsJacobian); - - *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/three21kins.c b/src/emc/kinematics/three21kins.c index 5cce5796a30..ae602858389 100644 --- a/src/emc/kinematics/three21kins.c +++ b/src/emc/kinematics/three21kins.c @@ -27,9 +27,18 @@ /* 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 */ @@ -41,20 +50,22 @@ static double angleDiff(double a, double b) 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; @@ -193,38 +204,40 @@ static int three21KinematicsForward(const double * joint, with the shoulder set A1 out along the first link and D1 up the base, D2 and D3 both along the axis the upper arm turns about, and D6 carrying the tool point out along the flange z. */ -static int three21KinematicsJacobian(const double * joint, - const EmcPose * world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int three21_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; const double alpha[6] = { 0, -90, 0, -90, 90, -90 }; - const double a[6] = { 0, hal_get_real(haldata->a1), hal_get_real(haldata->a2), - hal_get_real(haldata->a3), 0, 0 }; - const double d[6] = { hal_get_real(haldata->d1), hal_get_real(haldata->d2), - hal_get_real(haldata->d3), hal_get_real(haldata->d4), 0, 0 }; - - return kinsJacobianFromDhArm(alpha, a, d, joint, hal_get_real(haldata->d6), world, jac); -} // three21KinematicsJacobian() - -static int three21KinematicsInverse(const EmcPose * world, - double * joint, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) + const double a[6] = { 0, p->geometry[P_A1], p->geometry[P_A2], + p->geometry[P_A3], 0, 0 }; + const double d[6] = { p->geometry[P_D1], p->geometry[P_D2], + p->geometry[P_D3], p->geometry[P_D4], 0, 0 }; + + return kinsJacobianFromDhArm(alpha, a, d, joint, p->geometry[P_D6], world, jac); +} // three21_jacobian() + +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; @@ -367,31 +380,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, + .jacobian = three21_jacobian, +}; int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -399,41 +394,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; - switchkinsRegisterJacobian(1, three21KinematicsJacobian); - 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; - switchkinsRegisterJacobian(0, three21KinematicsJacobian); - - *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; } From ad5279d10c06a1b03cf8cb3eea4160ceff05a705 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:06:35 +1000 Subject: [PATCH 31/77] millturn, xyzab_tdr_kins, xyzacb_trsrn, xyzbca_trsrn: move onto the parameter block Each component declares its geometry as a table, writes its types as ops over the block, and supplies switchkinsSetup() like the C modules do; EXTRA_SETUP() runs it through switchkinsRunSetup() and initialises, so the components link switchkins_setup.o too and export kinsDescribe() with the rest. The trsrn TCP type registers its frames and Jacobian in its ops table and the TOOL type the identity frames, as they were registered before. The inverses go on reading the rotary angles from their joint argument, as they always have. Pin names and defaults are unchanged. --- src/hal/components/Submakefile | 2 +- src/hal/components/millturn.comp | 103 +++--- src/hal/components/xyzab_tdr_kins.comp | 173 +++++---- src/hal/components/xyzacb_trsrn.comp | 470 +++++++++++-------------- src/hal/components/xyzbca_trsrn.comp | 436 ++++++++++------------- 5 files changed, 540 insertions(+), 644 deletions(-) diff --git a/src/hal/components/Submakefile b/src/hal/components/Submakefile index 865b70ece96..d8975f3f8d1 100644 --- a/src/hal/components/Submakefile +++ b/src/hal/components/Submakefile @@ -97,7 +97,7 @@ obj-m += $(patsubst hal/drivers/%.comp, %.o, $(patsubst hal/components/%.comp, % # A component that links objects besides its own names them here as # -extra-objs. The list is expanded when the .mak is written, # so it has to be defined in this file (which the .mak depends on). -SWITCHKINS_OBJS := emc/kinematics/switchkins.o emc/kinematics/kins_util.o +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) diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index abbff00a375..b841ba3f0e0 100644 --- a/src/hal/components/millturn.comp +++ b/src/hal/components/millturn.comp @@ -26,7 +26,6 @@ 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; @@ -49,22 +48,16 @@ FUNCTION(fdemo) { fpin_set(fpin + 1); } -// the turn kinematics need no hal pins of their own -static int turnKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - (void)comp_id; - (void)coords; - (void)kp; - return 0; -} // turnKinematicsSetup() - -static int turnKinematicsForward(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; @@ -81,13 +74,16 @@ static int turnKinematicsForward(const double *j, pos->w = 0; return 0; -} // turnKinematicsForward() +} // turn_forward() -static int turnKinematicsInverse(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; @@ -97,53 +93,62 @@ static int turnKinematicsInverse(const EmcPose * pos, j[3] = pos->a; return 0; -} // turnKinematicsInverse() +} // turn_inverse() -static int turnKinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +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 turnKinematicsInverse(): which joint follows which - // pose coordinate, and in which sense + // 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; -} // turnKinematicsJacobian() +} // turn_jacobian() + +static const kins_ops turn_ops = { + .forward = turn_forward, + .inverse = turn_inverse, + .jacobian = turn_jacobian, +}; + +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 = {0}; + kparms kp; (void)__comp_inst; (void)prefix; (void)extra_arg; - kp.kinsname = "millturn"; - kp.halprefix = "millturn"; - kp.required_coordinates = "xyza"; - kp.allow_duplicates = 0; - kp.fwd_iterates_mask = 0; - kp.gui_kinstype = -1; - kp.sparm = NULL; - kp.max_joints = strlen(kp.required_coordinates); - - if (switchkinsRegister(0, identityKinematicsSetup, - identityKinematicsForward, - identityKinematicsInverse)) { return -1; } - if (switchkinsRegister(1, turnKinematicsSetup, - turnKinematicsForward, - turnKinematicsInverse)) { return -1; } - if (switchkinsRegisterJacobian(1, turnKinematicsJacobian)) { return -1; } - - if (switchkinsDeclare(0, KINSTYPE_IDENTITY)) { return -1; } - + if (switchkinsRunSetup(&kp, NULL)) { return -1; } return switchkinsInit(comp_id, &kp, coordinates); } // EXTRA_SETUP() diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index c5d4db81f66..ce21a7d8159 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -44,55 +44,33 @@ author "David Mueller"; static char *coordinates; RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); -static struct haldata { - hal_real_t tool_offset_z; - hal_real_t x_offset; - hal_real_t z_offset; - hal_real_t x_rot_point; - hal_real_t y_rot_point; - hal_real_t z_rot_point; -} *tdrdata; - -static int tdrKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - int res = 0; - (void)coords; - - tdrdata = hal_malloc(sizeof(*tdrdata)); - if (!tdrdata) return -1; - - res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->tool_offset_z, 0.0, - "%s.tool-offset-z", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->x_offset, 0.0, - "%s.x-offset", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->z_offset, 0.0, - "%s.z-offset", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->x_rot_point, 0.0, - "%s.x-rot-point", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->y_rot_point, 0.0, - "%s.y-rot-point", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->z_rot_point, 0.0, - "%s.z-rot-point", kp->halprefix); - if (res) return -1; - - return 0; -} // tdrKinematicsSetup() - -static int tdrKinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +// 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(tdrdata->x_rot_point); - double y_rot_point = hal_get_real(tdrdata->y_rot_point); - double z_rot_point = hal_get_real(tdrdata->z_rot_point); + double 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(tdrdata->z_offset); - double dt = hal_get_real(tdrdata->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 @@ -125,22 +103,24 @@ static int tdrKinematicsForward(const double *j, pos->w = 0; return 0; -} // tdrKinematicsForward() +} // tdr_forward() -static int tdrKinematicsInverse(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(tdrdata->x_rot_point); - double y_rot_point = hal_get_real(tdrdata->y_rot_point); - double z_rot_point = hal_get_real(tdrdata->z_rot_point); + double 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(tdrdata->x_offset); - double dz = hal_get_real(tdrdata->z_offset); - double dt = hal_get_real(tdrdata->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 @@ -167,21 +147,21 @@ static int tdrKinematicsInverse(const EmcPose * pos, j[4] = pos->b; return 0; -} // tdrKinematicsInverse() +} // tdr_inverse() -static int tdrKinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +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 = hal_get_real(tdrdata->x_rot_point); - double y_rot_point = hal_get_real(tdrdata->y_rot_point); - double z_rot_point = hal_get_real(tdrdata->z_rot_point); - double dx = hal_get_real(tdrdata->x_offset); - double dz = hal_get_real(tdrdata->z_offset); - double dt = hal_get_real(tdrdata->tool_offset_z); + 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); @@ -195,9 +175,9 @@ static int tdrKinematicsJacobian(const double *j, for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } } - // tdrKinematicsInverse() 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 + // 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; @@ -217,33 +197,42 @@ static int tdrKinematicsJacobian(const double *j, jac[3][3] = 1; jac[4][4] = 1; return 0; -} // tdrKinematicsJacobian() +} // tdr_jacobian() + +static const kins_ops tdr_ops = { + .forward = tdr_forward, + .inverse = tdr_inverse, + .jacobian = tdr_jacobian, +}; + +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; +} // switchkinsSetup() // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp = {0}; + kparms kp; (void)__comp_inst; (void)prefix; (void)extra_arg; - kp.kinsname = "xyzab_tdr_kins"; - kp.halprefix = "xyzab_tdr_kins"; - kp.required_coordinates = "xyzab"; - kp.allow_duplicates = 0; - kp.fwd_iterates_mask = 0; - kp.gui_kinstype = -1; - kp.sparm = NULL; - kp.max_joints = strlen(kp.required_coordinates); - - if (switchkinsRegister(0, identityKinematicsSetup, - identityKinematicsForward, - identityKinematicsInverse)) { return -1; } - if (switchkinsRegister(1, tdrKinematicsSetup, - tdrKinematicsForward, - tdrKinematicsInverse)) { return -1; } - if (switchkinsRegisterJacobian(1, tdrKinematicsJacobian)) { return -1; } - - if (switchkinsDeclare(0, KINSTYPE_IDENTITY)) { return -1; } - if (switchkinsDeclare(1, KINSTYPE_PRIMARY)) { return -1; } - + 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 3d26044943d..8fb6d8dae39 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -25,85 +25,46 @@ author "David Mueller"; static char *coordinates; RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); -static struct haldata { - // these should be parameters really but we want to be able to - // 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; -} *haldata; - -// the pins are shared by the TCP and TOOL kinematics; the TOOL type has -// no setup routine of its own -static int trsrnKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - int res = 0; - (void)coords; - - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) return -1; - - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_pivot, 0.0, "%s.y-pivot" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_rot_axis, 0.0, "%s.y-rot-axis" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle" ,kp->halprefix); - if (res) return -1; - - return 0; -} // trsrnKinematicsSetup() - -static int toolKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - (void)comp_id; - (void)coords; - (void)kp; - return 0; // pins created by trsrnKinematicsSetup() -} // toolKinematicsSetup() +// 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 double *j, EmcPose * pos, int tool_kins) +static int trsrnForward(const kins_params *p, const double *j, EmcPose * pos, int tool_kins) { - // 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); @@ -130,8 +91,6 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) double Py = j[1]; double Pz = j[2]; - // END of custom variable declaration for Forward kinematics - if (!tool_kins) { // ========================= TCP kinematics FORWARD // in TCP we use the current positions of the spindle joints Ss = sin(j[4]*TO_RAD); @@ -144,32 +103,32 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) 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]; @@ -187,27 +146,27 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) 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]; @@ -222,47 +181,35 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) return 0; } // trsrnForward() -static int tcpKinematicsForward(const double *j, +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; (void)iflags; - return trsrnForward(j, pos, 0); + return trsrnForward(p, j, pos, 0); } // tcpKinematicsForward() -static int toolKinematicsForward(const double *j, +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; (void)iflags; - return trsrnForward(j, pos, 1); + return trsrnForward(p, j, pos, 1); } // toolKinematicsForward() -static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) +// 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) { - // 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); - - // substitutions as used in mathematical documentation - // including degree -> radians angle conversion + GEOMETRY(p); // variables used in both, TCP and TOOL kinematics double Sw = sin(j[3]*TO_RAD); @@ -271,7 +218,7 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) 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; @@ -286,10 +233,8 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) // 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; if (!tool_kins) { // ========================= TCP kinematics INVERSE // in TCP we use the current positions of the spindle joints @@ -303,25 +248,25 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) 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 + 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 + 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 + j[2] = (Dt + Lz)*s + + Ly*t + - Cw*(Draz + Dt + Lz - Qz) + + (Dray + Dy + Ly - Qy)*Sw + Draz; j[3] = pos->a; @@ -329,80 +274,84 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) 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; + // 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 EmcPose * pos, +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(pos, j, 0); + return trsrnInverse(p, pos, j, 0); } // tcpKinematicsInverse() -static int toolKinematicsInverse(const EmcPose * pos, +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(pos, j, 1); + return trsrnInverse(p, pos, j, 1); } // toolKinematicsInverse() // The head answers in the convention already, so the native rotation -// registered with these frames is TOOL_FRAME_SPINDLE. -static int tcpKinematicsToolFrame(const double *j, +// 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 = hal_get_real(haldata->nut_angle); // degrees + 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); @@ -437,10 +386,11 @@ static int tcpKinematicsToolFrame(const double *j, return 0; } // tcpKinematicsToolFrame() -static int tcpKinematicsWorkFrame(const double *j, +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); @@ -456,23 +406,15 @@ static int tcpKinematicsWorkFrame(const double *j, return 0; } // tcpKinematicsWorkFrame() -static int tcpKinematicsJacobian(const double *j, +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; - - // the same geometry as trsrnInverse(), read the same way - 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 nu = hal_get_real(haldata->nut_angle); // degrees - double Dt = hal_get_real(haldata->tool_offset_z); + GEOMETRY(p); + (void)tc; (void)theta_1; (void)theta_2; double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); @@ -539,7 +481,7 @@ static int tcpKinematicsJacobian(const double *j, return 0; } // tcpKinematicsJacobian() -static int toolKinematicsJacobian(const double *j, +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) @@ -550,10 +492,10 @@ static int toolKinematicsJacobian(const double *j, // the head angles come from pins, so the inverse is linear in the pose // and the rows are its coefficients - 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 + 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); @@ -592,43 +534,55 @@ static int toolKinematicsJacobian(const double *j, return 0; } // toolKinematicsJacobian() +static const kins_ops tcp_ops = { + .forward = tcpKinematicsForward, + .inverse = tcpKinematicsInverse, + .work = tcpKinematicsWorkFrame, + .tool = tcpKinematicsToolFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = tcpKinematicsJacobian, +}; + +// 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() + // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp = {0}; + kparms kp; (void)__comp_inst; (void)prefix; (void)extra_arg; - kp.kinsname = "xyzacb_trsrn"; - kp.halprefix = "xyzacb_trsrn_kins"; - kp.required_coordinates = "xyzabc"; - kp.allow_duplicates = 0; - kp.fwd_iterates_mask = 0; - kp.gui_kinstype = -1; - kp.sparm = NULL; - kp.max_joints = strlen(kp.required_coordinates); - - if (switchkinsRegister(0, identityKinematicsSetup, - identityKinematicsForward, - identityKinematicsInverse)) { return -1; } - if (switchkinsRegister(1, trsrnKinematicsSetup, - tcpKinematicsForward, - tcpKinematicsInverse)) { return -1; } - if (switchkinsRegister(2, toolKinematicsSetup, - toolKinematicsForward, - toolKinematicsInverse)) { return -1; } - if (switchkinsRegisterFrames(1, tcpKinematicsWorkFrame, - tcpKinematicsToolFrame, - &TOOL_FRAME_SPINDLE)) { return -1; } - if (switchkinsRegisterJacobian(1, tcpKinematicsJacobian)) { return -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 - if (switchkinsRegisterFrames(2, identityKinematicsWorkFrame, - identityKinematicsToolFrame, - &TOOL_FRAME_SPINDLE)) { return -1; } - if (switchkinsRegisterJacobian(2, toolKinematicsJacobian)) { return -1; } - - if (switchkinsDeclare(0, KINSTYPE_IDENTITY)) { return -1; } - if (switchkinsDeclare(1, KINSTYPE_PRIMARY)) { return -1; } - + 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 075e3cbfeb2..151fde7a352 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -25,85 +25,46 @@ author "David Mueller"; static char *coordinates; RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); -static struct haldata { - // these should be parameters really but we want to be able to - // 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; -} *haldata; - -// the pins are shared by the TCP and TOOL kinematics; the TOOL type has -// no setup routine of its own -static int trsrnKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - int res = 0; - (void)coords; - - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) return -1; - - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_pivot, 0.0, "%s.x-pivot", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_rot_axis, 0.0, "%s.x-rot-axis", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle", kp->halprefix); - if (res) return -1; - - return 0; -} // trsrnKinematicsSetup() - -static int toolKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - (void)comp_id; - (void)coords; - (void)kp; - return 0; // pins created by trsrnKinematicsSetup() -} // toolKinematicsSetup() +// 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 double *j, EmcPose * pos, int tool_kins) +static int trsrnForward(const kins_params *p, const double *j, EmcPose * pos, int tool_kins) { - // 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); @@ -130,9 +91,6 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) double Py = j[1]; double Pz = j[2]; - // END of custom variable declaration for Forward kinematics - - if (!tool_kins) { // ========================= TCP kinematics FORWARD // in TCP we use the current positions of the spindle joints Ss = sin(j[3]*TO_RAD); @@ -144,37 +102,33 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) 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]; @@ -192,27 +146,25 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) 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]; @@ -227,44 +179,35 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) return 0; } // trsrnForward() -static int tcpKinematicsForward(const double *j, +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; (void)iflags; - return trsrnForward(j, pos, 0); + return trsrnForward(p, j, pos, 0); } // tcpKinematicsForward() -static int toolKinematicsForward(const double *j, +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; (void)iflags; - return trsrnForward(j, pos, 1); + return trsrnForward(p, j, pos, 1); } // toolKinematicsForward() -static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) +// 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) { - // 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); @@ -288,11 +231,8 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) // 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; if (!tool_kins) { // ========================= TCP kinematics INVERSE // in TCP we use the current positions of the spindle joints @@ -304,27 +244,27 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) 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; @@ -342,32 +282,31 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) 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; @@ -378,34 +317,38 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) return 0; } // trsrnInverse() -static int tcpKinematicsInverse(const EmcPose * pos, +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(pos, j, 0); + return trsrnInverse(p, pos, j, 0); } // tcpKinematicsInverse() -static int toolKinematicsInverse(const EmcPose * pos, +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(pos, j, 1); + return trsrnInverse(p, pos, j, 1); } // toolKinematicsInverse() // The head answers in the convention already, so the native rotation -// registered with these frames is TOOL_FRAME_SPINDLE. -static int tcpKinematicsToolFrame(const double *j, +// 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 = hal_get_real(haldata->nut_angle); // degrees + 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); @@ -440,10 +383,11 @@ static int tcpKinematicsToolFrame(const double *j, return 0; } // tcpKinematicsToolFrame() -static int tcpKinematicsWorkFrame(const double *j, +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); @@ -459,23 +403,15 @@ static int tcpKinematicsWorkFrame(const double *j, return 0; } // tcpKinematicsWorkFrame() -static int tcpKinematicsJacobian(const double *j, +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; - - // the same geometry as trsrnInverse(), read the same way - 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 nu = hal_get_real(haldata->nut_angle); // degrees - double Dt = hal_get_real(haldata->tool_offset_z); + GEOMETRY(p); + (void)tc; (void)theta_1; (void)theta_2; double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); @@ -542,7 +478,7 @@ static int tcpKinematicsJacobian(const double *j, return 0; } // tcpKinematicsJacobian() -static int toolKinematicsJacobian(const double *j, +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) @@ -553,10 +489,10 @@ static int toolKinematicsJacobian(const double *j, // the head angles come from pins, so the inverse is linear in the pose // and the rows are its coefficients - 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 + 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); @@ -595,43 +531,55 @@ static int toolKinematicsJacobian(const double *j, return 0; } // toolKinematicsJacobian() +static const kins_ops tcp_ops = { + .forward = tcpKinematicsForward, + .inverse = tcpKinematicsInverse, + .work = tcpKinematicsWorkFrame, + .tool = tcpKinematicsToolFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = tcpKinematicsJacobian, +}; + +// 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 = {0}; + kparms kp; (void)__comp_inst; (void)prefix; (void)extra_arg; - kp.kinsname = "xyzbca_trsrn"; - kp.halprefix = "xyzbca_trsrn_kins"; - kp.required_coordinates = "xyzabc"; - kp.allow_duplicates = 0; - kp.fwd_iterates_mask = 0; - kp.gui_kinstype = -1; - kp.sparm = NULL; - kp.max_joints = strlen(kp.required_coordinates); - - if (switchkinsRegister(0, identityKinematicsSetup, - identityKinematicsForward, - identityKinematicsInverse)) { return -1; } - if (switchkinsRegister(1, trsrnKinematicsSetup, - tcpKinematicsForward, - tcpKinematicsInverse)) { return -1; } - if (switchkinsRegister(2, toolKinematicsSetup, - toolKinematicsForward, - toolKinematicsInverse)) { return -1; } - if (switchkinsRegisterFrames(1, tcpKinematicsWorkFrame, - tcpKinematicsToolFrame, - &TOOL_FRAME_SPINDLE)) { return -1; } - if (switchkinsRegisterJacobian(1, tcpKinematicsJacobian)) { return -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 - if (switchkinsRegisterFrames(2, identityKinematicsWorkFrame, - identityKinematicsToolFrame, - &TOOL_FRAME_SPINDLE)) { return -1; } - if (switchkinsRegisterJacobian(2, toolKinematicsJacobian)) { return -1; } - - if (switchkinsDeclare(0, KINSTYPE_IDENTITY)) { return -1; } - if (switchkinsDeclare(1, KINSTYPE_PRIMARY)) { return -1; } - + if (switchkinsRunSetup(&kp, NULL)) { return -1; } return switchkinsInit(comp_id, &kp, coordinates); } // EXTRA_SETUP() From e1cf365aab60995fdac290541d418bbe8f59bb3f Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:13:52 +1000 Subject: [PATCH 32/77] genserkins, genhexkins, pentakins: move onto the parameter block The three that build a geometry from many pins build it from the block on each call: genser its link description, genhex its base and platform points, pentakins its base points and effector circles. What they reported through output pins, iteration counts, strut corrections, the hexapod's fwd-kins-fail and gui pose, goes through the scratch, so each caller keeps its own. genhex and pentakins declare their forward as iterating and the switchkins core seeds it as before. genhexkins's six gui pose pins were inputs written by the module and are outputs now; pentakins's HAL parameters become pins of the same names. genserfuncs.c loses its haldata and globals, and ugenserkins builds a block and calls the ops. --- src/Makefile | 2 + src/emc/kinematics/Submakefile | 1 + src/emc/kinematics/genhexkins.c | 529 ++++++++++++++----------------- src/emc/kinematics/genserfuncs.c | 296 +++++++---------- src/emc/kinematics/genserkins.c | 36 +-- src/emc/kinematics/genserkins.h | 36 +-- src/emc/kinematics/pentakins.c | 280 ++++++++-------- src/emc/kinematics/ugenserkins.c | 36 ++- 8 files changed, 515 insertions(+), 701 deletions(-) diff --git a/src/Makefile b/src/Makefile index 2ead045de1a..4206f66a218 100644 --- a/src/Makefile +++ b/src/Makefile @@ -1164,6 +1164,8 @@ 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) diff --git a/src/emc/kinematics/Submakefile b/src/emc/kinematics/Submakefile index c71c18696e2..dbbc783f21b 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 diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index 0964fa7d088..fa5276f5a56 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 @@ -114,49 +118,98 @@ #include "genhexkins.h" #include -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 +// 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() ***************************/ @@ -259,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) @@ -306,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; @@ -350,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 @@ -375,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; }; @@ -389,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; } @@ -402,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; } @@ -454,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; @@ -472,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; @@ -506,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; @@ -518,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; } @@ -541,9 +609,9 @@ static int genhexKinematicsInverse(const EmcPose * pos, } return 0; -} //genhexKinematicsInverse() +} //genhex_inverse() -/************************ genhexKinematicsJacobian() ***********************/ +/************************ 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 @@ -551,11 +619,18 @@ static int genhexKinematicsInverse(const EmcPose * pos, 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. */ -static int genhexKinematicsJacobian(const double * joints, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +// 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 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) { + genhex_geometry g; PmCartesian aw, RMatrix_a, strut, u, moment; PmRotationMatrix RMatrix; PmRpy rpy; @@ -563,13 +638,14 @@ static int genhexKinematicsJacobian(const double * joints, double sb, cb, sc, cc; int i, m; - genhex_read_hal_pins(); + 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 (hal_get_real(haldata->screw_lead) != 0.0) { - return kinsJacobianFromInverse(genhexKinematicsInverse, NUM_STRUTS, - joints, pos, iflags, jac); + if (g.screw_lead != 0.0) { + kins_scratch scratch; + kinsScratchInit(&scratch); + return kinsOpsJacobian(&genhex_diff_ops, p, &scratch, joints, pos, jac, iflags); } memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); @@ -590,9 +666,9 @@ static int genhexKinematicsJacobian(const double * joints, for (i = 0; i < NUM_STRUTS; i++) { double len; - pmMatCartMult(&RMatrix, &a[i], &RMatrix_a); + pmMatCartMult(&RMatrix, &g.a[i], &RMatrix_a); pmCartCartAdd(&pos->tran, &RMatrix_a, &aw); - pmCartCartSub(&aw, &b[i], &strut); + pmCartCartSub(&aw, &g.b[i], &strut); pmCartMag(&strut, &len); if (len <= 0) { return -1; } pmCartScalMult(&strut, 1.0/len, &u); @@ -608,145 +684,16 @@ static int genhexKinematicsJacobian(const double * joints, } } return 0; -} // genhexKinematicsJacobian() - -// 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, +} // 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, }; -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, -}; - -static -int genhexKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - (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; - } - - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) { - rtapi_print_msg(RTAPI_MSG_ERR,"genhexKinematicsSetup: hal_malloc fail\n"); - return -1; - } - - 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;} - } - - 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; - -error: - return res; -} // genhexKinematicsSetup() int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -754,46 +701,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); - switchkinsRegisterJacobian(1, genhexKinematicsJacobian); + 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; - switchkinsRegisterJacobian(0, genhexKinematicsJacobian); - - *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 02703735b2a..7a46d1cfdcc 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 @@ -49,44 +54,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; @@ -95,8 +109,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: @@ -315,7 +334,7 @@ int genser_kin_jac_fwd(void *kins, } /* The Jacobian in the terms of kinematics.h: joints in degrees per pose - word in EmcPose units, the derivative of genserKinematicsInverse(). + 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, @@ -327,13 +346,14 @@ int genser_kin_jac_fwd(void *kins, with the unit conversions and the unrotate coupling applied in the order the inverse applies them. */ -int genserKinematicsJacobian(const double *joint, - const EmcPose *world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +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 = 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; @@ -343,21 +363,14 @@ int genserKinematicsJacobian(const double *joint, double sb, cb, sc, cc; int link, i, a, m, retval; -#ifndef ULAPI - genser_kin_init(); - if (!genser_hal_inited) { - rtapi_print_msg(RTAPI_MSG_ERR, - "genserKinematicsJacobian: not initialized\n"); - return -1; - } -#endif + genser_links_of(p, genser); memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][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 = hal_get_si32(haldata->unrotate[link]); + rtapi_s32 unrotate = unrotate_of(p, link); jest[link] = joint[link] * (PM_PI / 180); if (link && unrotate) jest[link] -= unrotate * jest[link-1]; @@ -403,7 +416,7 @@ int genserKinematicsJacobian(const double *joint, // the unrotate coupling, in link order as the inverse applies it for (link = 1; link < genser->link_num; link++) { - rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[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]; @@ -412,86 +425,74 @@ int genserKinematicsJacobian(const double *joint, } // uvw pass through as joints 6, 7, 8 - if (total_joints > 6) jac[6][6] = 1; - if (total_joints > 7) jac[7][7] = 1; - if (total_joints > 8) jac[8][8] = 1; + 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; -} // genserKinematicsJacobian() +} // 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; } @@ -503,8 +504,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) @@ -518,22 +517,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; @@ -541,30 +543,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); @@ -576,9 +567,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]); @@ -597,8 +589,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); /* @@ -612,7 +603,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 @@ -641,9 +632,9 @@ int genserKinematicsInverse(const EmcPose * world, go_matrix_vector_mult(&Jinv, dvw, dj); //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++) { @@ -660,14 +651,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 */ @@ -681,6 +668,12 @@ int genserKinematicsInverse(const EmcPose * world, return GO_RESULT_ERROR; } +const kins_ops GENSER_OPS = { + .forward = genser_forward, + .inverse = genser_inverse, + .jacobian = genser_jacobian, +}; + /* 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 @@ -691,68 +684,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 be71f33ffc5..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) */ /******************************************************************** @@ -57,41 +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); - switchkinsRegisterJacobian(1, genserKinematicsJacobian); + 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; - switchkinsRegisterJacobian(0, genserKinematicsJacobian); - - *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 b74b826d2ec..c5a2d9526f8 100644 --- a/src/emc/kinematics/genserkins.h +++ b/src/emc/kinematics/genserkins.h @@ -81,8 +81,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 +123,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,23 +131,14 @@ extern int compute_jfwd(go_link * link_params, extern int compute_jinv(go_matrix * Jfwd, go_matrix * Jinv); -extern int genserKinematicsJacobian(const double *joint, - const EmcPose *world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags); - -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/pentakins.c b/src/emc/kinematics/pentakins.c index 551b79a4260..4ca870b392c 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 @@ -53,23 +57,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() ***************************/ @@ -180,31 +212,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]; - -/************************pentakins_read_hal_pins**************************/ +/* 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; -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; @@ -212,8 +242,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]; @@ -227,32 +255,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]; @@ -261,14 +287,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 || @@ -287,12 +310,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; }; @@ -300,22 +326,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; @@ -343,7 +370,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; @@ -358,34 +384,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; @@ -393,18 +422,19 @@ 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; } -int kinematicsJacobian(const double * joints, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +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) { + penta_geometry g; PmRotationMatrix R; PmRpy rpy; PmCartesian P, d, xyz, wa, wb, dxyz[5]; @@ -412,7 +442,7 @@ int kinematicsJacobian(const double * joints, (void)joints; (void)iflags; - pentakins_read_hal_pins(); + geometry_of(p, &g); memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); /* InvKins() differentiated. The effector end of each strut is found in @@ -433,7 +463,7 @@ int kinematicsJacobian(const double * joints, for (i = 0; i < NUM_STRUTS; i++) { double rho, A, B, len; - pmCartCartSub(&b[i], &P, &d); + 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; @@ -460,8 +490,8 @@ int kinematicsJacobian(const double * joints, } rho = sqrt(sqr(xyz.x) + sqr(xyz.y)); - A = xyz.z - za[i]; - B = rho - ra[i]; + 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++) { @@ -472,103 +502,43 @@ int kinematicsJacobian(const double * joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +// 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, +}; -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); +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; -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 -}; -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 -}; - 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/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) { From 6a7e6e00dc228946b9a009eefbeba8c8727daa25 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:24:57 +1000 Subject: [PATCH 33/77] switchkinscomp: write the template on the parameter block The out-of-tree template declares its geometry as a table, writes its example type as ops over the block and supplies switchkinsSetup() like the in-tree modules, with EXTRA_SETUP() running it through switchkinsRunSetup(). It includes switchkins_setup.c alongside the other two sources, so that file joins those installed in share/linuxcnc. The kparms it built was never zeroed, which the grown struct would have turned into a crash. --- .gitignore | 1 + debian/linuxcnc-uspace-dev.install | 1 + src/Makefile | 2 +- src/emc/kinematics/Submakefile | 1 + src/hal/components/switchkinscomp.comp | 134 +++++++++++++++---------- 5 files changed, 84 insertions(+), 55 deletions(-) diff --git a/.gitignore b/.gitignore index 14b60adb42f..a80d81ea584 100644 --- a/.gitignore +++ b/.gitignore @@ -21,6 +21,7 @@ share/desktop-directories/linuxcnc-cnc.directory share/desktop-directories/linuxcnc-ref.directory share/desktop-directories/linuxcnc-doc.directory share/linuxcnc/switchkins.c +share/linuxcnc/switchkins_setup.c share/linuxcnc/kins_util.c share/linuxcnc/kins_single.c src/modules.order diff --git a/debian/linuxcnc-uspace-dev.install b/debian/linuxcnc-uspace-dev.install index 1e845bd2fef..c4e9ee333a5 100644 --- a/debian/linuxcnc-uspace-dev.install +++ b/debian/linuxcnc-uspace-dev.install @@ -4,5 +4,6 @@ usr/lib/liblinuxcnc.a usr/lib/*.so usr/share/linuxcnc/Makefile.modinc 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/src/Makefile b/src/Makefile index 4206f66a218..597cd4e3210 100644 --- a/src/Makefile +++ b/src/Makefile @@ -788,7 +788,7 @@ ifeq ($(BUILD_GUI),yes) $(FILE) ../share/gtksourceview-4/language-specs/*.lang $(DESTDIR)$(datadir)/gtksourceview-4/language-specs/ endif - $(FILE) ../src/emc/kinematics/switchkins.c ../src/emc/kinematics/kins_util.c ../src/emc/kinematics/kins_single.c $(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 diff --git a/src/emc/kinematics/Submakefile b/src/emc/kinematics/Submakefile index dbbc783f21b..89c6173d5be 100644 --- a/src/emc/kinematics/Submakefile +++ b/src/emc/kinematics/Submakefile @@ -40,6 +40,7 @@ PYTARGETS += $(RDELTAMODULE) # 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 diff --git a/src/hal/components/switchkinscomp.comp b/src/hal/components/switchkinscomp.comp index 7e90edc380f..e5ca4034b9c 100644 --- a/src/hal/components/switchkinscomp.comp +++ b/src/hal/components/switchkinscomp.comp @@ -17,6 +17,12 @@ 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*). @@ -53,11 +59,15 @@ option extra_setup; // switchkins.c provides kinematicsForward(), kinematicsInverse(), // kinematicsSwitch() and the rest of the kinematics interface, and // dispatches each call to the currently selected switchkins-type. -// kins_util.c provides the identity kinematics and the coordinates -// letters-to-joints mapping they use. Both are installed with the -// headers, so halcompile finds them with no path of your own. +// 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 //===================================================================== @@ -66,36 +76,31 @@ static char *coordinates; RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); //--------------------------------------------------------------------- -// Example switchkins-type. A setup routine creating whatever hal pins -// the kinematics need, plus a forward and an inverse routine. Replace -// the arithmetic with the real kinematics. - -static struct { - hal_real_t x_offset; -} *mydata; - -static int myKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - (void)coords; // this type does not use the coordinates mapping - - mydata = hal_malloc(sizeof(*mydata)); - if (!mydata) return -1; +// 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. - return hal_pin_new_real(comp_id, HAL_IN, &mydata->x_offset, 0.0, - "%s.x-offset", kp->halprefix); -} // myKinematicsSetup() +static const kins_param_desc my_params[] = { + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, +}; +enum { P_X_OFFSET }; -static int myKinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +//--------------------------------------------------------------------- +// 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] + hal_get_real(mydata->x_offset); + pos->tran.x = j[0] + p->geometry[P_X_OFFSET]; pos->tran.y = j[1]; pos->tran.z = j[2]; @@ -104,50 +109,71 @@ static int myKinematicsForward(const double *j, pos->u = pos->v = pos->w = 0; return 0; -} // myKinematicsForward() +} // myForward() -static int myKinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +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 - hal_get_real(mydata->x_offset); + j[0] = pos->tran.x - p->geometry[P_X_OFFSET]; j[1] = pos->tran.y; j[2] = pos->tran.z; return 0; -} // myKinematicsInverse() +} // 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 the switchkins-types are registered and -// the implementation started from here. +// switchkinsInit() expects, so setup is run and the implementation +// started from here. EXTRA_SETUP() { kparms kp; (void)__comp_inst; (void)prefix; (void)extra_arg; - kp.kinsname = "switchkinscomp"; // must agree with the module name - kp.halprefix = "switchkinscomp"; // hal pin names - kp.required_coordinates = "xyz"; - kp.allow_duplicates = 0; - kp.fwd_iterates_mask = 0; // set bit N if type N iterates - kp.gui_kinstype = -1; // negative means: not used - kp.sparm = NULL; - kp.max_joints = strlen(kp.required_coordinates); - - // switchkins-type 0 is the startup default. Types run from 0 to - // SWITCHKINS_MAX_TYPES-1 with no gaps. - if (switchkinsRegister(0, identityKinematicsSetup, - identityKinematicsForward, - identityKinematicsInverse)) { return -1; } - if (switchkinsRegister(1, myKinematicsSetup, - myKinematicsForward, - myKinematicsInverse)) { return -1; } - + if (switchkinsRunSetup(&kp, NULL)) { return -1; } return switchkinsInit(comp_id, &kp, coordinates); } // EXTRA_SETUP() From cc7cd9317ab1ab3bb7783169016c718fdd7e7dcc Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 11 Sep 2026 19:55:34 +0800 Subject: [PATCH 34/77] switchkins: declare the primary and the machine frame kinstype from the ops table A kins_ops table now says what its type IS the same way it says the maths: .primary marks the module's working transform, and switchkinsRegisterOps() bridges it into the declared flags exactly like .identity. G43.4 resolves the switch target from the flag, so the number of the working kinematics stops being a guess. Like identity, primary may be declared for at most one kinstype; two answers fail the module load. Every in-tree working transform declares it: fiveaxis, the two trt tables, scara, puma, genser, genhex, three21, millturn, tdr and the trsrn tcp tables. The trsrn tool kinematics stays undeclared: it is a reporting mode, not the working transform. A third flag, KINSTYPE_MACHINE, names the machine frame type: the machine with the orientation left out, XYZ the pivot in machine coordinates and the rotary letters the rotary joints. On a machine whose slides line up with its frame that is the identity type, so a module that declares none has its identity type stand in, and every in-tree module is of that kind. A module whose carriage does not line up, a slanted slide or a composite Y on a mill-turn, declares its machine frame type separately and keeps identity for a type whose joints really are the axes, since the shortcuts taken on the identity flag (the Jacobian, the tool offset change, the user-space loader's KINEMATICS_IDENTITY) would be wrong on a transform. G13.1 and G49 cancel to the machine frame type; one at most, as for the others. --- docs/src/motion/switchkins.adoc | 42 +++++++++++++++++--------- src/emc/kinematics/5axiskins.c | 1 + src/emc/kinematics/genhexkins.c | 1 + src/emc/kinematics/genserfuncs.c | 1 + src/emc/kinematics/kinematics.h | 27 ++++++++++++++--- src/emc/kinematics/kins_single.c | 8 +++++ src/emc/kinematics/pumakins.c | 1 + src/emc/kinematics/scarakins.c | 1 + src/emc/kinematics/switchkins.c | 37 ++++++++++++++++++----- src/emc/kinematics/switchkins.h | 6 ++-- src/emc/kinematics/three21kins.c | 1 + src/emc/kinematics/trtfuncs.c | 2 ++ src/emc/rs274ngc/interp_convert.cc | 16 +++++----- src/emc/rs274ngc/rs274ngc_return.hh | 2 +- src/hal/components/millturn.comp | 1 + src/hal/components/xyzab_tdr_kins.comp | 1 + src/hal/components/xyzacb_trsrn.comp | 1 + src/hal/components/xyzbca_trsrn.comp | 1 + 18 files changed, 112 insertions(+), 38 deletions(-) diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 14fb1b7aabb..bc2aad05bb4 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -181,8 +181,9 @@ meant to be removed in the future. === G-code commands 'G12.1 P-' selects a kinstype and 'G13.1' cancels back to identity -kinematics. Which kinstype is identity is declared by the module (see -Code Notes), not fixed to a number: +kinematics. Which kinstype that is, the module declares (see Code +Notes), not fixed to a number; on a module whose slides do not line up +with its frame it is the machine frame type the module declares instead: [source,ngc] ---- @@ -218,9 +219,9 @@ Selection is not cancelled by the end of a program or by an abort, so that the kinstype continues to match the position readout. A program that should leave the machine in identity kinematics ends with 'G13.1'. -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. +A module that declares no identity or machine frame 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. 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 @@ -554,26 +555,37 @@ kinematics.h: . *KINSTYPE_IDENTITY* no transform: the joints are the world . *KINSTYPE_PRIMARY* the module's working transform +. *KINSTYPE_MACHINE* the machine frame: the pivot in machine coordinates, + the rotaries as joints 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 +the ops table itself, as the 'identity', 'primary' or 'machine' field; a +kinstype registered the older way gets it from a call, again from within switchkinsSetup(): ---- int switchkinsDeclare(int ktype, int flags); ---- -G-code reads these declarations: 'G13.1' cancels to the kinstype -declared KINSTYPE_IDENTITY, and 'G43.4' switches to the kinstype +G-code reads these declarations: 'G13.1' and 'G49' cancel to the +kinstype declared KINSTYPE_MACHINE, and 'G43.4' switches to the kinstype declared KINSTYPE_PRIMARY, whatever their numbers, so a module whose kinematics are not in the conventional order still gets working -spellings. 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. +spellings. The machine frame type is the machine with the orientation +left out, XYZ the pivot in machine coordinates and the rotary letters +the rotary joints; on a machine whose slides line up with its frame that +is the identity type, so a module that declares no machine frame +kinstype has its identity kinstype stand in, and every shipped module is +of that kind. A module whose carriage does not line up, a slanted slide +or a composite Y on a mill-turn, declares its machine frame kinstype +separately and keeps KINSTYPE_IDENTITY for a kinstype whose joints +really are the axes, since motion and the planner skip the maths on that +flag alone. At most one kinstype may be declared identity, at most one +primary and at most one machine frame, and declaring a kinstype the +module does not provide fails the module load. A module that declares +nothing keeps working exactly as before for 'G12.1 P-' and 'G49', but +'G13.1' and 'G43.4' are an error, since the numbers of the identity and +primary kinematics are then a guess. When every kinstype is registered, the module calls: diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index cf2e0254da4..d1829b483b0 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -204,6 +204,7 @@ static const kins_ops fiveaxis_ops = { .forward = fiveaxis_forward, .inverse = fiveaxis_inverse, .jacobian = fiveaxis_jacobian, + .primary = 1, }; int switchkinsSetup(kparms* kp, diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index fa5276f5a56..4deb523d9a3 100644 --- a/src/emc/kinematics/genhexkins.c +++ b/src/emc/kinematics/genhexkins.c @@ -693,6 +693,7 @@ static const kins_ops genhex_ops = { .inverse = genhex_inverse, .jacobian = genhex_jacobian, .fwd_iterates = 1, + .primary = 1, }; int switchkinsSetup(kparms* kp, diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index 7a46d1cfdcc..02859829cf3 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -672,6 +672,7 @@ const kins_ops GENSER_OPS = { .forward = genser_forward, .inverse = genser_inverse, .jacobian = genser_jacobian, + .primary = 1, }; /* diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index 97f2e7f2cb1..2b950744dbc 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -116,12 +116,25 @@ extern KINEMATICS_TYPE kinematicsType(void); /* 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. +** G13.1 resolves the machine frame type from these flags instead of +** assuming a number; a module that declares nothing leaves its types +** numeric-only and G13.1 refuses to guess. +** +** The machine frame type is the one whose world is the machine frame +** with the orientation left out: XYZ is the pivot, the point the rotary +** joints do not move, in machine coordinates, and the rotary letters are +** the rotary joints as they are. On a machine whose slides line up with +** its frame that is the identity type, and where a module declares no +** machine frame type its identity type stands in. A module whose +** carriage does not line up, a slanted slide or an offset pivot, declares +** its machine frame type separately and keeps identity for a type whose +** joints really are the axes, since a consumer skips the maths on that +** flag alone. */ #define KINSTYPE_IDENTITY 0x1 /* no transform: the joints are the world */ #define KINSTYPE_PRIMARY 0x2 /* the module's working transform */ +#define KINSTYPE_MACHINE 0x4 /* the machine frame: the pivot in machine + coordinates, the rotaries as joints */ /* 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) */ @@ -580,7 +593,11 @@ typedef int (*kins_jacobian_fn)(const kins_params *p, const double *joint, 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. */ + a consumer may use to skip the maths altogether. primary says this is + the module's working transform, the type G43.4 switches to. machine says + this is the machine frame type, the pivot in machine coordinates with the + rotaries as joints, which G13.1 and G49 select and G53.5 moves in; a module + that leaves it unset on every type has its identity type stand in. */ typedef struct kins_ops { kins_forward_fn forward; kins_inverse_fn inverse; @@ -590,6 +607,8 @@ typedef struct kins_ops { kins_jacobian_fn jacobian; int fwd_iterates; int identity; /* joints are axes */ + int primary; /* the working transform */ + int machine; /* the machine frame */ } kins_ops; /* A module described for a caller outside RT: its table, its joint diff --git a/src/emc/kinematics/kins_single.c b/src/emc/kinematics/kins_single.c index 58f914076d5..16461c49375 100644 --- a/src/emc/kinematics/kins_single.c +++ b/src/emc/kinematics/kins_single.c @@ -130,6 +130,13 @@ int kinematicsSwitch(int 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. @@ -152,4 +159,5 @@ EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); +EXPORT_SYMBOL(kinematicsTypeFlags); EXPORT_SYMBOL(kinsDescribe); diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index d93ed5ed3b9..1fc778a4bbe 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -420,6 +420,7 @@ static const kins_ops puma_ops = { .work = kinsIdentityFrame, .tool = puma_tool_frame, .native = &TOOL_FRAME_FLANGE, + .primary = 1, }; int switchkinsSetup(kparms* kp, diff --git a/src/emc/kinematics/scarakins.c b/src/emc/kinematics/scarakins.c index 34035b8adac..19820930c91 100644 --- a/src/emc/kinematics/scarakins.c +++ b/src/emc/kinematics/scarakins.c @@ -249,6 +249,7 @@ static const kins_ops scara_ops = { .forward = scara_forward, .inverse = scara_inverse, .jacobian = scara_jacobian, + .primary = 1, }; int switchkinsSetup(kparms* kp, diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index 24b432262fb..de5a79efa03 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -61,8 +61,8 @@ static int inited; static int kins_count; static int register_error; -// what each type IS (KINSTYPE_IDENTITY, KINSTYPE_PRIMARY), declared by -// the module with switchkinsDeclare(); 0==the module said nothing +// what each type IS (KINSTYPE_IDENTITY, KINSTYPE_PRIMARY, KINSTYPE_MACHINE), +// declared by the module with switchkinsDeclare(); 0==the module said nothing static int ktype_flags[SWITCHKINS_MAX_TYPES] = {0}; static int switchkins_type; @@ -482,6 +482,8 @@ int switchkinsRegisterOps(int ktype, const kins_ops *ops) } kops[ktype] = ops; if (ops->identity) { ktype_flags[ktype] |= KINSTYPE_IDENTITY; } + if (ops->primary) { ktype_flags[ktype] |= KINSTYPE_PRIMARY; } + if (ops->machine) { ktype_flags[ktype] |= KINSTYPE_MACHINE; } return 0; } // switchkinsRegisterOps() @@ -604,7 +606,7 @@ int switchkinsInit(const int comp_id, const char* coordinates) { int i; - int identities; + int identities, primaries, machines; int res = 0; char* emsg = "other"; @@ -632,9 +634,17 @@ int switchkinsInit(const int comp_id, } 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 + // declarations must name provided types, and each flag is unique: + // G13.1 and G49 resolve the machine frame type from the flags and + // G43.4 the primary, so two answers is a load error. A module that + // names no machine frame type has its identity type stand in, which + // is the truth on every machine whose slides line up with its frame. identities = 0; + primaries = 0; + machines = 0; + for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { + if (ktype_flags[i] & KINSTYPE_MACHINE) { machines++; } + } for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { if (!ktype_flags[i]) { continue; } if (i >= kins_count) { @@ -643,14 +653,25 @@ int switchkinsInit(const int comp_id, " not provided\n", i); emsg = "declared switchkins-type not provided"; goto error; } - if (ktype_flags[i] & KINSTYPE_IDENTITY) { identities++; } - rtapi_print("switchkins-type %d declared:%s%s\n", i, + if (ktype_flags[i] & KINSTYPE_IDENTITY) { + identities++; + if (!machines) { ktype_flags[i] |= KINSTYPE_MACHINE; } + } + if (ktype_flags[i] & KINSTYPE_PRIMARY) { primaries++; } + rtapi_print("switchkins-type %d declared:%s%s%s\n", i, (ktype_flags[i] & KINSTYPE_IDENTITY) ? " identity" : "", - (ktype_flags[i] & KINSTYPE_PRIMARY) ? " primary" : ""); + (ktype_flags[i] & KINSTYPE_PRIMARY) ? " primary" : "", + (ktype_flags[i] & KINSTYPE_MACHINE) ? " machine" : ""); } if (identities > 1) { emsg = "more than one identity switchkins-type declared"; goto error; } + if (primaries > 1) { + emsg = "more than one primary switchkins-type declared"; goto error; + } + if (machines > 1) { + emsg = "more than one machine frame switchkins-type declared"; goto error; + } for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { if (kp.fwd_iterates_mask & (1< still works, -// G13.1 refuses to guess which type is identity. +// (KINSTYPE_IDENTITY, KINSTYPE_PRIMARY, KINSTYPE_MACHINE, kinematics.h). +// A module that never calls it leaves its types numeric-only: G12.1 P +// still works, G13.1 refuses to guess which type is the machine frame. extern int switchkinsDeclare(int ktype, int flags); // KinematicsJACOBIAN function (optional, see kinematics.h) diff --git a/src/emc/kinematics/three21kins.c b/src/emc/kinematics/three21kins.c index ae602858389..903e3e08a07 100644 --- a/src/emc/kinematics/three21kins.c +++ b/src/emc/kinematics/three21kins.c @@ -386,6 +386,7 @@ static const kins_ops three21_ops = { .forward = three21_forward, .inverse = three21_inverse, .jacobian = three21_jacobian, + .primary = 1, }; int switchkinsSetup(kparms* kp, diff --git a/src/emc/kinematics/trtfuncs.c b/src/emc/kinematics/trtfuncs.c index 023f0a3d1fa..86a24e88410 100644 --- a/src/emc/kinematics/trtfuncs.c +++ b/src/emc/kinematics/trtfuncs.c @@ -263,6 +263,7 @@ const kins_ops XYZAC_OPS = { .tool = kinsIdentityFrame, .native = &TOOL_FRAME_SPINDLE, .jacobian = xyzac_jacobian, + .primary = 1, }; static int xyzbc_forward(const kins_params *p, kins_scratch *s, @@ -455,4 +456,5 @@ const kins_ops XYZBC_OPS = { .tool = kinsIdentityFrame, .native = &TOOL_FRAME_SPINDLE, .jacobian = xyzbc_jacobian, + .primary = 1, }; diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 82a69a240e1..b92c4f69bdd 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -6762,10 +6762,11 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu 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); + // to the machine frame type, identity kinematics on a machine whose + // slides line up, 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 such type keeps the plain cancel. + int identity = flagged_kins_type(KINSTYPE_MACHINE); if (identity >= 0) { switch_kins_type(identity, settings); } settings->kins_by_g43_4 = false; } @@ -6834,9 +6835,10 @@ int Interp::convert_kins_switch(int code, //!< G_12_1 or G_13_1 int kins_type; if (code == G_13_1) { - // G13.1 cancels to identity kinematics; which type that is, the - // module declares, the number is not the answer - kins_type = flagged_kins_type(KINSTYPE_IDENTITY); + // G13.1 cancels to the machine frame type, identity kinematics on a + // machine whose slides line up; which type that is, the module + // declares, the number is not the answer + kins_type = flagged_kins_type(KINSTYPE_MACHINE); if (kins_type < 0) { CHKS(kins_type_info_available(), NCE_NO_IDENTITY_KINEMATICS_TYPE); kins_type = 0; // no kinematics attached: standalone interpreter diff --git a/src/emc/rs274ngc/rs274ngc_return.hh b/src/emc/rs274ngc/rs274ngc_return.hh index f7f8dfcacfc..620f6ecfee6 100644 --- a/src/emc/rs274ngc/rs274ngc_return.hh +++ b/src/emc/rs274ngc/rs274ngc_return.hh @@ -207,7 +207,7 @@ #define NCE_QUEUE_IS_NOT_EMPTY_AFTER_INPUT _("Queue is not empty after external input") #define NCE_QUEUE_IS_NOT_EMPTY_AFTER_KINS_SWITCH _("Queue is not empty after Kinematics Switch") #define NCE_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_IDENTITY_KINEMATICS_TYPE _("G13.1 needs the kinematics module to declare its identity or machine frame 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") diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index b841ba3f0e0..bde6136efff 100644 --- a/src/hal/components/millturn.comp +++ b/src/hal/components/millturn.comp @@ -121,6 +121,7 @@ static const kins_ops turn_ops = { .forward = turn_forward, .inverse = turn_inverse, .jacobian = turn_jacobian, + .primary = 1, }; int switchkinsSetup(kparms* kp, diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index ce21a7d8159..a2d41593b4b 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -203,6 +203,7 @@ static const kins_ops tdr_ops = { .forward = tdr_forward, .inverse = tdr_inverse, .jacobian = tdr_jacobian, + .primary = 1, }; int switchkinsSetup(kparms* kp, diff --git a/src/hal/components/xyzacb_trsrn.comp b/src/hal/components/xyzacb_trsrn.comp index 8fb6d8dae39..5c0f09565a6 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -541,6 +541,7 @@ static const kins_ops tcp_ops = { .tool = tcpKinematicsToolFrame, .native = &TOOL_FRAME_SPINDLE, .jacobian = tcpKinematicsJacobian, + .primary = 1, }; // the tool kinematics report in tool axes, so the tool is square with the diff --git a/src/hal/components/xyzbca_trsrn.comp b/src/hal/components/xyzbca_trsrn.comp index 151fde7a352..9694b5b6f12 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -538,6 +538,7 @@ static const kins_ops tcp_ops = { .tool = tcpKinematicsToolFrame, .native = &TOOL_FRAME_SPINDLE, .jacobian = tcpKinematicsJacobian, + .primary = 1, }; // the tool kinematics report in tool axes, so the tool is square with the From 697ef0aaa28032a1ddcd0a2a00a5c69070a6b87c Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:24:58 +1000 Subject: [PATCH 35/77] docs: describe the parameter block form of a kinematics module The conventions chapter gains a section on the two blocks, the table, the ops table and what the shared code does with them in and outside realtime, and the Writing a Module list gains "no state". The frames and Jacobian sections point at the ops table where they pointed at the register calls. The switchkins chapter's Code Notes describe switchkinsRegisterOps(), the table in kparms, switchkinsRunSetup() and kinsDescribe(), keep the older registration as the older form, and the outline is a module written the new way. --- docs/src/motion/kinematics-conventions.adoc | 107 +++++++++++++-- docs/src/motion/switchkins.adoc | 137 ++++++++++++-------- 2 files changed, 181 insertions(+), 63 deletions(-) diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 5740fbc7012..3b58d13d653 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -178,7 +178,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 @@ -289,6 +289,8 @@ 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 @@ -438,28 +440,28 @@ every consumer would otherwise guess it. === What a module has to supply -Nothing. If a module registers no Jacobian, the shared code computes one by +Nothing. If a module supplies no Jacobian, the shared code computes one by central differences: it calls the module's inverse eighteen times, stepping each pose coordinate a small amount to either side on the solution branch the inverse flags select, and differences the results. The answer is as precise as the inverse itself; behind an inverse that iterates, that is the -iteration's convergence tolerance divided by the step. Modules built on `switchkins.c` answer this way for every type that -registers nothing; an identity type answers exactly. +iteration's 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. This cost matters because `kinematicsJacobian()` is meant to run in the servo thread, where checking a feed limit calls it once per cycle: each call then costs eighteen inverses. On the closed-form inverses in the tree that is under two microseconds. `genserkins`, the one module whose inverse iterates, would cost near eighty microseconds per call this way, against -under two for the geometric Jacobian it registers instead. +under two for the geometric Jacobian it supplies instead. -So a module whose inverse iterates should register a closed-form Jacobian, -as below. Nothing enforces this. A module that registers nothing still gets +So a module whose inverse iterates should supply a closed-form Jacobian, +as below. Nothing enforces this. A module that supplies none still gets a correct Jacobian, just one that costs eighteen inverses and carries the iteration's precision, which behind an iterating inverse is too slow for the servo thread. -A module with a closed form registers it with `switchkinsRegisterJacobian()`. +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 supplies one. The two arms whose inverse is @@ -476,6 +478,88 @@ 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` switches 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. 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 and takes the tool from motion's own offset pins, and says once when the +module's tool pin disagrees with them, which is a config that lost the tool on +the way. `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. 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 @@ -516,6 +600,13 @@ Geometry stays in the module:: 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 diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index bc2aad05bb4..908adf92a6b 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -441,7 +441,8 @@ 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 @@ -470,18 +471,20 @@ is included: [source,c] ---- #include +#include #include ---- A realtime module cannot link a library, so the implementation arrives -as source: switchkins.c and kins_util.c are installed beside the -headers, in share/linuxcnc, and halcompile already looks there. With +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 registers each of its kinstypes and calls switchkinsInit() -from EXTRA_SETUP(), which halcompile runs after hal_init() and before -hal_ready(). See <> for both -calls. +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 @@ -533,17 +536,26 @@ kinstype currently selected, and it creates the HAL pins common to all switchkins modules. It does not provide the module 'main' program, so a module can get that from wherever suits it. -A kinstype is supplied by calling switchkinsRegister(), once per -kinstype: +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): + +---- +int switchkinsRegisterOps(int ktype, const kins_ops *ops); +---- + +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. @@ -593,15 +605,15 @@ When every kinstype is registered, the module calls: int switchkinsInit(const int comp_id, kparms* kp, const char* coordinates); ---- -which checks the supplied parameters, creates the HAL pins, selects -kinstype 0, and then invokes the setup routine registered for each -kinstype. The caller owns the HAL component: it does hal_init() -before switchkinsInit() and hal_ready() after it. +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. -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. +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. === Module main program @@ -618,31 +630,59 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2); ---- -which identifies the setup, forward and inverse routines for -kinstypes 0,1,2 and sets a number of configuration settings. Those -three are registered for the module, so it can supply further -kinstypes by calling switchkinsRegister() itself, and registering -one that switchkinsSetup() has already filled in is the same error -as any other duplicate. +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 registers its kinstypes and calls -switchkinsInit() from its EXTRA_SETUP() routine, which halcompile -runs after hal_init() and before hal_ready(). The component names -the objects it needs in hal/components/Submakefile: +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/kins_util.o +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, @@ -653,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: From 38fd61d2d631b4b20df36ccd465e5072e5a64c44 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:24:57 +1000 Subject: [PATCH 36/77] kinematics_user: take the joints in as well as out, and bind modules lazily The loader zeroed the joints before a module's inverse and before an iterating forward. Motion hands a module the joints the machine is at and some read them, a nutating head its rotary angles, the hexapod its starting pose, so zeros put the loader on a different branch from realtime for xyzacb_trsrn. The caller's joints are the seed now, an iterating forward keeps the caller's pose, and the Jacobian runs its inverse from what the last inverse found. A halcompile component references hal_export_funct() and the rest of what its rtapi_app_main() needs, which only the realtime HAL library provides, so dlopen with RTLD_NOW refused every component; nothing here calls that main, so bind lazily. The module is looked up in the directory it is installed in, with the HAL library within reach. --- .../kinematics_userspace/kinematics_user.c | 44 +++++++++++++++---- .../kinematics_userspace/kinematics_user.h | 17 +++++-- 2 files changed, 49 insertions(+), 12 deletions(-) diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index b1ccec60eca..c7fe4f25bf5 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -28,7 +28,7 @@ #include #include -#include "config.h" /* EMC2_HOME */ +#include "config.h" /* EMC2_RTLIB_DIR, MODULE_EXT */ typedef int (*kins_describe_fn)(const char *coordinates, const char *sparm, kins_module_info *info); @@ -57,6 +57,7 @@ struct KinematicsUserContext { int cell_of_tool[AXIS_COUNT]; /* motion.tooloffset.*, -1 if absent */ int tool_param; /* the table's tool entry, -1 if none */ int warned_tool; + double last_joints[EMCMOT_MAX_JOINTS]; /* what the last inverse found */ }; /* ======================================================================== @@ -284,13 +285,29 @@ static int load_module(KinematicsUserContext *ctx, const char *sparm) { char module_path[512]; - void *handle; + void *handle, *hal_lib; kins_describe_fn describe; snprintf(module_path, sizeof(module_path), - "%s/rtlib/%s.so", EMC2_HOME, module_name); - - handle = dlopen(module_path, RTLD_NOW | RTLD_LOCAL); + "%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()); @@ -431,12 +448,18 @@ int kinematicsUserInverse(KinematicsUserContext* ctx, if (ctx->rt_only) return -1; refresh(ctx); - for (i = 0; i < EMCMOT_MAX_JOINTS; i++) j[i] = 0.0; + /* 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; } @@ -456,7 +479,11 @@ int kinematicsUserForward(KinematicsUserContext* ctx, for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { j[i] = (i < ctx->num_joints) ? joints[i] : 0.0; } - memset(world, 0, sizeof(*world)); + /* 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); } @@ -475,7 +502,8 @@ int kinematicsUserJacobian(KinematicsUserContext* ctx, if (ctx->rt_only) return -1; refresh(ctx); - for (r = 0; r < EMCMOT_MAX_JOINTS; r++) j[r] = 0.0; + /* 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; diff --git a/src/emc/kinematics_userspace/kinematics_user.h b/src/emc/kinematics_userspace/kinematics_user.h index 0a7187537f9..3d1e8c2bf8f 100644 --- a/src/emc/kinematics_userspace/kinematics_user.h +++ b/src/emc/kinematics_userspace/kinematics_user.h @@ -90,9 +90,14 @@ int kinematicsUserGetNumTypes(KinematicsUserContext* ctx); /** * 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 Output array of joint positions [KINEMATICS_USER_MAX_JOINTS] + * @param joints Joint positions in and out [KINEMATICS_USER_MAX_JOINTS] * @return 0 on success, -1 on failure */ int kinematicsUserInverse(KinematicsUserContext* ctx, @@ -102,9 +107,12 @@ int kinematicsUserInverse(KinematicsUserContext* ctx, /** * 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 + * @param world Output world coordinates, and the seed on input * @return 0 on success, -1 on failure */ int kinematicsUserForward(KinematicsUserContext* ctx, @@ -114,8 +122,9 @@ int kinematicsUserForward(KinematicsUserContext* ctx, /** * 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, so the - * derivative is taken on the solution branch the module picks there. + * 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 */ From 8542d01b73ff8825f07107b746d01fd167583094 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:24:58 +1000 Subject: [PATCH 37/77] tests: check that a module answers the same outside realtime tests/kins-params loads each module, evaluates it once in realtime through the classic entry points (paritycheck.c publishes the forward, the inverse and the Jacobian on pins) and once through the non-realtime loader from python (check.py: kinsDescribe(), the block filled from the module's pins, the same ops), and requires the same success and the same numbers to rounding. 34 runs over the 24 modules, every switchable type that has geometry of its own, the parallel machines from a pose their forward can be seeded with. Checked by mutation: the loader not refreshing the geometry fails 10 comparisons in the first module with a table; the loader seeding the inverse with zeros instead of the caller's joints fails the three translation joints of xyzacb_trsrn, which reads its rotary angles from that array. --- tests/kins-params/check.py | 136 +++++++++++++++++++++++++++ tests/kins-params/checkresult | 4 + tests/kins-params/paritycheck.c | 148 +++++++++++++++++++++++++++++ tests/kins-params/skip | 4 + tests/kins-params/test.sh | 161 ++++++++++++++++++++++++++++++++ 5 files changed, 453 insertions(+) create mode 100755 tests/kins-params/check.py create mode 100755 tests/kins-params/checkresult create mode 100644 tests/kins-params/paritycheck.c create mode 100755 tests/kins-params/skip create mode 100755 tests/kins-params/test.sh diff --git a/tests/kins-params/check.py b/tests/kins-params/check.py new file mode 100755 index 00000000000..b6f7758f892 --- /dev/null +++ b/tests/kins-params/check.py @@ -0,0 +1,136 @@ +#!/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 +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))) + +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..82f5d294e23 --- /dev/null +++ b/tests/kins-params/test.sh @@ -0,0 +1,161 @@ +#!/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 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" + for tok in $parms; do + case "$tok" in + joints=*) joints=${tok#joints=} ;; + frompose=*) frompose=${tok#frompose=} ;; + pose=*) pose=${tok#pose=} ;; + jnt=*) jnt=${tok#jnt=} ;; + esac + done + hal=$(mktemp --suffix=.hal) + { printf 'loadrt %s\n' "$loadrt" + printf '%s\n' "$setp" + printf 'loadrt paritycheck %s ktype=%s\n' "$parms" "${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\n' \ + "$module" "$joints" "${coords:--}" "${ktype:-0}" "$frompose" "$pose" "$jnt" "${sparm:--}" + } > "$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" +run "trivkins coordinates=XYZY kinstype=BOTH" "" "joints=4 jnt=10,20,30,20" +run "trivkins coordinates=XYZABCUVW" "" "joints=9" +run "userkins" "" "joints=3 jnt=10,20,30" +run "millturn" "" "joints=4 jnt=10,20,30,40" +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" +run "5axiskins coordinates=XYZBCW sparm=identityfirst" "" "joints=6 jnt=10,20,30,15,25,5" 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" + +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" + +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" 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" 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" 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" +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" From 94a4090003c2e717908570d97c85e6e49173f097 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 16:33:19 +1000 Subject: [PATCH 38/77] kinematics: take the tool offset from motion, not from a net A module whose maths needs the tool length read it from a pin the config had to net from motion.tooloffset.z: a copy of controller state motion already holds, one cycle late, and a missing net gave wrong joints with no error. The block carries the tool, so motion hands it over: kinematicsSetTool() is called whenever the offset changes, referenced weakly so an older module still loads. kins_single.c and switchkins.c export it for every module on the block; the pin is read only until motion has sent anything, and a pin left disagreeing is reported once, since it is a config setting a tool length where the tool table should. A pin nobody set is not a disagreement. The non-RT loader takes the tool from the caller through kinematicsUserSetTool() where one is given, a planner knowing what a segment runs under better than the machine does. tests/kins-tool-offset runs xyzac-trt-kins with nothing on its tool pin and checks that G43 reaches the joints, that netting the pin changes nothing and that G49 takes the length out; tests/kins-params checks the caller's tool on the loader. --- docs/src/code/code-notes.adoc | 3 +- docs/src/motion/5-axis-kinematics.adoc | 21 ++-- docs/src/motion/kinematics-conventions.adoc | 34 ++++-- docs/src/motion/switchkins.adoc | 8 +- src/emc/kinematics/kinematics.h | 9 ++ src/emc/kinematics/kins_rt.h | 20 ++++ src/emc/kinematics/kins_single.c | 13 +- src/emc/kinematics/kins_util.c | 37 ++++++ src/emc/kinematics/switchkins.c | 16 ++- .../kinematics_userspace/kinematics_user.c | 33 ++++- .../kinematics_userspace/kinematics_user.h | 15 ++- src/emc/motion/command.c | 8 ++ tests/kins-params/check.py | 29 +++++ tests/kins-tool-offset/README | 7 ++ tests/kins-tool-offset/checkresult | 2 + tests/kins-tool-offset/sim.hal | 20 ++++ tests/kins-tool-offset/test-ui.py | 113 ++++++++++++++++++ tests/kins-tool-offset/test.ini | 112 +++++++++++++++++ tests/kins-tool-offset/test.sh | 2 + tests/kins-tool-offset/tool.tbl | 1 + 20 files changed, 469 insertions(+), 34 deletions(-) create mode 100644 tests/kins-tool-offset/README create mode 100755 tests/kins-tool-offset/checkresult create mode 100644 tests/kins-tool-offset/sim.hal create mode 100755 tests/kins-tool-offset/test-ui.py create mode 100644 tests/kins-tool-offset/test.ini create mode 100755 tests/kins-tool-offset/test.sh create mode 100644 tests/kins-tool-offset/tool.tbl 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/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 3b58d13d653..02446cf425a 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -538,15 +538,26 @@ with `switchkinsRegisterOps()`; a module with one type describes itself in a 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. 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 and takes the tool from motion's own offset pins, and says once when the -module's tool pin disagrees with them, which is a config that lost the tool on -the way. `kinslimits` is built on it. +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. @@ -555,8 +566,9 @@ be evaluated outside realtime, which the loader reports. 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. The joint -map is in the block, from `coordinates=`. Nothing else the maths needs exists, +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. diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 908adf92a6b..ea5a1d98934 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -613,7 +613,13 @@ 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. +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. A kinstype registered the older +way reads its own pins and is not affected. === Module main program diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index 2b950744dbc..a57446063e8 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -707,6 +707,15 @@ extern int kinsOpsJacobian(const kins_ops *ops, const kins_params *p, 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 diff --git a/src/emc/kinematics/kins_rt.h b/src/emc/kinematics/kins_rt.h index 96d7309c739..78d0a61ef7d 100644 --- a/src/emc/kinematics/kins_rt.h +++ b/src/emc/kinematics/kins_rt.h @@ -42,6 +42,26 @@ 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. */ diff --git a/src/emc/kinematics/kins_single.c b/src/emc/kinematics/kins_single.c index 16461c49375..2ca2f057e2a 100644 --- a/src/emc/kinematics/kins_single.c +++ b/src/emc/kinematics/kins_single.c @@ -20,6 +20,7 @@ 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; @@ -28,11 +29,13 @@ static const kins_ops *ops(void) return inited ? kins_module.ops[0] : NULL; } -// the block sees the pins as they are now +// 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) @@ -117,6 +120,13 @@ int kinematicsJacobian(const double *joint, 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; @@ -157,6 +167,7 @@ EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsWorkFrame); EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsJacobian); +EXPORT_SYMBOL(kinematicsSetTool); EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsTypeFlags); diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index 5187c2598ce..4b5e59bc36b 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -1708,3 +1708,40 @@ void kinsParamsPinsWrite(const kins_pin_ref *pins, } } } // 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/switchkins.c b/src/emc/kinematics/switchkins.c index de5a79efa03..969fc819884 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -55,6 +55,7 @@ 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 @@ -113,13 +114,25 @@ static void get_lastpose(int ktype, EmcPose* pos) pos->w = lastpose[ktype].w; } // get_lastpose() -// the block sees the pins as they are now, and the type asked for +// 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]); @@ -550,6 +563,7 @@ EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsWorkFrame); EXPORT_SYMBOL(kinematicsToolFrameInverse); EXPORT_SYMBOL(kinematicsJacobian); +EXPORT_SYMBOL(kinematicsSetTool); EXPORT_SYMBOL(switchkinsRegister); EXPORT_SYMBOL(switchkinsRegisterFrames); EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index c7fe4f25bf5..b8b339f564e 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -6,7 +6,9 @@ * kinsDescribe(), and evaluates its kinematics through the parameter * block (see kinematics.h). The block is filled from HAL: one input pin * of the caller's component per table entry, connected to the signal the - * RT instance's pin reads, so the values are the live ones; and the tool + * RT instance's pin reads, so the values are the live ones. 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. @@ -57,6 +59,8 @@ struct KinematicsUserContext { int cell_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 */ }; @@ -232,7 +236,8 @@ static int bind_all(KinematicsUserContext *ctx) return 0; } -/* The block sees the pins as they are now. */ +/* 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; @@ -248,6 +253,14 @@ static void refresh(KinematicsUserContext *ctx) 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->cell_of_tool[i]; tool[i] = 0.0; @@ -259,8 +272,10 @@ static void refresh(KinematicsUserContext *ctx) /* 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. */ + 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;" @@ -435,6 +450,18 @@ int kinematicsUserGetNumTypes(KinematicsUserContext* ctx) 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) diff --git a/src/emc/kinematics_userspace/kinematics_user.h b/src/emc/kinematics_userspace/kinematics_user.h index 3d1e8c2bf8f..e3090a34bd5 100644 --- a/src/emc/kinematics_userspace/kinematics_user.h +++ b/src/emc/kinematics_userspace/kinematics_user.h @@ -9,8 +9,9 @@ * 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, and from motion's tool - * offset pins where motion is loaded, so the maths runs on live values. + * 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 @@ -87,6 +88,16 @@ int kinematicsUserSetType(KinematicsUserContext* ctx, int ktype); */ 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) * diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 17e9c767a37..6ee7f155971 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)) @@ -2017,6 +2022,9 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) case EMCMOT_SET_OFFSET: rtapi_print_msg(RTAPI_MSG_DBG, "SET_OFFSET"); emcmotStatus->tool_offset = emcmotCommand->tool_offset; + if (kinematicsSetTool) { + kinematicsSetTool(&emcmotStatus->tool_offset); + } break; case EMCMOT_SET_AXIS_POSITION_LIMITS: diff --git a/tests/kins-params/check.py b/tests/kins-params/check.py index b6f7758f892..265fb606e8c 100755 --- a/tests/kins-params/check.py +++ b/tests/kins-params/check.py @@ -128,6 +128,35 @@ def compare(what, ours, theirs): 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]) + kins.kinematicsUserFree(ctx) halc.hal_exit(comp_id) 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 From c428abb8418bcb4eb09a576bf485b252a950f537 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 14 Sep 2026 13:41:31 +1000 Subject: [PATCH 39/77] 5axiskins, maxkins: take the tool length from motion The trt modules, genhexkins and pentakins already have motion hand them the offset in effect; these two still read a pin the config had to net from motion.tooloffset.z, and a missing net gave wrong joints with no error. Flag the entry as the tool, drop the nets from the sims, and have the kins-switch test check that G43.4 reaches the joints with nothing netted: in the tilted pose the length moves them by its tilt term, the same length along the tool instead of along Z. --- .../vismach/5axis/bridgemill/5axisgui.hal | 1 - .../sim/axis/vismach/5axis/max5/max5kins.hal | 4 +- docs/src/man/man9/kins.9.adoc | 20 +++++---- src/emc/kinematics/5axiskins.c | 6 +-- src/emc/kinematics/maxkins.c | 2 +- tests/kins-switch/README | 5 ++- tests/kins-switch/test-ui.py | 42 ++++++++++++++++++- 7 files changed, 59 insertions(+), 21 deletions(-) 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/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/docs/src/man/man9/kins.9.adoc b/docs/src/man/man9/kins.9.adoc index f2bd1051d4c..79a3a116e21 100644 --- a/docs/src/man/man9/kins.9.adoc +++ b/docs/src/man/man9/kins.9.adoc @@ -252,9 +252,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. To avoid a joint jump, change the + tool offset only when B is 0. === pentakins - Pentapod Kinematics @@ -413,12 +414,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. To avoid a joint jump, change the + tool offset only when B is 0. 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/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index d1829b483b0..7af800fa693 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -12,8 +12,8 @@ * * Notes: * 1) pivot-length must agree with the mechanical design -* (including vismach simulation); the tool length comes -* in on the tool-length pin of its own +* (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) @@ -63,7 +63,7 @@ // 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, 0, 0.0 }, + { "tool-length", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, }; enum { P_PIVOT_LENGTH, P_TOOL_LENGTH }; diff --git a/src/emc/kinematics/maxkins.c b/src/emc/kinematics/maxkins.c index cce1fbc1797..24fe3a76b90 100644 --- a/src/emc/kinematics/maxkins.c +++ b/src/emc/kinematics/maxkins.c @@ -35,7 +35,7 @@ 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, 0, 0 }, + { "tool-length", KINS_PARAM_FLOAT, KINS_IN, 1, 0 }, }; enum { P_PIVOT_LENGTH, P_CON, P_TOOL_LENGTH }; diff --git a/tests/kins-switch/README b/tests/kins-switch/README index e031f2cd0ff..e426b4d7554 100644 --- a/tests/kins-switch/README +++ b/tests/kins-switch/README @@ -7,5 +7,6 @@ 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 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 7e999b9aa76..20544d5dd64 100755 --- a/tests/kins-switch/test-ui.py +++ b/tests/kins-switch/test-ui.py @@ -76,6 +76,7 @@ def mdi(cmd): said = [] at_switch = None +holding = False strayed = [0.0] * JOINTS w_reached = 0.0 deadline = time.time() + 60 @@ -88,7 +89,13 @@ def mdi(cmd): last = 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]) @@ -147,10 +154,41 @@ def mdi(cmd): else: print("G43.4 switched to primary with the offset, G49 cancelled both") -# ---- a negative kinematics number is refused ----------------------------- +# ---- the tool length reaches the kinematics with nothing netted ---------- +# +# Motion hands the module the offset G43 puts in effect. The head is +# tilted, so the offset moves the joints, by the tilt term of the length: +# the same length along the tool axis instead of along Z. +import math c.mode(linuxcnc.MODE_MDI) c.wait_complete(30) +drain() +mdi("G12.1 P0") +mdi("G0 X10 Y10 Z-5 B-22.5 C45") +mdi("G49") +before = mdi("G0 X10 Y10 Z-5") +after = mdi("G43.4 H1") +L = 12.5 # tool 1 in tool.tbl +b, cc = math.radians(-22.5), math.radians(45) +want = [-L * math.sin(math.pi - b) * math.cos(cc), + -L * math.sin(math.pi - b) * math.sin(cc), + -L * (1 + math.cos(math.pi - b))] +got = [after[j] - before[j] for j in (0, 1, 2)] +if max(abs(g - w) for g, w in zip(got, want)) > 1e-3: + error("G43.4 in the tilted pose moved the joints by %s, not %s" + % (" ".join("%.4f" % v for v in got), " ".join("%.4f" % v for v in want))) +else: + print("G43.4 moved the joints by the tilt term of the tool length") +# G49 would drop to identity and hold the joints where they are; a zero +# offset without a switch takes the length back out +back = mdi("G43.1 Z0") +if max(abs(back[j] - before[j]) for j in (0, 1, 2)) > 1e-3: + error("a zero tool length did not take the length back out of the joints") +mdi("G49") + +# ---- a negative kinematics number is refused ----------------------------- + drain() c.mdi("G12.1 P-1") c.wait_complete(30) From 66b8d7153d2726d3e96c4535b25e93035f13347b Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 7 Sep 2026 16:30:15 +1000 Subject: [PATCH 40/77] 5axiskins: supply the work and tool frames The head carries the tool and nothing turns the work, so the work frame is the machine frame and the tool frame is Rz(C) Ry(-B), the tilt left handed about y as the module's note 10 says. Its third column is the tool axis the forward already uses, from the tip towards the holder, so no native rotation is needed. Without them G53.1, G53.3, G53.6 and G68.3 refused on the bridgemill sim while every other five-axis module took them. tests/kins-frames takes the module and gains two checks: the type the module models must supply frames of its own, since a switchable module's identity type passed on its neighbour's answers; and the W joint runs the tool out along the reported axis reversed, which ties the frame to the forward. Reversing the tilt, reversing the head's turn and dropping the frames each fail. --- src/emc/kinematics/5axiskins.c | 21 +++++++++++++++++++++ tests/kins-frames/checkresult | 2 +- tests/kins-frames/framecheck.c | 27 ++++++++++++++++++++++++++- tests/kins-frames/test.sh | 3 +++ 4 files changed, 51 insertions(+), 2 deletions(-) diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index 7af800fa693..81a765253b4 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -200,9 +200,30 @@ static int fiveaxis_jacobian(const kins_params *p, 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); + + 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; +} // 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, }; 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" From bd614b4adade6238004ff1929ecac26e66dbfc71 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 5 Sep 2026 00:11:48 +1000 Subject: [PATCH 41/77] kinematics: reach the frames and the tool frame inverse from outside realtime The loader gains the two frames, the tool frame inverse and a survey of which joints turn the work, on kins_util.c code the library already carries; only the glue between the block form and the joint-only frame functions is new. The survey lets a caller hold the table still without a config entry naming it. A second survey tells the two orienting rotaries apart, the primary whose axis is fixed in the machine from the secondary whose axis it carries, by turning each a little and reading the axis of the resulting rotation; the sign of the secondary is how a caller names a pose rather than counting them. It answers only for exactly two such rotaries, so a robot wrist gets nothing. tests/kins-params checks the pair every module reports. kinematicsUserInitString() takes [KINS] KINEMATICS as the HAL file hands it to loadrt. --- src/emc/kinematics/kinematics.h | 24 +++ src/emc/kinematics/kins_util.c | 114 ++++++++++++++ .../kinematics_userspace/kinematics_user.c | 143 ++++++++++++++++++ .../kinematics_userspace/kinematics_user.h | 54 +++++++ tests/kins-params/check.py | 26 ++++ tests/kins-params/test.sh | 39 +++-- tests/tool-frame/test_tool_frame.c | 19 +++ 7 files changed, 403 insertions(+), 16 deletions(-) diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index a57446063e8..16a8cc58184 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -368,6 +368,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, diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index 4b5e59bc36b..486efce9829 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -562,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 @@ -970,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, diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index b8b339f564e..25e8b5c374a 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -589,6 +589,149 @@ int kinematicsUserIsRtOnly(KinematicsUserContext* ctx) 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) { int i; diff --git a/src/emc/kinematics_userspace/kinematics_user.h b/src/emc/kinematics_userspace/kinematics_user.h index e3090a34bd5..6d9b07a0859 100644 --- a/src/emc/kinematics_userspace/kinematics_user.h +++ b/src/emc/kinematics_userspace/kinematics_user.h @@ -201,6 +201,60 @@ int kinematicsUserRefreshParams(KinematicsUserContext* ctx); */ 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 * diff --git a/tests/kins-params/check.py b/tests/kins-params/check.py index 265fb606e8c..a3133840f52 100755 --- a/tests/kins-params/check.py +++ b/tests/kins-params/check.py @@ -42,6 +42,9 @@ class EmcPose(ctypes.Structure): 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)) @@ -157,6 +160,29 @@ def compare(what, ours, theirs): 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) diff --git a/tests/kins-params/test.sh b/tests/kins-params/test.sh index 82f5d294e23..e429bf5029c 100755 --- a/tests/kins-params/test.sh +++ b/tests/kins-params/test.sh @@ -11,7 +11,7 @@ ${SUDO} halcompile --install paritycheck.c >/dev/null # 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 hal tok + local module coords sparm joints frompose pose jnt orient rtparms hal tok case "$loadrt" in "${ONLY:-}"*) ;; *) return 0 ;; esac module=${loadrt%% *} coords=""; sparm="" @@ -22,21 +22,28 @@ run() { 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' "$parms" "${ktype:-0}" + 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\n' \ - "$module" "$joints" "${coords:--}" "${ktype:-0}" "$frompose" "$pose" "$jnt" "${sparm:--}" + 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" @@ -44,11 +51,11 @@ run() { } # identity, a gantry included -run "trivkins coordinates=XYZ" "" "joints=3 jnt=10,20,30" -run "trivkins coordinates=XYZY kinstype=BOTH" "" "joints=4 jnt=10,20,30,20" -run "trivkins coordinates=XYZABCUVW" "" "joints=9" +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" +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 @@ -69,8 +76,8 @@ 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" -run "5axiskins coordinates=XYZBCW sparm=identityfirst" "" "joints=6 jnt=10,20,30,15,25,5" 1 +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 @@ -79,7 +86,7 @@ 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" + "joints=5 jnt=10,20,30,15,25 orient=-" run "xyzbc-trt-kins coordinates=XYZBC" \ "setp xyzbc-trt-kins.conventional-directions 1 @@ -89,7 +96,7 @@ 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" + "joints=5 jnt=10,20,30,15,25 orient=-" run "xyzab_tdr_kins" \ "setp xyzab_tdr_kins.x-offset 3 @@ -112,7 +119,7 @@ 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" 1 + "joints=6 jnt=10,20,30,15,25,35 orient=5,4" 1 run "xyzacb_trsrn" \ "setp xyzacb_trsrn_kins.nut-angle 45 @@ -121,7 +128,7 @@ 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" 2 + "joints=6 jnt=10,20,30,15,25,35 orient=-" 2 run "xyzbca_trsrn" \ "setp xyzbca_trsrn_kins.nut-angle 45 @@ -135,7 +142,7 @@ 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" 1 + "joints=6 jnt=10,20,30,15,25,35 orient=5,3" 1 # polar run "rosekins" "" "joints=3 jnt=10,5,30" @@ -143,7 +150,7 @@ 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" +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" 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"); From 200d3ebae0debbbefcb308322819e4afe357d5b5 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 14 Sep 2026 16:48:22 +1000 Subject: [PATCH 42/77] kinematics_user: read the module's pins by name, making nothing in HAL The loader read the RT instance's pins through pins of its own, linked to the signal each RT pin was on, and where a pin had no signal it made one and linked the RT pin to it. That leaves the module's pins connected to signals the config never made, so a net of one of them after the module was first evaluated fails on a pin that is already connected. Read each pin by name with hal_get_p() instead, at every refresh: nothing is made in HAL, a pin netted later reads the signal from then on, and the loader no longer has to be initialised before the caller's hal_ready(). --- .../kinematics_userspace/kinematics_user.c | 199 +++++------------- .../kinematics_userspace/kinematics_user.h | 7 +- 2 files changed, 59 insertions(+), 147 deletions(-) diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index 25e8b5c374a..559bba46dc2 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -4,9 +4,10 @@ * * 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: one input pin - * of the caller's component per table entry, connected to the signal the - * RT instance's pin reads, so the values are the live ones. The tool is + * 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 @@ -35,8 +36,7 @@ typedef int (*kins_describe_fn)(const char *coordinates, const char *sparm, kins_module_info *info); -#define MAX_BOUND_PINS (KINS_MAX_PARAMS + AXIS_COUNT) -#define MAX_MADE_SIGNALS MAX_BOUND_PINS +#define MAX_PINS (KINS_MAX_PARAMS + AXIS_COUNT) struct KinematicsUserContext { int initialized; @@ -49,14 +49,11 @@ struct KinematicsUserContext { int ktype; /* kinematics type being evaluated */ int num_joints; char module_name[64]; - int comp_id; /* the caller's component, owns the pins made here */ - const char *prefix; /* its name, which those pin names start with */ - char made_signal[MAX_MADE_SIGNALS][HAL_NAME_LEN + 1]; - int num_made_signals; - hal_refs_u *cell; /* HAL storage those pins are made against */ - int num_cells; - int cell_of_param[KINS_MAX_PARAMS]; /* -1 if not bound */ - int cell_of_tool[AXIS_COUNT]; /* motion.tooloffset.*, -1 if absent */ + 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() */ @@ -65,55 +62,28 @@ struct KinematicsUserContext { }; /* ======================================================================== - * Pin binding + * Pin reading * ======================================================================== */ -/* - * Give the block a reference to a value it needs. - * - * The reference is to a pin of ours rather than into the RT instance's, - * so that its lifetime is ours. Ours is connected to the signal the RT - * pin reads, or, when the RT pin has no signal, to one made here and - * removed again in kinematicsUserFree(). - * - * The reference has to live in HAL shared memory, since that is where - * HAL rewrites it on connect and disconnect, so the pins are made - * against hal_malloc() cells and the block reads what a cell holds once - * the connection is in place. - */ -static int make_signal(KinematicsUserContext *ctx, const char *pin_name, - hal_type_t type, char *out, size_t outlen) -{ - if (ctx->num_made_signals >= MAX_MADE_SIGNALS) { - fprintf(stderr, "kinematicsUserInit: too many signals to create\n"); - return -1; - } - if ((size_t)snprintf(out, outlen, "%s-nonrt", pin_name) >= outlen) { - fprintf(stderr, "kinematicsUserInit: signal name for '%s' too long\n", - pin_name); - return -1; - } - if (hal_signal_new(out, type) != 0) return -1; - if (hal_link(pin_name, out) != 0) { - hal_signal_delete(out); - return -1; - } - snprintf(ctx->made_signal[ctx->num_made_signals++], - sizeof(ctx->made_signal[0]), "%s", out); - return 0; -} - -static int new_pin(int comp_id, hal_type_t type, hal_refs_u *out, - const char *name) +/* 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: return hal_pin_new_bool(comp_id, HAL_IN, &out->b, 0, "%s", name); - case HAL_FLOAT: return hal_pin_new_real(comp_id, HAL_IN, &out->r, 0.0, "%s", name); - case HAL_S32: return hal_pin_new_si32(comp_id, HAL_IN, &out->s, 0, "%s", name); - case HAL_U32: return hal_pin_new_ui32(comp_id, HAL_IN, &out->u, 0, "%s", name); - default: break; + 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 -1; + return 0; } /* Does a pin of this name exist? Silent: absence is an answer, not an error. */ @@ -126,61 +96,24 @@ static int pin_exists(const char *pin_name) return hal_getref_p(&q) == 0; } -/* Bind pin_name; returns the cell index, or -1. */ -static int bind_pin(KinematicsUserContext *ctx, const char *pin_name, +/* 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) { - char signal[HAL_NAME_LEN + 1]; - char mine[HAL_NAME_LEN + 1]; - hal_refs_u *cell; - hal_query_t q; - int idx; - - memset(&q, 0, sizeof(q)); - q.name = pin_name; - q.qtype = HAL_QTYPE_PIN; + double value; - if (hal_getref_p(&q) != 0) { - fprintf(stderr, "kinematicsUserInit: no such pin '%s'\n", pin_name); - return -1; - } - if (q.pp.type != type) { - fprintf(stderr, "kinematicsUserInit: pin '%s' has the wrong type\n", + if (pin_value(pin_name, type, &value) != 0) { + fprintf(stderr, "kinematicsUserInit: no pin '%s' of the type expected\n", pin_name); return -1; } - - if (q.pp.signal) { - snprintf(signal, sizeof(signal), "%s", q.pp.signal); - } else if (make_signal(ctx, pin_name, type, signal, sizeof(signal))) { - fprintf(stderr, "kinematicsUserInit: cannot reach '%s'\n", pin_name); + if (ctx->num_pins >= MAX_PINS) { + fprintf(stderr, "kinematicsUserInit: too many pins to read\n"); return -1; } - - if ((size_t)snprintf(mine, sizeof(mine), "%s.%s", ctx->prefix, pin_name) - >= sizeof(mine)) { - fprintf(stderr, "kinematicsUserInit: pin name for '%s' too long\n", - pin_name); - return -1; - } - if (ctx->num_cells >= MAX_BOUND_PINS) { - fprintf(stderr, "kinematicsUserInit: too many pins to bind\n"); - return -1; - } - idx = ctx->num_cells; - cell = &ctx->cell[idx]; - - if (new_pin(ctx->comp_id, type, cell, mine) != 0) { - fprintf(stderr, "kinematicsUserInit: cannot create pin '%s'\n", mine); - return -1; - } - if (hal_link(mine, signal) != 0) { - fprintf(stderr, "kinematicsUserInit: cannot link '%s' to '%s'\n", - mine, signal); - return -1; - } - ctx->num_cells++; - return idx; + 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) @@ -193,25 +126,15 @@ static hal_type_t hal_type_of(kins_param_type t) } } -static double cell_value(const hal_refs_u *cell, kins_param_type t) -{ - switch (t) { - case KINS_PARAM_BIT: return hal_get_bool(cell->b) ? 1.0 : 0.0; - case KINS_PARAM_S32: return hal_get_si32(cell->s); - case KINS_PARAM_U32: return hal_get_ui32(cell->u); - default: return hal_get_real(cell->r); - } -} - -/* Bind every input of the table, and motion's tool where motion is there. */ -static int bind_all(KinematicsUserContext *ctx) +/* 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->cell_of_param[i] = -1; - for (i = 0; i < AXIS_COUNT; i++) ctx->cell_of_tool[i] = -1; + 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++) { @@ -219,8 +142,8 @@ static int bind_all(KinematicsUserContext *ctx) 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->cell_of_param[i] = bind_pin(ctx, name, hal_type_of(d->type)); - if (ctx->cell_of_param[i] < 0) return -1; + 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 @@ -230,8 +153,8 @@ static int bind_all(KinematicsUserContext *ctx) for (i = 0; i < AXIS_COUNT; i++) { snprintf(name, sizeof(name), "motion.tooloffset.%c", letter[i]); if (!pin_exists(name)) continue; - ctx->cell_of_tool[i] = bind_pin(ctx, name, HAL_FLOAT); - if (ctx->cell_of_tool[i] < 0) return -1; + ctx->pin_of_tool[i] = note_pin(ctx, name, HAL_FLOAT); + if (ctx->pin_of_tool[i] < 0) return -1; } return 0; } @@ -244,10 +167,14 @@ static void refresh(KinematicsUserContext *ctx) 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->cell_of_param[i]; + int c = ctx->pin_of_param[i]; + double value; if (c < 0) continue; - ctx->params.geometry[i] = cell_value(&ctx->cell[c], ctx->info.params[i].type); + 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]; @@ -262,10 +189,10 @@ static void refresh(KinematicsUserContext *ctx) } for (i = 0; i < AXIS_COUNT; i++) { - int c = ctx->cell_of_tool[i]; + int c = ctx->pin_of_tool[i]; tool[i] = 0.0; if (c < 0) continue; - tool[i] = hal_get_real(ctx->cell[c].r); + if (pin_value(ctx->pin_name[c], HAL_FLOAT, &tool[i]) != 0) continue; have_motion_tool = 1; } if (!have_motion_tool) return; @@ -398,19 +325,12 @@ KinematicsUserContext* kinematicsUserInitSparm(const char* kins_type, ctx->num_joints = num_joints; ctx->comp_id = comp_id; - ctx->prefix = prefix; - - ctx->cell = (hal_refs_u *)hal_malloc(MAX_BOUND_PINS * sizeof(hal_refs_u)); - if (!ctx->cell) { - fprintf(stderr, "kinematicsUserInit: out of HAL memory\n"); - free(ctx); - return NULL; - } + (void)prefix; strncpy(ctx->module_name, kins_type, sizeof(ctx->module_name) - 1); if (load_module(ctx, kins_type, coordinates, sparm) == 0) { - if (bind_all(ctx) != 0) { - fprintf(stderr, "kinematicsUserInit: cannot bind the pins of '%s'\n", + if (note_all(ctx) != 0) { + fprintf(stderr, "kinematicsUserInit: cannot read the pins of '%s'\n", kins_type); ctx->rt_only = 1; } @@ -734,15 +654,8 @@ KinematicsUserContext* kinematicsUserInitString(const char* kinematics, void kinematicsUserFree(KinematicsUserContext* ctx) { - int i; - if (!ctx) return; - /* Removing one hands its value back to the RT pin, leaving the - machine as it was found. */ - for (i = 0; i < ctx->num_made_signals; i++) { - hal_signal_delete(ctx->made_signal[i]); - } if (ctx->rt_handle) dlclose(ctx->rt_handle); free(ctx); } diff --git a/src/emc/kinematics_userspace/kinematics_user.h b/src/emc/kinematics_userspace/kinematics_user.h index 6d9b07a0859..ff023113b95 100644 --- a/src/emc/kinematics_userspace/kinematics_user.h +++ b/src/emc/kinematics_userspace/kinematics_user.h @@ -47,15 +47,14 @@ typedef struct KinematicsUserContext KinematicsUserContext; /** * Initialize userspace kinematics context * - * The pins this creates belong to the caller's component, so call this - * after hal_init() and before hal_ready(): HAL refuses new pins once a - * component is ready. + * 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, which the created pin names start with + * @param prefix Its name * @return Allocated context, or NULL if kinematics type not supported */ KinematicsUserContext* kinematicsUserInit(const char* kins_type, From 9b368fe09a5741ec11cf69c4fba4557ebdb403a5 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 11 Sep 2026 20:25:25 +0800 Subject: [PATCH 43/77] kinematics: retire the KINS_NOT_SWITCHABLE macro Nothing uses it: every single-type module is on the parameter block and gets these entry points from kins_single.c, and the switchkins modules write their own. The man page now says so instead of pointing module authors at the macro. --- docs/src/motion/kinematics.adoc | 7 +++---- src/emc/kinematics/kinematics.h | 9 --------- 2 files changed, 3 insertions(+), 13 deletions(-) diff --git a/docs/src/motion/kinematics.adoc b/docs/src/motion/kinematics.adoc index 26f3e8a03d0..e1e25b57181 100644 --- a/docs/src/motion/kinematics.adoc +++ b/docs/src/motion/kinematics.adoc @@ -310,7 +310,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 +319,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/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index 16a8cc58184..80ff0ff84c6 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -744,15 +744,6 @@ extern int kinematicsSetTool(const EmcPose *tool); // 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; From f60bad27ba34e56f4b5746d06870731ebd7e2d69 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 15 Sep 2026 22:07:11 +1000 Subject: [PATCH 44/77] kinematics.h: keep the interface, move the module side to kins_module.h kinematics.h carried three things: the entry points motion, task and the interpreter call on a module, the helpers a module is written with, and the prototypes of particular modules that share their maths between two loadable modules. A consumer that wanted the flag values or the switch entry point read all of it. kinematics.h now declares what a module presents to its callers: the kinematics type, the flags, forward, inverse, home, the tool and work frames, the tool frame inverse, the Jacobian, the switch, the type flags and the tool offset. kins_module.h, exported alongside it, takes the module side: the joint map and identity helpers, the tool frame library, the generic Jacobian, the parameter block and the shared code that runs an ops table. What trtfuncs.c and userkfuncs.c share with the modules that load them is in trtfuncs.h and userkfuncs.h next to them, in tree only. SWITCHKINS_MAX_TYPES is defined in kinematics.h itself, where motion and NML read it, and kins_module.h aliases KINS_MAX_TYPES to it. Nothing declared moves or changes; only which header declares it. --- docs/src/motion/switchkins.adoc | 4 +- src/Makefile | 1 + src/emc/kinematics/5axiskins.c | 1 + src/emc/kinematics/corexykins.c | 2 +- src/emc/kinematics/genhexkins.c | 3 +- src/emc/kinematics/genhexkins.h | 2 +- src/emc/kinematics/genserfuncs.c | 4 +- src/emc/kinematics/genserkins.c | 3 +- src/emc/kinematics/genserkins.h | 4 +- src/emc/kinematics/kinematics.h | 452 +----------------- src/emc/kinematics/kins_module.h | 443 +++++++++++++++++ src/emc/kinematics/kins_rt.h | 2 +- src/emc/kinematics/kins_single.c | 2 +- src/emc/kinematics/kins_util.c | 2 +- src/emc/kinematics/lineardeltakins.c | 2 +- src/emc/kinematics/maxkins.c | 2 +- src/emc/kinematics/pentakins.c | 4 +- src/emc/kinematics/pumakins.c | 1 + src/emc/kinematics/rosekins.c | 2 +- src/emc/kinematics/rotarydeltakins.c | 2 +- src/emc/kinematics/rotatekins.c | 2 +- src/emc/kinematics/scarakins.c | 1 + src/emc/kinematics/scorbot-kins.c | 2 +- src/emc/kinematics/switchkins.c | 2 +- src/emc/kinematics/switchkins.h | 11 +- src/emc/kinematics/three21kins.c | 1 + src/emc/kinematics/tripodkins.c | 2 +- src/emc/kinematics/trivkins.c | 2 +- src/emc/kinematics/trtfuncs.c | 5 +- src/emc/kinematics/trtfuncs.h | 20 + src/emc/kinematics/userkfuncs.c | 5 +- src/emc/kinematics/userkfuncs.h | 30 ++ src/emc/kinematics/xyzac-trt-kins.c | 4 +- src/emc/kinematics/xyzbc-trt-kins.c | 4 +- .../kinematics_userspace/kinematics_user.c | 2 +- .../kinematics_userspace/kinematics_user.h | 4 +- src/hal/components/matrixkins.comp | 2 +- src/hal/components/millturn.comp | 2 +- src/hal/components/switchkinscomp.comp | 4 +- src/hal/components/userkins.comp | 8 +- src/hal/components/xyzab_tdr_kins.comp | 2 +- src/hal/components/xyzacb_trsrn.comp | 2 +- src/hal/components/xyzbca_trsrn.comp | 2 +- tests/kins-params/paritycheck.c | 2 +- tests/tool-frame/test_tool_frame.c | 2 +- 45 files changed, 560 insertions(+), 501 deletions(-) create mode 100644 src/emc/kinematics/kins_module.h create mode 100644 src/emc/kinematics/trtfuncs.h create mode 100644 src/emc/kinematics/userkfuncs.h diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index ea5a1d98934..11f120fd02b 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -554,7 +554,7 @@ int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv); ---- 'ktype' runs from 0 to SWITCHKINS_MAX_TYPES-1 (defined in -kinematics.h as KINS_MAX_TYPES). Registering a kinstype twice, by +kinematics.h). 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. @@ -686,7 +686,7 @@ static int my_inverse(const kins_params *p, kins_scratch *s, static const kins_ops my_ops = { .forward = my_forward, .inverse = my_inverse, - // .work, .tool, .native and .jacobian are optional, see kinematics.h + // .work, .tool, .native and .jacobian are optional, see kins_module.h }; int switchkinsSetup(kparms* kp, diff --git a/src/Makefile b/src/Makefile index 597cd4e3210..ae4fbe8c88c 100644 --- a/src/Makefile +++ b/src/Makefile @@ -403,6 +403,7 @@ SRCHEADERS := \ hal/drivers/mesa-hostmot2/hostmot2-serial.h \ emc/linuxcnc.h \ emc/kinematics/kinematics.h \ + emc/kinematics/kins_module.h \ emc/kinematics/switchkins.h \ emc/kinematics/kins_rt.h \ emc/kinematics_userspace/kinematics_user.h \ diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index 81a765253b4..d1df89d9f36 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -59,6 +59,7 @@ #include #include +#include "userkfuncs.h" // the geometry, one pin each; the maths reads it from the block static const kins_param_desc fiveaxis_params[] = { diff --git a/src/emc/kinematics/corexykins.c b/src/emc/kinematics/corexykins.c index ebc7a685d2a..b0de07233ef 100644 --- a/src/emc/kinematics/corexykins.c +++ b/src/emc/kinematics/corexykins.c @@ -8,7 +8,7 @@ #include #include #include -#include +#include #include static int corexy_forward(const kins_params *p, kins_scratch *s, diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index 4deb523d9a3..34b124ddef4 100644 --- a/src/emc/kinematics/genhexkins.c +++ b/src/emc/kinematics/genhexkins.c @@ -104,7 +104,7 @@ 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 + kins_module.h): the pins above are the table below, read into the block before every call and written from the scratch after it. ----------------------------------------------------------------------------*/ @@ -117,6 +117,7 @@ #include "genhexkins.h" #include +#include "userkfuncs.h" // the table: thirteen entries per strut, then the iteration controls, // the offsets and the reports. The macros index it. diff --git a/src/emc/kinematics/genhexkins.h b/src/emc/kinematics/genhexkins.h index 671419efcc9..60b48f7ec54 100644 --- a/src/emc/kinematics/genhexkins.h +++ b/src/emc/kinematics/genhexkins.h @@ -20,7 +20,7 @@ #ifndef GENHEXKINS_H #define GENHEXKINS_H -#include +#include #include #define GENHEX_MAX_JOINTS 6 diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index 02859829cf3..c43156490c9 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -29,7 +29,7 @@ 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 + kins_module.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. @@ -46,7 +46,7 @@ #include #include "libposemath/gotypes.h" /* go_result, go_integer */ #include "libposemath/gomath.h" /* go_pose */ -#include +#include #include "genserkins.h" /* these decls */ diff --git a/src/emc/kinematics/genserkins.c b/src/emc/kinematics/genserkins.c index 09c72a9fcbe..f2783b31951 100644 --- a/src/emc/kinematics/genserkins.c +++ b/src/emc/kinematics/genserkins.c @@ -5,7 +5,7 @@ * NOTEs: * 1) specify all kparms items * 2) the maths and the geometry table are in genserfuncs.c, written as -* pure functions of the parameter block (see kinematics.h) +* pure functions of the parameter block (see kins_module.h) */ /******************************************************************** @@ -44,6 +44,7 @@ frame-larger-than: #include "genserkins.h" #include +#include "userkfuncs.h" //-7 is system defined -3 ok, -4 ok, -5 ok,-6 ok (mm system) #undef GO_REAL_EPSILON diff --git a/src/emc/kinematics/genserkins.h b/src/emc/kinematics/genserkins.h index c5a2d9526f8..c50cddde3e3 100644 --- a/src/emc/kinematics/genserkins.h +++ b/src/emc/kinematics/genserkins.h @@ -36,7 +36,7 @@ #include /* HAL data types */ #include "libposemath/gotypes.h" /* go_result, go_integer */ #include "libposemath/gomath.h" /* go_pose */ -#include +#include /*! The maximum number of joints supported by the general serial @@ -131,7 +131,7 @@ extern int compute_jfwd(go_link * link_params, extern int compute_jinv(go_matrix * Jfwd, go_matrix * Jinv); -/* The kinematics as functions of the parameter block (see kinematics.h): +/* The kinematics as functions of the parameter block (see kins_module.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. */ diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index 80ff0ff84c6..5642ab92c3d 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -108,11 +108,10 @@ 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. +** 0..SWITCHKINS_MAX_TYPES-1, and motion runs one of them at a time. The +** count is here because motion and the NML status channel need it. */ -#define SWITCHKINS_MAX_TYPES KINS_MAX_TYPES +#define SWITCHKINS_MAX_TYPES 9 /* What a kinematics type IS, declared by the module with ** switchkinsDeclare() and read back with kinematicsTypeFlags(). @@ -194,96 +193,6 @@ extern int kinematicsWorkFrame(const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags); -/* parameters for use with switchkins.c */ -typedef struct kinematics_parms { - char* sparm; // module string parameter passed to kins - char* kinsname; // must agree with module(file) name - char* halprefix; // for hal pin hames - char* required_coordinates; - int max_joints; - int allow_duplicates; - int fwd_iterates_mask; // identify kins types that use iterative - // forward kinematics (typ: genhex) - // bitmask: 0x0 none - // bitmask: 0x1 bit0: switchkins_type==0 - // bitmask: 0x2 bit1: switchkins_type==1 - // 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 -** sequentially. Axis indices are 0:x,1:y,...,etc -** Example: coordinates=XYZYAC -** Result: axis_idx_for_jno[0] = 0 ==> X -** axis_idx_for_jno[1] = 1 ==> Y -** axis_idx_for_jno[2] = 2 ==> Z -** axis_idx_for_jno[3] = 1 ==> Y (duplicate allowed) -** axis_idx_for_jno[4] = 1 ==> A -** axis_idx_for_jno[5] = 1 ==> C -*/ -extern int map_coordinates_to_jnumbers(const char *coordinates, - const int max_joints, - const int allow_duplicates, - int axis_idx_for_jno[]); - -extern int mapped_joints_to_position(const int max_joints, - const double* joints, - EmcPose* pose); - -extern int position_to_mapped_joints(const int max_joints, - const EmcPose* pos, - double* joints); - -extern int identityKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* ksetup_parms); - -extern int identityKinematicsForward(const double *joint, - struct EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags); - -extern int identityKinematicsInverse(const struct EmcPose * world, - double *joint, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags); - -/* joints are axes, so neither frame ever turns */ -extern int identityKinematicsToolFrame(const double *joint, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -extern int identityKinematicsWorkFrame(const double *joint, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -/* Rotations relating a module's own frame to the tool frame convention. - TOOL_FRAME_SPINDLE is the identity, for maths already in the convention. - TOOL_FRAME_FLANGE is the half turn about tool x that turns an ISO 9787 - flange frame, whose z points out of the mechanical interface towards the - work, into the convention. */ -extern const PmRotationMatrix TOOL_FRAME_SPINDLE; -extern const PmRotationMatrix TOOL_FRAME_FLANGE; - -/* Post-multiply a module's native frame by the rotation it declared, in - place. Modules built on switchkins.c never call this, the dispatch does it - for them; a standalone module calls it before returning. - Returns 0, or -1 if native is not a proper rotation. */ -extern int toolFrameApplyNative(PmRotationMatrix *rot, - const PmRotationMatrix *native); - -/* out = transpose(work) * tool, the tool frame in workpiece coordinates. - out may alias neither input. */ -extern int toolFrameInWork(const PmRotationMatrix *work, - const PmRotationMatrix *tool, - PmRotationMatrix *out); - -/* True if m is orthonormal with determinant +1, so a frame a machine can - actually hold. Used to check a declared rotation once, at load. */ -extern int toolFrameIsProper(const PmRotationMatrix *m); /* The inverse of kinematicsToolFrame(): which joint values point the tool along a requested direction. This is the question a tilted work plane asks @@ -358,52 +267,6 @@ extern int kinematicsToolFrameInverse(const PmCartesian *axis_in_work, int *free_directions, double *tool_spin); -/* The generic implementation of the above, driven by a module's own frame - functions, so that a module gets it for free once it supplies them. A - module with a closed form registers that instead: it is faster, and it - knows its own degenerate poses without having to find them. - - num_joints is the length of seed and of each row of solutions. */ -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, - 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); - /* How each joint responds to a unit rate of each pose coordinate: jac[j][a] = d joint[j] / d pose[a] @@ -449,286 +312,6 @@ extern int kinematicsJacobian(const double *joint, 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]); - -/* The Jacobian of a serial arm of six revolute joints from its - Denavit-Hartenberg chain. Two conventions carry that name and put the - four parameters on different links; this is the modified one of John J. - Craig, Introduction to Robotics: Mechanics and Control, where link i is - Rx(alpha[i]) Tx(a[i]) Rz(joint[i]) Tz(d[i]), the joint turning about the - z of the frame Rx and Tx leave it in. (The original 1955 convention is - Rz(theta) Tz(d) Tx(a) Rx(alpha), and a table written for it does not fit - here.) The tool point `tool` lies along the z of the last frame, and - the pose of that point is reported as X Y Z and the RPY of the last - frame, R = Rz(C) Ry(B) Rx(A), as pmMatRpyConvert() does. - Each joint's axis crossed with the vector from it to the tool point - gives the point's rate per radian of the joint, the axis itself the - angular rate; that 6x6 inverted is the joint rate per unit of twist, and - the RPY rates reach the twist through the matrix of the axes each one - turns about, which is where world's B and C come in. alpha and joint in - degrees, a, d and tool in the module's length unit. Rows 0 to 5 of jac - are filled, the rest zero. Returns 0, or -1 at a singular pose, where no - finite joint rate follows the pose. */ -extern int kinsJacobianFromDhArm(const double alpha[6], const double a[6], - const double d[6], const double *joint, - double tool, const EmcPose *world, - 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. machine says - this is the machine frame type, the pivot in machine coordinates with the - rotaries as joints, which G13.1 and G49 select and G53.5 moves in; a module - that leaves it unset on every type has its identity type stand in. */ -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 */ - int machine; /* the machine frame */ -} 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); @@ -740,34 +323,7 @@ extern int kinematicsSwitch(int switchkins_type); 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 - -// 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); - -extern int userkKinematicsForward(const double *joint, - struct EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags); - -extern int userkKinematicsInverse(const struct EmcPose * world, - double *joint, - const KINEMATICS_INVERSE_FLAGS * iflags, - 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 diff --git a/src/emc/kinematics/kins_module.h b/src/emc/kinematics/kins_module.h new file mode 100644 index 00000000000..414b91196ee --- /dev/null +++ b/src/emc/kinematics/kins_module.h @@ -0,0 +1,443 @@ +/******************************************************************** +* Description: kins_module.h +* The module side of a kinematics module: what a module is written with, +* as opposed to kinematics.h, which is what motion and the interpreter +* call. The joint map and identity helpers, the tool frame library, the +* generic Jacobian, and the parameter block form of a module with the +* shared code that runs an ops table. Nothing here needs HAL; the pins +* made from a module's table are in kins_rt.h. +* +* License: GPL Version 2 +********************************************************************/ +#ifndef __LINUXCNC_KINS_MODULE_H +#define __LINUXCNC_KINS_MODULE_H + +#include "kinematics.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* parameters for use with switchkins.c */ +typedef struct kinematics_parms { + char* sparm; // module string parameter passed to kins + char* kinsname; // must agree with module(file) name + char* halprefix; // for hal pin hames + char* required_coordinates; + int max_joints; + int allow_duplicates; + int fwd_iterates_mask; // identify kins types that use iterative + // forward kinematics (typ: genhex) + // bitmask: 0x0 none + // bitmask: 0x1 bit0: switchkins_type==0 + // bitmask: 0x2 bit1: switchkins_type==1 + // 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 +** sequentially. Axis indices are 0:x,1:y,...,etc +** Example: coordinates=XYZYAC +** Result: axis_idx_for_jno[0] = 0 ==> X +** axis_idx_for_jno[1] = 1 ==> Y +** axis_idx_for_jno[2] = 2 ==> Z +** axis_idx_for_jno[3] = 1 ==> Y (duplicate allowed) +** axis_idx_for_jno[4] = 1 ==> A +** axis_idx_for_jno[5] = 1 ==> C +*/ +extern int map_coordinates_to_jnumbers(const char *coordinates, + const int max_joints, + const int allow_duplicates, + int axis_idx_for_jno[]); + +extern int mapped_joints_to_position(const int max_joints, + const double* joints, + EmcPose* pose); + +extern int position_to_mapped_joints(const int max_joints, + const EmcPose* pos, + double* joints); + +extern int identityKinematicsSetup(const int comp_id, + const char* coordinates, + kparms* ksetup_parms); + +extern int identityKinematicsForward(const double *joint, + struct EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags); + +extern int identityKinematicsInverse(const struct EmcPose * world, + double *joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags); + +/* joints are axes, so neither frame ever turns */ +extern int identityKinematicsToolFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + +extern int identityKinematicsWorkFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + +/* Rotations relating a module's own frame to the tool frame convention. + TOOL_FRAME_SPINDLE is the identity, for maths already in the convention. + TOOL_FRAME_FLANGE is the half turn about tool x that turns an ISO 9787 + flange frame, whose z points out of the mechanical interface towards the + work, into the convention. */ +extern const PmRotationMatrix TOOL_FRAME_SPINDLE; +extern const PmRotationMatrix TOOL_FRAME_FLANGE; + +/* Post-multiply a module's native frame by the rotation it declared, in + place. Modules built on switchkins.c never call this, the dispatch does it + for them; a standalone module calls it before returning. + Returns 0, or -1 if native is not a proper rotation. */ +extern int toolFrameApplyNative(PmRotationMatrix *rot, + const PmRotationMatrix *native); + +/* out = transpose(work) * tool, the tool frame in workpiece coordinates. + out may alias neither input. */ +extern int toolFrameInWork(const PmRotationMatrix *work, + const PmRotationMatrix *tool, + PmRotationMatrix *out); + +/* True if m is orthonormal with determinant +1, so a frame a machine can + actually hold. Used to check a declared rotation once, at load. */ +extern int toolFrameIsProper(const PmRotationMatrix *m); + + +/* The generic implementation of the above, driven by a module's own frame + functions, so that a module gets it for free once it supplies them. A + module with a closed form registers that instead: it is faster, and it + knows its own degenerate poses without having to find them. + + num_joints is the length of seed and of each row of solutions. */ +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, + 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); + +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]); + +/* The Jacobian of a serial arm of six revolute joints from its + Denavit-Hartenberg chain. Two conventions carry that name and put the + four parameters on different links; this is the modified one of John J. + Craig, Introduction to Robotics: Mechanics and Control, where link i is + Rx(alpha[i]) Tx(a[i]) Rz(joint[i]) Tz(d[i]), the joint turning about the + z of the frame Rx and Tx leave it in. (The original 1955 convention is + Rz(theta) Tz(d) Tx(a) Rx(alpha), and a table written for it does not fit + here.) The tool point `tool` lies along the z of the last frame, and + the pose of that point is reported as X Y Z and the RPY of the last + frame, R = Rz(C) Ry(B) Rx(A), as pmMatRpyConvert() does. + Each joint's axis crossed with the vector from it to the tool point + gives the point's rate per radian of the joint, the axis itself the + angular rate; that 6x6 inverted is the joint rate per unit of twist, and + the RPY rates reach the twist through the matrix of the axes each one + turns about, which is where world's B and C come in. alpha and joint in + degrees, a, d and tool in the module's length unit. Rows 0 to 5 of jac + are filled, the rest zero. Returns 0, or -1 at a singular pose, where no + finite joint rate follows the pose. */ +extern int kinsJacobianFromDhArm(const double alpha[6], const double a[6], + const double d[6], const double *joint, + double tool, const EmcPose *world, + 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 SWITCHKINS_MAX_TYPES + +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. machine says + this is the machine frame type, the pivot in machine coordinates with the + rotaries as joints, which G13.1 and G49 select and G53.5 moves in; a module + that leaves it unset on every type has its identity type stand in. */ +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 */ + int machine; /* the machine frame */ +} 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); + +#ifdef __cplusplus +} +#endif + +#endif // __LINUXCNC_KINS_MODULE_H diff --git a/src/emc/kinematics/kins_rt.h b/src/emc/kinematics/kins_rt.h index 78d0a61ef7d..fd96510b22f 100644 --- a/src/emc/kinematics/kins_rt.h +++ b/src/emc/kinematics/kins_rt.h @@ -14,7 +14,7 @@ #define __LINUXCNC_KINS_RT_H #include -#include "kinematics.h" +#include "kins_module.h" /* one HAL pin handle per table entry, of whichever type the entry has */ typedef union { diff --git a/src/emc/kinematics/kins_single.c b/src/emc/kinematics/kins_single.c index 2ca2f057e2a..2a0b0c8dc0a 100644 --- a/src/emc/kinematics/kins_single.c +++ b/src/emc/kinematics/kins_single.c @@ -14,7 +14,7 @@ #include #include -#include +#include #include static kins_params rt_params; diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index 486efce9829..763cafbfc97 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -49,7 +49,7 @@ #include #include #include -#include +#include #include // principal joint numbers based on module 'coordinates' parameter diff --git a/src/emc/kinematics/lineardeltakins.c b/src/emc/kinematics/lineardeltakins.c index 5b780635578..c4fef2ad660 100644 --- a/src/emc/kinematics/lineardeltakins.c +++ b/src/emc/kinematics/lineardeltakins.c @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include "lineardeltakins-common.h" diff --git a/src/emc/kinematics/maxkins.c b/src/emc/kinematics/maxkins.c index 24fe3a76b90..882ad15be47 100644 --- a/src/emc/kinematics/maxkins.c +++ b/src/emc/kinematics/maxkins.c @@ -21,7 +21,7 @@ #include #include #include -#include /* these decls */ +#include /* these decls */ #include #define d2r(d) ((d)*PM_PI/180.0) diff --git a/src/emc/kinematics/pentakins.c b/src/emc/kinematics/pentakins.c index 4ca870b392c..182d6eec289 100644 --- a/src/emc/kinematics/pentakins.c +++ b/src/emc/kinematics/pentakins.c @@ -46,7 +46,7 @@ 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 + kins_module.h): the pins above are the table below, read into the block before every call, and the entry points come from kins_single.c. ----------------------------------------------------------------------------*/ @@ -56,7 +56,7 @@ #include #include #include -#include /* these decls, KINEMATICS_FORWARD_FLAGS */ +#include /* these decls, KINEMATICS_FORWARD_FLAGS */ #include #include "pentakins.h" diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index 1fc778a4bbe..cbed57f8285 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -23,6 +23,7 @@ #include "pumakins.h" #include +#include "userkfuncs.h" // the five dimensions, one pin each; the maths reads them from the block static const kins_param_desc puma_params[] = { diff --git a/src/emc/kinematics/rosekins.c b/src/emc/kinematics/rosekins.c index 4d088ffe88b..618ce512d3e 100644 --- a/src/emc/kinematics/rosekins.c +++ b/src/emc/kinematics/rosekins.c @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include MODULE_LICENSE("GPL"); diff --git a/src/emc/kinematics/rotarydeltakins.c b/src/emc/kinematics/rotarydeltakins.c index 03c654cac1d..6d5f545cf3d 100644 --- a/src/emc/kinematics/rotarydeltakins.c +++ b/src/emc/kinematics/rotarydeltakins.c @@ -19,7 +19,7 @@ #include #include #include -#include +#include #include #include "rotarydeltakins-common.h" diff --git a/src/emc/kinematics/rotatekins.c b/src/emc/kinematics/rotatekins.c index 708bc412a7b..f5b65bc8643 100644 --- a/src/emc/kinematics/rotatekins.c +++ b/src/emc/kinematics/rotatekins.c @@ -17,7 +17,7 @@ #include #include #include -#include /* these decls */ +#include /* these decls */ #include static int rotate_forward(const kins_params *p, kins_scratch *s, diff --git a/src/emc/kinematics/scarakins.c b/src/emc/kinematics/scarakins.c index 19820930c91..a416724a5c2 100644 --- a/src/emc/kinematics/scarakins.c +++ b/src/emc/kinematics/scarakins.c @@ -21,6 +21,7 @@ #include #include +#include "userkfuncs.h" /* key dimensions diff --git a/src/emc/kinematics/scorbot-kins.c b/src/emc/kinematics/scorbot-kins.c index b840fb38969..2834160d38c 100644 --- a/src/emc/kinematics/scorbot-kins.c +++ b/src/emc/kinematics/scorbot-kins.c @@ -43,7 +43,7 @@ #include #include #include -#include +#include #include diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index 969fc819884..dfbc1360629 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -48,7 +48,7 @@ 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, +// types written as pure functions (see kins_module.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}; diff --git a/src/emc/kinematics/switchkins.h b/src/emc/kinematics/switchkins.h index a84c8f82fe5..024bed5d7f1 100644 --- a/src/emc/kinematics/switchkins.h +++ b/src/emc/kinematics/switchkins.h @@ -4,13 +4,10 @@ #ifndef __LINUXCNC_SWITCHKINS_H #define __LINUXCNC_SWITCHKINS_H -#include "kinematics.h" +#include "kins_module.h" -//SWITCHKINS_MAX_TYPES (max number of types a module may provide) -//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 +// SWITCHKINS_MAX_TYPES, the most types a module may provide, is in +// kinematics.h: motion and the NML status channel need it too // KinematicsFORWARD functions typedef int (*KF)(const double *joint, @@ -88,7 +85,7 @@ typedef int (*KJ)(const double *joint, // 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), +// provide one switchkins-type written as pure functions (see kins_module.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. diff --git a/src/emc/kinematics/three21kins.c b/src/emc/kinematics/three21kins.c index 903e3e08a07..438d36ecf0c 100644 --- a/src/emc/kinematics/three21kins.c +++ b/src/emc/kinematics/three21kins.c @@ -4,6 +4,7 @@ #include #include +#include "userkfuncs.h" /* default values for ar2 robot */ #define DEFAULT_THREE21_A1 64.2 diff --git a/src/emc/kinematics/tripodkins.c b/src/emc/kinematics/tripodkins.c index 47f4baf2b4a..e2e2847bee1 100644 --- a/src/emc/kinematics/tripodkins.c +++ b/src/emc/kinematics/tripodkins.c @@ -67,7 +67,7 @@ #include #include #include -#include /* these decls */ +#include /* these decls */ #include // the base geometry, one pin each, poked from HAL as before diff --git a/src/emc/kinematics/trivkins.c b/src/emc/kinematics/trivkins.c index 2de2368614a..f587ecbd989 100644 --- a/src/emc/kinematics/trivkins.c +++ b/src/emc/kinematics/trivkins.c @@ -15,7 +15,7 @@ #include #include #include -#include +#include #include // joints are axes, through whatever map coordinates= gives; the maths is diff --git a/src/emc/kinematics/trtfuncs.c b/src/emc/kinematics/trtfuncs.c index 86a24e88410..4aa6c99a925 100644 --- a/src/emc/kinematics/trtfuncs.c +++ b/src/emc/kinematics/trtfuncs.c @@ -29,7 +29,7 @@ * conventional axis directions. See * https://linuxcnc.org/docs/html/gcode/machining-center.html * -* Written as pure functions of the parameter block (see kinematics.h): +* Written as pure functions of the parameter block (see kins_module.h): * the geometry is the table below, the joint map comes from the block, * and the tool length is p->tool.tran.z. ********************************************************************/ @@ -37,7 +37,8 @@ #include #include #include -#include +#include +#include "trtfuncs.h" // the geometry both machines share, one pin each const kins_param_desc TRT_PARAMS[] = { diff --git a/src/emc/kinematics/trtfuncs.h b/src/emc/kinematics/trtfuncs.h new file mode 100644 index 00000000000..3a08d2987fa --- /dev/null +++ b/src/emc/kinematics/trtfuncs.h @@ -0,0 +1,20 @@ +/******************************************************************** +* Description: trtfuncs.h +* The two table-rotary tilting geometries, xyzac and xyzbc, written +* once in trtfuncs.c for the two modules that load them. +* +* License: GPL Version 2 +********************************************************************/ +#ifndef __LINUXCNC_TRTFUNCS_H +#define __LINUXCNC_TRTFUNCS_H + +#include "kins_module.h" + + +// 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; + +#endif diff --git a/src/emc/kinematics/userkfuncs.c b/src/emc/kinematics/userkfuncs.c index 0e51ed651e5..9dc5ee17ec2 100644 --- a/src/emc/kinematics/userkfuncs.c +++ b/src/emc/kinematics/userkfuncs.c @@ -24,7 +24,8 @@ // typical includes: //#include // if reqd #include -#include +#include +#include "userkfuncs.h" // Add for kins based on genserkins: // #include "genserkins.h" //includes gomath,hal @@ -53,7 +54,7 @@ static int userk_inverse(const kins_params *p, kins_scratch *s, const kins_ops USERK_OPS = { .forward = userk_forward, .inverse = userk_inverse, - // .work, .tool, .native and .jacobian are optional, see kinematics.h + // .work, .tool, .native and .jacobian are optional, see kins_module.h }; //********************************************************************** diff --git a/src/emc/kinematics/userkfuncs.h b/src/emc/kinematics/userkfuncs.h new file mode 100644 index 00000000000..5b997dbf60e --- /dev/null +++ b/src/emc/kinematics/userkfuncs.h @@ -0,0 +1,30 @@ +/******************************************************************** +* Description: userkfuncs.h +* The user-defined kinematics template, userkfuncs.c, shared by the +* switchkins modules as their third type. +* +* License: GPL Version 2 +********************************************************************/ +#ifndef __LINUXCNC_USERKFUNCS_H +#define __LINUXCNC_USERKFUNCS_H + +#include "kins_module.h" + + +extern const kins_ops USERK_OPS; + +extern int userkKinematicsSetup(const int comp_id, + const char* coordinates, + kparms* ksetup_parms); + +extern int userkKinematicsForward(const double *joint, + struct EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags); + +extern int userkKinematicsInverse(const struct EmcPose * world, + double *joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags); + +#endif diff --git a/src/emc/kinematics/xyzac-trt-kins.c b/src/emc/kinematics/xyzac-trt-kins.c index 3fc14fc9de4..2d99eb5f6aa 100644 --- a/src/emc/kinematics/xyzac-trt-kins.c +++ b/src/emc/kinematics/xyzac-trt-kins.c @@ -9,7 +9,7 @@ * 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) +* pure functions of the parameter block (see kins_module.h) */ #include @@ -17,6 +17,8 @@ #include #include +#include "userkfuncs.h" +#include "trtfuncs.h" int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, diff --git a/src/emc/kinematics/xyzbc-trt-kins.c b/src/emc/kinematics/xyzbc-trt-kins.c index 45c41b448dd..acb5fcc3b05 100644 --- a/src/emc/kinematics/xyzbc-trt-kins.c +++ b/src/emc/kinematics/xyzbc-trt-kins.c @@ -9,7 +9,7 @@ * 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) +* pure functions of the parameter block (see kins_module.h) */ #include @@ -17,6 +17,8 @@ #include #include +#include "userkfuncs.h" +#include "trtfuncs.h" int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index 559bba46dc2..976087839c1 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -4,7 +4,7 @@ * * 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 + * block (see kins_module.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 diff --git a/src/emc/kinematics_userspace/kinematics_user.h b/src/emc/kinematics_userspace/kinematics_user.h index ff023113b95..231d0b20929 100644 --- a/src/emc/kinematics_userspace/kinematics_user.h +++ b/src/emc/kinematics_userspace/kinematics_user.h @@ -7,7 +7,7 @@ * 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 + * its parameter block form (see kins_module.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 @@ -23,7 +23,7 @@ #define KINEMATICS_USER_H #include /* EmcPose */ -#include /* KINEMATICS_TYPE, flags */ +#include /* KINEMATICS_TYPE, flags */ #include /* hal_type_t, HAL_NAME_LEN */ #ifdef __cplusplus diff --git a/src/hal/components/matrixkins.comp b/src/hal/components/matrixkins.comp index f3b1834bbb1..4d37b2515da 100644 --- a/src/hal/components/matrixkins.comp +++ b/src/hal/components/matrixkins.comp @@ -180,7 +180,7 @@ option extra_setup; license "GPL"; ;; -#include +#include #include // the calibration matrix, one pin each; the maths reads it from the block diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index bde6136efff..55dd1b6569d 100644 --- a/src/hal/components/millturn.comp +++ b/src/hal/components/millturn.comp @@ -49,7 +49,7 @@ FUNCTION(fdemo) { } // the turn kinematics: no geometry, written as pure functions of the -// parameter block (see kinematics.h) +// parameter block (see kins_module.h) static int turn_forward(const kins_params *p, kins_scratch *s, const double *j, EmcPose * pos, diff --git a/src/hal/components/switchkinscomp.comp b/src/hal/components/switchkinscomp.comp index e5ca4034b9c..6947bcf4932 100644 --- a/src/hal/components/switchkinscomp.comp +++ b/src/hal/components/switchkinscomp.comp @@ -18,7 +18,7 @@ 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 +kins_module.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. @@ -88,7 +88,7 @@ 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. +// Jacobian are optional, see kins_module.h. static int myForward(const kins_params *p, kins_scratch *s, const double *j, diff --git a/src/hal/components/userkins.comp b/src/hal/components/userkins.comp index f382b0f0544..4e18ae089b8 100644 --- a/src/hal/components/userkins.comp +++ b/src/hal/components/userkins.comp @@ -50,7 +50,7 @@ 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* + kins_module.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, @@ -69,7 +69,7 @@ license "GPL"; author "Dewey Garrett"; ;; -#include +#include #include // the shared code for a module with one kinematics type, compiled in so @@ -159,7 +159,7 @@ static int userkins_jacobian(const kins_params *p, const double *j, (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. + // follows y and joint 2 follows z, each one for one. See kins_module.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; } @@ -174,7 +174,7 @@ 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 + // .work, .tool and .native report the frames, see kins_module.h }; const kins_module_info kins_module = { diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index a2d41593b4b..a4c73854a39 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -45,7 +45,7 @@ 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 +// kins_module.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 }, diff --git a/src/hal/components/xyzacb_trsrn.comp b/src/hal/components/xyzacb_trsrn.comp index 5c0f09565a6..c9a279ce916 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -27,7 +27,7 @@ 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 +// kins_module.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[] = { diff --git a/src/hal/components/xyzbca_trsrn.comp b/src/hal/components/xyzbca_trsrn.comp index 9694b5b6f12..9372bc22c76 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -27,7 +27,7 @@ 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 +// kins_module.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[] = { diff --git a/tests/kins-params/paritycheck.c b/tests/kins-params/paritycheck.c index 9043df69dac..1a3ef4664ab 100644 --- a/tests/kins-params/paritycheck.c +++ b/tests/kins-params/paritycheck.c @@ -27,7 +27,7 @@ #include #include #include -#include +#include MODULE_LICENSE("GPL"); diff --git a/tests/tool-frame/test_tool_frame.c b/tests/tool-frame/test_tool_frame.c index 9e38a5f6933..7ad78489d83 100644 --- a/tests/tool-frame/test_tool_frame.c +++ b/tests/tool-frame/test_tool_frame.c @@ -10,7 +10,7 @@ #include #include "emcpos.h" -#include "kinematics.h" +#include "kins_module.h" #define DEG (M_PI/180.0) #define NUTATION 45.0 From 5a6870533cc582ff9b34807d91c5d0657fcf8e3d Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 18:46:45 +1000 Subject: [PATCH 45/77] tests: put the nutating head kinematics next to the tilted work plane maths The two trsrn configs carry their orientation maths in python, written apart from the kinematics modules, and nothing had compared the two. tests/kins-twp loads each module in realtime with a small component answering frame and inverse requests over HAL pins and drives it from python holding the twp functions. Over a grid of primary, secondary and table angles the module's tool frame equals the python transformation matrix to 1e-9; for a set of requested tool axes with the table held as the remap holds it, the joint pairs the module finds are the python's candidates and the spin it reports is the python's virtual rotation. With nothing held the module may turn the table, which the python never does, so there each side is judged by the other's maths. --- tests/kins-twp/README | 9 ++ tests/kins-twp/check.py | 273 ++++++++++++++++++++++++++++++++++ tests/kins-twp/checkresult | 3 + tests/kins-twp/skip | 4 + tests/kins-twp/test.sh | 30 ++++ tests/kins-twp/twp-xyzacb.ini | 16 ++ tests/kins-twp/twp-xyzbca.ini | 16 ++ tests/kins-twp/twpcheck.c | 165 ++++++++++++++++++++ 8 files changed, 516 insertions(+) create mode 100644 tests/kins-twp/README create mode 100755 tests/kins-twp/check.py create mode 100755 tests/kins-twp/checkresult create mode 100755 tests/kins-twp/skip create mode 100755 tests/kins-twp/test.sh create mode 100644 tests/kins-twp/twp-xyzacb.ini create mode 100644 tests/kins-twp/twp-xyzbca.ini create mode 100644 tests/kins-twp/twpcheck.c diff --git a/tests/kins-twp/README b/tests/kins-twp/README new file mode 100644 index 00000000000..b9b3b7275fa --- /dev/null +++ b/tests/kins-twp/README @@ -0,0 +1,9 @@ +The C kinematics against the tilted work plane maths. + +The two nutating-head configs carry their orientation maths in python, +remap_funcs_twp.py, written independently of the kinematics modules. +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..86b77f6ff44 --- /dev/null +++ b/tests/kins-twp/test.sh @@ -0,0 +1,30 @@ +#!/bin/bash +set -e + +# RIP layout: $HEADERS is $TOPDIR/include +TOPDIR=$(dirname "$HEADERS") +CONFIGS=$TOPDIR/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating + +${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-trsrn_twp %s/twp-%s.ini\n' \ + "$machine" "$CONFIGS" "$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); } From 48205c677d9ce4cac280e16eed5ec9201edd65dc Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 5 Sep 2026 00:22:08 +1000 Subject: [PATCH 46/77] motion: add the point-to-point move, interpolated in joint space A segment whose endpoint is Cartesian but whose interpolation is in joint space: the inverse runs once, at the endpoint, when the move is queued, and the planner runs every joint from where the queue ends to there together, the slowest setting the pace. Its length is the joint space distance with the tightest per-joint velocity, acceleration and jerk limits scaled onto it. Nothing blends into or out of it. While it runs the servo thread takes the joints from the planner and reports the tool from the forward kinematics; a forward that fails, as an iterating one can at the singularity the move is there to cross, leaves the last solved position and carte_pos_cmd_ok cleared. This is how a program crosses a singularity or flips a head without leaving its coordinate system. The move is a rapid with the rapid override or a feed that takes a given time with the feed override; the endpoint can also be given as joints, whose world position motion finds with the forward. Canon JOINT_TRAVERSE and JOINT_FEED, NML EMC_TRAJ_JOINT_MOVE and EMCMOT_SET_JOINT_LINE carry it; the preview draws the straight line between its ends. An external offset cannot ride on a joint segment: the queue refuses the segment while one is applied, and a request arriving while one is queued waits until the last joint segment is done. A queue reset forgets the joint-space bookkeeping with the queue. --- src/emc/motion/command.c | 158 ++++++++++++++++++++++++++++ src/emc/motion/control.c | 34 +++++- src/emc/motion/motion.h | 8 ++ src/emc/nml_intf/canon.hh | 18 ++++ src/emc/nml_intf/emc.cc | 15 +++ src/emc/nml_intf/emc.hh | 2 + src/emc/nml_intf/emc_nml.hh | 21 ++++ src/emc/rs274ngc/gcodemodule.cc | 22 ++++ src/emc/sai/saicanon.cc | 43 ++++++++ src/emc/task/emccanon.cc | 41 ++++++++ src/emc/task/emctaskmain.cc | 12 +++ src/emc/task/taskintf.cc | 17 +++ src/emc/tp/tc.c | 74 ++++++++++++- src/emc/tp/tc.h | 1 + src/emc/tp/tc_types.h | 20 +++- src/emc/tp/tp.c | 181 +++++++++++++++++++++++++++++++- src/emc/tp/tp.h | 9 ++ src/emc/tp/tp_types.h | 11 ++ 18 files changed, 682 insertions(+), 5 deletions(-) diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 6ee7f155971..5d79bc6371f 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -1130,6 +1130,164 @@ 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 (!tpGetQueueEndJoints(&emcmotInternal->coord_tp, start)) { + EmcPose goal; + tpGetGoalPos(&emcmotInternal->coord_tp, &goal); + for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { + start[joint_num] = (joint_num < ALL_JOINTS) ? joints[joint_num].pos_cmd : 0.0; + } + if (kinematicsInverse(&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 (kinematicsInverse(&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 */ diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 43ad5854168..715af6ee2f7 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -1427,10 +1427,41 @@ 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 */ + 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; @@ -1439,6 +1470,7 @@ static void get_pos_cmds(long period) /* OUTPUT KINEMATICS - convert to joints in local array */ 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/motion.h b/src/emc/motion/motion.h index b107b232811..f16b175cb49 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,13 @@ 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 */ } emcmot_command_t; /*! \todo FIXME - these packed bits might be replaced with chars diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index 0e6f6a0c25f..4f5da55a2e8 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -298,6 +298,24 @@ extern void STRAIGHT_TRAVERSE(int lineno, double x, double y, double z, double a, double b, double c, double u, double v, double w); + +/* A traverse interpolated in joint space. The endpoint x..w is in program + coordinates like STRAIGHT_TRAVERSE's; motion runs the inverse once there + and interpolates the joints to it. With have_joints the joints given + (machine units, one per joint) are the endpoint instead and x..w say + where the interpreter believes that is. Nothing blends into or out of + it. */ +extern void JOINT_TRAVERSE(int lineno, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w); +/* The same move at feed: it is to take 'seconds' seconds at the programmed + feed, the feed override applies, and the joint limits still cap it. */ +extern void JOINT_FEED(int lineno, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w, + double seconds); /* Move at traverse rate so that at any time during the move, all axes diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index 99e62837172..94942565fcd 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -311,6 +311,9 @@ int emcFormat(NMLTYPE type, void *buffer, CMS * cms) case EMC_TRAJ_SET_G68_TYPE: ((EMC_TRAJ_SET_G68 *) buffer)->update(cms); break; + case EMC_TRAJ_JOINT_MOVE_TYPE: + ((EMC_TRAJ_JOINT_MOVE *) buffer)->update(cms); + break; case EMC_TRAJ_SET_SCALE_TYPE: ((EMC_TRAJ_SET_SCALE *) buffer)->update(cms); break; @@ -536,6 +539,8 @@ const char *emc_symbol_lookup(uint32_t type) return "EMC_TRAJ_SET_ROTATION"; case EMC_TRAJ_SET_G68_TYPE: return "EMC_TRAJ_SET_G68"; + case EMC_TRAJ_JOINT_MOVE_TYPE: + return "EMC_TRAJ_JOINT_MOVE"; case EMC_TRAJ_SET_SCALE_TYPE: return "EMC_TRAJ_SET_SCALE"; case EMC_TRAJ_SET_RAPID_SCALE_TYPE: @@ -1699,6 +1704,16 @@ void EMC_TRAJ_SET_G68::update(CMS * cms) cms->update(active); } +// cppcheck-suppress duplInheritedMember +void EMC_TRAJ_JOINT_MOVE::update(CMS * cms) +{ + EMC_TRAJ_CMD_MSG::update(cms); + EmcPose_update(cms, &end); + cms->update(joints, EMCMOT_MAX_JOINTS); + cms->update(have_joints); + cms->update(seconds); +} + /* * NML/CMS Update function for EMC_SPINDLE_BRAKE_ENGAGE * Automatically generated by NML CodeGen Java Applet. diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index 0628cfd3e78..a9b26278601 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -112,6 +112,7 @@ struct PM_CARTESIAN; #define EMC_TRAJ_SET_FH_ENABLE_TYPE ((NMLTYPE) 236) #define EMC_TRAJ_RIGID_TAP_TYPE ((NMLTYPE) 237) #define EMC_TRAJ_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) @@ -376,6 +377,7 @@ 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); diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index 06e96999de6..a1dd0bc5443 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -960,6 +960,27 @@ class EMC_TRAJ_PROBE:public EMC_TRAJ_CMD_MSG { unsigned char probe_type; }; +// a move interpolated in joint space: to the world endpoint, whose joints +// motion finds with the inverse; or to the joints given, whose world +// position motion finds with the forward +class EMC_TRAJ_JOINT_MOVE:public EMC_TRAJ_CMD_MSG { + public: + EMC_TRAJ_JOINT_MOVE() + : EMC_TRAJ_CMD_MSG(EMC_TRAJ_JOINT_MOVE_TYPE, sizeof(EMC_TRAJ_JOINT_MOVE)), + end{}, joints{}, have_joints(0), seconds(0.0) + {}; + + // For internal NML/CMS use only. + // Sub-class update() calls base-class update() + // cppcheck-suppress duplInheritedMember + void update(CMS * cms); + + EmcPose end; + double joints[EMCMOT_MAX_JOINTS]; + int have_joints; + double seconds; /* 0 for a rapid, else the time it is to take */ +}; + class EMC_TRAJ_RIGID_TAP:public EMC_TRAJ_CMD_MSG { public: EMC_TRAJ_RIGID_TAP() diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index caa4103db1f..655498872f8 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -517,6 +517,28 @@ void STRAIGHT_FEED(int line_number, ensure_inch({x, y, z, a, b, c, u, v, w})); } +// the preview draws a joint interpolated move as the traverse between its +// ends: the path between them depends on the kinematics, which the +// preview does not have +void JOINT_TRAVERSE(int line_number, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w) { + (void)joints; + (void)have_joints; + STRAIGHT_TRAVERSE(line_number, x, y, z, a, b, c, u, v, w); +} + +void JOINT_FEED(int line_number, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double u, double v, double w, double seconds) { + (void)joints; + (void)have_joints; + (void)seconds; + STRAIGHT_FEED(line_number, x, y, z, a, b, c, u, v, w); +} + void STRAIGHT_TRAVERSE(int line_number, double x, double y, double z, double a, double b, double c, diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index 61fa7637a9b..73be89400a6 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -222,6 +222,49 @@ void SET_TRAVERSE_RATE(double rate) _sai._traverse_rate = rate; } +void JOINT_TRAVERSE(int /*line_number*/, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double /*u*/, double /*v*/, double /*w*/) +{ + if (have_joints && joints) { + ECHO_WITH_ARGS("[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f], " + "%.4f, %.4f, %.4f, %.4f, %.4f, %.4f", + joints[0], joints[1], joints[2], joints[3], joints[4], + joints[5], joints[6], joints[7], joints[8], x, y, z, a, b, c); + } else { + ECHO_WITH_ARGS("%.4f, %.4f, %.4f, %.4f, %.4f, %.4f", x, y, z, a, b, c); + } + _sai._program_position_x = x; + _sai._program_position_y = y; + _sai._program_position_z = z; + _sai._program_position_a = a; + _sai._program_position_b = b; + _sai._program_position_c = c; +} + +void JOINT_FEED(int /*line_number*/, const double *joints, int have_joints, + double x, double y, double z, + double a, double b, double c, + double /*u*/, double /*v*/, double /*w*/, + double seconds) +{ + if (have_joints && joints) { + ECHO_WITH_ARGS("[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f], " + "%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f", + joints[0], joints[1], joints[2], joints[3], joints[4], + joints[5], joints[6], joints[7], joints[8], x, y, z, a, b, c, seconds); + } else { + ECHO_WITH_ARGS("%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f", x, y, z, a, b, c, seconds); + } + _sai._program_position_x = x; + _sai._program_position_y = y; + _sai._program_position_z = z; + _sai._program_position_a = a; + _sai._program_position_b = b; + _sai._program_position_c = c; +} + void STRAIGHT_TRAVERSE( int /*line_number*/, double x, double y, double z , double a /*AA*/ diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 9f2d6d691d1..4b01d744cfb 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -1157,6 +1157,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) diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index d26c65de714..4374cb5c03e 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -508,6 +508,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; @@ -1555,6 +1558,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: @@ -1918,6 +1922,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); @@ -2581,6 +2592,7 @@ static EMC_TASK_EXEC emcTaskCheckPostconditions(NMLmsg * cmd) return EMC_TASK_EXEC::WAITING_FOR_SYSTEM_CMD; break; + case EMC_TRAJ_JOINT_MOVE_TYPE: case EMC_TRAJ_LINEAR_MOVE_TYPE: case EMC_TRAJ_CIRCULAR_MOVE_TYPE: case EMC_TRAJ_SET_VELOCITY_TYPE: diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 9b6fab24c62..ece2c8ce33c 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -1537,6 +1537,23 @@ double emcTrajGetAngularUnits() return TrajConfig.AngularUnits; } +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) { emcmotCommand.command = EMCMOT_SET_OFFSET; diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index f6267ab056b..c5ccbf87e0c 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -200,6 +200,7 @@ int tcGetStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const ou tcCircleStartAccelUnitVector(tc,out); break; case TC_SPHERICAL: + case TC_JOINT: return -1; default: return -1; @@ -207,6 +208,24 @@ int tcGetStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const ou return 0; } +/** + * The world direction of a joint interpolated segment, end minus start, + * for the status fields that want a direction. The path between them is + * not straight, so this is the chord, and there is none when only the + * rotaries move. + */ +static int tcJointChordUnitVector(TC_STRUCT const * const tc, PmCartesian * const out) +{ + PmCartesian d; + double mag; + + pmCartCartSub(&tc->coords.joint.world_end.tran, &tc->coords.joint.world_start.tran, &d); + pmCartMag(&d, &mag); + if (mag < TP_POS_EPSILON) { return -1; } + pmCartScalMult(&d, 1.0 / mag, out); + return 0; +} + /** * Get the acceleration direction unit vector for blend velocity calculations. * This calculates the direction of acceleration at the end of a segment. @@ -325,6 +344,8 @@ int tcGetStartTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const case TC_CIRCULAR: pmCircleTangentVector(&tc->coords.circle.xyz, 0.0, out); break; + case TC_JOINT: + return tcJointChordUnitVector(tc, out); default: rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d!\n",tc->motion_type); return -1; @@ -348,6 +369,8 @@ int tcGetEndTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const ou pmCircleTangentVector(&tc->coords.circle.xyz, tc->coords.circle.xyz.angle, out); break; + case TC_JOINT: + return tcJointChordUnitVector(tc, out); default: rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d!\n",tc->motion_type); return -1; @@ -403,6 +426,8 @@ int tcGetCurrentTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * cons arcTangent(arc, out, at_end); } break; + case TC_JOINT: + return tcJointChordUnitVector(tc, out); default: rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d in tcGetCurrentTangentUnitVector!\n", tc->motion_type); return -1; @@ -526,6 +551,30 @@ int tcGetPosReal(TC_STRUCT const * const tc, int of_point, EmcPose * const pos) abc = tc->coords.arc.abc; uvw = tc->coords.arc.uvw; break; + case TC_JOINT: { + // the ends are exact; between them this is the chord, a proxy + // for the status fields, and the servo thread reports the + // real position from the forward kinematics + const PmJointLine *jl = &tc->coords.joint; + double f = (tc->target > 0.0) ? progress / tc->target : 0.0; + EmcPose d; + + emcPoseSub(&jl->world_end, &jl->world_start, &d); + pos->tran.x = jl->world_start.tran.x + f * d.tran.x; + pos->tran.y = jl->world_start.tran.y + f * d.tran.y; + pos->tran.z = jl->world_start.tran.z + f * d.tran.z; + pos->a = jl->world_start.a + f * d.a; + pos->b = jl->world_start.b + f * d.b; + pos->c = jl->world_start.c + f * d.c; + pos->u = jl->world_start.u + f * d.u; + pos->v = jl->world_start.v + f * d.v; + pos->w = jl->world_start.w + f * d.w; + if (of_point == TC_GET_ENDPOINT) { *pos = jl->world_end; } + return TP_ERR_OK; + } + default: + rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d in tcGetPosReal!\n", tc->motion_type); + return TP_ERR_FAIL; } if (res_fit == TP_ERR_OK) { @@ -536,6 +585,26 @@ int tcGetPosReal(TC_STRUCT const * const tc, int of_point, EmcPose * const pos) } +/** + * The joints of a joint interpolated segment at its progress. + * Returns the joint count, or 0 for any other segment. + */ +int tcGetJointPos(TC_STRUCT const * const tc, double * const joints) +{ + const PmJointLine *jl; + double f; + int i; + + if (!tc || tc->motion_type != TC_JOINT) { return 0; } + jl = &tc->coords.joint; + f = (tc->target > 0.0) ? tc->progress / tc->target : 1.0; + if (f > 1.0) { f = 1.0; } + for (i = 0; i < jl->num_joints; i++) { + joints[i] = jl->start[i] + f * (jl->end[i] - jl->start[i]); + } + return jl->num_joints; +} + /** * Set the terminal condition of a segment. * This function will eventually handle state changes associated with altering a terminal condition. @@ -621,7 +690,7 @@ int tcIsBlending(TC_STRUCT * const tc) { //FIXME Disabling blends for rigid tap cycle until changes can be verified. int is_blending_next = (tc->term_cond == TC_TERM_COND_PARABOLIC ) && tc->on_final_decel && (tc->currentvel < tc->blend_vel) && - tc->motion_type != TC_RIGIDTAP; + tc->motion_type != TC_RIGIDTAP && tc->motion_type != TC_JOINT; //Latch up the blending_next status here, so that even if the prev conditions //aren't necessarily true we still blend to completion once the blend @@ -1068,6 +1137,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 ef9c96501b5..b197b2c23d6 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; } @@ -4406,6 +4513,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) { @@ -4498,6 +4671,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; From 63ed4baf8f6db52bf7b2b36b7a53425d82ab0ccd Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 5 Sep 2026 00:33:42 +1000 Subject: [PATCH 47/77] interpreter: the point-to-point moves and the tool orientation codes, G53.1 to G53.6, and G68.3 The interpreter evaluates the kinematics module ahead of motion through the loader in kinematics_userspace, opened on first use on a HAL component of its own, so G53.n can ask the tool frame inverse where the rotaries go, G68.3 can read the tool direction, and the point-to-point codes can tell where their joints put the tool. G53.4, G53.5 and G53.7 are point-to-point moves, non-modal modifiers of G0 and G1 like G53: the path is interpolated in joint space, only the endpoint is defined. G53.4 takes a program point through the offsets. G53.5 reads the axis letters as a point in the machine frame with the orientation left out, XYZ the carriage in machine coordinates and the rotary letters the rotary joints, no offset applied: the world of the type the module flags KINSTYPE_MACHINE, the identity type where the slides line up with the frame, so there a letter names the joints the module's identity mapping gives it, and on a module with a slanted or composite slide the point goes through that module's own machine frame maths, a letter not given holding its machine coordinate. Where that type is a plain identity it is refused on a machine whose letters name joints of the other unit class, as a serial robot's X; a module that declares no types is read as letters = joints. G53.7 takes joint values as J= words, a form the lexer reads when the J value is followed by an equals sign, the joint's own position in its own units, which works on every machine. With G1 the move takes the time the straight move would at the programmed feed. The inverse ahead of motion is run to a fixed point, because a module may read the joints it is handed. G53.1 moves the rotaries to the plane's normal with the linear joints where they are; G53.6 does the same holding the tool centre point, a Cartesian move; G53.3 X Y Z goes to a point in the plane with the tool oriented. P picks among the poses: nearest by default, P1 and P2 by the sign of the secondary rotary, the one whose axis the other carries, found by turning each rotary through the module's tool frame, Heidenhain's SEQ+ and SEQ-. Q says whether the joints that carry the work take part: Q0 holds them, COORD ROT, falling back to everything free; Q1 frees them, TABLE ROT. G68.3 takes the plane from the tool, Z the tool axis as the joints have it, X the default tool X of the conventions chapter. tests/twp-native runs the xyzacb nutating head through all of it against the python maths in tests/kins-twp; tests/ptp-robot puts pumakins through the point-to-point codes; tests/ptp-machine-frame builds a module with a slanted Y slide out of tree and checks G53.5 holds machine X across a Y move, which the slides would not. Reversing the joint interpolation fails three checks in twp-native. --- docs/src/gcode/g-code.adoc | 242 +++++++- docs/src/gcode/overview.adoc | 6 +- docs/src/motion/kinematics-conventions.adoc | 40 +- docs/src/motion/kinematics.adoc | 22 + docs/src/motion/switchkins.adoc | 5 +- src/emc/motion/command.c | 24 +- src/emc/rs274ngc/Submakefile | 2 +- src/emc/rs274ngc/interp_array.cc | 4 +- src/emc/rs274ngc/interp_check.cc | 59 +- src/emc/rs274ngc/interp_convert.cc | 36 +- src/emc/rs274ngc/interp_internal.cc | 6 +- src/emc/rs274ngc/interp_internal.hh | 22 + src/emc/rs274ngc/interp_read.cc | 18 +- src/emc/rs274ngc/interp_setup.cc | 6 + src/emc/rs274ngc/interp_workplane.cc | 584 ++++++++++++++++++ src/emc/rs274ngc/rs274ngc_interp.hh | 17 + src/emc/rs274ngc/rs274ngc_pre.cc | 17 + tests/kins-twp/README | 6 +- tests/kins-twp/test.sh | 8 +- .../kins-twp/xyzacb}/remap_funcs_twp.py | 0 .../kins-twp/xyzbca}/remap_funcs_twp.py | 0 tests/ptp-machine-frame/README | 16 + tests/ptp-machine-frame/checkresult | 3 + tests/ptp-machine-frame/sim.hal | 15 + tests/ptp-machine-frame/skip | 4 + tests/ptp-machine-frame/slantkins.comp | 164 +++++ tests/ptp-machine-frame/test-ui.py | 150 +++++ tests/ptp-machine-frame/test.ini | 114 ++++ tests/ptp-machine-frame/test.sh | 5 + tests/ptp-machine-frame/tool.tbl | 1 + tests/ptp-robot/README | 6 + tests/ptp-robot/checkresult | 3 + tests/ptp-robot/sim.hal | 16 + tests/ptp-robot/skip | 4 + tests/ptp-robot/test-ui.py | 99 +++ tests/ptp-robot/test.ini | 134 ++++ tests/ptp-robot/test.sh | 4 + tests/ptp-robot/tool.tbl | 1 + tests/twp-native/README | 13 + tests/twp-native/abort.ngc | 11 + tests/twp-native/sim.hal | 16 + tests/twp-native/skip | 4 + tests/twp-native/test-ui.py | 489 +++++++++++++++ tests/twp-native/test.ini | 147 +++++ tests/twp-native/test.sh | 4 + tests/twp-native/tool.tbl | 1 + 46 files changed, 2509 insertions(+), 39 deletions(-) rename {configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp => tests/kins-twp/xyzacb}/remap_funcs_twp.py (100%) rename {configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp => tests/kins-twp/xyzbca}/remap_funcs_twp.py (100%) create mode 100644 tests/ptp-machine-frame/README create mode 100755 tests/ptp-machine-frame/checkresult create mode 100644 tests/ptp-machine-frame/sim.hal create mode 100755 tests/ptp-machine-frame/skip create mode 100644 tests/ptp-machine-frame/slantkins.comp create mode 100755 tests/ptp-machine-frame/test-ui.py create mode 100644 tests/ptp-machine-frame/test.ini create mode 100755 tests/ptp-machine-frame/test.sh create mode 100644 tests/ptp-machine-frame/tool.tbl create mode 100644 tests/ptp-robot/README create mode 100755 tests/ptp-robot/checkresult create mode 100644 tests/ptp-robot/sim.hal create mode 100755 tests/ptp-robot/skip create mode 100755 tests/ptp-robot/test-ui.py create mode 100644 tests/ptp-robot/test.ini create mode 100755 tests/ptp-robot/test.sh create mode 100644 tests/ptp-robot/tool.tbl create mode 100644 tests/twp-native/README create mode 100644 tests/twp-native/abort.ngc create mode 100644 tests/twp-native/sim.hal create mode 100755 tests/twp-native/skip create mode 100755 tests/twp-native/test-ui.py create mode 100644 tests/twp-native/test.ini create mode 100755 tests/twp-native/test.sh create mode 100644 tests/twp-native/tool.tbl diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index e5e642a40c5..332de91c619 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -89,11 +89,13 @@ as the 'L number', and so on for any other letter. |<> |Cancel Tool Length Offset |<> |Local Coordinate System Offset |<> |Move in Machine Coordinates +|<> |Orient the Tool to the Tilted Work Plane +|<> |Point-to-Point Move |<> |Select Coordinate System (1 - 9) |<> |Exact Path Mode |<> |Exact Stop Mode |<> |Path Control Mode with Optional Tolerance -|<> |Tilted Work Plane +|<> |Tilted Work Plane |<> |Lathe finishing cycle |<> |Lathe roughing cycle |<> |Drilling Cycle with Chip Breaking @@ -1817,6 +1819,225 @@ 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.3, G53.6 Orient the Tool to the Work Plane(((G53.1 Orient the Tool))) + +[source,ngc] +---- +G53.1 +G53.3 X- Y- Z- +G53.6 +---- + +Each of these moves the rotary joints so that the tool axis is normal to the +active <>, the plane's Z. They differ in what +happens to the tool tip on the way: + +* '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. + +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: on a five axis machine the two +poses lean the head or the table 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-`. + +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. A machine that is not of this shape, a robot among +them, has no such sign to name, and there only the nearest form is +available. + +'Q' says whether the joints that carry the work, the table, take part. + +* 'Q0', the default, holds them and lets the head do the work. On a machine + whose head has only two rotaries the plane's X is then not something the + joints can place, and it is left to the coordinate system, which the plane + already carries; nothing else is done with it. This is what Heidenhain + calls `COORD ROT`. If nothing is reachable with the table held, the move + is tried again with every joint free. +* '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' or 'G53.6', or words other than 'X', 'Y' + and 'Z' with 'G53.3'. +* Cutter compensation is on. + +[[gcode:g53.4]] +== G53.4, G53.5, G53.7 Point-to-Point Move(((G53.4 Point-to-Point Move))) + +[source,ngc] +---- +G53.4 G0 +G53.4 G1 F- +G53.5 G0 +G53.5 G1 F- +G53.7 G0 J= ... +G53.7 G1 F- J= ... +---- + +A point-to-point move defines its two ends and leaves the path between them +to the joints. The inverse kinematics runs once, at the destination, and every +joint then travels from where it is to where it must be, all together, the +slowest setting the pace. The tool does not follow a straight line; on a +machine with rotary joints it swings. The joints are the motion controller's, +<>, the ones the kinematics module maps the axes +to; what HAL connects behind each of them is not part of the interpolation. + +All three are non-modal, like 'G53': they apply to the one block they are +written in, which must have 'G0' or 'G1' in force or on the line. Each strips +one more layer of interpretation from the destination than the one before: + +* 'G53.4' takes a destination in program coordinates, through the offsets and + the tilted work plane like any other move. This is the move a program uses + to cross a kinematic singularity, or to turn a rotary head right round, + without leaving its coordinate system and without the trajectory planner + trying to hold the tool tip on a line the joints cannot follow at speed. +* 'G53.5' takes the same axis letters as a position in the machine frame + with the orientation left out: 'X', 'Y' and 'Z' are the carriage, the + point the rotary joints do not move, in machine coordinates, and the + rotary letters are the rotary joints themselves. On a machine whose slides + line up with its frame, which every shipped module is, that is the slides: + each letter names the joints the module's identity mapping gives it, which + the `coordinates=` parameter and the `[TRAJ]COORDINATES` line describe, + and the two joints of a gantry take one value together. A module whose + carriage does not line up, a slanted slide or a composite Y on a mill-turn, + declares a machine frame type of its own (see the + <> chapter) and 'G53.5' + goes through it, so that 'Y10' is machine Y whichever slides make it, and + a letter not given holds its machine coordinate rather than a joint. + 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 where the +machine frame is the joints '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, the machine's letters name joints of the other kind, or the + machine frame kinematics cannot reach the point. +* With 'G53.7', an axis word or a plain 'J' word is used, no joint word is + given, a joint number is not a whole number from 0 to 15 or is beyond the + joints of the kinematics, or one joint of a gantry pair is given without the + other or with a different value. +* With 'G53.5' or 'G53.7', polar coordinates are used, incremental distance + mode is in force, or the kinematics module cannot be evaluated by the + interpreter. +* A joint word, 'J=', is used without 'G53.7'. +* With 'G1', the feed is zero, or under G94 the two ends coincide, or feed per + revolution (G95) is in force. +* Cutter compensation is on. +* An external offset is applied when the move reaches motion. + [[gcode:g54-g59.3]] == G54-G59.3 Select Coordinate System(((G54-G59.3 Select Coordinate System))) @@ -1992,7 +2213,7 @@ G64 P0.015 Q2 image::images/G64_Heart_Q2.png["G64 Heart",align="center"] [[gcode:g68.2]] -== G68.2, G68.4, G69 Tilted Work Plane(((G68.2 Tilted Work Plane))) +== G68.2, G68.3, G68.4, G69 Tilted Work Plane(((G68.2 Tilted Work Plane))) [source,ngc] ---- @@ -2004,6 +2225,7 @@ G68.2 P2 Q2 X- Y- Z- (a second point on the plane's +X,) G68.2 P2 Q3 X- Y- Z- (a third point on its +Y side) 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) ---- @@ -2052,6 +2274,14 @@ 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. @@ -2080,7 +2310,8 @@ before a program runs, however small the program. an abort: the plane is not persistent and nothing about it is written to the parameter file. -Defining the plane does not move anything. +Defining the plane does not move anything. To bring the tool normal to it +use <>. While a plane is active the codes that define the coordinate system the plane sits on are refused: 'G92', 'G92.1', 'G92.2', 'G92.3', 'G52', 'G10 L2', @@ -2097,6 +2328,7 @@ plane coordinates or draw the plane. ---- G54 G68.2 X50 Y50 Z0 I30 J20 K0 (Euler: 30 about Z, 20 about the new X) +G53.1 (tool normal to it) G0 X0 Y0 Z5 (5 above the plane's origin, along its normal) G1 Z-3 F150 (a hole 3 deep, straight into the plane) G0 Z5 @@ -2114,6 +2346,10 @@ It is an error if: * 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. +* 'G68.3' is given 'I', 'J', 'K', 'P' or 'Q', or 'G69' any of those or 'R': + a word the code does not use is an error, not ignored. * Cutter compensation is on. * Polar coordinates or a motion code are used on the same line. diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index bf1feabbd0e..d801b25cca4 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 @@ -964,7 +964,7 @@ The modal groups are shown in the following Table. [width="80%",cols="4,6",options="header"] |=== |Modal Group Meaning | Member Words -|Non-modal codes (Group 0) | G4, G10 G28, G28.2, G30, G52, G53, G92, G92.1, G92.2, G92.3, +|Non-modal codes (Group 0) | G4, G10 G28, G28.2, G30, G52, G53, G53.1, G53.3, G53.4, G53.5, G53.6, G53.7, G92, G92.1, G92.2, G92.3, |Motion (Group 1) | G0, G1, G2, G3, G33, G38.n, G73, G76, G80, G81 G82, G83, G84, G85, G86, G87, G88, G89 |Plane selection (Group 2) | G17, G18, G19, G17.1, G18.1, G19.1 @@ -974,7 +974,7 @@ The modal groups are shown in the following Table. |Units (Group 6) | G20, G21 |Cutter Diameter Compensation (Group 7) | G40, G41, G42, G41.1, G42.1 |Tool Length Offset (Group 8) | G43, G43.1, G43.2, G43.4, G49 -|Tilted Work Plane (Group 9) | G68.2, G68.4, G69 +|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/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 02446cf425a..64596b2c62d 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -63,6 +63,28 @@ against, and the two halves of this chapter use different pairs deliberately. Positions are measured in the work frame, which is what makes a program independent of how the table is set. +With every rotary joint at zero, the work frame position is what the linear +joints read. The shipped head and table modules fold the pivot lengths and +the offsets in, so that the tool tip and the slides agree at the zero pose +and the pivot shows only as the compensation, `L(1 - cos B)` on a head, +once a rotary leaves zero. A module with a switchable identity type keeps +this of necessity, since the identity and the geometric type must agree at +the zero pose or a switch would jump the reported position; a module without +one keeps it too, so that `G53.5` machine frame positions and `G53` positions +coincide with the rotaries at zero. + +The machine frame is the frame with the orientation left out: XYZ is the +pivot, the point the rotary joints do not move, in machine coordinates, and +the rotary letters are the rotary joints as they are. On a machine whose +slides line up with its frame that is the identity type, and it is what +`G13.1` and `G49` select and `G53.5` moves in. A machine whose carriage does +not line up, a slanted slide, a composite Y made by two slides, an offset +pivot, declares a machine frame type of its own next to its identity type, +and its identity type then keeps the plain meaning, joints are axes, which a +consumer uses to skip the maths. The zero-pose rule holds for the machine +frame type as it does for the identity: the working transform and the machine +frame type agree with the rotaries at zero. + Orientations are measured against the machine frame, and there are two of them. A module reports the tool frame and the work frame separately, each in machine coordinates. A consumer that wants the tool in workpiece coordinates composes @@ -329,6 +351,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.3` and `G53.6`, which ask this inverse where the rotaries go, +with the plane's normal and its X as the request. Their `Q` word is the +`held` mask: `Q0` holds the joints the work frame survey finds and takes the +reported turn as the coordinate rotation it is, `Q1` holds nothing. See the +G-code chapter. + Some requests still do not pin the machine down. A five-axis machine asked to point the tool along the axis its primary rotary turns about can hold any primary angle; a machine with three orientation joints asked only for a tool @@ -527,8 +556,10 @@ the optional work and tool frames with the native rotation that relates the tool frame to the convention, and the optional Jacobian. A type whose forward iterates from the pose it is handed says so, and the shared code seeds it with the last answer after a switch. A type also says what it IS: `identity` marks -the no-transform type `G13.1` cancels to, `primary` the working transform -`G43.4` switches to (see the Switchable Kinematics chapter). A module with +the no-transform type, joints are axes; `primary` the working transform +`G43.4` switches to; `machine` the machine frame type `G13.1` and `G49` +cancel to and `G53.5` moves in, which a module leaves unset when its +identity type is that frame (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`. @@ -581,6 +612,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 diff --git a/docs/src/motion/kinematics.adoc b/docs/src/motion/kinematics.adoc index e1e25b57181..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 diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 11f120fd02b..01dd48b32f8 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -580,8 +580,9 @@ int switchkinsDeclare(int ktype, int flags); ---- G-code reads these declarations: 'G13.1' and 'G49' cancel to the -kinstype declared KINSTYPE_MACHINE, and 'G43.4' switches to the kinstype -declared KINSTYPE_PRIMARY, whatever their numbers, so a module whose +kinstype declared KINSTYPE_MACHINE, 'G53.5' moves in its world, and +'G43.4' switches to the kinstype declared KINSTYPE_PRIMARY, whatever +their numbers, so a module whose kinematics are not in the conventional order still gets working spellings. The machine frame type is the machine with the orientation left out, XYZ the pivot in machine coordinates and the rotary letters diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 5d79bc6371f..4bd9193e1b0 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -120,6 +120,26 @@ 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; +} + /* 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) @@ -1176,7 +1196,7 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { start[joint_num] = (joint_num < ALL_JOINTS) ? joints[joint_num].pos_cmd : 0.0; } - if (kinematicsInverse(&goal, start, &iflags, &fflags) != 0) { + 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; @@ -1207,7 +1227,7 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) SET_MOTION_ERROR_FLAG(1); break; } - if (kinematicsInverse(&end, target, &iflags, &fflags) != 0) { + 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; diff --git a/src/emc/rs274ngc/Submakefile b/src/emc/rs274ngc/Submakefile index e1b246520a5..40ecea5cc8f 100644 --- a/src/emc/rs274ngc/Submakefile +++ b/src/emc/rs274ngc/Submakefile @@ -42,7 +42,7 @@ TARGETS += ../lib/librs274.so ../lib/librs274.so.0 ../lib/librs274.so.0: $(patsubst %.cc,objects/%.o,$(LIBRS274SRCS)) \ ../lib/liblinuxcncini.so ../lib/libpyplugin.so ../lib/liblinuxcnchal.so.0 \ - ../lib/libtooldata.so.0 + ../lib/libtooldata.so.0 ../lib/libkinslimits.so.0 ../lib/libposemath.so.0 $(ECHO) Linking $(notdir $@) @mkdir -p ../lib @rm -f $@ diff --git a/src/emc/rs274ngc/interp_array.cc b/src/emc/rs274ngc/interp_array.cc index 2a4fb2c469e..fcfb7942d33 100644 --- a/src/emc/rs274ngc/interp_array.cc +++ b/src/emc/rs274ngc/interp_array.cc @@ -96,7 +96,7 @@ const int Interp::gees[] = { /* 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,-1, 0, 0, 0, 0, 0,-1,-1, /* 540 */ 12,-1,-1,-1,-1,-1,-1,-1,-1,-1,12,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 560 */ 12,-1,-1,-1,-1,-1,-1,-1,-1,-1,12,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 580 */ 12,-1,-1,-1,-1,-1,-1,-1,-1,-1,12,12,12,12,-1,-1,-1,-1,-1,-1, @@ -104,7 +104,7 @@ const int Interp::gees[] = { /* 620 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 640 */ 13,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 660 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, -/* 680 */ -1,-1, 9,-1, 9,-1,-1,-1,-1,-1, 9,-1,-1,-1,-1,-1,-1,-1,-1,-1, +/* 680 */ -1,-1, 9, 9, 9,-1,-1,-1,-1,-1, 9,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 700 */ 1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 1, 1, 1,-1,-1,-1,-1,-1,-1,-1, /* 720 */ 1, 1, 1,-1,-1,-1,-1,-1,-1,-1, 1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 740 */ 1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, diff --git a/src/emc/rs274ngc/interp_check.cc b/src/emc/rs274ngc/interp_check.cc index a0a2fe3f069..a440aa124da 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -110,6 +110,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_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 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); @@ -290,28 +314,39 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block _("H word with no G43, G43.4 or G76 to use it")); } + // G68.2 and G68.4 take I J K, P and Q; G68.3 only R, X Y Z; G69 nothing + int plane_words = block->g_modes[GM_WORK_PLANE] == G_68_2 || block->g_modes[GM_WORK_PLANE] == G_68_4; + int plane_r = plane_words || block->g_modes[GM_WORK_PLANE] == G_68_3; + if (block->i_flag) { /* could still be useless if yz_plane arc */ CHKS(((motion != G_2) && (motion != G_3) && (motion != G_5) && (motion != G_5_1) && (motion != G_6) && (motion != G_6_1) && (motion != G_71) && (motion != G_71_1) && (motion != G_71_2) && (motion != G_72) && (motion != G_72_1) && (motion != G_72_2) && (motion != G_76) && (motion != G_87) && (motion != G_33_1) && (block->g_modes[GM_MODAL_0] != G_10) && - (block->g_modes[GM_WORK_PLANE] == -1)), - _("I word with no G2, G3, G5, G5.1, G6, G6.1, G10, G33.1, G68.2, G76, or G87 to use it")); + !plane_words), + _("I word with no G2, G3, G5, G5.1, G6, G6.1, G10, G33.1, G68.2, G68.4, G76, or G87 to use it")); } if (block->j_flag) { /* could still be useless if xz_plane arc */ CHKS(((motion != G_2) && (motion != G_3) && (motion != G_5) && (motion != G_5_1) && (motion != G_6) && (motion != G_6_1) && (motion != G_76) && (motion != G_87) && (block->g_modes[GM_MODAL_0] != G_10) && - (block->g_modes[GM_WORK_PLANE] == -1)), - _("J word with no G2, G3, G5, G5.1, G6, G6.1, G10, G68.2, G76 or G87 to use it")); + !plane_words), + _("J word with no G2, G3, G5, G5.1, G6, G6.1, G10, G68.2, G68.4, G76 or G87 to use it")); + } + + for (int n = 0; n < EMCMOT_MAX_JOINTS; n++) { + if (block->joint_flag[n]) { + CHKS((block->g_modes[GM_MODAL_0] != G_53_7), _("J%d= word with no G53.7 to use it"), n); + break; + } } if (block->k_flag) { /* could still be useless if xy_plane arc */ CHKS(((motion != G_2) && (motion != G_3) && (motion != G_6_2) && (motion != G_33) && (motion != G_33_1) && (motion != G_76) && (motion != G_87) && - (block->g_modes[GM_WORK_PLANE] == -1)), - _("K word with no G2, G3, G6.2, G33, G33.1, G68.2, G76, or G87 to use it")); + !plane_words), + _("K word with no G2, G3, G6.2, G33, G33.1, G68.2, G68.4, G76, or G87 to use it")); } if (block->l_number != -1) { @@ -329,7 +364,8 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block if (block->p_flag) { CHKS(((block->g_modes[GM_MODAL_0] != G_10) && (block->g_modes[GM_MODAL_0] != G_4) && (block->g_modes[GM_CONTROL_MODE] != G_64 && (block->g_modes[GM_MODAL_0] != G_12_1)) && - (block->g_modes[GM_WORK_PLANE] == -1) && + !plane_words && + (block->g_modes[GM_MODAL_0] != G_53_1) && (block->g_modes[GM_MODAL_0] != G_53_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) && @@ -342,7 +378,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block (block->m_modes[5] != 64) && (block->m_modes[5] != 65) && (block->m_modes[5] != 66) && (block->m_modes[7] != 19) && (block->user_m != 1) && (block->o_type != M_98)), - _("P word with no G2 G3 G4 G10 G12.1 G64 G68.2 G5 G5.2 G6, G6.2, G76 G82 G86 G88 G89" + _("P word with no G2 G3 G4 G10 G12.1 G53.1 G53.3 G53.6 G64 G68.2 G68.4 G5 G5.2 G6, G6.2, G76 G82 G86 G88 G89" " G28.2" " or M50 M51 M52 M53 M62 M63 M64 M65 M66 M98 " "or user M code to use it")); @@ -360,12 +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) && + !plane_words && + (block->g_modes[GM_MODAL_0] != G_53_1) && (block->g_modes[GM_MODAL_0] != G_53_3) && (block->g_modes[GM_MODAL_0] != G_53_6) && (motion != G_70) && (motion != G_71) && (motion != G_71_1) && (motion != G_71_2) && (motion != G_72) && (motion != G_72_1) && (motion != G_72_2) && (block->m_modes[7] != 19), - _("Q word with no G5, G6, G10, G64, G68.2, G73, G76, G83, M19, M66, M67, M68 or user M code that uses it")); + _("Q word with no G5, G6, G10, G53.1, G53.3, G53.6, G64, G68.2, G68.4, G73, G76, G83, M19, M66, M67, M68 or user M code that uses it")); } if (block->r_flag) { @@ -376,7 +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) && + !plane_r && (block->g_modes[GM_CONTROL_MODE] != G_64) ), /* G64_R_PLANNER: R selects planner on G64 */ NCE_R_WORD_WITH_NO_G_CODE_THAT_USES_IT); /* G64_R_PLANNER: a block has one shared R word; with G64 it is the planner diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index b92c4f69bdd..cbb698aa72b 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -4500,7 +4500,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_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 @@ -5693,6 +5697,11 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 } settings->motion_mode = move; + 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 + 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)); @@ -5706,7 +5715,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), @@ -6557,7 +6587,7 @@ static int kins_type_info_available() // the type carrying a KINSTYPE_ flag, or -1 when the module declares none; // -1 for a type is "no information", and it matches every flag, so it must // be excluded before the bit test -static int flagged_kins_type(int flag) +int flagged_kins_type(int flag) { int k, f; diff --git a/src/emc/rs274ngc/interp_internal.cc b/src/emc/rs274ngc/interp_internal.cc index e7f3f32bdf9..37fbe304f88 100644 --- a/src/emc/rs274ngc/interp_internal.cc +++ b/src/emc/rs274ngc/interp_internal.cc @@ -174,7 +174,7 @@ 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_4) { CHKS(polar_flag, _("Polar coordinates cannot define a tilted work plane")); @@ -286,6 +286,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 acd4eeb39ae..cbfed8c4954 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -255,6 +255,12 @@ enum GCodes G_51 = 510, G_52 = 520, G_53 = 530, + G_53_1 = 531, + 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, @@ -265,6 +271,7 @@ enum GCodes G_59_2 = 592, G_59_3 = 593, G_68_2 = 682, + G_68_3 = 683, G_68_4 = 684, G_69 = 690, G_61 = 610, @@ -503,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{}; @@ -763,6 +773,14 @@ struct setup 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 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 @@ -1070,4 +1088,8 @@ struct scoped_locale { }; #define FORCE_LC_NUMERIC_C scoped_locale force_lc_numeric_c(LC_NUMERIC, "C") + +// the kinematics type carrying a KINSTYPE_ flag, or -1 when the module +// declares none (interp_convert.cc) +int flagged_kins_type(int flag); #endif // INTERP_INTERNAL_HH diff --git a/src/emc/rs274ngc/interp_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 566188109ee..bdfcada8020 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -112,6 +112,12 @@ setup::setup() : g68_seq_p(0), g68_seq_have(0), g68_seq_word{}, + kins_ctx(nullptr), + kins_comp_id(0), + kins_module{}, + kins_joints(0), + kins_angular_joints(0), + kins_seed{}, parameters{0}, parameter_occurrence(0), diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 5f14ceea40e..093091239a2 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -412,6 +412,10 @@ int Interp::convert_work_plane(int g_code, block_pointer block, setup_pointer s) _("Cannot cancel a tilted work plane with cutter radius compensation on")); return work_plane_cancel(s, true); } + if (g_code == G_68_3) { + CHKS((s->g68_seq_code != 0), _("G68.3 cannot interrupt a G68.2 sequence")); + return convert_work_plane_from_tool(block, s); + } CHKS((g_code != G_68_2 && g_code != G_68_4), "BUG: code not G68.2, G68.4 or G69"); CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), @@ -435,3 +439,583 @@ int Interp::convert_work_plane(int g_code, block_pointer block, setup_pointer s) } return work_plane_set(s, g_code, origin, rotation); } + +//---------------------------------------------------------------------- +// The kinematics. G68.3 and the orientation moves need the frames and +// the tool frame inverse of the module motion runs, evaluated here, ahead +// of motion, through the loader in kinematics_userspace/. The loader +// binds its pins to a HAL component, so the interpreter makes one, 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 loaded module, on the kinematics type the program is in +int Interp::kins_context(setup_pointer s, void **out) +{ + KinematicsUserContext *ctx; + + *out = NULL; + 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; } + } + 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); + *out = ctx; + return INTERP_OK; +} + +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; +} + +// 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; + EmcPose pose; + int pass, i; + + current_machine_pose(s, &pose); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = s->kins_seed[i]; } + for (pass = 0; pass < 8; pass++) { + double prev[EMCMOT_MAX_JOINTS], worst = 0.0; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { prev[i] = 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; +} + +// a direction of the plane in world coordinates: the plane's rotation +// then the XY rotation of the coordinate system it sits on +static void plane_axis_in_world(setup_pointer s, int column, double rotation_xy, PmCartesian *out) +{ + double x = s->g68_rotation[0][column]; + double y = s->g68_rotation[1][column]; + double z = s->g68_rotation[2][column]; + double t = rotation_xy * M_PI / 180.0; + + out->x = x * cos(t) - y * sin(t); + out->y = x * sin(t) + y * cos(t); + out->z = z; +} + +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.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. P picks the pose, +// nearest first 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::convert_orient_tool(int code, block_pointer block, setup_pointer s) +{ + void *vctx; + KinematicsUserContext *ctx; + double now[EMCMOT_MAX_JOINTS]; + 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]; + PmCartesian axis, xdir; + EmcPose end_pose; + double end_prog[9]; + unsigned int held = 0; + int p, q, n, i, j, chosen, njoints; + const double *sol; + const char *name = (code == G_53_1) ? "G53.1" : (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); + njoints = kinematicsUserGetNumJoints(ctx); + 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); + 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 plane's normal cannot be reached by the rotary joints"), name); + + // nearest first, by rotary travel in joint units + for (i = 0; i < n; i++) { + distance[i] = 0.0; + for (j = 0; j < njoints; j++) { distance[i] += fabs(solutions[i*njoints + j] - now[j]); } + order[i] = i; + } + for (i = 1; i < n; i++) { + int k = order[i]; + for (j = i; j > 0 && distance[order[j-1]] > distance[k]; j--) { order[j] = order[j-1]; } + order[j] = k; + } + if (p == 0) { + chosen = order[0]; + } else { + // P names the pose rather than its rank, so that the same program + // reaches the same pose from wherever the machine is standing + int 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"); + } + sol = solutions + chosen * njoints; + + // where that puts the machine, and what the program calls it + end_pose = (EmcPose){}; + current_machine_pose(s, &end_pose); + { + double full[EMCMOT_MAX_JOINTS]; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { full[i] = (i < njoints) ? sol[i] : 0.0; } + CHKS((kinematicsUserForward(ctx, full, &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] = full[i]; } + } + machine_pose_to_program(s, &end_pose, end_prog); + + 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; +} + +// 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 axis letters read as machine frame coordinates, the words the module's +// machine frame type answers to: the pivot in machine coordinates, the +// rotaries as joints, in program units. The joints are read in on the type +// in force and come out as the joints of the machine frame point, the letters +// not given standing where they are. Where the machine frame type is a plain +// identity the letters name joints, and a letter carries a unit class where +// a joint does not: on a serial robot the first joint answers to X and turns +// in degrees, and the whole machine is refused rather than that one letter, +// since a mapping that lies about X is telling nothing useful about A. A +// module that declares no types at all is read as letters = joints through +// its identity mapping, gantry pairs sharing the value. +int Interp::slide_joints(const char *name, setup_pointer s, void *vctx, const struct kins_params *p, + int njoints, const int flags[9], const double words[9], double *joints) +{ + KinematicsUserContext *ctx = (KinematicsUserContext *)vctx; + const int machine = flagged_kins_type(KINSTYPE_MACHINE); + int a, given = 0, status; + + for (a = 0; a < 9; a++) { given += flags[a] ? 1 : 0; } + CHKS((given == 0), _("%s needs at least one axis word"), name); + if (machine < 0) { + return slide_words(name, s, ctx, p, njoints, 1, flags, words, joints); + } + CHKS((kinematicsUserSetType(ctx, machine) != 0), + _("%s: kinematics type %d is not available outside realtime"), name, machine); + status = slide_words(name, s, ctx, p, njoints, 0, flags, words, joints); + // back on the type in force whatever happened: the caller's context + kinematicsUserSetType(ctx, s->kins_type); + return status; +} + +// the words on the context as set: the module's machine frame type, or the +// type in force with letters = joints where the module declares none +int Interp::slide_words(const char *name, setup_pointer s, void *vctx, const struct kins_params *p, + int njoints, int undeclared, const int flags[9], const double words[9], + double *joints) +{ + KinematicsUserContext *ctx = (KinematicsUserContext *)vctx; + static const char letters[9] = { 'X', 'Y', 'Z', 'A', 'B', 'C', 'U', 'V', 'W' }; + int a, j; + + if (undeclared || kinematicsUserIsIdentity(ctx)) { + CHKS((!p), _("%s: the kinematics module gives no joint mapping"), name); + for (a = 0; a < 9; a++) { + const int angular = (a >= 3 && a <= 5); + for (j = 0; j < njoints; j++) { + int turns; + if (!(p->joints_of_axis[a] & (1 << j))) { continue; } + turns = (s->kins_angular_joints & (1 << j)) ? 1 : 0; + CHKS((turns != angular), + _("%s: on this machine %c names joint %d, which the INI file" + " declares %s, so the axis letters do not name the joints they" + " look like; give joints by number with G53.7 J%d="), + name, letters[a], j, turns ? "angular" : "linear", j); + } + } + } + if (!undeclared) { + return machine_frame_joints(name, s, ctx, flags, words, joints); + } + for (a = 0; a < 9; a++) { + const int angular = (a >= 3 && a <= 5); + double value; + if (!flags[a]) { continue; } + CHKS((p->joints_of_axis[a] == 0), + _("%s: %c is not a joint of this kinematics"), name, letters[a]); + value = angular ? PROGRAM_TO_USER_ANG(words[a]) : PROGRAM_TO_USER_LEN(words[a]); + for (j = 0; j < njoints; j++) { + if (p->joints_of_axis[a] & (1 << j)) { joints[j] = value; } + } + } + return INTERP_OK; +} + +// The joints of a machine frame point, on the machine frame type the +// context is set to: where the joints handed in stand in that frame, the +// letters given replacing their coordinate, and the inverse iterated to a +// fixed point as current_joints() does, since an iterative inverse takes +// its seed from the joints handed in. +int Interp::machine_frame_joints(const char *name, setup_pointer s, void *vctx, + const int flags[9], const double words[9], double *joints) +{ + KinematicsUserContext *ctx = (KinematicsUserContext *)vctx; + EmcPose pose; + double *coord[9]; + int a, i, pass; + + current_machine_pose(s, &pose); + CHKS((kinematicsUserForward(ctx, joints, &pose) != 0), + _("%s: the machine frame kinematics cannot place the current joints"), name); + coord[0] = &pose.tran.x; coord[1] = &pose.tran.y; coord[2] = &pose.tran.z; + coord[3] = &pose.a; coord[4] = &pose.b; coord[5] = &pose.c; + coord[6] = &pose.u; coord[7] = &pose.v; coord[8] = &pose.w; + for (a = 0; a < 9; a++) { + const int angular = (a >= 3 && a <= 5); + if (!flags[a]) { continue; } + *coord[a] = angular ? PROGRAM_TO_USER_ANG(words[a]) : PROGRAM_TO_USER_LEN(words[a]); + } + for (pass = 0; pass < 8; pass++) { + double prev[EMCMOT_MAX_JOINTS], worst = 0.0; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { prev[i] = joints[i]; } + CHKS((kinematicsUserInverse(ctx, &pose, joints) != 0), + _("%s: the machine frame kinematics cannot reach that point"), name); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { worst = fmax(worst, fabs(joints[i] - prev[i])); } + if (worst < 1e-9) { break; } + } + return INTERP_OK; +} + +// The two point-to-point codes below G53.4: G53.5 by axis letter in the +// machine frame, the module's machine frame type, refused where that type is +// a plain identity whose letters name joints of the other unit class; G53.7 +// by J= in the joint's own units. A letter or joint left out keeps +// its position. +int Interp::convert_ptp_joints(int code, int move, block_pointer block, setup_pointer s) +{ + void *vctx; + KinematicsUserContext *ctx; + const kins_params *p; + double joints[EMCMOT_MAX_JOINTS]; + 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 }; + 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 { + CHP(slide_joints(name, s, ctx, p, njoints, flags, words, joints)); + } + + // 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/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index 23fd0f0d3c2..517a82e5d52 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -371,6 +371,23 @@ public: 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 convert_ptp_joints(int code, int move, block_pointer block, setup_pointer settings); + int slide_joints(const char *name, setup_pointer settings, void *ctx, const struct kins_params *params, + int njoints, const int flags[9], const double words[9], double *joints); + int slide_words(const char *name, setup_pointer settings, void *ctx, const struct kins_params *params, + int njoints, int undeclared, const int flags[9], const double words[9], double *joints); + int machine_frame_joints(const char *name, setup_pointer settings, void *ctx, + const int flags[9], const double words[9], double *joints); + int ptp_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_context(setup_pointer settings, void **ctx); + void kins_release(setup_pointer settings); + void current_machine_pose(setup_pointer settings, EmcPose *pose); + void machine_pose_to_program(setup_pointer settings, const EmcPose *pose, double prog[9]); + int current_joints(setup_pointer settings, void *ctx, double *joints); void g68_apply(setup_pointer settings, double *x, double *y, double *z); void g68_remove(setup_pointer settings, double *x, double *y, double *z); void g68_unrotate(setup_pointer settings, double *x, double *y, double *z); diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index 899d507eb4f..0a3dc73c306 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); @@ -899,6 +900,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); diff --git a/tests/kins-twp/README b/tests/kins-twp/README index b9b3b7275fa..d114c3a2afc 100644 --- a/tests/kins-twp/README +++ b/tests/kins-twp/README @@ -1,7 +1,9 @@ The C kinematics against the tilted work plane maths. -The two nutating-head configs carry their orientation maths in python, -remap_funcs_twp.py, written independently of the kinematics modules. +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 diff --git a/tests/kins-twp/test.sh b/tests/kins-twp/test.sh index 86b77f6ff44..d50c9bea68e 100755 --- a/tests/kins-twp/test.sh +++ b/tests/kins-twp/test.sh @@ -1,10 +1,6 @@ #!/bin/bash set -e -# RIP layout: $HEADERS is $TOPDIR/include -TOPDIR=$(dirname "$HEADERS") -CONFIGS=$TOPDIR/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating - ${SUDO} halcompile --install twpcheck.c >/dev/null # One hal file per machine. twpcheck answers frame and inverse requests @@ -18,8 +14,8 @@ run() { 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-trsrn_twp %s/twp-%s.ini\n' \ - "$machine" "$CONFIGS" "$machine" "$PWD" "$machine" + 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" diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py b/tests/kins-twp/xyzacb/remap_funcs_twp.py similarity index 100% rename from configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/remap_funcs_twp.py rename to tests/kins-twp/xyzacb/remap_funcs_twp.py diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py b/tests/kins-twp/xyzbca/remap_funcs_twp.py similarity index 100% rename from configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/remap_funcs_twp.py rename to tests/kins-twp/xyzbca/remap_funcs_twp.py diff --git a/tests/ptp-machine-frame/README b/tests/ptp-machine-frame/README new file mode 100644 index 00000000000..c7e76a61ea1 --- /dev/null +++ b/tests/ptp-machine-frame/README @@ -0,0 +1,16 @@ +The point-to-point moves on a machine whose slides do not line up with +its frame. + +slantkins.comp is built here, out of tree the way the switchkinscomp +template builds: XYZBC on five joints, joint 1 a slide slanted 30 degrees +in the XY plane, so machine X and Y are made by joints 0 and 1 together. +Type 0 is the carriage in machine coordinates, declared the machine frame +type and not an identity; type 1 the tool tip with the head tilted about Y +and the tool length from the tool table; type 2 a plain identity. + +The test checks that G13.1 and G49 select the machine frame type rather +than the identity, that G53.5 puts the carriage at a machine frame point, +the letters not given holding their machine coordinate and not their +joint, from the machine frame type and from under the tilted tip +kinematics alike, and that a plain move after a machine frame move starts +from the right place. diff --git a/tests/ptp-machine-frame/checkresult b/tests/ptp-machine-frame/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/ptp-machine-frame/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/ptp-machine-frame/sim.hal b/tests/ptp-machine-frame/sim.hal new file mode 100644 index 00000000000..ecfa81b6035 --- /dev/null +++ b/tests/ptp-machine-frame/sim.hal @@ -0,0 +1,15 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/ptp-machine-frame/skip b/tests/ptp-machine-frame/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/ptp-machine-frame/skip @@ -0,0 +1,4 @@ +#!/bin/sh +# Builds a realtime component with halcompile, which needs the build +# tools present. Skip when testing installed packages. +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/ptp-machine-frame/slantkins.comp b/tests/ptp-machine-frame/slantkins.comp new file mode 100644 index 00000000000..db9d694a9bb --- /dev/null +++ b/tests/ptp-machine-frame/slantkins.comp @@ -0,0 +1,164 @@ +component slantkins "Test kinematics whose machine frame is not its joints"; + +description +""" +A switchable kinematics for the ptp-machine-frame test: XYZBC on five +joints, where joint 1 is a slide slanted in the XY plane by the 'slant' +angle, so that machine X and Y are made by joints 0 and 1 together, the way +a mill-turn with a composite Y slide is built. + +type0 is the machine frame: the carriage in machine coordinates, the two +rotaries as joints, declared as the machine frame type and not an identity. + +type1 is the working transform: the tool tip, with the head tilted about Y +by B and the tool length from the tool table along the tool axis, folded so +that tip and carriage agree at B zero. + +type2 is a plain identity, the joints as the axes, declared identity. +"""; +pin out si32 dummy=0 "one pin, which halcompile requires"; +option period no; +option extra_setup; +license "GPL"; +author "Luca Toniolo"; +;; + +#include +// out of tree, the way the switchkinscomp template builds: the switchkins +// core and the parameter block helpers compiled into the module +#include +#include +#include + +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); + +static const kins_param_desc slant_params[] = { + { "tool-offset-z", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + { "slant", KINS_PARAM_FLOAT, KINS_IN, 0, 30.0 }, +}; +enum { P_TOOL, P_SLANT }; + +static void unused_axes(EmcPose *pos) +{ + pos->a = 0; + pos->u = 0; + pos->v = 0; + pos->w = 0; +} + +// the carriage: joint 1 runs at the slant angle from X, in the XY plane +static int frame_forward(const kins_params *p, kins_scratch *s, + const double *j, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + double th = p->geometry[P_SLANT] * PM_PI / 180.0; + (void)s; (void)fflags; (void)iflags; + + pos->tran.x = j[0] + j[1] * cos(th); + pos->tran.y = j[1] * sin(th); + pos->tran.z = j[2]; + pos->b = j[3]; + pos->c = j[4]; + unused_axes(pos); + return 0; +} + +static int frame_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *j, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + double th = p->geometry[P_SLANT] * PM_PI / 180.0; + (void)s; (void)iflags; (void)fflags; + + j[1] = pos->tran.y / sin(th); + j[0] = pos->tran.x - j[1] * cos(th); + j[2] = pos->tran.z; + j[3] = pos->b; + j[4] = pos->c; + return 0; +} + +// the tip: the head tilts about Y by B and carries the tool length below +// the pivot, folded so that the tip is the carriage at B zero +static void tip_offset(const kins_params *p, double b, double *dx, double *dz) +{ + double L = p->tool.tran.z; + double rb = b * PM_PI / 180.0; + + *dx = -L * sin(rb); + *dz = L * (1.0 - cos(rb)); +} + +static int tip_forward(const kins_params *p, kins_scratch *s, + const double *j, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + double dx, dz; + + frame_forward(p, s, j, pos, fflags, iflags); + tip_offset(p, j[3], &dx, &dz); + pos->tran.x += dx; + pos->tran.z += dz; + return 0; +} + +static int tip_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *j, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + EmcPose carriage = *pos; + double dx, dz; + + tip_offset(p, pos->b, &dx, &dz); + carriage.tran.x -= dx; + carriage.tran.z -= dz; + return frame_inverse(p, s, &carriage, j, iflags, fflags); +} + +static const kins_ops frame_ops = { + .forward = frame_forward, + .inverse = frame_inverse, + .machine = 1, +}; + +static const kins_ops tip_ops = { + .forward = tip_forward, + .inverse = tip_inverse, + .primary = 1, +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "slantkins"; + kp->halprefix = "slantkins"; + kp->required_coordinates = "xyzbc"; + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + kp->params = slant_params; + kp->nparams = sizeof(slant_params)/sizeof(slant_params[0]); + + switchkinsRegisterOps(0, &frame_ops); + switchkinsRegisterOps(1, &tip_ops); + switchkinsRegisterOps(2, &KINS_IDENTITY_OPS); + return 0; +} + +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + if (switchkinsRunSetup(&kp, NULL)) { return -1; } + return switchkinsInit(comp_id, &kp, coordinates); +} diff --git a/tests/ptp-machine-frame/test-ui.py b/tests/ptp-machine-frame/test-ui.py new file mode 100755 index 00000000000..28021cc33f5 --- /dev/null +++ b/tests/ptp-machine-frame/test-ui.py @@ -0,0 +1,150 @@ +#!/usr/bin/env python3 +# The point-to-point moves on a machine whose slides do not line up with its +# frame: see README. + +import linuxcnc +import hal +import sys +import os +import time +import math + +JOINTS = 5 +SLANT = math.radians(30.0) +TOOL = 50.0 # T1 in tool.tbl + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.home(-1) +c.wait_complete() +c.mode(linuxcnc.MODE_MDI) + +errors = 0 + +def error(msg): + global errors + errors += 1 + print("*** ERROR " + msg) + +def drain(): + while True: + m = e.poll() + if not m: + return + print("channel:", m) + if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reported: %s" % m[1]) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(JOINTS)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.05) + error("timed out waiting for the move") + return last + +def mdi(*cmds): + for cmd in cmds: + c.mdi(cmd) + c.wait_complete(60) + return settled() + +def kins_type(): + return int(hal.get_value("motion.kins-type")) + +def show(what, j): + print("%-34s %s" % (what, " ".join("%.4f" % v for v in j))) + +def expect(what, j, want, tol=1e-4): + show(what, j) + if max(abs(a - b) for a, b in zip(j, want)) > tol: + error("%s: expected %s" % (what, " ".join("%.4f" % v for v in want))) + +# the joints of a carriage position: joint 1 is the slanted slide +def carriage(x, y, z, b=0.0, cc=0.0): + j1 = y / math.sin(SLANT) + return [x - j1 * math.cos(SLANT), j1, z, b, cc] + +def expect_type(what, want): + k = kins_type() + print("%-34s kins-type %d" % (what, k)) + if k != want: + error("%s: kins-type %d, expected %d" % (what, k, want)) + +# --- G13.1 and G49 select the machine frame type, not the identity --------- +mdi("G12.1 P2") +expect_type("G12.1 P2, the plain identity", 2) +mdi("G13.1") +expect_type("G13.1", 0) +mdi("G43.4 H1") +expect_type("G43.4", 1) +mdi("G49") +expect_type("G49", 0) +mdi("G12.1 P1", "G13.1") +expect_type("G12.1 P1 then G13.1", 0) +drain() + +# --- G53.5 is the machine frame, not the slides ------------------------------ +# on the machine frame type G53 and G53.5 agree, and a Y word alone moves +# both slides: X is held, which the slanted slide would otherwise pull +j = mdi("G53 G0 X5 Y0 Z0 B0 C0") +expect("G53 to (5, 0, 0)", j, carriage(5, 0, 0)) +j = mdi("G53.5 G0 Y10") +expect("G53.5 Y10 holds machine X", j, carriage(5, 10, 0)) +j = mdi("G53.5 G0 X0") +expect("G53.5 X0 holds machine Y", j, carriage(0, 10, 0)) + +# under the tip kinematics with the head tilted and a tool on, G53.5 still +# means the carriage: the tilt and the tool length are left out. The tilt +# holds the tip, so the carriage has moved by the tool's swing, and that is +# where X is held +mdi("G43.4 H1") +j = mdi("G0 B30") +expect_type("G43.4 with the head at B30", 1) +swing_x = TOOL * math.sin(math.radians(30)) +swing_z = TOOL * (1 - math.cos(math.radians(30))) +expect("the tilt holds the tip", j, carriage(swing_x, 10, -swing_z, 30)) +j = mdi("G53.5 G0 Y0 Z0") +expect("G53.5 Y0 Z0 under the tip, tilted", j, carriage(swing_x, 0, 0, 30)) +j = mdi("G53.5 G0 X20 Y10 Z-5") +expect("G53.5 X20 Y10 Z-5 under the tip", j, carriage(20, 10, -5, 30)) +j = mdi("G53.5 G0 B0") +expect("G53.5 B0 is the joint", j, carriage(20, 10, -5, 0)) +mdi("G49", "G0 B0", "G53 G0 X0 Y0 Z0") +drain() + +# --- the program position agrees with the move ------------------------------ +# the interpreter reports the tip where the joints put it, so a plain move +# after a machine frame move starts from the right place +mdi("G43.4 H1", "G0 B30", "G53.5 G0 X20 Y10 Z0") +s.poll() +before = list(s.position[:3]) +j = mdi("G91 G0 X1", "G90") +after = carriage(20, 10, 0, 30) +after[0] += 1.0 +expect("G91 X1 after G53.5", j, after) +s.poll() +d = [s.position[i] - before[i] for i in range(3)] +print("%-34s dx %.4f dy %.4f dz %.4f" % ("the tip moved", d[0], d[1], d[2])) +if abs(d[0] - 1.0) > 1e-4 or abs(d[1]) > 1e-4 or abs(d[2]) > 1e-4: + error("the tip did not move by X1 alone") +mdi("G49", "G0 B0", "G53 G0 X0 Y0 Z0") +drain() + +for f in ("sim.var", "sim.var.bak"): + try: + os.unlink(f) + except OSError: + pass + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/ptp-machine-frame/test.ini b/tests/ptp-machine-frame/test.ini new file mode 100644 index 00000000000..20053c9a2db --- /dev/null +++ b/tests/ptp-machine-frame/test.ini @@ -0,0 +1,114 @@ +[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 = slantkins +JOINTS = 5 + +[HAL] +HALFILE = sim.hal + +[TRAJ] +COORDINATES = XYZBC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 100 +MAX_LINEAR_VELOCITY = 120 +MAX_LINEAR_ACCELERATION = 700 +DEFAULT_LINEAR_ACCELERATION = 300 +NO_FORCE_HOMING = 1 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Y] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_Z] +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 + +[AXIS_B] +MIN_LIMIT = -185 +MAX_LIMIT = 185 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[AXIS_C] +MIN_LIMIT = -360 +MAX_LIMIT = 360 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 + +[JOINT_0] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 700 +MIN_LIMIT = -5000 +MAX_LIMIT = 5000 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -185 +MAX_LIMIT = 185 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 90 +MAX_ACCELERATION = 900 +MIN_LIMIT = -360 +MAX_LIMIT = 360 +HOME_SEQUENCE = 0 diff --git a/tests/ptp-machine-frame/test.sh b/tests/ptp-machine-frame/test.sh new file mode 100755 index 00000000000..48c8eebedaf --- /dev/null +++ b/tests/ptp-machine-frame/test.sh @@ -0,0 +1,5 @@ +#!/bin/bash -e +${SUDO} halcompile --install slantkins.comp >/dev/null +# a failed run leaves the var file behind, and it carries stored positions +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/ptp-machine-frame/tool.tbl b/tests/ptp-machine-frame/tool.tbl new file mode 100644 index 00000000000..67f23122e9e --- /dev/null +++ b/tests/ptp-machine-frame/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z50 D6 diff --git a/tests/ptp-robot/README b/tests/ptp-robot/README new file mode 100644 index 00000000000..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/skip b/tests/ptp-robot/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/ptp-robot/skip @@ -0,0 +1,4 @@ +#!/bin/sh +# Builds a realtime component with halcompile, which needs the build +# tools present. Skip when testing installed packages. +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/ptp-robot/test-ui.py b/tests/ptp-robot/test-ui.py new file mode 100755 index 00000000000..92362c901a7 --- /dev/null +++ b/tests/ptp-robot/test-ui.py @@ -0,0 +1,99 @@ +#!/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])) + +# 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/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/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/skip b/tests/twp-native/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/twp-native/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/twp-native/test-ui.py b/tests/twp-native/test-ui.py new file mode 100755 index 00000000000..9550b123782 --- /dev/null +++ b/tests/twp-native/test-ui.py @@ -0,0 +1,489 @@ +#!/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 --------------------------- +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 = max(abs(smp[0][i] - start[i]) for smp in samples for i in range(3)) +print("linear joints moved at most %.9f through G53.1" % worst) +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.3 goes to a point in the plane with the tool oriented ---------- +before = mdi("G69") +R3 = rot_y(-25).dot(rot_x(35)) +mdi("G68.2 P1 Q123 I35 J-25 K0") +after, samples = sampled("G53.3 X5 Y5 Z5") +show("G53.3", after) +drain() +joint_line("G53.3", samples, before, after) +s.poll() +prog = R3.T.dot(np.array(s.position[:3]) - np.array(s.g68_offset[:3])) +if not close(list(prog), [5, 5, 5], 1e-3): + error("G53.3 ended at %s in the plane, not 5 5 5" % list(prog)) +if not close(tool_axis(after), list(R3[:, 2]), 1e-6): + error("the tool axis after G53.3 is not the plane normal") + +# --- G68.3 reads the plane back off the tool --------------------------- +mdi("G69", "G68.3 X1 Y2 Z3") +drain() +s.poll() +x, y, z = plane_axes() +if not s.g68_active or not close(list(s.g68_offset[:3]), [1, 2, 3], 1e-9): + error("G68.3 did not set the origin asked for") +if not close(z, list(R3[:, 2]), 1e-6): + error("G68.3's normal %s is not the tool axis %s" % (z, list(R3[:, 2]))) +if abs(x[2]) > 1e-6: + error("G68.3's X is not parallel to the machine XY plane: %s" % x) +mdi("G69", "G68.3 R90") +xr, yr, zr = plane_axes() +if not close(xr, y, 1e-6): + error("G68.3 R90 did not turn the plane about its normal") + +# the words a plane code does not use are refused, not ignored +for cmd in ("G68.3 I1 J0 K0", "G68.3 P1", "G68.3 Q123", "G69 R45", "G69 I1", "G69 P1"): + mdi("G69") + c.mdi(cmd) + c.wait_complete(30) + m = e.poll() + if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("%s was accepted" % cmd) + else: + print("refused as expected:", m[1]) + c.mode(linuxcnc.MODE_MDI) + +# --- Q1 lets the table take part --------------------------------------- +mdi("G69", "G0 X0 Y0 Z0 A0 B0 C0") +R4 = rot_x(30) +mdi("G68.2 P1 Q123 I30 J0 K0") +after = mdi("G53.1 Q1") +show("G53.1 Q1", after) +drain() +if not close(tool_axis(after), list(R4[:, 2]), 1e-6): + error("the tool axis after G53.1 Q1 is not the plane normal") +# with the plane X requested as well and the table free, three joints +# place three constraints: the plane's X is reached by the machine rather +# than by the frame +m = twp.kins_calc_transformation_matrix(math.radians(after[PRIMARY]), + math.radians(after[SECONDARY]), 0, I4, 'inv') +a = math.radians(after[TABLE]) +W = np.array([[1, 0, 0], [0, math.cos(a), math.sin(a)], [0, -math.sin(a), math.cos(a)]]) +xm = np.array([m[0, 0], m[1, 0], m[2, 0]]) +if not close(list(W.T.dot(xm)), list(R4[:, 0]), 1e-6): + error("with Q1 the machine did not place the plane's X") + +# --- G53.4, G53.5 and G69 ------------------------------------------------ +before = mdi("G69") +after, samples = sampled("G53.4 G0 X0 Y0 Z0 A0 B0 C0") +show("G53.4 G0", after) +drain() +joint_line("G53.4 G0", samples, before, after) +s.poll() +if s.g68_active: + error("G69 left the plane active") +if not close(list(s.g68_rotation), [1, 0, 0, 0, 1, 0, 0, 0, 1], 1e-12): + error("G69 left a rotation in status") +if not close(list(s.position[:3]), [0, 0, 0], 1e-6) or any(abs(after[j]) > 1e-6 for j in (TABLE, SECONDARY, PRIMARY)): + error("G53.4 G0 did not bring the tool and the rotaries back to zero") + +# G53.7 takes joint values by joint number: with the head tilted, J2=-5 +# puts joint 2 at -5 whatever that does to the tool tip, and touches no +# other joint; the value is the joint's own, untouched by G20 +before = mdi("G0 B30") +show("before G53.7", before) +after = mdi("G20 G53.7 G0 J2=-5") +mdi("G21") +show("G53.7 G0 J2=-5", after) +drain() +if abs(after[2] + 5) > 1e-6: + error("G53.7 J2=-5 left joint 2 at %.6f" % after[2]) +for j in (0, 1, 3, 4, 5): + if abs(after[j] - before[j]) > 1e-6: + error("G53.7 J2=-5 moved joint %d from %.9f to %.9f" % (j, before[j], after[j])) +after = mdi("G53.7 G0 J2=0 J[2+2]=0") +if abs(after[2]) > 1e-6 or abs(after[SECONDARY]) > 1e-6: + error("G53.7 J2=0 J4=0 did not put joints 2 and 4 at zero") + +# G53.5 takes the same destination by axis letter, in program units: Z-5 +# is joint 2 in millimetres, and under G20 the same words are inches +before = mdi("G0 B30") +after = mdi("G53.5 G0 Z-5") +show("G53.5 G0 Z-5", after) +drain() +if abs(after[2] + 5) > 1e-6: + error("G53.5 Z-5 left joint 2 at %.6f" % after[2]) +for j in (0, 1, 3, 4, 5): + if abs(after[j] - before[j]) > 1e-6: + error("G53.5 Z-5 moved joint %d from %.9f to %.9f" % (j, before[j], after[j])) +after = mdi("G20 G53.5 G0 Z-1") +mdi("G21") +show("G20 G53.5 G0 Z-1", after) +if abs(after[2] + 25.4) > 1e-6: + error("an inch of G53.5 Z left joint 2 at %.6f, not -25.4" % after[2]) +after = mdi("G53.5 G0 Z0 B0") +if abs(after[2]) > 1e-6 or abs(after[SECONDARY]) > 1e-6: + error("G53.5 Z0 B0 did not put joints 2 and 4 at zero") + +# 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 From 7d7e79d883fa7cb43f4252d57368f9537777042c Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 5 Sep 2026 00:58:37 +1000 Subject: [PATCH 48/77] configs: the nutating-head sims on the native tilted work plane The two trsrn_twp configs drop the python remap of G68.2, G68.3, G68.4, G69, G53.1, G53.3 and G53.6, its ngc wrappers, the abort handler that unwound it and the twp-status analog pin: the interpreter does all of it now, with nothing to restore on abort. The helper component that feeds the vismach model reads the plane from status. The demos select the TCP kinematics up front, since the orientation codes need it, and put kinematics 0 back at the end; square.ngc loses its G52, which a plane refuses. --- .../README | 33 +- .../demos/incremental_repetition.ngc | 2 + .../incremental_repetition_back_and_forth.ngc | 2 + .../demos/incremental_repetition_g533.ngc | 4 +- .../demos/simple_example.ngc | 2 + .../demos/square.ngc | 1 - .../python/remap.py | 1574 ----------------- .../python/toplevel.py | 20 - .../python/twp-helper-comp.py | 94 +- .../python/util.py | 67 - .../remap_subs/g531remap.ngc | 14 - .../remap_subs/g533remap.ngc | 20 - .../remap_subs/g536remap.ngc | 14 - .../remap_subs/g69remap.ngc | 11 - .../remap_subs/on_abort_with_twp_reset.ngc | 15 - .../xyzacb-trsrn_twp/xyzacb-trsrn.ini | 39 - .../xyzbca-trsrn_twp/xyzbca-trsrn.ini | 40 +- tests/twp-native/checkresult | 3 + tests/twp-native/test-ui.py | 12 +- 19 files changed, 63 insertions(+), 1904 deletions(-) delete mode 100755 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py delete mode 100755 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/toplevel.py delete mode 100755 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/util.py delete mode 100755 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g531remap.ngc delete mode 100755 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g533remap.ngc delete mode 100755 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g536remap.ngc delete mode 100755 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g69remap.ngc delete mode 100755 configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/on_abort_with_twp_reset.ngc create mode 100755 tests/twp-native/checkresult 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 05fc53261a1..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/remap.py +++ /dev/null @@ -1,1574 +0,0 @@ -# This is a python remap for LinuxCNC implementing 'Tilted Work Plane' -# G68.2, G68.3, G68.4 and related Gcodes G53.1, G53.3, G53.6, G69 -# -# Copyright ()c) 2025 David Mueller -# -# This program is free software: you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation, either version 2 of the License, or -# (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# -''' -The remap does the following: - -- Parses the G68.[2,4] gcodes and constructs the requested tool orientation vectors (x,z). -- Writes and reads hal pins created and updated by'twp-helper-comp.py' (mostly for updating the gui). -- Parses the G53.[1,3,6] and uses the functions in 'remap_funcs_twp.py' to calculate all rotary joint position that result in the correct tool orientation (there may be more than just one). -- Selects the appropriate rotary angles that will respect rotary limits set in the ini file and also follow any orientation strategy requested by the operator using the 'P' word. -- Sets the kinematic modes -- Calculates new work offset values so the WCS origin after switching to TWP mode is in the requested physical position. -- Used MDI commands to: - - Move the rotary joints to the calculated positions - - Switch the WCS system to 'G59' and set the values of G59, G59.[1,2.3] to the calculated coordinates -- Parses the G69 gcodes, resets the relevant parameters and switches back to Identity kinematic mode -''' - - -import sys -import traceback -import numpy as np -from math import sin,cos,tan,asin,acos,atan,atan2,sqrt,pi,degrees,radians,fabs -from interpreter import * -import emccanon -from util import lineno, call_pydevd -import hal - -# logging -import logging -# this name will be printed first on each log message -log = logging.getLogger('remap.py; TWP') -# we have to setup a handler to be able to set the log level for this module -handler = logging.StreamHandler() -formatter = logging.Formatter('%(name)s %(levelname)s: %(message)s') -handler.setFormatter(formatter) -log.addHandler(handler) - -# set up parsing of the inifile -import os -import linuxcnc -# get the path for the ini file used to start this config -inifile = os.environ.get("INI_FILE_NAME") - -# adding the remap_funcs folder to the system path. The machine specific -# functions live beside the ini file, which is the working directory, and the -# parent is searched too so a config may keep them one level up and share them -# between variants. -cwd = os.getcwd() -parent = os.path.abspath(os.path.join(cwd, os.pardir)) -sys.path.insert(0, parent) -sys.path.insert(0, cwd) -from remap_funcs_twp import * - -# instantiate the LinuxCNC ini-parser -config = linuxcnc.ini(inifile) - -# debug setting -try: - debug_setting = config.getint('TWP', 'LOG_LEVEL', fallback=1) - if debug_setting > 4: debug_setting = 4 - if debug_setting < 0: debug_setting = 0 -except Exception as error: - debug_setting = 1 - log.warning("Unable to parse debug setting given in INI. Setting it to 1.") -debug_levels = (logging.CRITICAL, logging.ERROR, logging.WARNING, logging.INFO, logging.DEBUG) -log.setLevel(debug_levels[debug_setting]) - -## ROTARY JOINT LETTERS -# primary rotary joint (independent of the secondary joint) -joint_letter_primary = config.getstring('TWP', 'PRIMARY', fallback="").capitalize() -# secondary rotary joint (dependent on the primary joint) -joint_letter_secondary = config.getstring('TWP', 'SECONDARY', fallback="").capitalize() - -if not joint_letter_primary in ('A','B','C') or not joint_letter_secondary in ('A','B','C'): - log.error("Unable to parse joint letters given in INI [TWP].") -elif joint_letter_primary == joint_letter_secondary: - log.error("Letters for primary and secondary joints in INI [TWP] must not be the same.") -else: - # get the MIN/MAX limits of the respective rotary joint letters - category = 'AXIS_' + joint_letter_primary - primary_min_limit = radians(config.getreal(category, 'MIN_LIMIT', fallback=0.0)) - primary_max_limit = radians(config.getreal(category, 'MAX_LIMIT', fallback=0.0)) - log.info('Joint letter for primary is %s with MIN/MAX limits: %s,%s', - joint_letter_primary, degrees(primary_min_limit), degrees(primary_max_limit)) - category = 'AXIS_' + joint_letter_secondary - secondary_min_limit = radians(config.getreal(category, 'MIN_LIMIT', fallback=0.0)) - secondary_max_limit = radians(config.getreal(category, 'MAX_LIMIT', fallback=0.0)) - log.info('Joint letter for secondary is %s with MIN/MAX Limits: %s,%s', - joint_letter_secondary, degrees(secondary_min_limit), degrees(secondary_max_limit)) - - -## CONNECTIONS TO THE HELPER COMPONENT -twp_comp = 'twp-helper-comp.' -twp_is_defined = twp_comp + 'twp-is-defined' -twp_is_active = twp_comp + 'twp-is-active' - -# Which rotary joint should be prioritized when calculating optimal joint rotation angles -try: - optimization_priority = config.getint('TWP', 'PRIORITY', fallback=1) -except Exception as error: - log.warning("Unable to parse orientation priority given in INI. Setting it to 1.") - optimization_priority = 1 - -# raise InterpreterException if execute() or read() fail -throw_exceptions = 1 - -## VALUE INITIALIZATION -# we start with the identity matrix (ie the twp is equal to the world coordinates) -twp_matrix = np.asmatrix(np.identity(4)) - -# some g68.2 p-word modes require several calls to enter all the required parameters so we -# need a flag that indicates when the twp has been defined and is ready for G53.n -# [current p-word, number of calls required, (state of calls required for that p mode added by g68.2)] -# note that we use string since boolean True == 1, which gives wrong results if we want -# to count the elements that are True because it is counted as integer '1' -# eg: twp_flag = [0, 1, 'empty'] -twp_flag = [] -# we need a place to store the twp-build-parameters if the mode needs more than one call -twp_build_params = {} -# container to store the current work offset during twp operations -current_work_offset_number = 1 -saved_work_offset = [0,0,0] -# orientation mode refers to the strategy used to choose from the different rotary angles for a given -# z-vector vector. The optimization is applied to the primary axis only with mode 0 (shortest path) being -# the default. (0=shortest_path , 1=positive_rotation only, 2=negative_rotation only, ) -orient_mode = 0 - - - -# define the basic rotation matrices -def Rx(th): - return np.array([[1, 0 , 0 ], - [0, cos(th), -sin(th)], - [0, sin(th), cos(th)]]) - -def Ry(th): - return np.array([[ cos(th), 0, sin(th)], - [ 0 , 1, 0 ], - [-sin(th), 0, cos(th)]]) - -def Rz(th): - return np.array([[cos(th), -sin(th), 0], - [sin(th), cos(th), 0], - [0 , 0 , 1]]) - - - -def calc_euler_rot_matrix(th1, th2, th3, order): # expects radians - # returns the rotation matrices for given order and angles - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - debug_msg = (f' Euler order {order} requested with angles: ' - f'{degrees(th1):.4f}, {degrees(th2):.4f}, {degrees(th3):.4f}') - log.debug(debug_msg) - if order == '131': - matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Rx(th3)) - elif order=='121': - matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rx(th3)) - elif order=='212': - matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Ry(th3)) - elif order=='232': - matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Ry(th3)) - elif order=='323': - matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rz(th3)) - elif order=='313': - matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Rz(th3)) - elif order=='123': - matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rz(th3)) - elif order=='132': - matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Ry(th3)) - elif order=='213': - matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Rz(th3)) - elif order=='231': - matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Rx(th3)) - elif order=='321': - matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rx(th3)) - elif order=='312': - matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Ry(th3)) - #log.debug(' Returning euler rotation as matrix: \n %s', matrix) - return matrix - - -def calc_joint_angles(z_vector_req, x_vector_req): - # returns a list of valid primary/secondary rotary joint positions in radians for a given orientation vector - # returns an empty list if no valid position could be found - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - log.debug(' z_vector_requested: %s', z_vector_req) - log.debug(' x_vector_requested: %s', x_vector_req) - # set the tolerance value - epsilon = 0.0001 - # create np.array so we can easily calculate differences and check elements - z_vector_req = np.array([z_vector_req[0], z_vector_req[1], z_vector_req[2]]) - # calculate joint values using kinematic specific formula - try: - (theta_1_calcd, theta_2_calcd) = kins_calc_possible_joint_angles(log, z_vector_req, x_vector_req) - except Exception as error: - log.error('Remap_funcs: kins_calc_possible_joint_angles failure, %s', error) - - # remove any duplicate values from the results - theta_1_calcd = tuple(set(theta_1_calcd)) - theta_2_calcd = tuple(set(theta_2_calcd)) - log.debug(' Got possible angles theta_1: ' + ' '.join("{:.4f}°".format(degrees(theta)) for theta in theta_1_calcd)) - log.debug(' Got possible angles theta_2: ' + ' '.join("{:.4f}°".format(degrees(theta)) for theta in theta_2_calcd)) - if theta_1_calcd == None or theta_2_calcd == None: - return [] - angle_pairs_list = [] - # create a list of paired combinations of returned angles (theta_1 , theta_2) - for i in range(len(theta_1_calcd)): - for j in range(len(theta_2_calcd)): - angle_pairs_list.append((theta_1_calcd[i], theta_2_calcd[j])) - angle_pairs_list = list(set(angle_pairs_list)) - # iterate through the list and check if a particular pair actually produces the requested z-vector orientation - joint_angles_list = [] - for i in range(len(angle_pairs_list)): - debug_msg = (f' Checking angle pair {i}: ({angle_pairs_list[i][0]:.4f}, {angle_pairs_list[i][1]:.4f}) ' - f'({degrees(angle_pairs_list[i][0]):.4f}°, {degrees(angle_pairs_list[i][1]):.4f}°)') - log.debug(debug_msg) - # we start with an identity matrix (ie oriented to world) - matrix_in = np.asmatrix(np.identity(4)) - try: - direction = kins_calc_transformation_get_direction() - except Exception as error: - log.error('kins_calc_transformation_get_direction, %s', error) - try: - matrix_out = kins_calc_transformation_matrix(angle_pairs_list[i][0], angle_pairs_list[i][1], 0, matrix_in, direction) - except Exception as error: - log.error('kins_calc_transformation_matrix, %s', error) - # the resulting z-vector for this pair of (theta_1, theta_2) is found in the third column - z_vector_would_be = np.array([matrix_out[0,2], matrix_out[1,2], matrix_out[2,2]]) - # calculate the difference of the respective elements - z_vector_diff = z_vector_req - z_vector_would_be - log.debug(' z_vector_diff: %s', z_vector_diff) - # and check if all elements are within [-epsilon,epsilon] - match_z = np.all((z_vector_diff > -epsilon) & (z_vector_diff < epsilon)) - log.debug(' Is the z-vector-vector close enough ? %s', match_z) - if match_z: - joint_angles_list.append((angle_pairs_list[i][0], angle_pairs_list[i][1])) - for (theta_1, theta_2) in joint_angles_list: - log.debug(f'Returning valid joint angles found: {degrees(theta_1):.4f}°, {degrees(theta_2):.4f}°') - return joint_angles_list # returns radians - - -def calc_shortest_distance(pos, trgt, mode): - pos = degrees(pos) - trgt = degrees(trgt) - # calculate the shortest distance in [-180°, 180°] eg if pos=170° and trgt=-170° then dist will be 20° - # If the operator requests positive or negative rotation we may need to return the long distance instead - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - dist_short = (trgt - pos + 180) % 360 - 180 - # calculate short and long distance - if dist_short >= 0: # ie dist_long should be negative - dist_long = -(360 - dist_short) - else: - dist_long = 360 + dist_short - log.debug(f' Calculated dist_short: {dist_short:.4f}°, dist_long: {dist_long:.4f}°') - if mode == 1: # positive rotation only, ie we want a positive distance - if dist_short >= 0: # ie we want this one - dist = dist_short - else: # ie we need to go the other way - dist = dist_long - elif mode == 2: # negative rotation only ie we want a positive distance - if dist_short > 0: # ie we need to go the other way - dist = dist_long - else: # ie we want this one - dist = dist_short - else: # mode = 0 ie we want the shortest distance either way - dist = dist_short - log.debug(f'Returning distance: {dist:.4f}°') - return radians(dist) - - -def calc_rotary_move_with_joint_limits(pos, trgt, max_limit, min_limit, mode): # expects radians - # this takes a target angle in [-pi,pi] and finds the closest move within [min_limit, max_limit] - # from a given position in [min_limit, max_limit], returns the optimized target angle and the distance - # from the given position to that target angle - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - log.debug(f' Current position: {degrees(pos):.4f}°, target position: {degrees(trgt):.4f}°') - # calculate the shortest distance from position to target for the strategy given by - # the operator (ie shortest (= default), positive rotation only, negative rotation only ) - dist = calc_shortest_distance(pos, trgt, mode) - # check that the result is within the rotary axis limits defined in the ini file - if dist >= 0: # shortest way is in the positive direction - if (pos + dist) <= max_limit: # if the limits allow we rotate the joint in the positive sense - log.debug(f' Max_limit OK, setting target to: {degrees(pos + dist):.4f}°') - theta = pos + dist - else: # if positive limits would be exceeded we need to go the longer way in the other direction - log.debug(f' Maximum axis limit of {degrees(max_limit):.4f} would be violated.') - if mode == 0: - dist = dist - 2*pi - log.debug(f' Changing target to: {degrees(trgt):.4f}°, distance to: {degrees(dist):.4f}°') - theta = trgt - else: # if the rotation direction was set by the operator then we can not change direction - log.debug(f' Unable to change direction because orient mode is set to {mode:.0f}.\n') - theta = None - else: # shortest way is in the negative direction - if (pos + dist) >= min_limit: # if the limits allow we rotate the joint in the negative sense - log.debug(f' Min_limit OK, setting target to: {degrees(pos + dist):.4f}°') - theta = pos + dist - else: # if negative limits would be exceeded we need to go the longer way int the other direction - log.debug(f' Minimum axis limit of {degrees(min_limit):.4f} would be violated.') - if mode == 0: - dist = dist + 2*pi - log.debug(f' Changing target to: {degrees(trgt):.4f}°, distance to: {degrees(dist):.4f}°') - theta = trgt - else: # if the rotation direction was set by the operator then we can not change direction - log.debug(f' Unable to change direction because orient mode is set to {mode:.0f}.\n') - theta = None - if theta is not None: - log.debug(f'Returning: angle {degrees(theta):.4f}° with distance {degrees(dist):.4f}° for requested mode {mode:.0f}\n') - # we also attach the distance for this particular move and mode - return theta, dist # returns radians - - -def calc_angle_pairs_and_distances(self, possible_prim_sec_angle_pairs): # expects radians - # this takes a list of joint angle pairs in [-pi,pi] and optimizes them for shortest moves - # in (min_limit, max_linit) from the current joint positions using the orient_mode set by - # the operator: 0=shortest (default), 1=positive rotation only, 2=negative rotation only - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global primary_min_limit, primary_max_limit, secondary_min_limit, secondary_max_limit - global orient_mode - # get the current joint positions - prim_pos, sec_pos = get_current_rotary_positions(self) # returns radians - # we want to return a list of angles that are optimized for the orient_mode and the - # rotary axes limits as set in the ini file - target_dist_list= [] - for prim_trgt, sec_trgt in possible_prim_sec_angle_pairs: - # For the priortized joint we apply the orient mode requested by the operator - # the other we optimize for shortest move - if optimization_priority == 2: - primary_strategy = 0 - secondary_strategy = orient_mode - else: - primary_strategy = orient_mode - secondary_strategy = 0 - # primary joint - prim_move, prim_dist = calc_rotary_move_with_joint_limits(prim_pos, prim_trgt, - primary_max_limit, primary_min_limit, - primary_strategy) - # secondary joint - sec_move, sec_dist = calc_rotary_move_with_joint_limits(sec_pos, sec_trgt, - secondary_max_limit, secondary_min_limit, - secondary_strategy) - # if a solution has been found for this particular pair then we add it to the list - if not (prim_move == None) and not (sec_move == None): - target_dist_list.append(((prim_move, sec_move),(prim_dist, sec_dist))) - for ((prim_move, sec_move),(prim_dist, sec_dist)) in target_dist_list: - debug_msg = (f'Returning prim_move: {degrees(prim_move):.4f}°, sec_move: {degrees(sec_move):.4f}°, ' - f'prim_dist: {degrees(prim_dist):.4f}°, sec_dist: {degrees(sec_dist):.4f}°') - log.debug(debug_msg) - return target_dist_list # returns radians - - -def calc_optimal_joint_move(self, possible_prim_sec_angle_pairs): - # find the optimal joint move from current to target positions in the list - # orient_mode is 0=shortest, 1=positive rotation only, 2=negative rotation only - # For orient_mode=(1,2): If no move can be found within joint limits we return None - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global orient_mode - # this returns a list with all moves ((prim_move, sec_move),(prim_dist, sec_dist)) that - # will result in correct tool orientation, stay within the rotary axis limits and respect the - # orient_mode if set by the operator - valid_joint_moves_and_distances = calc_angle_pairs_and_distances(self, possible_prim_sec_angle_pairs) - if len(valid_joint_moves_and_distances) < 1: - log.error(f' No valid joint moves found.') - return (None, None) - # now we need to pick and return the (primary angle, secondary angle) that results in the - # shortest move of the prioritized joint - (theta_1, theta_2) = (None, None) - joint = optimization_priority - 1 - dist = 10 # some large initial value - for trgt_angles, dists in valid_joint_moves_and_distances: - if orient_mode == 0 and fabs(dists[joint]) < fabs(dist): # shortest move requested - (theta_1, theta_2) = trgt_angles - dist = dists[0] - elif orient_mode == 1 and fabs(dists[joint]) < fabs(dist) and dists[joint] >= 0: # positive primary rotation only - (theta_1, theta_2) = trgt_angles - dist = dists[0] - elif orient_mode == 2 and fabs(dists[joint]) < fabs(dist) and dists[joint] <= 0: # negative primary rotation only - (theta_1, theta_2) = trgt_angles - dist = dists[0] - if theta_1 is not None: - debug_msg = (f'Returning shortest move selected for orient_mode {orient_mode:.0f}: ' - f'primary: {degrees(theta_1):.4f}°, secondary: {degrees(theta_2):.4f}°\n') - log.debug(debug_msg) - return theta_1, theta_2 # returns radians - - -def calc_virtual_rotation(theta_1, theta_2, x_vector_req, z_vector_req, matrix_in, direction): # expects radians - # calculates a required virtual-rotation around tool- or work-z so the x-vector matches the requested - # orientation after rotation - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - # tolerance setting for check if x-vector-vector needs to be rotated at all - epsilon = 0.00000001 - log.info(" x-vector-requested: %s", x_vector_req) - debug_msg = (f' got joint angles: primary {theta_1:.4f} {degrees(theta_1):.4f}°, ' - f'secondary {theta_2:.4f}° {degrees(theta_2):.4f}°') - log.debug(debug_msg) - # run matrix_in through the kinematic transformation in the requested direction - # using the given joint angles and zero virtual-rotation - try: - matrix_out = kins_calc_transformation_matrix(theta_1, theta_2, 0, matrix_in, direction) - except Exception as error: - log.error('calc_virtual_rotation, %s', error) - # the x-vector for the given machine joint rotations is found directly in the first column - x_vector_is = [matrix_out[0,0], matrix_out[1,0], matrix_out[2,0]] - log.debug(" X-vector after machine rotation would be: %s", x_vector_is) - # we calculate the angular difference between the two vectors so we can add a virtual rotation - # around z-vector or work-z to match the requested x orientation after machine rotation - # just to be sure we normalize the two vectors - x_vector_is = x_vector_is / np.linalg.norm(x_vector_is) - x_vector_req = x_vector_req / np.linalg.norm(x_vector_req) - # check if the x-vector is already in the required orientation (ie parallel) - log.debug(" checking if vectors are parallel: %s", np.dot(x_vector_is,x_vector_req)) - if np.dot(x_vector_is, x_vector_req) > 1 - epsilon: - log.info(" X-vector already oriented, setting virtual-rotation = 0") - # if we are already parallel then we don't need to add a virtual rotation - virtual_rot = 0 - else: - # we can use the cross product to determine the direction we need to rotate - cross = np.cross(x_vector_req, x_vector_is) - log.debug(" cross product (x_vector_req, x_vector_is): %s", cross) - virtual_rot = np.arccos(np.dot(x_vector_req, x_vector_is)) - log.debug(f' raw virtual_rot: {virtual_rot:.4f} {degrees(virtual_rot):.4f}°') - # To find out which quadrant we need the angle to be in we create a list of them all - virtual_rot_list = [virtual_rot, -virtual_rot, 2*pi-virtual_rot, -(2*pi-virtual_rot)] - log.debug(' Got possible virtual_rot angles: ' + ' '.join("{:.4f}°".format(degrees(angle)) for angle in virtual_rot_list)) - # then we run all of them through the kinematic model and see which gives us the requested x-vector-vector - for virtual_rot in virtual_rot_list: - log.debug(f' Checking virtual_rot = {degrees(virtual_rot):.4f}°') - zeta = 0.0001 - # run the identity matrix through the kinematic transformation in the requested direction - # using the given joint angles and virtual-rotation angle in the list - try: - matrix_out = kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, matrix_in, direction) - except Exception as error: - log.error('calc_virtual_rotation, %s', error) - # the oriented x-vector is found directly in the first column - x_vector_would_be = [matrix_out[0,0], matrix_out[1,0], matrix_out[2,0]] - log.debug(' x_vector_would_be: %s', x_vector_would_be) - # calculate the difference of the respective elements - x_vector_diff = x_vector_req - x_vector_would_be - # and check if all elements are within [-epsilon,epsilon] - match = np.all((x_vector_diff > -zeta) & (x_vector_diff < zeta)) - log.debug(' Is the X-vector close enough ? %s', match) - if match: - # if we have a match we leave the loop and use this angle - break - log.info(f'Returning virtual-rotation calculated {degrees(virtual_rot):.4f}°') - return virtual_rot # returns radians - - -def calc_twp_matrix_from_joint_position(self, matrix_in, virtual_rot, direction): # expects radians - # transforms a 4x4 input matrix using the current transformation matrix - # (forward or inverse) using the kinematic model of the machine - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global kins_virtual_rotation - # read current spindle rotary angles (radians) - theta_1, theta_2 = get_current_rotary_positions(self) - # virtual-rot is the virtual rotary axis around the z-vector or work-z axis to align the x-vector - log.debug(f" requested virtual-rot value {degrees(virtual_rot):.4f}°") - # run matrix_in through the kinematic transformation in the requested direction - # using the current joint angles and virtual-rotation as requested - try: - twp_matrix = kins_calc_transformation_matrix(theta_1, theta_2, virtual_rot, matrix_in, direction) - except Exception as error: - log.error('calc_twp_matrix_from_joint_position, %s', error) - return twp_matrix - - -def gui_update_twp(): - # The tilted-work-plane is created in identity mode and must NOT be updated after a switch - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global twp_matrix, saved_work_offset - # twp origin as vector (in world coords) from current work-offset to the origin of the twp - try: - hal.set_p("twp-helper-comp.twp-ox-in",str(twp_matrix[0,3])) - hal.set_p("twp-helper-comp.twp-oy-in",str(twp_matrix[1,3])) - hal.set_p("twp-helper-comp.twp-oz-in",str(twp_matrix[2,3])) - # twp x-vector - hal.set_p("twp-helper-comp.twp-xx-in",str(twp_matrix[0,0])) - hal.set_p("twp-helper-comp.twp-xy-in",str(twp_matrix[1,0])) - hal.set_p("twp-helper-comp.twp-xz-in",str(twp_matrix[2,0])) - # twp z-vector - hal.set_p("twp-helper-comp.twp-zx-in",str(twp_matrix[0,2])) - hal.set_p("twp-helper-comp.twp-zy-in",str(twp_matrix[1,2])) - hal.set_p("twp-helper-comp.twp-zz-in",str(twp_matrix[2,2])) - except Exception as error: - log.error('gui_update_twp failed, %s', error) - # publish the twp offset coordinates in world coordinates (ie identity) - [work_offset_x, work_offset_y, work_offset_z] = saved_work_offset - log.debug(" Setting work_offsets in the simulation: %s", (work_offset_x, work_offset_y, work_offset_z)) - # this is used to translate the rotated twp to the correct position - # care must be taken that only the work_offsets in identity mode are sent as that is - # what the model uses. The visuals for the offsets are created in the origin, - # then rotated according to the rotary joint position and then translated. - # The twp has to be rotated out of the machine xy plane using the g68.2 parameters and is then - # translated by the offset values of the identity mode. - try: - hal.set_p("twp-helper-comp.twp-ox-world-in",str(work_offset_x)) - hal.set_p("twp-helper-comp.twp-oy-world-in",str(work_offset_y)) - hal.set_p("twp-helper-comp.twp-oz-world-in",str(work_offset_z)) - except Exception as error: - log.error('gui_update_twp failed, %s', error) - - -# NOTE: Due to easier abort handling we currently restrict the use of twp to G54 -# as LinuxCNC seems to revert to G54 as the default system -def get_current_work_offset(self): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - # get which offset is active (g54=1 .. g59.3=9) - active_offset = int(self.params[5220]) - current_work_offset_number = active_offset - # set the relevant parameter numbers that hold the active offset values - # (G54_x: #5221, G55_x:#[5221+20], G56_x:#[5221+40] ....) - work_offset_x = (active_offset-1)*20 + 5221 - work_offset_y = work_offset_x + 1 - work_offset_z = work_offset_x + 2 - co_x = self.params[work_offset_x] - co_y = self.params[work_offset_y] - co_z = self.params[work_offset_z] - current_work_offset = (co_x, co_y, co_z) - return [current_work_offset_number, current_work_offset] - - -def get_current_rotary_positions(self): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global joint_letter_primary, joint_letter_secondary - if joint_letter_primary == 'A': - theta_1 = radians(self.AA_current) - elif joint_letter_primary == 'B': - theta_1 = radians(self.BB_current) - elif joint_letter_primary == 'C': - theta_1 = radians(self.CC_current) - log.debug(f' Current position Primary joint: {degrees(theta_1):.4f}°') - # read current spindle rotary angles and convert to radians - if joint_letter_secondary == 'A': - theta_2 = radians(self.AA_current) - elif joint_letter_secondary == 'B': - theta_2 = radians(self.BB_current) - elif joint_letter_secondary == 'C': - theta_2 = radians(self.CC_current) - log.debug(f' Current position Secondary joint: {degrees(theta_2):.4f}°') - return theta_1, theta_2 - - -def reset_twp_params(): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global virtual_rot, twp_matrix, twp_flag, twp_build_params - virtual_rot = 0 - # we must not change tool kins parameters when TOOL kins are active or we get sudden joint position changes - # ie don't do this: kins_comp_set_virtual_rot(0)! - twp_flag = [] - twp_build_params = {} - log.info(" Resetting TWP-matrix") - twp_matrix = np.asmatrix(np.identity(4)) - - -def g53n_core(self): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - # Orient the tool to the current twp (with TCP for G53.1 or IDENTITY for G53.6) - # Note: To avoid that this python code is run prematurely by the read ahead we need a quebuster at the - # beginning but because we need self.execute() to switch the WCS properly this remap needs to be called from - # an ngc reamp that contains a quebuster before calling this code. - # IMPORTANT: - # The correct kinematic mode (ie TCP for 53.1 / IDENTITY for G53.6) must be active when this code is called - # (ie do it in the ngc remap mentioned above!) - global saved_work_offset, twp_matrix, twp_flag, virtual_rot - global joint_letter_primary, joint_letter_secondary, twp_error_status - global orient_mode - if self.task == 0: # ignore the preview interpreter - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - if not hal.get_value(twp_is_defined): - # reset the twp parameters - reset_twp_params() - msg = "G53.n: No TWP defined." - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - elif hal.get_value(twp_is_active): - # reset the twp parameters - reset_twp_params() - msg = "G53.n: TWP already active" - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # Check if any words have been passed with the respective G53.n command - c = self.blocks[self.remap_level] - p = c.p_number if c.p_flag else 0 - x = c.i_number if c.i_flag else None - y = c.j_number if c.j_flag else None - z = c.k_number if c.k_flag else None - log.debug(' G53.n Words passed: (P, X,Y,Z): %s', (p,x,y,z)) - - if p not in [0,1,2]: - # reset the twp parameters - reset_twp_params() - msg = "G53.n : unrecognised P-Word found." - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - orient_mode = p - z_vector_requested = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] - x_vector_requested = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] - # calculate all possible pairs of (primary, secondary) angles to matches the requested orientation - try: - # angles are returned in [-pi,pi] - possible_prim_sec_angle_pairs = calc_joint_angles(z_vector_requested, x_vector_requested) # returns radians - except Exception as error: - log.error('calc_joint_angles, %s', error) - # reset the twp parameters - reset_twp_params() - msg = ("G53.n ERROR: Calculation of joint angles has failed. -> aborting G53.n") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - if possible_prim_sec_angle_pairs == []: - # reset the twp parameters - log.error('G53.n: No possible primary/secondary angle pairs found.') - reset_twp_params() - msg = "G53.n ERROR: Requested tool orientation not reachable -> aborting G53.n" - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # this returns one pair of optimized angles in degrees, or (None, None) if no solution could be found - try: - theta_1, theta_2 = calc_optimal_joint_move(self, possible_prim_sec_angle_pairs) # returns radians - except Exception as error: - log.error('G53.n: Calculation of optimal joint move failed, %s', error) - if theta_1 == None or theta_2 == None: - # reset the twp parameters - reset_twp_params() - msg = ("G53.n ERROR: Requested tool orientation not reachable -> aborting G53.n") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # get the particular conditions to be met for the kinematic at hand - try: - (x_vector_requested, z_vector_requested, matrix_in, direction) = kins_calc_virtual_rot_get_values(x_vector_requested, - z_vector_requested, - twp_matrix) - except Exception as error: - log.error('G53.n: kins_calc_virtual_rot_get_values failed, %s', error) - # calculate the virtual-rotation needed - virtual_rot = calc_virtual_rotation(theta_1, - theta_2, - x_vector_requested, - z_vector_requested, - matrix_in, - direction) # returns radians - log.debug(f" Calculated virtual-rotation to match requested x-vector: {degrees(virtual_rot):.4f}°") - - # mark twp-flag as active - twp_flag = [0, 'active'] - gui_update_twp() - - # set the virtual-rotation value in the kinematic component - debug_msg = (f' G53.n: Setting angle values in kins comp to theta1: {degrees(theta_1):.4f}°, ' - f'theta2: {degrees(theta_2):.4f}°, virtual_rot: {degrees(virtual_rot):.4f}°') - log.debug(debug_msg) - try: - kins_set_values(theta_1, theta_2, virtual_rot) - except Exception as error: - log.error('G53.n: kins_set_values failed, %s', error) - - # calculate the work offset in transformed-coordinatess - log.debug(" G53.n: Saved work offset: %s", saved_work_offset) - twp_offset = (twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]) - try: - new_offset = kins_calc_transformed_work_offset(saved_work_offset, twp_offset, theta_1, theta_2, virtual_rot) - except Exception as error: - log.error('G53.n: Calculation of kins_calc_transformed_work_offset failed, %s', error) - debug_msg = (f' G53.n: Setting transformed work-offsets for twp-kins in G59, G59.1, ' - f'G59.2 and G59.3 to: {new_offset[0]:.4f}, {new_offset[1]:.4f}, {new_offset[2]:.4f}') - log.debug(debug_msg) - # set the dedicated TWP work offset values (G53, G53.1, G53.2, G53.3) - self.execute("G10 L2 P6 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) - self.execute("G10 L2 P7 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) - self.execute("G10 L2 P8 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) - self.execute("G10 L2 P9 X%f Y%f Z%f" % (new_offset[0], new_offset[1], new_offset[2]), lineno()) - - log.debug(f" G53.n: Moving primary joint to {degrees(theta_1):.4f}° and secondary joint to {degrees(theta_2):.4f}° ") - if (x,y,z) == (None,None,None): - # Move rotary joints to align the tool and the requested work plane - self.execute("G0 %s%f %s%f" % (joint_letter_primary, degrees(theta_1), joint_letter_secondary, degrees(theta_2)), lineno()) - # switch to the dedicated TWP work offsets - self.execute("G59", lineno()) - # activate TWP kinematics - self.execute("G12.1 P2") - if (x,y,z) != (None,None,None): - log.debug(' G53.3 called') - self.execute("G0 X%s Y%s Z%s %s%f %s%f" % - (x, y, z, joint_letter_primary, degrees(theta_1), joint_letter_secondary, degrees(theta_2)), lineno()) - # set twp-state to 'active' (2) - self.execute("M68 E2 Q2") - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - -# Cancel an active TWP definition and reset the parameters to zero -# Note: To avoid that this python code is run prematurely by the read ahead we need a quebuster at the beginning but -# because we need self.execute() to switch the WCS properly this remap needs to be called from -# an ngc that contains a quebuster before calling this code -def g69_core(self): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global twp_flag, saved_work_offset_number, saved_work_offset - if self.task == 0: # ignore the preview interpreter - yield INTERP_EXECUTE_FINISH - return INTERP_OK - log.info('G69 called') - # reset the twp parameters - reset_twp_params() - gui_update_twp() - # set twp-state to 'undefined' (0) - self.execute("M68 E2 Q0") - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - -# define a virtual tilted-work-plane (twp) that is perpendicular to the current tool-orientation -def g683(self, **words): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global twp_matrix, virtual_rot, twp_flag, saved_work_offset_number, saved_work_offset - - if self.task == 0: # ignore the preview interpreter - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - # ! IMPORTANT ! - # We need to use 'yield INTERP_EXECUTE_FINISH' here to stop the read ahead - # and avoid it executing the rest of the remap ahead of time - ## NOTE: No 'self.execute(..)' command can be used after 'yield INTERP_EXECUTE_FINISH' - yield INTERP_EXECUTE_FINISH - - if hal.get_value(twp_is_defined): - # reset the twp parameters - reset_twp_params() - msg =("G68.3 ERROR: TWP already defined.") - log.debug(msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # NOTE: Due to easier abort handling we currently restrict the use of twp to G54 - # as LinuxCNC seems to revert to G54 as the default system - # get which offset is active (g54=1 .. g59.3=9) - (n, offsets) = get_current_work_offset(self) - if n != 1: - # reset the twp parameters - reset_twp_params() - msg = "G68.3 ERROR: Must be in G54 to define TWP." - log.debug(msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - c = self.blocks[self.remap_level] - # parse the requested origin - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested rotation of x-vector around the origin - r = radians(c.r_number) if c.r_flag else 0 - - twp_flag = [0, 1, 'empty'] # one call to define the twp in this mode - theta_1, theta_2 = get_current_rotary_positions(self) # radians - # calculate virtual rotation to have the oriented x-vector in the direction required for the kinematic at hand - try: - virtual_rot = kins_calc_virtual_rot_for_g683(theta_1, theta_2 ) - except Exception as error: - log.error('remap_func: kins_calc_virtual_rot_for_g683 failed, %s', error) - log.info("G68.3: virtual-Rotation calculated for x-vector in machine-xy plane [deg]: %s", degrees(virtual_rot)) - # then we need to calculate the transformation matrix of the current orientation with the including the - # calculated virtual-rotation. - # for this we take the 4x4 identity matrix and pass it through the kinematic transformation using the - # current rotary joint positions and the calculated virtual-rotation angle plus any additional angle - # passed in the R word of the G68.3 command - start_matrix = np.asmatrix(np.identity(4)) - log.info('G68.3: Requested R-word rotation [deg]: %s', degrees(r)) - # the required transformation direction may depend on the kinematic at hand - try: - direction = kins_calc_transformation_get_direction() - except Exception as error: - log.error('kins_calc_transformation_get_direction, %s', error) - twp_matrix = calc_twp_matrix_from_joint_position(self, start_matrix, virtual_rot + r, direction) - log.debug("G68.3: TWP matrix with oriented x-vector: \n%s", twp_matrix) - # put the requested origin into the twp_matrix - (twp_matrix[0,3], twp_matrix[1,3], twp_matrix[2,3]) = (x, y, z) - # update the build state of the twp call - twp_flag[2] = 'done' - log.info("G68.3: Built twp-transformation-matrix: \n%s", twp_matrix) - # collect the currently active work offset values (ie g54, g55 or other) - saved_work_offset = offsets - saved_work_offset_number = n - log.debug("G68.3: Saved work offsets: %s", (n, saved_work_offset)) - # set twp-state to 'defined' (1) - self.execute("M68 E2 Q1") - yield INTERP_EXECUTE_FINISH - - gui_update_twp() - return INTERP_OK - - -# definition of a virtual work-plane (twp) using different methods set by the 'p'-word -def g682(self, **words): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global twp_matrix, virtual_rot, twp_flag, twp_build_params, saved_work_offset_number, saved_work_offset - - if self.task == 0: # ignore the preview interpreter - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - # ! IMPORTANT ! - # We need to use 'yield INTERP_EXECUTE_FINISH' here to stop the read ahead - # and avoid it executing the rest of the remap ahead of time - ## NOTE: No 'self.execute(..)' command can be used after 'yield INTERP_EXECUTE_FINISH' - yield INTERP_EXECUTE_FINISH - - if hal.get_value(twp_is_defined): # ie TWP has already been defined - # reset the twp parameters - reset_twp_params() - msg = ("G68.2: TWP already defined.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # NOTE: Due to easier abort handling we currently restrict the use of twp to G54 - # as LinuxCNC seems to revert to G54 as the default system - (n, offsets) = get_current_work_offset(self) - if n != 1: - # reset the twp parameters - reset_twp_params() - msg = "G68.2 ERROR: Must be in G54 to define TWP." - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # collect the currently active work offset values (ie g54, g55 or other) - saved_work_offset_number = n - saved_work_offset = offsets - log.debug(" G68.2: Saved work offsets %s", (n, saved_work_offset)) - - c = self.blocks[self.remap_level] - p = c.p_number if c.p_flag else 0 - if p == 0: # true euler angles (this is the default mode) - twp_flag = [int(p), 1, 'empty'] # one call to define the twp in this mode - # parse requested order of rotations (default is '313' ie: ZXZ) - q = str(int(c.q_number if c.q_flag else 313)) - if q not in ['121','131','212','232','313','323']: - # reset the twp parameters - reset_twp_params() - msg = ("G68.2 (P0): No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # parse the requested origin - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - # parse the requested euler rotation angles - th1 = radians(c.i_number) if c.i_flag else 0 - th2 = radians(c.j_number) if c.j_flag else 0 - th3 = radians(c.k_number) if c.k_flag else 0 - - # build the translation vector of the twp_matrix - twp_origin = [[x], [y], [z]] - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.2 (P0): Twp_origin_rotation \n%s',twp_origin_rotation) - # build the rotation matrix for the requested euler rotation - twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) - log.debug(' G68.2 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) - # calculate the total twp_rotation using matrix multiplication - 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() - msg = ("G68.2 P1: No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # parse the requested origin - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - # parse the requested euler rotation angles - th1 = radians(c.i_number) if c.i_flag else 0 - th2 = radians(c.j_number) if c.j_flag else 0 - th3 = radians(c.k_number) if c.k_flag else 0 - - # build the translation vector of the twp_matrix - twp_origin = [[x], [y], [z]] - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.2 P1: Twp_origin_rotation \n%s',twp_origin_rotation) - # build the rotation matrix for the requested euler rotation - twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) - log.debug(' G68.2 P1: Twp_euler_rotation \n%s',twp_euler_rotation) - # calculate the total twp_rotation using matrix multiplication - 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 - # TODO implement operator errors as outlined in the twp README - #- G68.2 P2 (Q0),Q1,Q2,Q3 commands are not entered consecutively - #- two to the points entered in Q1,Q2,Q3 are identical - #- all three points entered in Q1,Q2,Q3 are on a line - #- the distance between a line defined by any two points entered in (Q1,Q2,Q3) and - #the remaining point is less than 10mm or 0.5inch (just some arbitrary values for now) - - # if this is the first call for this mode reset the twp_flag flag - if not twp_flag: - twp_flag = [int(p), 4 , 'empty', 'empty', 'empty', 'empty'] # four calls needed - twp_build_params = {'q0':[], 'q1':[], 'q2':[], 'q3':[]} - # Point 1: defines the origin of the twp - # Point 2: direction from P1 to P2 defines the positive x direction on the twp (x-vector) - # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (z-vector) - q = int(c.q_number if c.q_flag else 0) - # this mode needs four calls to fill all required parameters - if q == 0: # define new origin and rotation - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - twp_build_params['q0'] = [x,y,z,r] - twp_flag[2] = 'done' - elif q == 1: # define point 1 - 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() - 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 z-vector vector - # note: if P3 is on the right side of the vector P1->P2 - # then the z-vector will be below the twp (ie z-vector will be downwards) - twp_vect_z = np.cross(twp_vect_x , v2) - log.debug(" G68.2 P2: Twp_vect_z %s",twp_vect_z) - # we can use the cross product to calculate the y vector - twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug(" G68.2 P2: Twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated vectors - # first stack the vectors (lists) and then flip diagonally (transpose) - # so the vectors are now vertical - twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) - twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) - twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug(" G68.2 P2: Built the twp-rotation-matrix: \n%s", twp_vect_rotation) - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.2 P2: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) - # calculate the total twp_rotation using matrix multiplication - 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 x-vector and vector 2 defines the z-vector) - # TODO implement operator errors as outlined in the twp README - #- G68.2 P3 Q1 and Q2 commands are not entered consecutively - #- one of the vectors is the zero vector - #- the enclosed angle between the 1. and 2. vector is <85° or >95° (re fanuc twp pdf) - q = int(c.q_number if c.q_flag else 0) - # if this is the first call for this mode reset the twp_flag flag - if not twp_flag: - log.info(' first call') - twp_flag = [int(p), 2 , 'empty', 'empty'] # two calls needed - twp_build_params = {'q0':[], 'q1':[]} - 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 xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - # first vector (direction of x in the twp) - i = c.i_number if c.i_flag else 0 - j = c.j_number if c.j_flag else 0 - k = c.k_number if c.k_flag else 0 - twp_build_params['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() - 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 x-vector direction - twp_vect_x = [i-x, j-y, k-z] - twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) - twp_vect_z = [i1, j1, k1] - twp_vect_z = twp_vect_z / np.linalg.norm(twp_vect_z) - orth = np.dot(twp_vect_x, twp_vect_z) - log.debug(" orth check: %s", orth) - # the two vectors must be orthogonal - if orth > 0.001: - reset_twp_params() - 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 y vector - twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug(" G68.2 P3: twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated vectors - # first stack the vectors (lists) and then flip diagonally (transpose) - # so the vectors are now vertical - twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) - twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) - twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug(" G68.2 P3: Built twp-rotation-matrix: \n%s", twp_vect_rotation) - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.2 P3: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) - # calculate the total twp_rotation using matrix multiplication - 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) - - # TODO implement G68.2 P4 as outlined in the fanuc twp pdf (the exact meaning of which is unclear to me) - - else: - # reset the twp parameters - reset_twp_params() - msg = ("G68.2: No recognised P-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - log.debug(" G68.2: twp_flag: %s", twp_flag) - log.debug(" G68.2: calls required: %s", twp_flag.count('done')) - log.debug(" G68.2: number of calls made: %s", twp_flag.count('done')) - - if twp_flag.count('done') == twp_flag[1]: - log.info(' G68.2: requested rotation (degrees): %s', degrees(r)) - log.info(" G68.2: twp-tranformation-matrix: \n%s",twp_matrix) - twp_origin = [twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]] - log.info(" G68.2: twp origin: %s", twp_origin) - twp_vect_x = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] - log.info(" G68.2: twp vector-x: %s", twp_vect_x) - twp_vect_z = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] - log.info(" G68.2: twp vector-z: %s", twp_vect_z) - # set twp-state to 'defined' (1) - self.execute("M68 E2 Q1") - yield INTERP_EXECUTE_FINISH - - gui_update_twp() - return INTERP_OK - - -# incremental definition of a virtual work-plane (twp) using different methods set by the 'p'-word -def g684(self, **words): - log.debug('Entering: %s', sys._getframe( ).f_code.co_name) - global twp_matrix, virtual_rot, twp_flag, twp_build_params, saved_work_offset_number, saved_work_offset - - if self.task == 0: # ignore the preview interpreter - yield INTERP_EXECUTE_FINISH - return INTERP_OK - - # ! IMPORTANT ! - # We need to use 'yield INTERP_EXECUTE_FINISH' here to stop the read ahead - # and avoid it executing the rest of the remap ahead of time - ## NOTE: No 'self.execute(..)' command can be used after 'yield INTERP_EXECUTE_FINISH' - yield INTERP_EXECUTE_FINISH - - if not hal.get_value(twp_is_active): # ie there is currently no TWP defined - # reset the twp parameters - reset_twp_params() - msg = ("G68.4: No TWP active to increment from. Run G68.2 or G68.3 first.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # collect the currently active work offset values (ie g54, g55 or other) - n = get_current_work_offset(self)[0] - # Must be in one of the dedicated offset systems for TWP - if False: #n < 6: - # reset the twp parameters - reset_twp_params() - msg = ("G68.4 ERROR: Must be in G59, G59.x to increment TWP.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # store the current TWP to - twp_matrix_current = np.matrix.copy(twp_matrix) - c = self.blocks[self.remap_level] - p = c.p_number if c.p_flag else 0 - - if p == 0: # true euler angles (this is the default mode) - twp_flag = [int(p), 1, 'empty'] # one call to define the twp in this mode - # parse requested order of rotations (default is '313' ie: ZXZ) - q = str(int(c.q_number if c.q_flag else 313)) - - if q not in ['121','131','212','232','313','323']: - # reset the twp parameters - reset_twp_params() - msg = ("G68.4 (P0): No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # parse the requested origin - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - # parse the requested euler rotation angles - th1 = radians(c.i_number) if c.i_flag else 0 - th2 = radians(c.j_number) if c.j_flag else 0 - th3 = radians(c.k_number) if c.k_flag else 0 - - # build the translation vector of the twp_matrix - twp_origin = [[x], [y], [z]] - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.4 (P0): Twp_origin_rotation \n%s',twp_origin_rotation) - # build the rotation matrix for the requested euler rotation - twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) - log.debug(' G68.4 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) - # calculate the total twp_rotation using matrix multiplication - 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() - msg = ("G68.4 P1: No recognised Q-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - # parse the requested origin - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - # parse the requested euler rotation angles - th1 = radians(c.i_number) if c.i_flag else 0 - th2 = radians(c.j_number) if c.j_flag else 0 - th3 = radians(c.k_number) if c.k_flag else 0 - - # build the translation vector of the twp_matrix - twp_origin = [[x], [y], [z]] - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.4 P1: Twp_origin_rotation \n%s',twp_origin_rotation) - # build the rotation matrix for the requested euler rotation - twp_euler_rotation = calc_euler_rot_matrix(th1, th2, th3, q) - log.debug(' G68.4 P1: Twp_euler_rotation \n%s',twp_euler_rotation) - # calculate the total twp_rotation using matrix multiplication - 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 - # TODO implement operator errors as outlined in the twp README - #- G68.2 P2 (Q0),Q1,Q2,Q3 commands are not entered consecutively - #- two to the points entered in Q1,Q2,Q3 are identical - #- all three points entered in Q1,Q2,Q3 are on a line - #- the distance between a line defined by any two points entered in (Q1,Q2,Q3) and - #the remaining point is less than 10mm or 0.5inch (just some arbitrary values for now) - - # if this is the first call for this mode reset the twp_flag flag - if not twp_flag: - twp_flag = [int(p), 4 , 'empty', 'empty', 'empty', 'empty'] # four calls needed - twp_build_params = {'q0':[], 'q1':[], 'q2':[], 'q3':[]} - # Point 1: defines the origin of the twp - # Point 2: direction from P1 to P2 defines the positive x direction on the twp (x-vector) - # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (z-vector) - q = int(c.q_number if c.q_flag else 0) - # this mode needs four calls to fill all required parameters - if q == 0: # define new origin and rotation - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - twp_build_params['q0'] = [x,y,z,r] - twp_flag[2] = 'done' - elif q == 1: # define point 1 - 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() - 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 z-vector vector - # note: if P3 is on the right side of the vector P1->P2 - # then the z-vector will be below the twp (ie z-vector will be downwards) - twp_vect_z = np.cross(twp_vect_x , v2) - log.debug(" G68.4 P2: Twp_vect_z %s",twp_vect_z) - # we can use the cross product to calculate the y vector - twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug(" G68.4 P2: Twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated vectors - # first stack the vectors (lists) and then flip diagonally (transpose) - # so the vectors are now vertical - twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) - twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) - twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug(" G68.4 P2: Built the twp-rotation-matrix: \n%s", twp_vect_rotation) - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.4 P2: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) - # calculate the total twp_rotation using matrix multiplication - 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 x-vector and vector 2 defines the z-vector) - # TODO implement operator errors as outlined in the twp README - #- G68.2 P3 Q1 and Q2 commands are not entered consecutively - #- one of the vectors is the zero vector - #- the enclosed angle between the 1. and 2. vector is <85° or >95° (re fanuc twp pdf) - q = int(c.q_number if c.q_flag else 0) - # if this is the first call for this mode reset the twp_flag flag - if not twp_flag: - twp_flag = [int(p), 2 , 'empty', 'empty'] # two calls needed - twp_build_params = {'q0':[], 'q1':[]} - if q == 0: # define new origin and first vector (direction of x in the twp) - x = c.x_number if c.x_flag else 0 - y = c.y_number if c.y_flag else 0 - z = c.z_number if c.z_flag else 0 - # parse the requested xy-rotation around the origin - r = radians(c.r_number) if c.r_flag else 0 - # first vector (direction of x in the twp) - i = c.i_number if c.i_flag else 0 - j = c.j_number if c.j_flag else 0 - k = c.k_number if c.k_flag else 0 - twp_build_params['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() - 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 x-vector direction - twp_vect_x = [i-x, j-y, k-z] - twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) - twp_vect_z = [i1, j1, k1] - twp_vect_z = twp_vect_z / np.linalg.norm(twp_vect_z) - orth = np.dot(twp_vect_x, twp_vect_z) - log.debug(" orth check: %s", orth) - # the two vectors must be orthogonal - if orth != 0: - # reset the twp parameters - reset_twp_params() - ## reset the parameter values - #twp_flag = [int(p), 2 , 'empty', 'empty'] # two calls needed - #twp_build_params = {'q0':[], 'q1':[]} - msg = ("G68.4 P3: Vectors are not orthogonal.") - log.debug(' ' + msg) - 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 y vector - twp_vect_y = np.cross(twp_vect_z, twp_vect_x) - log.debug(" G68.4 P3: twp_vect_y %s",twp_vect_y) - # build the rotation matrix of the twp_matrix from the calculated vectors - # first stack the vectors (lists) and then flip diagonally (transpose) - # so the vectors are now vertical - twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) - twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) - twp_vect_rotation = np.transpose(twp_vect_rotation_t) - log.debug(" G68.4 P3: Built twp-rotation-matrix: \n%s", twp_vect_rotation) - # create the rotation matrix for the requested origin rotation - try: - twp_origin_rotation = kins_calc_twp_origin_rot_matrix(r) - except Exception as error: - log.error('remap_func: kins_calc_twp_origin_rot_matrix failed, %s', error) - log.debug(' G68.4 P3: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) - # calculate the total twp_rotation using matrix multiplication - 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) - - # TODO implement G68.4 P4 as outlined in the fanuc twp pdf (the exact meaning of which is unclear to me) - - else: - # reset the twp parameters - reset_twp_params() - msg = ("G68.4: No recognised P-Word found.") - log.debug(' ' + msg) - emccanon.CANON_ERROR(msg) - yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed - yield INTERP_EXIT # w/o this the error does not abort a running gcode program - return INTERP_ERROR - - log.debug(" G68.4: twp_flag: %s", twp_flag) - log.debug(" G68.4: calls required: %s", twp_flag.count('done')) - log.debug(" G68.4: number of calls made: %s", twp_flag.count('done')) - - if twp_flag.count('done') == twp_flag[1]: - log.info(' G68.4: requested rotation (degrees) %s', degrees(r)) - log.info(" G68.4: twp_matrix_current: \n%s", twp_matrix_current) - log.info(" G68.4: incremental twp_matrix requested: \n%s",twp_matrix) - log.info(" G68.4: calculating new twp_matrix...") - twp_matrix_new = twp_matrix_current * twp_matrix - log.info(" G68.4: twp_matrix_new: \n%s",twp_matrix_new) - twp_origin = [twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]] - log.info(" G68.4: twp origin: %s", twp_origin) - twp_vect_x = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] - log.info(" G68.4: twp vector-x: %s", twp_vect_x) - twp_vect_z = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] - log.info(" G68.4: twp vector-z: %s", twp_vect_z) - log.info(" G68.4: incremented twp_matrix: \n%s", twp_matrix_new) - twp_matrix = twp_matrix_new - # set twp-state to 'defined' (1) - self.execute("M68 E2 Q1") - yield INTERP_EXECUTE_FINISH - - gui_update_twp() - return INTERP_OK diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/toplevel.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/toplevel.py deleted file mode 100755 index c7ce432a045..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/toplevel.py +++ /dev/null @@ -1,20 +0,0 @@ -# This is a component of LinuxCNC -# Copyright 2011, 2012, 2013 Dewey Garrett , -# Michael Haberler -# -# This program is free software; you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation; either version 2 of the License, or -# (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. -# -import remap - diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/twp-helper-comp.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/twp-helper-comp.py index 44748312c4e..ee6994e7196 100755 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/twp-helper-comp.py +++ b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/twp-helper-comp.py @@ -1,87 +1,47 @@ #!/usr/bin/env python3 +# Publishes the tilted work plane for the vismach model: the origin and the +# X and Z directions in the coordinate system the plane was defined in, +# read from status where the interpreter keeps them (G68.2, G68.3, G68.4, +# G69), and the active work offset the model translates the plane by. import hal import linuxcnc +import time h = hal.component("twp-helper-comp") -# this pin reflects the machine.analog pin used for the -# twp-status -h.newpin("twp-status", hal.Type.REAL, hal.Dir.IN) -# these pins are created here from 'twp-status'' -h.newpin("twp-is-redefined", hal.Type.BOOL, hal.Dir.OUT) +h.newpin("twp-status", hal.Type.REAL, hal.Dir.OUT) # 0 undefined, 1 defined h.newpin("twp-is-defined", hal.Type.BOOL, hal.Dir.OUT) h.newpin("twp-is-active", hal.Type.BOOL, hal.Dir.OUT) -# twp origin vector -h.newpin("twp-ox-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-oy-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-oz-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-ox", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-oy", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-oz", hal.Type.REAL, hal.Dir.OUT) -# twp x-orientation vector -h.newpin("twp-xx-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-xy-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-xz-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-xx", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-xy", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-xz", hal.Type.REAL, hal.Dir.OUT) -# twp z-orientation vector -h.newpin("twp-zx-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-zy-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-zz-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-zx", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-zy", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-zz", hal.Type.REAL, hal.Dir.OUT) -# twp origin vector in machine coordinate system -h.newpin("twp-ox-world-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-oy-world-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-oz-world-in", hal.Type.REAL, hal.Dir.IN) -h.newpin("twp-ox-world", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-oy-world", hal.Type.REAL, hal.Dir.OUT) -h.newpin("twp-oz-world", hal.Type.REAL, hal.Dir.OUT) +for name in ("twp-ox", "twp-oy", "twp-oz", + "twp-xx", "twp-xy", "twp-xz", + "twp-zx", "twp-zy", "twp-zz", + "twp-ox-world", "twp-oy-world", "twp-oz-world"): + h.newpin(name, hal.Type.REAL, hal.Dir.OUT) h.ready() -# create a connection to the status channel s = linuxcnc.stat() try: while 1: - # publish twp-status - if h['twp-status'] == 1: - h['twp-is-defined'] = 1 - h['twp-is-active'] = 0 - elif h['twp-status'] == 2: - h['twp-is-defined'] = 1 - h['twp-is-active'] = 1 - else: - h['twp-is-defined'] = 0 - h['twp-is-active'] = 0 - - # passthrough the twp arguments - h['twp-ox'] = h['twp-ox-in'] - h['twp-oy'] = h['twp-oy-in'] - h['twp-oz'] = h['twp-oz-in'] - h['twp-xx'] = h['twp-xx-in'] - h['twp-xy'] = h['twp-xy-in'] - h['twp-xz'] = h['twp-xz-in'] - h['twp-zx'] = h['twp-zx-in'] - h['twp-zy'] = h['twp-zy-in'] - h['twp-zz'] = h['twp-zz-in'] - - # we only want to expose offsets when twp is not defined - if not h['twp-is-defined']: - s.poll() # get current values - g5x_offset = s.g5x_offset - h['twp-ox-world'] = g5x_offset[0] - h['twp-oy-world'] = g5x_offset[1] - h['twp-oz-world'] = g5x_offset[2] - else : # use the values from the remap - h['twp-ox-world'] = h['twp-ox-world-in'] - h['twp-oy-world'] = h['twp-oy-world-in'] - h['twp-oz-world'] = h['twp-oz-world-in'] + s.poll() + active = 1 if s.g68_active else 0 + h['twp-status'] = active + h['twp-is-defined'] = active + h['twp-is-active'] = active + + o = s.g68_offset + r = s.g68_rotation + h['twp-ox'], h['twp-oy'], h['twp-oz'] = o[0], o[1], o[2] + # columns of the rotation: the plane's X and Z + h['twp-xx'], h['twp-xy'], h['twp-xz'] = r[0], r[3], r[6] + h['twp-zx'], h['twp-zy'], h['twp-zz'] = r[2], r[5], r[8] + + g5x = s.g5x_offset + h['twp-ox-world'], h['twp-oy-world'], h['twp-oz-world'] = g5x[0], g5x[1], g5x[2] + time.sleep(0.05) except KeyboardInterrupt: raise SystemExit diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/util.py b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/util.py deleted file mode 100755 index 59b012058ad..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/python/util.py +++ /dev/null @@ -1,67 +0,0 @@ -# This is a component of LinuxCNC -# Copyright 2011, 2013 Dewey Garrett , Michael -# Haberler -# -# This program is free software; you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation; either version 2 of the License, or -# (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. -# -import inspect -import emccanon - -# O-word procedure to trap into the Pydevd debugger -# start debug server in Eclipse, then -# call as 'O call' from MDI - -# example setup for debugging embedded Python code -# see http://pydev.org/manual_adv_remote_debugger.html -# if this points to a valid directory, - -def call_pydevd(): - """ trap into the pydevd debugger""" - - import os,sys - - pydevdir= '/home/mah/.eclipse/org.eclipse.platform_3.5.0_155965261/plugins/org.python.pydev.debug_2.0.0.2011040403/pysrc/' - - # the 'emctask' module is present only in the milltask instance, otherwise both the UI and - # milltask would try to connect to the debug server. - - if os.path.isdir(pydevdir) and 'emctask' in sys.builtin_module_names: - sys.path.append(pydevdir) - sys.path.insert(0,pydevdir) - try: - import pydevd - emccanon.MESSAGE("pydevd imported, connecting to Eclipse debug server...") - pydevd.settrace() - except: - emccanon.MESSAGE("no pydevd module found") - pass - - - -def lineno(): - """ return line number in the current Python script """ - return inspect.currentframe().f_back.f_lineno - -def error_stack(self): - """ print the Interpreters error stack (function names) """ - print("error stack level=%d" % (self.stack_index)) - for s in self.stack(): - print("--'%s'" % (s)) - -def callstack(self): - """ print the O-Word call stack """ - for i in range(self.call_level): - c = self.sub_context[i] - print("%d: pos=%d seq=%d filename=%s sub=%s" % (i,c.position, c.sequence_number,c.filename,c.subname)) diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g531remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g531remap.ngc deleted file mode 100755 index 4b2fd293c61..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g531remap.ngc +++ /dev/null @@ -1,14 +0,0 @@ -; this is the wrapper remap to orient the spindle using IDENTITY kinematics G53.1 - -osub -M66 L0 E0 ;force sync, stop read ahead -o100 if [EXISTS [#

]] -o100 else - #

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

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

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

) -o105 else - #

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

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

]] -o100 else - #

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

;orient the spindle with P word -M66 L0 E0 -oendsub -m2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g69remap.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g69remap.ngc deleted file mode 100755 index 9efd1b7db29..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/g69remap.ngc +++ /dev/null @@ -1,11 +0,0 @@ -; this is the wrapper remap to cancel TWP - -osub -M66 L0 E0 ; force sync, stop read ahead -M469 ; call the python G69_core code -G13.1 ; back to identity kins -M68 E2 Q0 ; reset twp-state to 'undefined' (0) -G54 ; switch to G54 -M66 L0 E0 ; force sync, stop read ahead -oendsub -m2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/on_abort_with_twp_reset.ngc b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/on_abort_with_twp_reset.ngc deleted file mode 100755 index 1cbf3d41db7..00000000000 --- a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/remap_subs/on_abort_with_twp_reset.ngc +++ /dev/null @@ -1,15 +0,0 @@ -;This is a workaround for a bug that leads to stat.gcodes and parameter[5250] -;to get out of sync after some program aborts -;in [RS274NGC] section of the ini add: ON_ABORT_COMMAND = o call -;save this to a path specified in SUBROUTINE_PATH = -;NOTE: we cannot run remapped codes here only custom Mcodes (ie M100..M199) - -o sub -;(msg, on_abort START) -M68 E2 Q0 ; reset twp-state to 'undefined' (0) -G13.1 ; back to identity kins -G64 P0.01 ; reset the toolpath tolerance as this sometimes gets set to zero on estop events -G54 ; switch to G54 -(msg, on_abort END) -o endsub -M2 diff --git a/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini b/configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini index 2be585e6ef8..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=g53n_core - - REMAP = G68.2 modalgroup=1 argspec=pqxyzijkr python=g682 - REMAP = G68.3 modalgroup=1 argspec=xyzr python=g683 - REMAP = G68.4 modalgroup=1 argspec=pqxyzijkr python=g684 - - REMAP = G69 modalgroup=1 ngc=g69remap - REMAP = M469 modalgroup=10 python=g69_core - PARAMETER_FILE = xyzacb-trsrn.var -[PYTHON] -# where to find the Python code: -# code specific for this configuration -PATH_APPEND = ../python -# import the following Python module -TOPLEVEL = ../python/toplevel.py -# the higher the more verbose tracing of the Python plugin -LOG_LEVEL = 3 - [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 d3032855aee..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=g53n_core - - REMAP = G68.2 modalgroup=1 argspec=pqxyzijkr python=g682 - REMAP = G68.3 modalgroup=1 argspec=xyzr python=g683 - REMAP = G68.4 modalgroup=1 argspec=pqxyzijkr python=g684 - - REMAP = G69 modalgroup=1 ngc=g69remap - REMAP = M469 modalgroup=10 python=g69_core - PARAMETER_FILE = xyzbca-trsrn.var -[PYTHON] -# where to find the Python code: -# code specific for this configuration -PATH_APPEND = ../python -# import the following Python module -TOPLEVEL = ../python/toplevel.py -# the higher the more verbose tracing of the Python plugin -LOG_LEVEL = 3 - [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/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/test-ui.py b/tests/twp-native/test-ui.py index 9550b123782..0d88df07503 100755 --- a/tests/twp-native/test-ui.py +++ b/tests/twp-native/test-ui.py @@ -182,8 +182,16 @@ def rot_y(d): % (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 = max(abs(smp[0][i] - start[i]) for smp in samples for i in range(3)) -print("linear joints moved at most %.9f through G53.1" % worst) +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): From 69d3beb7d337571a52383164d0f8128ab56e973a Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 8 Sep 2026 16:04:42 +1000 Subject: [PATCH 49/77] tests: give the tilted work plane a plane that tells the forms apart Every plane in the frame test was one rotation of ninety degrees about a single axis, written four ways. With two angles zero, composing about the frame as it turns and about the fixed axes give the same matrix, so the difference between G68.2 P0 and P1, the whole reason they are separate codes, went unobserved. One plane at 25, 40 and -15 degrees instead, still written four ways, with the points and vectors derived from it; G68.4 and R take odd angles too, and G53 inside the plane lands somewhere that is not a permutation of the axes. Caught now: P0 composing the way P1 does, R composed before the plane rotation, the three-point form taking Y as X cross Z. --- tests/interp/g68-frame/expected | 88 +++++++++++++++++---------------- tests/interp/g68-frame/g68.ngc | 26 +++++----- 2 files changed, 59 insertions(+), 55 deletions(-) diff --git a/tests/interp/g68-frame/expected b/tests/interp/g68-frame/expected index 98c0321fffe..79e5b75b286 100644 --- a/tests/interp/g68-frame/expected +++ b/tests/interp/g68-frame/expected @@ -10,46 +10,48 @@ 10 N..... SET_G5X_OFFSET(2, 100.0000, 200.0000, 300.0000, 0.0000, 0.0000, 0.0000) 11 N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) 12 N..... SET_XY_ROTATION(0.0000) - 13 N..... COMMENT("a plane rotated 90 about X: plane Y is world Z, plane Z is world -Y") - 14 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 15 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 16 N..... COMMENT("the same plane by fixed axis angles about X") - 17 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 18 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 19 N..... COMMENT("the same plane by three points") - 20 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 21 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 22 N..... COMMENT("the same plane by two vectors, X nudged off square") - 23 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) - 24 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 25 N..... COMMENT("R turns the plane about its own Z") - 26 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.0000, -1.0000, 0.0000, 0.0000, 0.0000, -1.0000, 1.0000, 0.0000, 0.0000], 1) - 27 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 28 N..... COMMENT("an arc in the plane") - 29 N..... SET_FEED_RATE(100.0000) - 30 N..... STRAIGHT_FEED(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 31 N..... ARC_FEED(2.0000, 0.0000, 1.0000, 0.0000, -1, 0.0000, 0.0000, 0.0000, 0.0000) - 32 N..... COMMENT("G53 inside the plane goes to absolute coordinates") - 33 N..... STRAIGHT_TRAVERSE(-30.0000, 10.0000, 20.0000, 0.0000, 0.0000, 0.0000) - 34 N..... COMMENT("and #5021 reports them") - 35 N..... MESSAGE(" abs 100.000000 200.000000 300.000000 prog -30.000000 10.000000 20.000000") - 36 N..... COMMENT("a probe result comes back in plane coordinates: nothing to run here") - 37 N..... COMMENT("a tool length change moves the program coordinates along the plane axis that is world Z") - 38 N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 7.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000) - 39 N..... MESSAGE(" prog -37.000000 10.000000 20.000000") - 40 N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000) - 41 N..... COMMENT("G68.4 composes: a further 90 about the plane's X") - 42 N..... SET_G68_FRAME(10.0000, 20.0000, 30.0000, [0.0000, 0.0000, 1.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000, 0.0000], 1) - 43 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 44 N..... COMMENT("G69 cancels") - 45 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) - 46 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) - 47 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) - 48 N..... SET_XY_ROTATION(0.0000) - 49 N..... SET_FEED_MODE(0, 0) - 50 N..... SET_FEED_RATE(0.0000) - 51 N..... STOP_SPINDLE_TURNING(0) - 52 N..... SET_SPINDLE_MODE(0 0.0000) - 53 N..... PROGRAM_END() - 54 N..... ON_RESET() - 55 N..... ON_RESET() + 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 index 656cce9b906..544f5a16e7a 100644 --- a/tests/interp/g68-frame/g68.ngc +++ b/tests/interp/g68-frame/g68.ngc @@ -3,24 +3,26 @@ g21 g90 g10 l2 p2 x100 y200 z300 r0 g55 -(a plane rotated 90 about X: plane Y is world Z, plane Z is world -Y) -g68.2 x10 y20 z30 i0 j90 k0 +(one plane, four ways. Three distinct angles, none of them right, so that) +(the order the rotations compose in shows in the answer) +(P0 turns about the frame as it goes, ZXZ by default) +g68.2 x10 y20 z30 i25 j40 k-15 g0 x1 y2 z3 -(the same plane by fixed axis angles about X) -g68.2 p1 q123 x10 y20 z30 i90 j0 k0 +(the same plane about the fixed axes of the system it sits in, XYZ) +g68.2 p1 q123 x10 y20 z30 i39.025043525 j9.576578516 k13.400523974 g0 x1 y2 z3 -(the same plane by three points) +(the 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 x5 y0 z0 -g68.2 p2 q3 x0 y0 z5 +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 i1 j0 k0.000001 -g68.2 p3 q2 i0 j-1 k0 +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 p1 q123 x10 y20 z30 i90 j0 k0 r90 +g68.2 x10 y20 z30 i25 j40 k-15 r37.5 g0 x1 y2 z3 (an arc in the plane) g1 f100 x0 y0 z0 @@ -34,8 +36,8 @@ g53 g0 x100 y200 z300 g43.1 z7 (debug, prog #5420 #5421 #5422) g49 -(G68.4 composes: a further 90 about the plane's X) -g68.4 p1 q123 i90 j0 k0 +(G68.4 composes onto the plane, in its own axes) +g68.4 p1 q123 i35 j-20 k10 g0 x1 y2 z3 (G69 cancels) g69 From 51b91253beaa7e2a158c6aed70261b53f72d2733 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 8 Sep 2026 18:07:36 +1000 Subject: [PATCH 50/77] docs: say where the tilted work plane conventions come from The chapter and the G-code reference described the two angle forms but never said whose conventions they are: P0 is Heidenhain's PLANE EULER, P1 its PLANE SPATIAL, the numbering is Fanuc's, and Fanuc's fifth form is not implemented. Two differences from the control they borrow from are stated: P1 and P2 refuse where a control offering SEQ falls back to the nearer pose, and P names a pose only where a rotary turns the tool, so a machine whose rotaries all carry the work has only the nearest form; the G53.1 section read as though a tilting table had a pose to name. --- docs/src/gcode/g-code.adoc | 29 ++++++++++++++------- docs/src/motion/kinematics-conventions.adoc | 28 ++++++++++++++++++++ 2 files changed, 47 insertions(+), 10 deletions(-) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 332de91c619..a3aa8cc013c 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -1849,19 +1849,23 @@ 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: on a five axis machine the two -poses lean the head or the table 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-`. +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. A machine that is not of this shape, a robot among -them, has no such sign to name, and there only the nearest form is -available. +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. @@ -2269,6 +2273,11 @@ is. Words left out are zero. '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 diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 64596b2c62d..6a799e7c6f0 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -669,6 +669,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 From 368a33c66cf8cacf68080006915926fb8d6e57d2 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 8 Sep 2026 20:23:32 +1000 Subject: [PATCH 51/77] interpreter, motion: hold the joints a point-to-point move asks for A point does not name one joint set: a robot wrist reaches it again with the forearm turned half a revolution, and both ends of a point-to-point move read the joints back from the point. The interpreter keeps its seed while it still explains where the machine is, instead of inverting the point every time and reporting joints the machine is not standing in, which had the next G53.7 refused on a joint limit. Motion holds the joints a joint segment ended on while the planner stays there, instead of inverting again the next cycle and undoing the move. tests/ptp-robot drives a wrist joint through zero and back and checks the joints not named stay put. --- src/emc/motion/control.c | 50 +++++++++++++++++++++++++--- src/emc/rs274ngc/interp_workplane.cc | 17 +++++++++- tests/ptp-robot/test-ui.py | 16 +++++++++ 3 files changed, 78 insertions(+), 5 deletions(-) diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 715af6ee2f7..745db82b0b7 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -929,6 +929,25 @@ 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 the same, to a hair either way */ +static int same_carte_pos(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; +} + static void set_operating_mode(void) { int joint_num; @@ -1449,7 +1468,7 @@ static void get_pos_cmds(long period) 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 */ - tpTakeJointEnd(&emcmotInternal->coord_tp, positions); + int joint_end_fresh = tpTakeJointEnd(&emcmotInternal->coord_tp, positions); /* get new commanded traj pos */ tpGetPos(&emcmotInternal->coord_tp, &emcmotStatus->carte_pos_cmd); @@ -1467,9 +1486,32 @@ static void get_pos_cmds(long period) 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) { diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 093091239a2..25401a75672 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -536,17 +536,32 @@ void Interp::machine_pose_to_program(setup_pointer s, const EmcPose *pose, doubl 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; - EmcPose pose; + EmcPose pose, seeded; int pass, i; 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; + } 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]; } diff --git a/tests/ptp-robot/test-ui.py b/tests/ptp-robot/test-ui.py index 92362c901a7..c013a42f2ff 100755 --- a/tests/ptp-robot/test-ui.py +++ b/tests/ptp-robot/test-ui.py @@ -74,6 +74,22 @@ def refused(cmd, expect): 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") From 59f90b0ba1db21d30bfc4e8e7512dd9f045a8374 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 8 Sep 2026 23:13:58 +1000 Subject: [PATCH 52/77] interpreter: seed the kinematics from the joints the machine stands in The interpreter works out the joints behind a point by inverting it, and had nothing to start the first inverse from but zeros. A module that answers the inverse by iterating cannot be started anywhere: genserkins takes the Jacobian at the seed, and a serial arm with every joint at zero stands in a singular pose, so on a robot every G53.5 and G53.7 was refused with "the kinematics cannot invert the current position". Canon now reports the joint positions, and the interpreter falls back on them when its own seed no longer explains where the machine is. They also name the arm the machine is standing in, which the inverse of a point cannot: a robot reaches the same point again with the elbow the other way up. --- src/emc/nml_intf/canon.hh | 6 ++++++ src/emc/rs274ngc/gcodemodule.cc | 1 + src/emc/rs274ngc/interp_workplane.cc | 12 +++++++++++- src/emc/sai/saicanon.cc | 5 +++++ src/emc/task/emccanon.cc | 11 +++++++++++ 5 files changed, 34 insertions(+), 1 deletion(-) diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index 4f5da55a2e8..11092070206 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -1018,6 +1018,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/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index 655498872f8..9f2b328b0b6 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -759,6 +759,7 @@ double GET_EXTERNAL_POSITION_C() { return parse_state.pos[P9_C]; } double GET_EXTERNAL_POSITION_U() { return parse_state.pos[P9_U]; } double GET_EXTERNAL_POSITION_V() { return parse_state.pos[P9_V]; } double GET_EXTERNAL_POSITION_W() { return parse_state.pos[P9_W]; } +int GET_EXTERNAL_JOINT_POSITIONS(double * /*joints*/, int /*max*/) { return 0; } void INIT_CANON() {} void SET_PARAMETER_FILE_NAME(const char *name) diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 25401a75672..c697b53909e 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -553,8 +553,9 @@ static bool same_pose(const EmcPose *a, const EmcPose *b) 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; + int pass, i, n; current_machine_pose(s, &pose); for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joints[i] = s->kins_seed[i]; } @@ -562,6 +563,15 @@ int Interp::current_joints(setup_pointer s, void *vctx, double *joints) 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]; } diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index 73be89400a6..9e146037f27 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -926,6 +926,11 @@ double GET_EXTERNAL_POSITION_W() return 0.; } +int GET_EXTERNAL_JOINT_POSITIONS(double * /*joints*/, int /*max*/) +{ + return 0; +} + double GET_EXTERNAL_PROBE_POSITION_U() { return 0.; diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 4b01d744cfb..c9d3fae41f2 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -3969,6 +3969,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; From 8ec7033059348d4bd694aa7c1c72823ed5133dc8 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 9 Sep 2026 11:55:19 +1000 Subject: [PATCH 53/77] preview: let the canon report where the joints stand The preview has a machine to ask, through the status buffer it already reads for the tool table and the offsets, so it can answer for the joints as well. Without them every inverse started from zeros, which previewed a point-to-point move from a pose the machine is not standing in, and on a serial arm could not be inverted at all. The one method on the mixin covers AXIS, gremlin and the Qt screens, which all take their canon from it. --- lib/python/rs274/interpret.py | 3 +++ src/emc/rs274ngc/gcodemodule.cc | 25 ++++++++++++++++++++++++- 2 files changed, 27 insertions(+), 1 deletion(-) diff --git a/lib/python/rs274/interpret.py b/lib/python/rs274/interpret.py index 3c83b5502a8..0662adc90bc 100644 --- a/lib/python/rs274/interpret.py +++ b/lib/python/rs274/interpret.py @@ -193,5 +193,8 @@ 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]) + # vim:ts=8:sts=4:et: diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index 9f2b328b0b6..d28b0a30065 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -759,7 +759,30 @@ double GET_EXTERNAL_POSITION_C() { return parse_state.pos[P9_C]; } double GET_EXTERNAL_POSITION_U() { return parse_state.pos[P9_U]; } double GET_EXTERNAL_POSITION_V() { return parse_state.pos[P9_V]; } double GET_EXTERNAL_POSITION_W() { return parse_state.pos[P9_W]; } -int GET_EXTERNAL_JOINT_POSITIONS(double * /*joints*/, int /*max*/) { return 0; } + +// Where the machine's joints stand. A point does not name one joint set, so +// an iterative inverse has to start somewhere: a canon that watches the +// status buffer says where. One that cannot, or that answers something that +// is not a sequence of numbers, answers nothing, and the interpreter works +// from the point alone. +int GET_EXTERNAL_JOINT_POSITIONS(double *joints, int max) { + int n = 0; + if(parse_state.interp_error) return 0; + py::handle canon(parse_state.callback); + if(!py::hasattr(canon, "get_external_joint_positions")) return 0; + try { + py::sequence seq = canon.attr("get_external_joint_positions")().cast(); + for(py::handle value : seq) { + if(n == max) break; + joints[n++] = value.cast(); + } + } catch(py::error_already_set &) { + return 0; // the error goes with the exception + } catch(py::builtin_exception &) { + return 0; + } + return n; +} void INIT_CANON() {} void SET_PARAMETER_FILE_NAME(const char *name) From d07403ebf36c76ad077002fce9511a980252c25e Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 9 Sep 2026 15:56:21 +1000 Subject: [PATCH 54/77] tests: the point-to-point moves and their preview on an iterative module pumakins answers the inverse in closed form and never looks at the joints it is handed, so tests/ptp-robot says nothing about the seed. genserkins takes the Jacobian at those joints: the machine here homes well away from all zeros, so the seed has to come from where the machine stands, and all zeros is the singular pose it cannot come from at all. The same program goes through the interpreter and through the preview, and the two have to agree on where it ends up. Then the arm is parked in the singular pose, where the module reports through the HAL library it prints with: the preview refuses, rather than the process going down. --- tests/ptp-iterative/README | 9 ++ tests/ptp-iterative/checkresult | 3 + tests/ptp-iterative/sim.hal | 16 +++ tests/ptp-iterative/test-ui.py | 175 ++++++++++++++++++++++++++++++++ tests/ptp-iterative/test.ini | 140 +++++++++++++++++++++++++ tests/ptp-iterative/test.ngc | 3 + tests/ptp-iterative/test.sh | 4 + tests/ptp-iterative/tool.tbl | 1 + tests/ptp-robot/skip | 4 - tests/twp-native/skip | 4 - 10 files changed, 351 insertions(+), 8 deletions(-) create mode 100644 tests/ptp-iterative/README create mode 100755 tests/ptp-iterative/checkresult create mode 100644 tests/ptp-iterative/sim.hal create mode 100755 tests/ptp-iterative/test-ui.py create mode 100644 tests/ptp-iterative/test.ini create mode 100644 tests/ptp-iterative/test.ngc create mode 100755 tests/ptp-iterative/test.sh create mode 100644 tests/ptp-iterative/tool.tbl delete mode 100755 tests/ptp-robot/skip delete mode 100755 tests/twp-native/skip 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/skip b/tests/ptp-robot/skip deleted file mode 100755 index a12f31a77c2..00000000000 --- a/tests/ptp-robot/skip +++ /dev/null @@ -1,4 +0,0 @@ -#!/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/twp-native/skip b/tests/twp-native/skip deleted file mode 100755 index a12f31a77c2..00000000000 --- a/tests/twp-native/skip +++ /dev/null @@ -1,4 +0,0 @@ -#!/bin/sh -# Builds a realtime component with halcompile, which needs the build -# tools present. Skip when testing installed packages. -[ -z "$SYSTEM_BUILD" ] From 15d26073e6908930341cd3c36026ad9fa774ace8 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 9 Sep 2026 21:08:54 +1000 Subject: [PATCH 55/77] genserkins: hold the inverse to a step the Jacobian is good for The inverse took the whole Newton step the Jacobian asked for, and the Jacobian holds only near the estimate: an endpoint far from the seed asked for many radians, the arm landed somewhere unrelated and converged on a pose reached through whole turns. Seeded at 90 -90 0 0 90 -17.7 and asked for X450 Y-200 Z150 it answered joint 0 at -3263.962, the right pose nine turns out, which the joint limits refuse on a move the arm could make. Cap the step at GENSER_MAX_ANGLE_STEP and scale the whole vector so its direction survives, measured on the rotary links; a servo cycle asks for far less and is untouched. --- src/emc/kinematics/genserfuncs.c | 21 +++++++++++++++++++++ src/emc/kinematics/genserkins.h | 2 ++ 2 files changed, 23 insertions(+) diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index c43156490c9..c9a140a7ab6 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -631,6 +631,27 @@ static int genser_inverse(const kins_params *p, kins_scratch *s, /* push the Cartesian velocity vector through the inverse Jacobian */ go_matrix_vector_mult(&Jinv, dvw, dj); + /* The Jacobian holds only near the estimate, and a far pose asks + for a step of many radians: the arm lands somewhere unrelated + and converges by way of whole turns its limits refuse. Cap the + step, keeping its direction, and let the iteration walk there. */ + { + double worst = 0.0; + + for (link = 0; link < genser->link_num; link++) { + if (GO_QUANTITY_ANGLE == linkout[link].quantity + && fabs(dj[link]) > worst) { + worst = fabs(dj[link]); + } + } + if (worst > GENSER_MAX_ANGLE_STEP) { + double scale = GENSER_MAX_ANGLE_STEP / worst; + for (link = 0; link < genser->link_num; link++) { + dj[link] *= scale; + } + } + } + //pass through 678 as uvw if (p->max_joints > 6) joints[6] = world->u; if (p->max_joints > 7) joints[7] = world->v; diff --git a/src/emc/kinematics/genserkins.h b/src/emc/kinematics/genserkins.h index c50cddde3e3..52f369a958e 100644 --- a/src/emc/kinematics/genserkins.h +++ b/src/emc/kinematics/genserkins.h @@ -45,6 +45,8 @@ #define GENSER_MAX_JOINTS 6 #define GENSER_DEFAULT_MAX_ITERATIONS 100 +/* the most a rotary joint moves in one pass of the inverse, in radians */ +#define GENSER_MAX_ANGLE_STEP 0.2 #define PI_2 GO_PI_2 From 0ea00f42c462a6a4242cd9683d9e9d7af238f71a Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 9 Sep 2026 21:09:03 +1000 Subject: [PATCH 56/77] motion: check a move against the joints the queue leaves behind The joint limit check inverted a move's endpoint from the joints the machine stands in. The reading runs far ahead, so every endpoint of a short program was checked from the starting pose, and an iterating inverse answers nearest whatever it is handed: park a robot at J4=-90, run a program that takes it to J4=90 and asks for a point, and the point is refused because it was inverted from the parked pose with the other wrist. Seed from the end of the queue instead: a joint segment knows its own end and the rest carry the answer the last endpoint came out with; with nothing queued the machine is the reference. The joint interpolated path already worked this way and shares the helper. --- src/emc/motion/command.c | 48 +++++++++++++++++++++++++++++++--------- 1 file changed, 38 insertions(+), 10 deletions(-) diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 4bd9193e1b0..bfa2495e304 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -140,6 +140,30 @@ static int inverse_settled(EmcPose *pos, double *joints, 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) @@ -288,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; } @@ -329,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; } @@ -1190,12 +1221,9 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) } /* where the queue ends in joint space */ - if (!tpGetQueueEndJoints(&emcmotInternal->coord_tp, start)) { + if (!queue_end_joints(start)) { EmcPose goal; tpGetGoalPos(&emcmotInternal->coord_tp, &goal); - for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { - start[joint_num] = (joint_num < ALL_JOINTS) ? joints[joint_num].pos_cmd : 0.0; - } if (inverse_settled(&goal, start, &iflags, &fflags) != 0) { reportError(_("joint interpolated move on line %d: the queue end fails kinematicsInverse"), emcmotCommand->id); From 358d662db41a5f7d6881facf3948f6b77b892eba Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 17 Sep 2026 23:16:09 +1000 Subject: [PATCH 57/77] interpreter: add G53.2, solve the tool orientation without moving G53.2 asks where the rotaries have to go for the tool to be normal to the active plane, like G53.1, and moves nothing: it publishes the pose on #<_orient_x> and kin and on #5071 to #5079, with #<_orient_valid> and #5080 saying a pose is held, so the program reaches it with a move of its own, for instance one G0 naming the linear and rotary words together, a single Cartesian move under TCP. Heidenhain's STAY. Reading the parameters before the first G53.2 is an error. Two fixes found on the way: a G68.3 block with axis words and a modal G0 emitted a stray move; and the solver's answers in (-180, 180] are unwrapped onto the turn nearest the present rotary position before the nearest-first ranking, or a rotary crossing 180 swings the long way round. --- docs/src/gcode/g-code.adoc | 25 ++++++++--- docs/src/gcode/overview.adoc | 14 +++++- docs/src/motion/kinematics-conventions.adoc | 4 +- src/emc/rs274ngc/interp_array.cc | 3 +- src/emc/rs274ngc/interp_check.cc | 12 ++--- src/emc/rs274ngc/interp_convert.cc | 2 +- src/emc/rs274ngc/interp_internal.cc | 3 +- src/emc/rs274ngc/interp_internal.hh | 4 ++ src/emc/rs274ngc/interp_namedparams.cc | 32 ++++++++++++++ src/emc/rs274ngc/interp_setup.cc | 2 + src/emc/rs274ngc/interp_workplane.cc | 38 +++++++++++++--- tests/remap/introspect/expected | 4 +- tests/twp-native/test-ui.py | 49 +++++++++++++++++++++ 13 files changed, 165 insertions(+), 27 deletions(-) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index a3aa8cc013c..9aa577c95a8 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -89,7 +89,7 @@ as the 'L number', and so on for any other letter. |<> |Cancel Tool Length Offset |<> |Local Coordinate System Offset |<> |Move in Machine Coordinates -|<> |Orient the Tool to the Tilted Work Plane +|<> |Orient the Tool to the Tilted Work Plane |<> |Point-to-Point Move |<> |Select Coordinate System (1 - 9) |<> |Exact Path Mode @@ -1820,18 +1820,19 @@ It is an error if: * or G53 is used while cutter compensation is on. [[gcode:g53.1]] -== G53.1, G53.3, G53.6 Orient the Tool to the Work Plane(((G53.1 Orient the Tool))) +== 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 ---- -Each of these moves the rotary joints so that the tool axis is normal to the -active <>, the plane's Z. They differ in what -happens to the tool tip on the way: +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 @@ -1840,6 +1841,16 @@ happens to the tool tip on the way: 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 @@ -1910,8 +1921,8 @@ It is an error if: 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' or 'G53.6', or words other than 'X', 'Y' - and 'Z' with 'G53.3'. +* 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]] diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index d801b25cca4..0079306428b 100644 --- a/docs/src/gcode/overview.adoc +++ b/docs/src/gcode/overview.adoc @@ -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, G28.2, G30, G52, G53, G53.1, G53.3, G53.4, G53.5, G53.6, G53.7, G92, G92.1, G92.2, G92.3, +|Non-modal codes (Group 0) | G4, G10 G28, G28.2, 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 diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 6a799e7c6f0..7d691d6b547 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -352,8 +352,8 @@ 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.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 +`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. diff --git a/src/emc/rs274ngc/interp_array.cc b/src/emc/rs274ngc/interp_array.cc index fcfb7942d33..ddff76a0118 100644 --- a/src/emc/rs274ngc/interp_array.cc +++ b/src/emc/rs274ngc/interp_array.cc @@ -96,7 +96,7 @@ const int Interp::gees[] = { /* 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, 0,-1, 0, 0, 0, 0, 0,-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, @@ -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_check.cc b/src/emc/rs274ngc/interp_check.cc index a440aa124da..fcc457c1682 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -110,10 +110,10 @@ 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_6) { + } 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 or G53.6")); + _("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")); @@ -365,7 +365,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block if (block->p_flag) { CHKS(((block->g_modes[GM_MODAL_0] != G_10) && (block->g_modes[GM_MODAL_0] != G_4) && (block->g_modes[GM_CONTROL_MODE] != G_64 && (block->g_modes[GM_MODAL_0] != G_12_1)) && !plane_words && - (block->g_modes[GM_MODAL_0] != G_53_1) && (block->g_modes[GM_MODAL_0] != G_53_3) && (block->g_modes[GM_MODAL_0] != G_53_6) && + (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) && @@ -378,7 +378,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 G53.1 G53.3 G53.6 G64 G68.2 G68.4 G5 G5.2 G6, G6.2, G76 G82 G86 G88 G89" + _("P word with no G2 G3 G4 G10 G12.1 G53.1 G53.2 G53.3 G53.6 G64 G68.2 G68.4 G5 G5.2 G6, G6.2, G76 G82 G86 G88 G89" " G28.2" " or M50 M51 M52 M53 M62 M63 M64 M65 M66 M98 " "or user M code to use it")); @@ -397,12 +397,12 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block (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) && !plane_words && - (block->g_modes[GM_MODAL_0] != G_53_1) && (block->g_modes[GM_MODAL_0] != G_53_3) && (block->g_modes[GM_MODAL_0] != G_53_6) && + (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, G53.1, G53.3, G53.6, G64, G68.2, G68.4, G73, G76, G83, M19, M66, M67, M68 or user M code that uses it")); + _("Q word with no G5, G6, G10, G53.1, G53.2, G53.3, G53.6, G64, G68.2, G68.4, G73, G76, G83, M19, M66, M67, M68 or user M code that uses it")); } if (block->r_flag) { diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index cbb698aa72b..fdd477f8053 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -4502,7 +4502,7 @@ int Interp::convert_modal_0(int code, //!< G-code, must be from group 0 CHP(convert_nurbs(code, block, settings)); } 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_3) || (code == G_53_6)) { + 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)) { diff --git a/src/emc/rs274ngc/interp_internal.cc b/src/emc/rs274ngc/interp_internal.cc index 37fbe304f88..01b1e73e047 100644 --- a/src/emc/rs274ngc/interp_internal.cc +++ b/src/emc/rs274ngc/interp_internal.cc @@ -176,7 +176,8 @@ int Interp::enhance_block(block_pointer block, //!< pointer to a block to be c ((mode0 == G_10) || (mode0 == G_28) || (mode0 == G_30) || (mode0 == G_52) || (mode0 == G_92) || (mode0 == G_53_3)); // a tilted work plane definition takes the axis words the same way - if (block->g_modes[GM_WORK_PLANE] == G_68_2 || block->g_modes[GM_WORK_PLANE] == G_68_4) { + if (block->g_modes[GM_WORK_PLANE] == G_68_2 || block->g_modes[GM_WORK_PLANE] == G_68_3 + || block->g_modes[GM_WORK_PLANE] == G_68_4) { CHKS(polar_flag, _("Polar coordinates cannot define a tilted work plane")); mode_zero_covets_axes = 1; } diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index cbfed8c4954..961537359e4 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -256,6 +256,7 @@ enum GCodes 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, @@ -773,6 +774,9 @@ struct setup 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 diff --git a/src/emc/rs274ngc/interp_namedparams.cc b/src/emc/rs274ngc/interp_namedparams.cc index 9ab4bae38b5..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: @@ -899,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_setup.cc b/src/emc/rs274ngc/interp_setup.cc index bdfcada8020..aa762016211 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -112,6 +112,8 @@ setup::setup() : 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{}, diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index c697b53909e..5f88bf47eea 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -682,11 +682,12 @@ int Interp::convert_work_plane_from_tool(block_pointer block, setup_pointer s) return work_plane_set(s, G_68_3, origin, rotation); } -// G53.1, 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. P picks the pose, -// nearest first or by the sign of the tilting joint; Q0 holds the joints that -// carry the work (Heidenhain COORD ROT), Q1 frees them (TABLE ROT). +// 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 picks the pose, nearest +// first or by the sign of the tilting joint; Q0 holds the joints that carry +// the work (COORD ROT), Q1 frees them (TABLE ROT). int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) { void *vctx; @@ -702,7 +703,7 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) unsigned int held = 0; int p, q, n, i, j, chosen, njoints; const double *sol; - const char *name = (code == G_53_1) ? "G53.1" : (code == G_53_3) ? "G53.3" : "G53.6"; + 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), @@ -737,6 +738,18 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) CHKS((n < 0), _("%s: the kinematics cannot answer the orientation"), name); CHKS((n == 0), _("%s: the plane's normal 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; @@ -782,6 +795,19 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) } 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 diff --git a/tests/remap/introspect/expected b/tests/remap/introspect/expected index 2f33b4bbe08..206bc4d96af 100644 --- a/tests/remap/introspect/expected +++ b/tests/remap/introspect/expected @@ -29,8 +29,8 @@ speed= 3000.0 global parameter set in test.ngc: 47.11 parameter set via test.ini: 3.14159 locals: ['a_new_local'] -globals: ['_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] -params(): ['a_new_local', '_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] +globals: ['_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_orient_a', '_orient_b', '_orient_c', '_orient_valid', '_orient_x', '_orient_y', '_orient_z', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] +params(): ['a_new_local', '_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_orient_a', '_orient_b', '_orient_c', '_orient_valid', '_orient_x', '_orient_y', '_orient_z', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] 14 N..... MESSAGE(" after introspect: return value=2.718280 call_level= 0.000000") 15 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) 16 N..... SET_XY_ROTATION(0.0000) diff --git a/tests/twp-native/test-ui.py b/tests/twp-native/test-ui.py index 0d88df07503..4008e987d0a 100755 --- a/tests/twp-native/test-ui.py +++ b/tests/twp-native/test-ui.py @@ -162,6 +162,13 @@ def rot_y(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)) @@ -271,6 +278,48 @@ def in_plane(): 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)) From f98003cf035e81833ccf25812f517821669a20c6 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 23 Sep 2026 11:21:29 +1000 Subject: [PATCH 58/77] interpreter: keep the orientation poses inside the rotary travel The orientation codes took each pose the solver reported onto the turn nearest where the rotaries stood and ranked them by travel, without asking whether that turn was inside the joint's limits. A program that winds a rotary, a G68.4 incremental plane in a loop with G53.6, walked it past its limit, and motion refused the move in the middle of the program: "Linear move on line 9 would exceed joint 5's positive limit". The tilted work plane remap it replaces checked the limits and went the long way round. The interpreter reads [JOINT_n] MIN_LIMIT and MAX_LIMIT beside TYPE. Each angular joint of each pose goes onto the turn nearest where it stands that lies inside its travel, which is the long way round where the short way runs out, and a pose no turn brings inside is dropped before the ranking, so P0 takes the next nearest and P1 or P2 refuse. Under Q0 the table is freed before a pose is refused, as when nothing reaches the direction with it held. Where every pose that reaches the direction is outside the travel, the code refuses with that reason, at plan time rather than in motion. tests/twp-native stands C at 300 with a travel of 320, picks a plane whose nearest pose is the turn past the limit, and checks that G53.6 ends on the nearest pose inside the travel, with C inside it all the way. --- docs/src/gcode/g-code.adoc | 2 + src/emc/rs274ngc/interp_internal.hh | 2 + src/emc/rs274ngc/interp_setup.cc | 2 + src/emc/rs274ngc/interp_workplane.cc | 56 +++++++++++++++++++++------ src/emc/rs274ngc/rs274ngc_pre.cc | 5 ++- tests/twp-native/README | 2 +- tests/twp-native/test-ui.py | 57 ++++++++++++++++++++++++++++ 7 files changed, 112 insertions(+), 14 deletions(-) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 9aa577c95a8..329fa3dae64 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -1873,6 +1873,8 @@ 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. +Every form stays inside the travel `[JOINT_n] MIN_LIMIT` and `MAX_LIMIT` give the rotaries. A rotary reaches its angle by the turn nearest where it stands, and where that turn is outside its travel, by the nearest turn inside it, so it goes the long way round rather than run out of travel. A pose no turn brings inside the travel is not taken: without 'P' the next nearest is, and with 'P1' or 'P2' the code refuses. Under 'Q0' the table is freed before a pose is refused, as when no pose reaches the direction with it held. + 'P' names a pose only 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 diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index 961537359e4..ea0c78d60f3 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -784,6 +784,8 @@ struct setup char kins_module[LINELEN]; // [KINS] KINEMATICS, as loadrt gets it int kins_joints; // [KINS] JOINTS int kins_angular_joints; // bit per joint, [JOINT_n] TYPE = ANGULAR + double kins_joint_min[EMCMOT_MAX_JOINTS]; // [JOINT_n] MIN_LIMIT + double kins_joint_max[EMCMOT_MAX_JOINTS]; // [JOINT_n] MAX_LIMIT double kins_seed[EMCMOT_MAX_JOINTS]; // the last inverse, seeding the next double parameters[interp_param_global::RS274NGC_MAX_PARAMETERS]; // system parameters int parameter_occurrence; // parameter buffer index diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index aa762016211..f9083a0726e 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -119,6 +119,8 @@ setup::setup() : kins_module{}, kins_joints(0), kins_angular_joints(0), + kins_joint_min{}, + kins_joint_max{}, kins_seed{}, parameters{0}, diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 5f88bf47eea..4978343f9a5 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -682,6 +682,43 @@ int Interp::convert_work_plane_from_tool(block_pointer block, setup_pointer s) return work_plane_set(s, G_68_3, origin, rotation); } +// The solver reports each answer in (-180, 180], but the machine stands +// somewhere in turn space: every angular joint of each pose goes onto the +// turn nearest where it stands, or the nearest pose is not the nearest move +// and a free rotary swings the long way round. Of the turns, only those +// inside the joint's [JOINT_n] travel count, so a rotary that would run out +// of travel the short way goes the long way, and a pose no turn brings +// inside is dropped. Returns how many poses are left, packed at the front. +static int orient_fit(setup_pointer s, double *solutions, int n, int njoints, const double *now) +{ + int i, j, kept = 0; + + for (i = 0; i < n; i++) { + double *pose = &solutions[i*njoints]; + bool inside = true; + for (j = 0; j < njoints; j++) { + if (!(s->kins_angular_joints & (1 << j))) { continue; } + double turns = floor((now[j] - pose[j]) / 360.0 + 0.5); + if (j < s->kins_joints) { + double first = ceil((s->kins_joint_min[j] - pose[j]) / 360.0 - 1e-9); + double last = floor((s->kins_joint_max[j] - pose[j]) / 360.0 + 1e-9); + if (first > last) { + inside = false; + break; + } + turns = fmin(fmax(turns, first), last); + } + pose[j] += 360.0 * turns; + } + if (!inside) { continue; } + if (kept != i) { + for (j = 0; j < njoints; j++) { solutions[kept*njoints + j] = pose[j]; } + } + kept++; + } + return kept; +} + // 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 @@ -702,6 +739,7 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) double end_prog[9]; unsigned int held = 0; int p, q, n, i, j, chosen, njoints; + bool reached; const double *sol; const char *name = (code == G_53_1) ? "G53.1" : (code == G_53_2) ? "G53.2" : (code == G_53_3) ? "G53.3" : "G53.6"; @@ -729,27 +767,21 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) } n = kinematicsUserToolFrameInverse(ctx, &axis, &xdir, now, held, solutions, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); + reached = (n > 0); + if (n > 0) { n = orient_fit(s, solutions, n, njoints, now); } 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); + reached = reached || (n > 0); + if (n > 0) { n = orient_fit(s, solutions, n, njoints, now); } } CHKS((n < 0), _("%s: the kinematics cannot answer the orientation"), name); + CHKS((n == 0 && reached), + _("%s: every pose that reaches the plane's normal puts a rotary joint outside its travel"), name); CHKS((n == 0), _("%s: the plane's normal 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; diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index 0a3dc73c306..d16a14bbadb 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -906,7 +906,8 @@ int Interp::init() } _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 + // refused where it would name a joint of the other kind, and the + // travel of each, so that a rotary is taken the way that stays in it _setup.kins_angular_joints = 0; for (int jno = 0; jno < _setup.kins_joints && jno < EMCMOT_MAX_JOINTS; jno++) { char section[16]; @@ -914,6 +915,8 @@ int Interp::init() if (auto type = inifile.findString("TYPE", section)) { if (*type == "ANGULAR") { _setup.kins_angular_joints |= 1 << jno; } } + _setup.kins_joint_min[jno] = inifile.findRealV("MIN_LIMIT", section, -1e99); + _setup.kins_joint_max[jno] = inifile.findRealV("MAX_LIMIT", section, 1e99); } _setup.tolerance_default = inifile.findRealV("G64_DEFAULT_TOLERANCE", "RS274NGC", 0.0); diff --git a/tests/twp-native/README b/tests/twp-native/README index 9a49937006d..868da2c4f14 100644 --- a/tests/twp-native/README +++ b/tests/twp-native/README @@ -7,7 +7,7 @@ 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.6 standing near the end of C's travel reaches the plane by the nearest pose inside it, never running C past its limit. Q1 lets the table take part. The point-to-point moves are checked too: G53.4 G0 to a program point, G53.5 and G53.7 G0 to a slide position with the head tilted, 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/test-ui.py b/tests/twp-native/test-ui.py index 4008e987d0a..aed1f65744c 100755 --- a/tests/twp-native/test-ui.py +++ b/tests/twp-native/test-ui.py @@ -320,6 +320,63 @@ def in_plane(): error("#<_orient_a> after G53.2: %s" % m[1]) drain() +# --- the orientation stays inside the rotary travel -------------------- +# the primary rotary C travels -320 to 320: standing at C300, a plane whose +# nearest pose puts C on the turn past 320 is reached the long way round or +# by the other pose, whichever is nearer inside the travel, and never by +# running C out of it +C_MIN, C_MAX, B_MIN, B_MAX = -320.0, 320.0, -185.0, 185.0 + +def turn_near(v, now, lo=None, hi=None): + turns = math.floor((now - v) / 360.0 + 0.5) + if lo is not None: + turns = min(max(turns, math.ceil((lo - v) / 360.0)), math.floor((hi - v) / 360.0)) + return v + 360.0 * turns + +def pose_near(pairs, b_now, c_now, limited): + best = None + for b, cc in pairs: + if limited: + bb, cc = turn_near(b, b_now, B_MIN, B_MAX), turn_near(cc, c_now, C_MIN, C_MAX) + else: + bb, cc = turn_near(b, b_now), turn_near(cc, c_now) + d = abs(bb - b_now) + abs(cc - c_now) + if best is None or d < best[0]: + best = (d, bb, cc) + return best[1], best[2] + +mdi("G69") +stand = mdi("G0 A0 B0 C300") +plane = None +for i in range(-60, 61, 5): + for j in range(-60, 61, 5): + z = list(rot_y(j).dot(rot_x(i))[:, 2]) + pairs = oracle_pairs(z) + if pairs and pose_near(pairs, stand[SECONDARY], stand[PRIMARY], False)[1] > C_MAX: + plane = (i, j, z, pose_near(pairs, stand[SECONDARY], stand[PRIMARY], True)) + break + if plane: + break +if not plane: + error("no plane on the grid puts the nearest C past the travel") +else: + i, j, z, want = plane + mdi("G68.2 P1 Q123 I%d J%d K0" % (i, j)) + after, samples = sampled("G53.6") + show("G53.6 at the C limit", after) + drain() + top = max(smp[0][PRIMARY] for smp in samples) + bottom = min(smp[0][PRIMARY] for smp in samples) + print("plane I%d J%d from C300: C went %.4f to %.4f, the nearest pose inside the travel is B %.4f C %.4f" + % (i, j, bottom, top, want[0], want[1])) + if top > C_MAX + 1e-6 or bottom < C_MIN - 1e-6: + error("G53.6 ran C out of its travel") + if abs(after[SECONDARY] - want[0]) > 1e-3 or abs(after[PRIMARY] - want[1]) > 1e-3: + error("G53.6 ended at B %.4f C %.4f, not the nearest pose inside the travel" + % (after[SECONDARY], after[PRIMARY])) + if not close(tool_axis(after), z, 1e-6): + error("the tool axis after G53.6 at the C limit is not the plane normal") + # --- G53.3 goes to a point in the plane with the tool oriented ---------- before = mdi("G69") R3 = rot_y(-25).dot(rot_x(35)) From a1f375239af05248106697b92d4573d05685478f Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 10 Sep 2026 19:03:51 +0800 Subject: [PATCH 59/77] configs: the bridgemill sim shows off the tilted work plane family The old 5axisgui.ngc "drilled" a sphere with W words, W standing in for a tool length: the W axis of 5axiskins never drove a physical joint, the stroke folds into the XYZ slides, and the model's drawn quill hid that by cancelling the very motion that makes a W word work. The quill and its drawing lock go, so a W word shows the slides plunging, and 5axisgui.ngc keeps its name with comments saying what the controller cannot see. The sphere is then drilled two honest ways: g532-fused-orient-move.ngc, where G53.2 solves each plane and one fused G0 turns the head and travels to the hole, and g536-orient-then-move.ngc, where G53.6 reorients about the tip and a separate move travels in the plane. The demos and the M428 remap select the TCP kinematics with G12.1 P0, since on 5axiskins G13.1 lands on the identity. The AXIS GEOMETRY drops W: under TCP a W word never moves the tip, so W in the live plot only ever lied. --- .../axis/vismach/5axis/bridgemill/5axis.ini | 9 +++- .../vismach/5axis/bridgemill/5axisgui.ngc | 15 ++++++ .../sim/axis/vismach/5axis/bridgemill/README | 50 +++++++++++++++++-- .../bridgemill/g532-fused-orient-move.ngc | 50 +++++++++++++++++++ .../bridgemill/g536-orient-then-move.ngc | 49 ++++++++++++++++++ 5 files changed, 166 insertions(+), 7 deletions(-) create mode 100644 configs/sim/axis/vismach/5axis/bridgemill/g532-fused-orient-move.ngc create mode 100644 configs/sim/axis/vismach/5axis/bridgemill/g536-orient-then-move.ngc 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.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 From 8cc637610b7a8378eaa330f07ff84650e2e5dfcd Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 14 Sep 2026 15:37:46 +1000 Subject: [PATCH 60/77] motion, interp: a tool offset change under a tilt keeps the joints A module that applies the tool length itself reads it from motion, so a G43 with the head tilted moves the programmed point, not the joints; motion, task and the interpreter each did something else. Motion re-reads the point from the joints under the new tool, as after a kinematics switch, takes the external offsets off, moves the planner onto it and latches the joint hold there. The interpreter predicts the same point through the loader, from the joints the machine stands on, or for a G43.4 that switches on the same line from the joints read in the old kinematics before the switch, evaluating with the tool offset the program is under rather than the one motion has reached; the point rides along with the offset on EMC_TRAJ_SET_OFFSET, and motion aborts when its own answer differs. Under the identity, and where the module cannot be evaluated, the interpreter shifts by the offset difference as before and motion aborts only if the point moved. Task issues the offset once the queue has drained, so no queue busting is needed. tests/kins-switch applies G43.4 and a zero length in a tilted pose and after a point-to-point move: the joints hold, the point moves by the tilt term, and a move of nothing from the interpreter's point goes nowhere. --- docs/src/gcode/g-code.adoc | 13 ++++ docs/src/man/man9/kins.9.adoc | 16 +++-- docs/src/motion/switchkins.adoc | 9 ++- lib/python/rs274/interpret.py | 3 + src/emc/motion/command.c | 10 ++- src/emc/motion/control.c | 95 ++++++++++++++++++++++---- src/emc/motion/mot_priv.h | 2 + src/emc/motion/motion.h | 4 ++ src/emc/nml_intf/canon.hh | 9 +++ src/emc/nml_intf/emc.cc | 2 + src/emc/nml_intf/emc.hh | 2 +- src/emc/nml_intf/emc_nml.hh | 6 +- src/emc/rs274ngc/canonmodule.cc | 2 +- src/emc/rs274ngc/gcodemodule.cc | 21 ++++++ src/emc/rs274ngc/interp_convert.cc | 99 ++++++++++++++++++++-------- src/emc/rs274ngc/interp_workplane.cc | 92 +++++++++++++++++++++++--- src/emc/rs274ngc/rs274ngc_interp.hh | 5 ++ src/emc/sai/saicanon.cc | 10 +++ src/emc/task/emccanon.cc | 24 ++++++- src/emc/task/emctaskmain.cc | 9 ++- src/emc/task/taskintf.cc | 6 +- tests/kins-switch/README | 6 +- tests/kins-switch/test-ui.py | 68 ++++++++++++++----- 23 files changed, 433 insertions(+), 80 deletions(-) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 329fa3dae64..172a5d1e140 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -1617,6 +1617,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 diff --git a/docs/src/man/man9/kins.9.adoc b/docs/src/man/man9/kins.9.adoc index 79a3a116e21..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 @@ -254,8 +256,8 @@ replacing it. Put a given length in one column or the other, not both. Tool length, applied along the tool rather than along Z, so that the tip stays on the programmed point as B tilts. Motion hands the module the offset in effect (G43, G49), so the pin needs no connection; it is read - only until motion has sent anything. To avoid a joint jump, change the - tool offset only when B is 0. + only until motion has sent anything. A tool offset change with B tilted + moves the programmed point, not the joints. === pentakins - Pentapod Kinematics @@ -291,7 +293,9 @@ The forward kinematics iteration is controlled by HAL pins. *pentakins.tool-offset*:: TCP offset from effector origin along Z to implement RTCP function. - To avoid joints jump change tool offset only when the platform is not tilted. + Motion hands the module the offset in effect (G43, G49), so the pin needs + no connection. A tool offset change with the platform tilted moves the + programmed point, not the joints. === pumakins - kinematics for puma typed robots @@ -416,8 +420,8 @@ expected by it (XYZBCW `->` joints 0..5) Tool length, applied along the tool rather than along Z, so that the tip stays on the programmed point as B and C move. Motion hands the module the offset in effect (G43, G49), so the pin needs no connection; it is read - only until motion has sent anything. To avoid a joint jump, change the - tool offset only when B is 0. A tool length in the W column of the tool + only until motion has sent anything. A tool offset change with B tilted + moves the programmed point, not the joints. A tool length in the W column of the tool table reaches the same place, once a block commands W, and adds to this one rather than replacing it. Put a given length in one column or the other, not both. diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 01dd48b32f8..75bfb3a5e9c 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -619,8 +619,13 @@ exports kinematicsSetTool() as well, through which motion hands it the tool offset in effect whenever that changes. A table entry flagged as the tool is overwritten with it, and the entry's pin only matters until motion has sent anything, so a config need not net -motion.tooloffset.z to the module. A kinstype registered the older -way reads its own pins and is not affected. +motion.tooloffset.z to the module. When the offset changes, motion +keeps the joints where they are and reads the point back from them +under the new offset, so a tool length change with the tool axis +tilted moves the programmed point rather than the machine; the +interpreter reads the point the same way through the non-realtime +loader, and motion stops the program if the two disagree. A kinstype +registered the older way reads its own pins and is not affected. === Module main program diff --git a/lib/python/rs274/interpret.py b/lib/python/rs274/interpret.py index 0662adc90bc..055909ade05 100644 --- a/lib/python/rs274/interpret.py +++ b/lib/python/rs274/interpret.py @@ -196,5 +196,8 @@ def get_block_delete(self): 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/emc/motion/command.c b/src/emc/motion/command.c index bfa2495e304..d94d8ee0686 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -2227,10 +2227,16 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) case EMCMOT_SET_OFFSET: rtapi_print_msg(RTAPI_MSG_DBG, "SET_OFFSET"); - emcmotStatus->tool_offset = emcmotCommand->tool_offset; if (kinematicsSetTool) { - kinematicsSetTool(&emcmotStatus->tool_offset); + /* the module applies the offset, so the point the joints + stand on changes with it */ + kinematicsSetTool(&emcmotCommand->tool_offset); + emcmotToolOffsetChanged(&emcmotStatus->tool_offset, + &emcmotCommand->tool_offset, + &emcmotCommand->pos, + emcmotCommand->have_point); } + emcmotStatus->tool_offset = emcmotCommand->tool_offset; break; case EMCMOT_SET_AXIS_POSITION_LIMITS: diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 745db82b0b7..35275c93144 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,8 +364,6 @@ static void handle_kinematicsSwitch(void) { return; // the kinematics in force is unchanged } - KINEMATICS_FORWARD_FLAGS tmpFFlags = fflags; - KINEMATICS_INVERSE_FLAGS tmpIFlags = iflags; #ifdef SWITCHKINS_DEBUG double beforePose[EMCMOT_MAX_AXIS]; int anum; @@ -376,9 +375,7 @@ static void handle_kinematicsSwitch(void) { solve them is one the machine cannot run in from here: put the old one back, or the inverse would run the joints to wherever the pose we know lands in the new one */ - EmcPose poseKinsSwitch = emcmotStatus->carte_pos_cmd; - if (kinematicsForward(joint_posKinsSwitch, &poseKinsSwitch, - &tmpFFlags, &tmpIFlags)) { + if (reanchor_pose(joint_posKinsSwitch, NULL) != 0) { kinematicsSwitch(switchkins_type); reportError(_("kinematicsForward failed for kinematics type %d," " type %d is still in force"), @@ -386,7 +383,6 @@ static void handle_kinematicsSwitch(void) { SET_MOTION_ERROR_FLAG(1); // abort return; // the kinematics in force and the position are unchanged } - emcmotStatus->carte_pos_cmd = poseKinsSwitch; switchkins_type = requested_type; hal_set_real(emcmot_hal_data->kins_type, (double)switchkins_type); @@ -398,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) { @@ -937,17 +953,74 @@ static int joint_hold_valid = 0; static double joint_hold[EMCMOT_MAX_JOINTS]; static EmcPose joint_hold_pose; -/* whether two machine points are the same, to a hair either way */ -static int same_carte_pos(const EmcPose *a, const EmcPose *b) +/* 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) { - 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; } +/* 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; 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 f16b175cb49..038bace9856 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -281,6 +281,10 @@ extern "C" { double joint_target[EMCMOT_MAX_JOINTS]; int have_joint_target; double joint_seconds; /* 0 for a rapid, else the time the move is to take */ + + /* SET_OFFSET: pos is where the interpreter expects the point to be + once the offset is on, for motion to check its own answer against */ + int have_point; } emcmot_command_t; /*! \todo FIXME - these packed bits might be replaced with chars diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index 11092070206..d0dc274e05c 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -698,6 +698,11 @@ extern void USE_NO_SPINDLE_FORCE(); extern void SET_TOOL_TABLE_ENTRY(int pocket, int toolno, const EmcPose& offset, double diameter, double frontangle, double backangle, int orientation); extern void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset); +/* The same, with the point the interpreter expects the machine to stand + on once the offset is applied, in program coordinates: where a + kinematics applies the offset itself, motion keeps the joints and + re-reads the point from them, and compares it with this one. */ +extern void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset, const EmcPose& point); extern void CHANGE_TOOL(); @@ -952,6 +957,10 @@ extern int GET_EXTERNAL_KINS_TYPE(); kinematics.h); -1 where it says nothing: no such type, plain kinematics, or no motion controller attached (sai, preview) */ extern int GET_EXTERNAL_KINS_TYPE_FLAGS(int ktype); +/* whether the machine's kinematics is the identity, the joints being the + world, so that no tool offset is applied by the kinematics; true where + no motion controller is attached (sai, a preview without a machine) */ +extern bool GET_EXTERNAL_KINEMATICS_IDENTITY(); // Returns the current motion path-following tolerance extern double GET_EXTERNAL_MOTION_CONTROL_TOLERANCE(); diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index 94942565fcd..6595ca4f8e2 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -1607,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 diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index a9b26278601..ca7b7f6eb07 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -382,7 +382,7 @@ extern int emcTrajCircularMove(const EmcPose& end, const PM_CARTESIAN& center, c 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 a1dd0bc5443..00927480158 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 { 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 d28b0a30065..5f313c9c572 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -57,6 +57,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 @@ -651,6 +652,10 @@ void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) { offset.u, offset.v, offset.w})); } +void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset, const EmcPose& /*point*/) { + USE_TOOL_LENGTH_OFFSET(offset); +} + void SET_FEED_REFERENCE(double /*reference*/) { } void SET_CUTTER_RADIUS_COMPENSATION(double /*radius*/) {} void START_CUTTER_RADIUS_COMPENSATION(int /*direction*/) {} @@ -945,6 +950,22 @@ 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() { + if(parse_state.interp_error) return true; + py::handle canon(parse_state.callback); + if(!py::hasattr(canon, "get_kinematics_type")) return true; + try { + return canon.attr("get_kinematics_type")().cast() == KINEMATICS_IDENTITY; + } catch(py::error_already_set &) { + return true; // the error goes with the exception + } catch(py::builtin_exception &) { + return true; + } +} void SET_NAIVECAM_TOLERANCE(double /*tolerance*/) { } #define RESULT_OK (result == INTERP_OK || result == INTERP_EXECUTE_FINISH) diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index fdd477f8053..c107caf83a3 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -6645,6 +6645,8 @@ 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; @@ -6656,7 +6658,18 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu // apply the offset, as if the switch line had run and drained. With // no kinematics attached there is nothing to switch to. CHKS(primary < 0 && kins_type_info_available(), NCE_NO_PRIMARY_KINEMATICS_TYPE); - if (primary >= 0) { switch_kins_type(primary, settings); } + if (primary >= 0 && primary != settings->kins_type) { + // the switch keeps the joints and moves the point, so the point + // the offset is read from is not the one the program is at: take + // the joints, while the kinematics they are known in is in force + void *vctx; + CHP(kins_here(settings, &vctx)); + if (vctx) { + CHP(current_joints(settings, vctx, standing)); + have_standing = true; + } + switch_kins_type(primary, settings); + } settings->kins_by_g43_4 = true; } else if (g_code != G_49) { // the offset in effect is no longer G43.4's, so G49 has no switch to undo @@ -6746,31 +6759,65 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu } else { ERS("BUG: Code not G43, G43.1, G43.2, G43.4, or G49"); } - USE_TOOL_LENGTH_OFFSET(tool_offset); - - double dx, dy, dz; - - // the tool does not move, so its program coordinates change by the - // offset difference seen from the program: the XY rotation and the - // tilted work plane taken off it - dx = settings->tool_offset.tran.x - tool_offset.tran.x; - dy = settings->tool_offset.tran.y - tool_offset.tran.y; - dz = settings->tool_offset.tran.z - tool_offset.tran.z; - - rotate(&dx, &dy, -settings->rotation_xy); - g68_unrotate(settings, &dx, &dy, &dz); - - settings->current_x += dx; - settings->current_y += dy; - settings->current_z += dz; - settings->AA_current += settings->tool_offset.a - tool_offset.a; - settings->BB_current += settings->tool_offset.b - tool_offset.b; - settings->CC_current += settings->tool_offset.c - tool_offset.c; - settings->u_current += settings->tool_offset.u - tool_offset.u; - settings->v_current += settings->tool_offset.v - tool_offset.v; - settings->w_current += settings->tool_offset.w - tool_offset.w; - - settings->tool_offset = tool_offset; + // 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; + } // Update parameters #5081-#5089 to reflect the tool length offset // actually applied to motion (covers G43, G43Hn with n != loaded tool, diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 4978343f9a5..8817da03b10 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -441,11 +441,11 @@ int Interp::convert_work_plane(int g_code, block_pointer block, setup_pointer s) } //---------------------------------------------------------------------- -// The kinematics. G68.3 and the orientation moves need the frames and -// the tool frame inverse of the module motion runs, evaluated here, ahead -// of motion, through the loader in kinematics_userspace/. The loader -// binds its pins to a HAL component, so the interpreter makes one, named -// by its process, the first time it is asked. +// 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 @@ -456,12 +456,11 @@ int Interp::convert_work_plane(int g_code, block_pointer block, setup_pointer s) #define KINS_CTX(s) ((KinematicsUserContext *)(s)->kins_ctx) -// the loaded module, on the kinematics type the program is in -int Interp::kins_context(setup_pointer s, void **out) +// the module loaded, once +int Interp::kins_load(setup_pointer s) { KinematicsUserContext *ctx; - *out = NULL; if (!s->kins_ctx) { char name[HAL_NAME_LEN + 1]; int comp; @@ -481,15 +480,46 @@ int Interp::kins_context(setup_pointer s, void **out) 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) { @@ -584,6 +614,52 @@ int Interp::current_joints(setup_pointer s, void *vctx, double *joints) 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 of the plane in world coordinates: the plane's rotation // then the XY rotation of the coordinate system it sits on static void plane_axis_in_world(setup_pointer s, int column, double rotation_xy, PmCartesian *out) diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index 517a82e5d52..cb7771282b8 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -383,7 +383,12 @@ public: 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]); diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index 9e146037f27..268102004b7 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -605,6 +605,11 @@ void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) offset.tran.x, offset.tran.y, offset.tran.z, offset.a, offset.b, offset.c, offset.u, offset.v, offset.w); } +void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset, const EmcPose& /*point*/) +{ + USE_TOOL_LENGTH_OFFSET(offset); +} + void CHANGE_TOOL() { PRINT("CHANGE_TOOL()\n"); @@ -847,6 +852,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); diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index c9d3fae41f2..7d8fa5f3722 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -3049,7 +3049,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(); @@ -3078,6 +3078,13 @@ void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) set_offset_msg->offset.v = TO_EXT_AX(7, canon.toolOffset.v); set_offset_msg->offset.w = TO_EXT_AX(8, 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); @@ -3086,6 +3093,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() { @@ -4065,6 +4082,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/emctaskmain.cc b/src/emc/task/emctaskmain.cc index 4374cb5c03e..0602c9fc96a 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -1992,11 +1992,14 @@ 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; diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index ece2c8ce33c..1b709fb0292 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -1554,10 +1554,14 @@ int emcTrajJointMove(const EmcPose& end, const double *joints, int have_joints, return usrmotWriteEmcmotCommand(&emcmotCommand); } -int emcTrajSetOffset(const EmcPose& tool_offset) +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/tests/kins-switch/README b/tests/kins-switch/README index e426b4d7554..0815a214c3c 100644 --- a/tests/kins-switch/README +++ b/tests/kins-switch/README @@ -8,5 +8,7 @@ the selection reaches the motion controller and the interpreter, that a negative P word and a kinematics the module does not provide are refused, that G13.1 cancels to the identity kinematics the module declares (type 1 here, not 0), that the tool length G43.4 puts in effect reaches the -kinematics with nothing netted to its pin, and that G13.1 in an -ON_ABORT_COMMAND routine does not swallow the rest of the routine. +kinematics with nothing netted to its pin and, the head being tilted, +moves the programmed point rather than the joints, with the interpreter +agreeing on where the point went, 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 20544d5dd64..c4ec15923d7 100755 --- a/tests/kins-switch/test-ui.py +++ b/tests/kins-switch/test-ui.py @@ -154,37 +154,75 @@ def mdi(cmd): else: print("G43.4 switched to primary with the offset, G49 cancelled both") -# ---- the tool length reaches the kinematics with nothing netted ---------- +# ---- a tool length under a tilt keeps the joints ------------------------ # -# Motion hands the module the offset G43 puts in effect. The head is -# tilted, so the offset moves the joints, by the tilt term of the length: -# the same length along the tool axis instead of along Z. +# Motion hands the module the offset G43 puts in effect, with nothing +# netted. The head is tilted, so the length lies along the tool axis +# instead of along Z: the point the joints stand on moves by the tilt +# term of the length, and the joints stay where they are. The +# interpreter works out the same point ahead of motion, so a move to +# where it thinks the machine is goes nowhere. import math c.mode(linuxcnc.MODE_MDI) c.wait_complete(30) drain() + +def pose(): + s.poll() + return list(s.position[:3]) + +def tool_length_check(what, before_joints, before_pose, after_joints, after_pose, want): + if max(abs(after_joints[j] - before_joints[j]) for j in range(JOINTS)) > 1e-6: + error("%s moved the joints by %s" % (what, + " ".join("%.4f" % (after_joints[j] - before_joints[j]) for j in range(JOINTS)))) + got = [after_pose[j] - before_pose[j] for j in (0, 1, 2)] + if max(abs(g - w) for g, w in zip(got, want)) > 1e-3: + error("%s moved the point by %s, not %s" % (what, + " ".join("%.4f" % v for v in got), " ".join("%.4f" % v for v in want))) + said = [m[1].strip() for m in drain() if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR)] + if said: + error("%s raised %s" % (what, said)) + # the interpreter's point: a move of nothing from it goes nowhere + mdi("G91") + stayed = mdi("G0 X0 Y0 Z0") + mdi("G90") + if max(abs(stayed[j] - after_joints[j]) for j in range(JOINTS)) > 1e-6: + error("after %s the interpreter has the point elsewhere: a move of nothing moved the joints by %s" + % (what, " ".join("%.4f" % (stayed[j] - after_joints[j]) for j in range(JOINTS)))) + mdi("G12.1 P0") mdi("G0 X10 Y10 Z-5 B-22.5 C45") mdi("G49") +errors_before = errors before = mdi("G0 X10 Y10 Z-5") +before_pose = pose() after = mdi("G43.4 H1") +pose_after = pose() L = 12.5 # tool 1 in tool.tbl b, cc = math.radians(-22.5), math.radians(45) -want = [-L * math.sin(math.pi - b) * math.cos(cc), - -L * math.sin(math.pi - b) * math.sin(cc), - -L * (1 + math.cos(math.pi - b))] -got = [after[j] - before[j] for j in (0, 1, 2)] -if max(abs(g - w) for g, w in zip(got, want)) > 1e-3: - error("G43.4 in the tilted pose moved the joints by %s, not %s" - % (" ".join("%.4f" % v for v in got), " ".join("%.4f" % v for v in want))) -else: - print("G43.4 moved the joints by the tilt term of the tool length") +# the length along the tool axis, less the length along Z the offset stands for +want = [L * math.sin(math.pi - b) * math.cos(cc), + L * math.sin(math.pi - b) * math.sin(cc), + L * (1 + math.cos(math.pi - b))] +tool_length_check("G43.4 in the tilted pose", before, before_pose, after, pose_after, want) # G49 would drop to identity and hold the joints where they are; a zero # offset without a switch takes the length back out back = mdi("G43.1 Z0") -if max(abs(back[j] - before[j]) for j in (0, 1, 2)) > 1e-3: - error("a zero tool length did not take the length back out of the joints") +tool_length_check("a zero tool length", after, pose_after, back, pose(), [-w for w in want]) +if errors == errors_before: + print("a tool length under a tilt moved the point, not the joints") + +# a point-to-point move ends on joints motion holds while the point +# stays; a tool length moves the point, and the joints are held on +errors_before = errors +p2p = mdi("G53.4 G0 X10 Y10 Z-5 B-22.5 C45") +p2p_pose = pose() +after = mdi("G43.1 Z%g" % L) +tool_length_check("a tool length after a point-to-point move", p2p, p2p_pose, after, pose(), want) +if errors == errors_before: + print("a tool length after a point-to-point move keeps the joints too") +mdi("G43.1 Z0") mdi("G49") # ---- a negative kinematics number is refused ----------------------------- From 05d70609ee5281458e8829169fff6f9e57a68647 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 15 Sep 2026 15:58:17 +1000 Subject: [PATCH 61/77] interp: G43.5, the tool axis as a vector G43.5 is G43.4 with one thing more: while it is in effect a G0 or G1 line may give the tool axis as I J K in place of rotary words, the form of a five-axis program written for the tool rather than the machine, Fanuc's tool center point control type 2. The rotaries are solved through the module's tool frame inverse, the solver G53.1 uses, split out as orient_solve(): every orienting joint free, the pose nearest the present one, so a path of vectors runs continuous; on the pole of the primary rotary the free joint stays. The pose enters the move as program rotary words through machine_pose_to_program(), so rotary offsets are right by construction and the move interpolates like any other. The vector is read in the coordinate system the line's X Y Z are in, the tilted plane where one is active, the machine's on a G53 line. A direction is never incremental, so G91 applies to the axis words only, and I J K alone turns the tool in place. No configuration names the rotaries: the same program orients a head, a table and a robot. Refused: a vector with a rotary word, a zero vector, the identity kinematics, a vector on a G53.5 or G53.7 line. Left out: Fanuc's interpolation of the vector between lines, which needs the kinematics inside the planner. The semantics follow the G43.5 greatEndian built for xyzab_tdr_kins on the fork, including that the vector is absolute under G91; this is the generic form. tests/kins-switch covers the vector against rotary words, G91, the pole, a rotary offset and the refusals; docs in g-code.adoc. --- docs/src/gcode/g-code.adoc | 69 ++++- docs/src/gcode/overview.adoc | 2 +- docs/src/motion/kinematics-conventions.adoc | 4 +- docs/src/motion/switchkins.adoc | 7 +- src/emc/rs274ngc/interp_array.cc | 2 +- src/emc/rs274ngc/interp_check.cc | 17 +- src/emc/rs274ngc/interp_convert.cc | 22 +- src/emc/rs274ngc/interp_internal.cc | 7 +- src/emc/rs274ngc/interp_internal.hh | 2 + src/emc/rs274ngc/interp_setup.cc | 1 + src/emc/rs274ngc/interp_workplane.cc | 277 ++++++++++++-------- src/emc/rs274ngc/interp_write.cc | 9 +- src/emc/rs274ngc/rs274ngc_interp.hh | 3 + tests/kins-switch/README | 8 +- tests/kins-switch/test-ui.py | 72 +++++ 15 files changed, 372 insertions(+), 130 deletions(-) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 172a5d1e140..541d544431d 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -86,6 +86,7 @@ as the 'L number', and so on for any other letter. |<> |Dynamic Tool Length Offset |<> |Apply additional Tool Length Offset |<> |Tool Length Offset on Primary Kinematics +|<> |Tool Length Offset with the Tool Axis as a Vector |<> |Cancel Tool Length Offset |<> |Local Coordinate System Offset |<> |Move in Machine Coordinates @@ -1776,13 +1777,79 @@ It is an error if: kinematics, or * any of the 'G43' error conditions holds. +[[gcode:g43.5]] +== G43.5 Tool Length Offset with the Tool Axis as a Vector(((G43.5 Tool Length Offset with the Tool Axis as a Vector))) + +[source,ngc] +---- +G43.5 +G0 X- Y- Z- I- J- K- +G1 X- Y- Z- I- J- K- F- +---- + +* 'H' - tool number (optional) +* 'I J K' - the direction the tool axis is to point along, on a 'G0' or + 'G1' line while 'G43.5' is in effect + +'G43.5' is '<>' with one thing more: while it is in +effect, a 'G0' or 'G1' line may give the direction of the tool axis as +a vector 'I J K' in place of rotary words, and the interpreter works +out where the rotary joints have to go to point the tool that way. +This is the form of a five-axis program written for the tool rather +than for the machine, the one Fanuc calls tool center point control +type 2, and it runs on any machine whose kinematics module supplies its +tool frame (see the <> chapter): the same program orients a tilting head, a +tilting table and a robot. + +The vector points from the tool tip towards the holder, in the +coordinate system the line's 'X Y Z' are in: the active coordinate +system with its rotation, the <> where +one is active, and the machine's on a '<>' line. Its +length does not matter. A word left out is zero, so 'K1' alone is the +tool vertical. The words describe a direction, so they are never +incremental: 'G91' applies to the 'X Y Z' of the line, not to 'I J K', +and 'G91.1' does not apply to them either. A line with 'I J K' and no +axis word turns the tool where it stands. + +Where more than one pose of the rotaries points the tool along the +vector, two on a five-axis machine, the interpreter takes the one +nearest where the rotaries stand inside their travel, every orienting joint free to take +part, so a path of vectors runs continuous from wherever it starts and +a rotary swings the long way round only where the short way runs out of travel. On the pole of the primary +rotary, the tool vertical on a mill, one joint no longer matters to the +direction and it stays where it is. + +The pose the interpreter finds goes into the move as ordinary rotary +words, in program coordinates, so the rotary offsets of the coordinate +system apply, and the move interpolates like any other move with rotary +words: the rotaries turn together with the linear axes between the +poses of consecutive lines. The vector itself is not interpolated +along the way; a program that needs the tool to sweep between two +directions writes the lines in between. + +'G2' and 'G3' keep 'I J K' as the arc centre; the tool direction on an +arc is given with rotary words. 'G43', 'G43.1', 'G43.2' and 'G49' end +the vector form along with the offset they replace; 'G49' cancels the +offset and undoes the kinematics switch, as after 'G43.4'. + +It is an error if: + +* 'I J K' are given together with a rotary word on the same line, +* the vector is zero, +* the kinematics selected is the identity one, or the module supplies + no tool frame or cannot be evaluated by the interpreter, +* the direction cannot be reached by the rotary joints, +* 'I J K' are given on a 'G53.5' or 'G53.7' line, or +* any of the 'G43.4' error conditions holds. + [[gcode:g49]] == G49 Cancel Tool Length Compensation(((G49 Cancel Tool Length Offset))) * 'G49' - cancels tool length compensation 'G49' also switches a switchable kinematics module back to its identity -kinematics when it cancels a 'G43.4', undoing the switch that made. It +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. diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index 0079306428b..7151d2bc6db 100644 --- a/docs/src/gcode/overview.adoc +++ b/docs/src/gcode/overview.adoc @@ -985,7 +985,7 @@ The modal groups are shown in the following Table. |Feed Rate Mode (Group 5) | G93, G94, G95 |Units (Group 6) | G20, G21 |Cutter Diameter Compensation (Group 7) | G40, G41, G42, G41.1, G42.1 -|Tool Length Offset (Group 8) | G43, G43.1, G43.2, G43.4, G49 +|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 diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 7d691d6b547..7d4fa9834fe 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -557,8 +557,8 @@ 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, joints are axes; `primary` the working transform -`G43.4` switches to; `machine` the machine frame type `G13.1` and `G49` -cancel to and `G53.5` moves in, which a module leaves unset when its +`G43.4` and `G43.5` switch to; `machine` the machine frame type `G13.1` and +`G49` cancel to and `G53.5` moves in, which a module leaves unset when its identity type is that frame (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 diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 75bfb3a5e9c..4e28826ca7b 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -226,9 +226,10 @@ see Code Notes for how a module declares its types. For tool length work there are spellings that name the kinematics by what it is rather than by number: 'G43.4' applies the tool length offset and switches to the kinstype the module declares its working -transform, and the 'G49' that cancels it switches back to identity. -See the G-code documentation for 'G43.4' and 'G49', and for 'G12.1' -and 'G13.1', for the full description. +transform, 'G43.5' does the same and lets the lines after it give the +tool axis as a vector, and the 'G49' that cancels either switches back +to 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 diff --git a/src/emc/rs274ngc/interp_array.cc b/src/emc/rs274ngc/interp_array.cc index ddff76a0118..e3931cca6ba 100644 --- a/src/emc/rs274ngc/interp_array.cc +++ b/src/emc/rs274ngc/interp_array.cc @@ -91,7 +91,7 @@ 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, 8,-1,-1,-1,-1,-1, +/* 420 */ 7, 7,-1,-1,-1,-1,-1,-1,-1,-1, 8, 8, 8,-1, 8, 8,-1,-1,-1,-1, /* 440 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 460 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 480 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 8,-1,-1,-1,-1,-1,-1,-1,-1,-1, diff --git a/src/emc/rs274ngc/interp_check.cc b/src/emc/rs274ngc/interp_check.cc index fcc457c1682..5c01f743b9c 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -310,15 +310,22 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block } if (block->h_flag) { - CHKS((block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43 && motion != G_76 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_2 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_4), - _("H word with no G43, G43.4 or G76 to use it")); + CHKS((block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43 && motion != G_76 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_2 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_4 && block->g_modes[GM_TOOL_LENGTH_OFFSET] != G_43_5), + _("H word with no G43, G43.4, G43.5 or G76 to use it")); } // G68.2 and G68.4 take I J K, P and Q; G68.3 only R, X Y Z; G69 nothing int plane_words = block->g_modes[GM_WORK_PLANE] == G_68_2 || block->g_modes[GM_WORK_PLANE] == G_68_4; int plane_r = plane_words || block->g_modes[GM_WORK_PLANE] == G_68_3; - 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) && @@ -328,7 +335,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block _("I word with no G2, G3, G5, G5.1, G6, G6.1, G10, G33.1, G68.2, G68.4, G76, or G87 to use it")); } - if (block->j_flag) { /* could still be useless if xz_plane arc */ + if (block->j_flag && !tool_vector) { /* could still be useless if xz_plane arc */ CHKS(((motion != G_2) && (motion != G_3) && (motion != G_5) && (motion != G_5_1) && (motion != G_6) && (motion != G_6_1) && (motion != G_76) && (motion != G_87) && (block->g_modes[GM_MODAL_0] != G_10) && @@ -343,7 +350,7 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block } } - if (block->k_flag) { /* could still be useless if xy_plane arc */ + if (block->k_flag && !tool_vector) { /* could still be useless if xy_plane arc */ CHKS(((motion != G_2) && (motion != G_3) && (motion != G_6_2) && (motion != G_33) && (motion != G_33_1) && (motion != G_76) && (motion != G_87) && !plane_words), _("K word with no G2, G3, G6.2, G33, G33.1, G68.2, G68.4, G76, or G87 to use it")); diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index c107caf83a3..714f6c23107 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -4193,7 +4193,8 @@ int Interp::convert_m(block_pointer block, //!< pointer to a block of RS27 if (FEATURE(RETAIN_G43)) { if (((settings->active_g_codes[9] == G_43) || - (settings->active_g_codes[9] == G_43_4)) && ONCE(STEP_RETAIN_G43)) { + (settings->active_g_codes[9] == G_43_4) || + (settings->active_g_codes[9] == G_43_5)) && ONCE(STEP_RETAIN_G43)) { if(settings->selected_pocket > 0) { struct block_struct g43; init_block(&g43); @@ -5697,13 +5698,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, @@ -6652,11 +6662,12 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu CHKS((settings->cutter_comp_side != CUTTER_COMP::OFF), (_("Cannot change tool offset with cutter radius compensation on"))); - if (g_code == G_43_4) { + if (g_code == G_43_4 || g_code == G_43_5) { int primary = flagged_kins_type(KINSTYPE_PRIMARY); // G43.4 is G43 on the module's working transform: switch first, then // apply the offset, as if the switch line had run and drained. With - // no kinematics attached there is nothing to switch to. + // no kinematics attached there is nothing to switch to. G43.5 is + // the same, and the lines after it may give the tool axis as I J K. CHKS(primary < 0 && kins_type_info_available(), NCE_NO_PRIMARY_KINEMATICS_TYPE); if (primary >= 0 && primary != settings->kins_type) { // the switch keeps the joints and moves the point, so the point @@ -6675,9 +6686,10 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu // the offset in effect is no longer G43.4's, so G49 has no switch to undo settings->kins_by_g43_4 = false; } + settings->tool_vector = (g_code == G_43_5); if (g_code == G_49) { idx = 0; - } else if (g_code == G_43 || g_code == G_43_4) { + } else if (g_code == G_43 || g_code == G_43_4 || g_code == G_43_5) { logDebug("convert_tool_length_offset h_flag=%d h_number=%d toolchange_flag=%d current_pocket=%d\n", block->h_flag,block->h_number,settings->toolchange_flag,settings->current_pocket); if(block->h_flag) { @@ -6757,7 +6769,7 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu if(block->w_flag) tool_offset.w += block->w_number; } } else { - ERS("BUG: Code not G43, G43.1, G43.2, G43.4, or G49"); + ERS("BUG: Code not G43, G43.1, G43.2, G43.4, G43.5, or G49"); } // The machine does not move, so the program coordinates of the point change. // A kinematics that applies the offset itself moves the point by more than diff --git a/src/emc/rs274ngc/interp_internal.cc b/src/emc/rs274ngc/interp_internal.cc index 01b1e73e047..ea3b1212ab5 100644 --- a/src/emc/rs274ngc/interp_internal.cc +++ b/src/emc/rs274ngc/interp_internal.cc @@ -216,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")); diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index ea0c78d60f3..cd025b5e6bc 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -250,6 +250,7 @@ enum GCodes G_43_1 = 431, G_43_2 = 432, G_43_4 = 434, + G_43_5 = 435, G_49 = 490, G_50 = 500, G_51 = 510, @@ -801,6 +802,7 @@ struct setup bool kinsSwitch_flag; // flag indicating waiting for kinematics switch done int kins_type; // kinematics selected by G12.1/G13.1 bool kins_by_g43_4; // G43.4 selected the kinematics, for G49 to undo + bool tool_vector; // G43.5: I J K on G0 and G1 give the tool axis bool toolchange_flag; // flag indicating we just had a tool change bool home_flag; // flag indicating a G28.2 homing cycle just ran int input_index; // channel queried diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index f9083a0726e..43c38c6cae5 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -137,6 +137,7 @@ setup::setup() : kinsSwitch_flag(0), kins_type(0), kins_by_g43_4(false), + tool_vector(false), toolchange_flag(0), home_flag(0), input_index(0), diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 8817da03b10..4a9e8a4a07e 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -660,20 +660,32 @@ int Interp::tool_offset_point(setup_pointer s, const EmcPose *offset, const doub return INTERP_OK; } -// a direction of the plane in world coordinates: the plane's rotation -// then the XY rotation of the coordinate system it sits on -static void plane_axis_in_world(setup_pointer s, int column, double rotation_xy, PmCartesian *out) +// 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 = s->g68_rotation[0][column]; - double y = s->g68_rotation[1][column]; - double z = s->g68_rotation[2][column]; + 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 @@ -758,6 +770,102 @@ int Interp::convert_work_plane_from_tool(block_pointer block, setup_pointer s) 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 solver reports each answer in (-180, 180], but the machine stands // somewhere in turn space: every angular joint of each pose goes onto the // turn nearest where it stands, or the nearest pose is not the nearest move @@ -795,68 +903,43 @@ static int orient_fit(setup_pointer s, double *solutions, int n, int njoints, co return kept; } -// 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 picks the pose, nearest -// first or by the sign of the tilting joint; Q0 holds the joints that carry -// the work (COORD ROT), Q1 frees them (TABLE ROT). -int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) +// The joints that point the tool along axis, and its x along xdir where given, +// from the joints the machine is at: every pose the module reports, on the +// nearest turn inside the travel, ranked by rotary travel. P picks by rank or by +// the sign of the tilting joint; Q0 holds the joints that carry the work +// (Heidenhain COORD ROT), Q1 frees them (TABLE ROT). +int Interp::orient_solve(setup_pointer s, void *vctx, const PmCartesian *axis, const PmCartesian *xdir, + int p, int q, const double *now, double *joints, const char *name) { - void *vctx; - KinematicsUserContext *ctx; - double now[EMCMOT_MAX_JOINTS]; + 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]; - PmCartesian axis, xdir; - EmcPose end_pose; - double end_prog[9]; unsigned int held = 0; - int p, q, n, i, j, chosen, njoints; + int n, i, j, chosen, njoints; bool reached; - const double *sol; - 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); njoints = kinematicsUserGetNumJoints(ctx); - 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); if (q == 0) { if (kinematicsUserWorkJoints(ctx, now, &held) != 0) { held = 0; } } - n = kinematicsUserToolFrameInverse(ctx, &axis, &xdir, now, held, + n = kinematicsUserToolFrameInverse(ctx, axis, xdir, now, held, solutions, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); reached = (n > 0); if (n > 0) { n = orient_fit(s, solutions, n, njoints, now); } if (n == 0 && held) { // nothing reachable with the work held still: let it move held = 0; - n = kinematicsUserToolFrameInverse(ctx, &axis, &xdir, now, held, + n = kinematicsUserToolFrameInverse(ctx, axis, xdir, now, held, solutions, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin); reached = reached || (n > 0); if (n > 0) { n = orient_fit(s, solutions, n, njoints, now); } } CHKS((n < 0), _("%s: the kinematics cannot answer the orientation"), name); CHKS((n == 0 && reached), - _("%s: every pose that reaches the plane's normal puts a rotary joint outside its travel"), name); - CHKS((n == 0), _("%s: the plane's normal cannot be reached by the rotary joints"), name); + _("%s: every pose that reaches the direction puts a rotary joint outside its travel"), name); + CHKS((n == 0), _("%s: the direction asked for cannot be reached by the rotary joints"), name); // nearest first, by rotary travel in joint units for (i = 0; i < n; i++) { @@ -889,70 +972,56 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) CHKS((chosen < 0), _("%s P%d: no reachable pose has joint %d %s"), name, p, secondary, (p == 1) ? "positive" : "negative"); } - sol = solutions + chosen * njoints; - - // where that puts the machine, and what the program calls it - end_pose = (EmcPose){}; - current_machine_pose(s, &end_pose); - { - double full[EMCMOT_MAX_JOINTS]; - for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { full[i] = (i < njoints) ? sol[i] : 0.0; } - CHKS((kinematicsUserForward(ctx, full, &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] = full[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; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + joints[i] = (i < njoints) ? solutions[chosen * njoints + i] : 0.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); +// 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 { - 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]; + 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; } diff --git a/src/emc/rs274ngc/interp_write.cc b/src/emc/rs274ngc/interp_write.cc index 54dfe128f98..755c7c8fc0f 100644 --- a/src/emc/rs274ngc/interp_write.cc +++ b/src/emc/rs274ngc/interp_write.cc @@ -111,16 +111,17 @@ int Interp::write_g_codes(block_pointer block, //!< pointer to a block of RS27 7) ? (530 + (10 * settings->origin_index)) : (584 + settings->origin_index); // the kins type, not the label, is the authority: a G43 given on the - // module's primary type shows as G43.4, and the label follows the type - // motion reports after a resync. -1 is "no information" and matches - // every flag, so it is excluded before the bit test. + // module's primary type shows as G43.4, or G43.5 while I J K give the + // tool axis, and the label follows the type motion reports after a + // resync. -1 is "no information" and matches every flag, so it is + // excluded before the bit test. kf = GET_EXTERNAL_KINS_TYPE_FLAGS(settings->kins_type); settings->active_g_codes[9] = (settings->g43_with_zero_offset || settings->tool_offset.tran.x || settings->tool_offset.tran.y || settings->tool_offset.tran.z || settings->tool_offset.a || settings->tool_offset.b || settings->tool_offset.c || settings->tool_offset.u || settings->tool_offset.v || settings->tool_offset.w) ? - ((kf >= 0 && (kf & KINSTYPE_PRIMARY)) ? G_43_4 : G_43) : G_49; + ((kf >= 0 && (kf & KINSTYPE_PRIMARY)) ? (settings->tool_vector ? G_43_5 : G_43_4) : G_43) : G_49; settings->active_g_codes[10] = (settings->retract_mode == RETRACT_MODE::OLD_Z) ? G_98 : G_99; // Three modes: G_64, G_61, G_61_1 or CANON_CONTINUOUS/EXACT_PATH/EXACT_STOP settings->active_g_codes[11] = diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index cb7771282b8..7b9a3df8df6 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -373,6 +373,9 @@ public: 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 slide_joints(const char *name, setup_pointer settings, void *ctx, const struct kins_params *params, int njoints, const int flags[9], const double words[9], double *joints); diff --git a/tests/kins-switch/README b/tests/kins-switch/README index 0815a214c3c..e409e2b3cf9 100644 --- a/tests/kins-switch/README +++ b/tests/kins-switch/README @@ -10,5 +10,9 @@ that G13.1 cancels to the identity kinematics the module declares (type 1 here, not 0), that the tool length G43.4 puts in effect reaches the kinematics with nothing netted to its pin and, the head being tilted, moves the programmed point rather than the joints, with the interpreter -agreeing on where the point went, and that G13.1 in an ON_ABORT_COMMAND -routine does not swallow the rest of the routine. +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 c4ec15923d7..b8f540e8908 100755 --- a/tests/kins-switch/test-ui.py +++ b/tests/kins-switch/test-ui.py @@ -225,6 +225,78 @@ def tool_length_check(what, before_joints, before_pose, after_joints, after_pose 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() From 4ab39f506bde0b0cd01fdc9e9eb3c57fd74185ec Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 18 Sep 2026 22:50:06 +1000 Subject: [PATCH 62/77] interp: G28.5 and G30.5, the machine frame forms of G28 and G30, and MACHINE_MOVES_NEED_MACHINE_FRAME G53, G28 and G30 move in the world of the kinematics in force with the offsets removed, and on a table-side kinematics that world turns with the table: G53 X0 Y0 Z0 under TCP with the table at A10 takes the tool tip to the turned origin, which the slides reach elsewhere. That is what the existing switchkins configs rely on, so it stays; the G53 section says so now, and points at G53.5 for the machine frame. G28.5 and G30.5 are to G28 and G30 what G53.5 is to G53: the stored position is read in the machine frame with the orientation left out, the way G53.5 reads its words, the slides on a machine whose slides line up with its frame, and the move is a joint-space traverse there, with no offset, tool length or work plane. Axis words on the line are a machine frame waypoint, and then only those letters go to their stored values, as G28 does with its words. The stored position has to be saved on the machine frame type, where the world is that frame. [RS274NGC] MACHINE_MOVES_NEED_MACHINE_FRAME, off by default, refuses G53, G28, G30, G28.1 and G30.1 whenever the kinematics in force is not the machine frame type, for a configuration that wants the machine codes to mean that frame and nothing else. The decision is the flags the module declares for the selected type, with the machine's kinematics type as the fallback for a module that switches nothing; a plain identity type that is not the machine frame is refused like any other. The letter mapping and the joint move come out of convert_ptp_joints into slide_joints and ptp_joint_move, shared by the three codes. The twp-native test stores a position on the identity, checks G28.5 and G30.5 with the head tilted under TCP, the waypoint included, sets the flag and checks what it refuses under TCP and accepts on the identity. The ptp-machine-frame test does the same on the slanted module, where G28.5 with the head tilted lands the carriage, and the flag refuses the codes under the plain identity too. The waypoint check wants the joint to reach its turn, and a status snapshot per task cycle misses a turn by up to 9e-3 mm at the test's deceleration, which failed on a loaded runner: the twp-native test now reads the path of every move from a sampler on the servo thread, logged by halsampler, where the turn falls inside one cycle. --- docs/src/config/ini-config.adoc | 4 + docs/src/gcode/g-code.adoc | 82 +++++++++++++++- docs/src/gcode/overview.adoc | 2 +- docs/src/motion/switchkins.adoc | 38 ++++---- src/emc/rs274ngc/interp_array.cc | 4 +- src/emc/rs274ngc/interp_check.cc | 21 +++- src/emc/rs274ngc/interp_convert.cc | 2 + src/emc/rs274ngc/interp_internal.cc | 1 + src/emc/rs274ngc/interp_internal.hh | 4 + src/emc/rs274ngc/interp_setup.cc | 1 + src/emc/rs274ngc/interp_workplane.cc | 137 ++++++++++++++++++++------- src/emc/rs274ngc/rs274ngc_interp.hh | 4 + src/emc/rs274ngc/rs274ngc_pre.cc | 3 + tests/ptp-machine-frame/README | 7 +- tests/ptp-machine-frame/test-ui.py | 32 +++++++ tests/ptp-machine-frame/test.ini | 2 + tests/twp-native/README | 13 ++- tests/twp-native/sim.hal | 16 ++++ tests/twp-native/test-ui.py | 108 +++++++++++++++++---- tests/twp-native/test.ini | 2 + tests/twp-native/test.sh | 2 +- 21 files changed, 402 insertions(+), 83 deletions(-) diff --git a/docs/src/config/ini-config.adoc b/docs/src/config/ini-config.adoc index b5c3c5af8bb..04c8b551cae 100644 --- a/docs/src/config/ini-config.adoc +++ b/docs/src/config/ini-config.adoc @@ -639,6 +639,10 @@ The maximum number of `USER_M_PATH` directories is defined at compile time (typ: When set M2 and M30 will no longer automatically reset the active WCS to 'G54'. * `DISABLE_FANUC_STYLE_SUB = 0` (Default: 0) If there is reason to disable Fanuc subroutines set it to 1. +* `MACHINE_MOVES_NEED_MACHINE_FRAME = 0` (bool, Default: 0) + + When set, 'G53', 'G28', 'G30', 'G28.1' and 'G30.1' are refused whenever the kinematics in force is not the machine frame type: the type the module flags as such, the identity kinematics on a machine whose slides line up with its frame, and neither the tool centre point nor a raw joint identity on one whose slides do not. + These codes work in the world of the kinematics in force, which on a table-side kinematics turns with the table; the machine frame positions are 'G53.5', 'G28.5' and 'G30.5'. + See <>. * 'G73_PECK_CLEARANCE = .020' (default: Metric machine: 1mm, imperial machine: .050 inches) Chip breaking back-off distance in machine units * 'G83_PECK_CLEARANCE = .020' (default: Metric machine: 1mm, imperial machine: .050 inches) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 541d544431d..c3d8d24cb52 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -74,6 +74,7 @@ as the 'L number', and so on for any other letter. |<> |Plane Select |<> |Set Units of Measure |<> |Go to Predefined Position +|<> |Go to Predefined Slide Position |<> |Home from G-code |<> |Go to Predefined Position |<> |Spindle Synchronized Motion @@ -1072,9 +1073,17 @@ all axes will go to the <>. G28 Z2.5 (rapid to Z2.5 then to Z location specified in #5163) ---- +The 'absolute' position is the machine coordinate system of the kinematics in +force, see <>: on a kinematics that is not the identity it is +that kinematics' world, not the slides, and the stored position is a world +position too. <> is the form that moves the slides. + It is an error if : * Cutter Compensation is turned on +* `MACHINE_MOVES_NEED_MACHINE_FRAME` is set in the `[RS274NGC]` section of the INI + file and the kinematics in force is not the machine frame kinematics, for + 'G28' and for 'G28.1'. [[gcode:g28.2]] == G28.2 Home from G-code(((G28.2 Home from G-code))) @@ -1186,9 +1195,61 @@ if TOOL_CHANGE_AT_G30=1 is in the [EMCIO] section of the INI file. G30 Z2.5 (rapid to Z2.5 then to the Z location specified in #5183) ---- +The 'absolute' position is the machine coordinate system of the kinematics in +force, see <>; <> is the form that moves +the slides. + It is an error if : * Cutter Compensation is turned on +* `MACHINE_MOVES_NEED_MACHINE_FRAME` is set in the `[RS274NGC]` section of the INI + file and the kinematics in force is not the machine frame kinematics, for + 'G30' and for 'G30.1'. + +[[gcode:g28.5]] +== G28.5, G30.5 Go to Predefined Slide Position(((G28.5 Go to Predefined Slide Position))) + +[source,ngc] +---- +G28.5 +G30.5 +---- + +'G28.5' and 'G30.5' are to 'G28' and 'G30' what <> is to +'G53': the stored position, parameters 5161-5169 for 'G28.5' and 5181-5189 +for 'G30.5', is read in the machine frame with the orientation left out, the +way 'G53.5' reads its words, the slides on a machine whose slides line up +with its frame, and the move is a <> there, +rapid, with no offset, tool length, rotation or work plane applied. The +values are in the machine units of the INI file, as for 'G28'. The stored +position has to be a machine frame position: store it with 'G28.1' or 'G30.1' +while the identity kinematics is in force, where the world is that frame. + +* 'G28.5' - moves every letter the machine has to the stored position. +* 'G28.5 axes' - moves the letters named by 'axes' to the given machine + frame position first, then the same letters to their stored positions. A + letter not named holds its machine frame coordinate. + +On a mill whose head is tilted and a table-side kinematics in force, 'G28' +takes the tool tip to the stored point, where the head's tilt puts the slides +somewhere else; 'G28.5' takes the slides there, whatever the head is doing, +which is what a park or a tool change position wants. + +.G28.5 Example Line +[source,ngc] +---- +G28.5 Z0 (the Z slide to zero, then to the Z stored in #5163) +G30.5 (every slide to the position stored with G30.1) +---- + +It is an error if: + +* Cutter Compensation is turned on +* Polar coordinates are used, or incremental distance mode is in force. +* An axis letter is used that is not a joint of the kinematics, the + machine's letters name joints of the other kind, or the machine frame + kinematics cannot reach the point, see <>. +* The kinematics module cannot be evaluated by the interpreter. [[gcode:g33]] == G33 Spindle Synchronized Motion(((G33 Spindle Synchronized Motion))) @@ -1885,6 +1946,23 @@ programmed on the same line if one is currently active. For example 'G53 G0 X0 Y0 Z0' will move the axes to the home position even if the currently selected coordinate system has offsets in effect. +The machine coordinate system is the world of the kinematics in force with the +offsets removed. With the identity kinematics, or a head-side kinematics with +the rotaries at zero, that is the slides. With a table-side kinematics, such as +'xyzac-trt-kins' in TCP or the tilted work plane types of the 'trsrn' +modules, the world turns with the table, so 'G53 X0 Y0 Z0' takes the tool tip +to the origin of that turned frame, which the slides reach at some other +position. The move in the machine frame with the orientation left out, the +slides on a machine whose slides line up with its frame, whatever the +kinematics in force, is <>. A configuration that wants +'G53' to mean that frame and nothing else sets `MACHINE_MOVES_NEED_MACHINE_FRAME` +in the `[RS274NGC]` section of the INI file, and 'G53', 'G28', 'G30' and +their stores are then refused whenever the kinematics in force is not the +machine frame type: the type the module flags as such (see the +<> chapter), the identity +kinematics on a machine whose slides line up with its frame, and neither the +tool centre point nor a raw joint identity on one whose slides do not. + .G53 Example [source,ngc] ---- @@ -1897,7 +1975,9 @@ See <> section for more information. It is an error if: * G53 is used without G0 or G1 being active, -* or G53 is used while cutter compensation is on. +* or G53 is used while cutter compensation is on, +* or `MACHINE_MOVES_NEED_MACHINE_FRAME` is set and the kinematics in force is not + the machine frame kinematics. [[gcode:g53.1]] == G53.1, G53.2, G53.3, G53.6 Orient the Tool to the Work Plane(((G53.1 Orient the Tool))) diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index 7151d2bc6db..24fdd07724f 100644 --- a/docs/src/gcode/overview.adoc +++ b/docs/src/gcode/overview.adoc @@ -976,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, G28.2, G30, G52, G53, G53.1, G53.2, G53.3, G53.4, G53.5, G53.6, G53.7, G92, G92.1, G92.2, G92.3, +|Non-modal codes (Group 0) | G4, G10 G28, G28.2, G28.5, G30, G30.5, G52, G53, G53.1, G53.2, G53.3, G53.4, G53.5, G53.6, G53.7, G92, G92.1, G92.2, G92.3, |Motion (Group 1) | G0, G1, G2, G3, G33, G38.n, G73, G76, G80, G81 G82, G83, G84, G85, G86, G87, G88, G89 |Plane selection (Group 2) | G17, G18, G19, G17.1, G18.1, G19.1 diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 4e28826ca7b..56d82baa261 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -581,25 +581,25 @@ int switchkinsDeclare(int ktype, int flags); ---- G-code reads these declarations: 'G13.1' and 'G49' cancel to the -kinstype declared KINSTYPE_MACHINE, 'G53.5' moves in its world, and -'G43.4' switches to the kinstype declared KINSTYPE_PRIMARY, whatever -their numbers, so a module whose -kinematics are not in the conventional order still gets working -spellings. The machine frame type is the machine with the orientation -left out, XYZ the pivot in machine coordinates and the rotary letters -the rotary joints; on a machine whose slides line up with its frame that -is the identity type, so a module that declares no machine frame -kinstype has its identity kinstype stand in, and every shipped module is -of that kind. A module whose carriage does not line up, a slanted slide -or a composite Y on a mill-turn, declares its machine frame kinstype -separately and keeps KINSTYPE_IDENTITY for a kinstype whose joints -really are the axes, since motion and the planner skip the maths on that -flag alone. At most one kinstype may be declared identity, at most one -primary and at most one machine frame, and declaring a kinstype the -module does not provide fails the module load. A module that declares -nothing keeps working exactly as before for 'G12.1 P-' and 'G49', but -'G13.1' and 'G43.4' are an error, since the numbers of the identity and -primary kinematics are then a guess. +kinstype declared KINSTYPE_MACHINE, 'G53.5', 'G28.5' and 'G30.5' move +in its world, and 'G43.4' switches to the kinstype declared +KINSTYPE_PRIMARY, whatever their numbers, so a module whose kinematics +are not in the conventional order still gets working spellings. The +machine frame type is the machine with the orientation left out, XYZ the +pivot in machine coordinates and the rotary letters the rotary joints; +on a machine whose slides line up with its frame that is the identity +type, so a module that declares no machine frame kinstype has its +identity kinstype stand in, and every shipped module is of that kind. A +module whose carriage does not line up, a slanted slide or a composite Y +on a mill-turn, declares its machine frame kinstype separately and keeps +KINSTYPE_IDENTITY for a kinstype whose joints really are the axes, since +motion and the planner skip the maths on that flag alone. At most one +kinstype may be declared identity, at most one primary and at most one +machine frame, and declaring a kinstype the module does not provide +fails the module load. A module that declares nothing keeps working +exactly as before for 'G12.1 P-' and 'G49', but 'G13.1' and 'G43.4' +are an error, since the numbers of the identity and primary kinematics +are then a guess. When every kinstype is registered, the module calls: diff --git a/src/emc/rs274ngc/interp_array.cc b/src/emc/rs274ngc/interp_array.cc index e3931cca6ba..be6f9d1b433 100644 --- a/src/emc/rs274ngc/interp_array.cc +++ b/src/emc/rs274ngc/interp_array.cc @@ -84,8 +84,8 @@ const int Interp::gees[] = { /* 220 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 240 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 260 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, -/* 280 */ 0, 0, 0,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, // 282=G28.2 -/* 300 */ 0, 0,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, +/* 280 */ 0, 0, 0,-1,-1, 0,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, // 282=G28.2 +/* 300 */ 0, 0,-1,-1,-1, 0,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 320 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 1, 1,-1,-1,-1,-1,-1,-1,-1,-1, /* 340 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 360 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, diff --git a/src/emc/rs274ngc/interp_check.cc b/src/emc/rs274ngc/interp_check.cc index 5c01f743b9c..11c27bc3fba 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -93,18 +93,35 @@ int Interp::check_g_codes(block_pointer block, //!< pointer to a block to be c CHKS((((block->p_number + 0.0001) - p_int) > 0.0002), _("P value not an integer with G10")); CHKS((((block->l_number == 2 || block->l_number == 20) && ((p_int < 0) || (p_int > 9)))), _("P value out of range (0-9) with G10 L%d"), block->l_number); CHKS((((block->l_number == 1 || block->l_number == 10 || block->l_number == 11) && p_int < 1)), _("P value out of range with G10 L%d"), block->l_number); - } else if (mode0 == G_28) { - } else if (mode0 == G_30) { + } else if (mode0 == G_28 || mode0 == G_30) { + CHKS((settings->machine_moves_need_machine_frame && !kins_machine_frame(settings)), + _("%s moves in the world of the active kinematics, which is not the machine frame" + " (MACHINE_MOVES_NEED_MACHINE_FRAME): the machine frame is %s"), + (mode0 == G_28) ? "G28" : "G30", (mode0 == G_28) ? "G28.5" : "G30.5"); + } else if (mode0 == G_28_5 || mode0 == G_30_5) { + CHKS((block->radius_flag || block->theta_flag), + _("Cannot use polar coordinates with G28.5 or G30.5")); + CHKS(((block->g_modes[GM_DISTANCE_MODE] == G_91) || + ((block->g_modes[GM_DISTANCE_MODE] != G_90) && + (settings->distance_mode == DISTANCE_MODE::INCREMENTAL))), + _("Cannot use G28.5 or G30.5 in incremental distance mode")); } else if (mode0 == G_5_3) { CHKS(((mode1 != G_5_2) && (mode1 != -1)), _("Between G5.2 and G5.3 codes, only additional G5.2 codes are allowed.")); } else if (mode1 == G_5_2){ } else if (mode1 == G_6_2){ } else if (mode0 == G_28_1 || mode0 == G_30_1) { + CHKS((settings->machine_moves_need_machine_frame && !kins_machine_frame(settings)), + _("%s stores a world position, and the active kinematics is not the machine frame" + " (MACHINE_MOVES_NEED_MACHINE_FRAME): store it on the machine frame kinematics"), + (mode0 == G_28_1) ? "G28.1" : "G30.1"); } else if (mode0 == G_28_2) { // G-code homing } else if (mode0 == G_52) { } else if (mode0 == G_53) { CHKS(((block->motion_to_be != G_0) && (block->motion_to_be != G_1)), NCE_MUST_USE_G0_OR_G1_WITH_G53); + CHKS((settings->machine_moves_need_machine_frame && !kins_machine_frame(settings)), + _("G53 moves in the world of the active kinematics, which is not the machine frame" + " (MACHINE_MOVES_NEED_MACHINE_FRAME): the machine frame is G53.5")); CHKS(((block->g_modes[GM_DISTANCE_MODE] == G_91) || ((block->g_modes[GM_DISTANCE_MODE] != G_90) && (settings->distance_mode == DISTANCE_MODE::INCREMENTAL))), diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 714f6c23107..3a683e6c4b2 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -4493,6 +4493,8 @@ int Interp::convert_modal_0(int code, //!< G-code, must be from group 0 CHP(convert_setup(block, settings)); } else if ((code == G_28) || (code == G_30)) { CHP(convert_home(code, block, settings)); + } else if ((code == G_28_5) || (code == G_30_5)) { + CHP(convert_home_slides(code, block, settings)); } else if ((code == G_28_1) || (code == G_30_1)) { CHP(convert_savehome(code, block, settings)); } else if (code == G_28_2) { diff --git a/src/emc/rs274ngc/interp_internal.cc b/src/emc/rs274ngc/interp_internal.cc index ea3b1212ab5..a1bd764f130 100644 --- a/src/emc/rs274ngc/interp_internal.cc +++ b/src/emc/rs274ngc/interp_internal.cc @@ -174,6 +174,7 @@ 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_28_5) || (mode0 == G_30_5) || (mode0 == G_52) || (mode0 == G_92) || (mode0 == G_53_3)); // a tilted work plane definition takes the axis words the same way if (block->g_modes[GM_WORK_PLANE] == G_68_2 || block->g_modes[GM_WORK_PLANE] == G_68_3 diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index cd025b5e6bc..45a97736cb7 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -233,8 +233,10 @@ enum GCodes G_28 = 280, G_28_1 = 281, G_28_2 = 282, /* G-code homing cycle (home one/all joints) */ + G_28_5 = 285, G_30 = 300, G_30_1 = 301, + G_30_5 = 305, G_33 = 330, G_33_1 = 331, G_38_2 = 382, @@ -803,6 +805,7 @@ struct setup int kins_type; // kinematics selected by G12.1/G13.1 bool kins_by_g43_4; // G43.4 selected the kinematics, for G49 to undo bool tool_vector; // G43.5: I J K on G0 and G1 give the tool axis + bool machine_moves_need_machine_frame; // [RS274NGC] MACHINE_MOVES_NEED_MACHINE_FRAME: G53, G28, G30 refused off the machine frame type bool toolchange_flag; // flag indicating we just had a tool change bool home_flag; // flag indicating a G28.2 homing cycle just ran int input_index; // channel queried @@ -1100,4 +1103,5 @@ struct scoped_locale { // the kinematics type carrying a KINSTYPE_ flag, or -1 when the module // declares none (interp_convert.cc) int flagged_kins_type(int flag); + #endif // INTERP_INTERNAL_HH diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index 43c38c6cae5..aeab7986b01 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -138,6 +138,7 @@ setup::setup() : kins_type(0), kins_by_g43_4(false), tool_vector(false), + machine_moves_need_machine_frame(false), toolchange_flag(0), home_flag(0), input_index(0), diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 4a9e8a4a07e..fa3e2bbb452 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -1048,6 +1048,17 @@ int Interp::ptp_seconds(block_pointer block, setup_pointer s, return INTERP_OK; } +// Whether the kinematics in force is the machine frame type, the one G13.1 +// and G49 cancel to: the flags the module declares for the type the program +// selected, else what the machine reports (a module that switches nothing, +// or no machine attached). +bool Interp::kins_machine_frame(setup_pointer s) +{ + int flags = GET_EXTERNAL_KINS_TYPE_FLAGS(s->kins_type); + if (flags >= 0) { return (flags & KINSTYPE_MACHINE) != 0; } + return GET_EXTERNAL_KINEMATICS_IDENTITY(); +} + // The axis letters read as machine frame coordinates, the words the module's // machine frame type answers to: the pivot in machine coordinates, the // rotaries as joints, in program units. The joints are read in on the type @@ -1157,6 +1168,47 @@ int Interp::machine_frame_joints(const char *name, setup_pointer s, void *vctx, return INTERP_OK; } +// The move to a set of joints: where that puts the tool, and what the +// program calls it, then the joint-space traverse or feed there. +int Interp::ptp_joint_move(const char *name, int move, block_pointer block, setup_pointer s, + void *vctx, const double *joints) +{ + KinematicsUserContext *ctx = (KinematicsUserContext *)vctx; + EmcPose pose; + double prog[9]; + int j; + + current_machine_pose(s, &pose); + CHKS((kinematicsUserForward(ctx, joints, &pose) != 0), + _("%s: the kinematics cannot place those joints"), name); + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { s->kins_seed[j] = joints[j]; } + machine_pose_to_program(s, &pose, prog); + + write_canon_state_tag(block, s); + if (move == G_0) { + JOINT_TRAVERSE(block->line_number, joints, 1, + prog[0], prog[1], prog[2], prog[3], prog[4], prog[5], + prog[6], prog[7], prog[8]); + } else { + double seconds; + CHP(ptp_seconds(block, s, prog[0], prog[1], prog[2], prog[3], prog[4], prog[5], + prog[6], prog[7], prog[8], &seconds)); + JOINT_FEED(block->line_number, joints, 1, + prog[0], prog[1], prog[2], prog[3], prog[4], prog[5], + prog[6], prog[7], prog[8], seconds); + } + s->current_x = prog[0]; + s->current_y = prog[1]; + s->current_z = prog[2]; + s->AA_current = prog[3]; + s->BB_current = prog[4]; + s->CC_current = prog[5]; + s->u_current = prog[6]; + s->v_current = prog[7]; + s->w_current = prog[8]; + return INTERP_OK; +} + // The two point-to-point codes below G53.4: G53.5 by axis letter in the // machine frame, the module's machine frame type, refused where that type is // a plain identity whose letters name joints of the other unit class; G53.7 @@ -1168,8 +1220,6 @@ int Interp::convert_ptp_joints(int code, int move, block_pointer block, setup_po 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 }; @@ -1195,12 +1245,8 @@ int Interp::convert_ptp_joints(int code, int move, block_pointer block, setup_po given++; } CHKS((given == 0), _("G53.7 needs at least one J= joint word")); - } else { - CHP(slide_joints(name, s, ctx, p, njoints, flags, words, joints)); - } - // the joints of a gantry pair move together: both given, one value - if (code == G_53_7) { + // the joints of a gantry pair move together: both given, one value for (a = 0; p && a < EMCMOT_MAX_AXIS; a++) { int bits = p->joints_of_axis[a]; int first = -1; @@ -1214,36 +1260,59 @@ int Interp::convert_ptp_joints(int code, int move, block_pointer block, setup_po _("G53.7: joints %d and %d are a pair on this kinematics, give them one value"), first, j); } } + } else { + CHP(slide_joints(name, s, ctx, p, njoints, flags, words, joints)); } - // 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); + return ptp_joint_move(name, move, block, s, ctx, joints); +} - 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]); +// G28.5 and G30.5: the machine frame forms of G28 and G30, as G53.5 is of +// G53. The stored position, parameters 5161 to 5169 or 5181 to 5189, is +// read in the machine frame the way G53.5 reads its words: no offset, no +// work plane, no orientation. Axis words on the line are a machine frame +// point to pass through first, and then only those letters go to the stored +// position; with no words every letter the machine has goes. +int Interp::convert_home_slides(int code, block_pointer block, setup_pointer s) +{ + void *vctx; + KinematicsUserContext *ctx; + const kins_params *p; + const double *parameters = s->parameters; + double joints[EMCMOT_MAX_JOINTS]; + double home[9]; + int flags[9] = { block->x_flag, block->y_flag, block->z_flag, + block->a_flag, block->b_flag, block->c_flag, + block->u_flag, block->v_flag, block->w_flag }; + const double words[9] = { block->x_number, block->y_number, block->z_number, + block->a_number, block->b_number, block->c_number, + block->u_number, block->v_number, block->w_number }; + const char *name = (code == G_28_5) ? "G28.5" : "G30.5"; + const int base = (code == G_28_5) ? 5161 : 5181; + int a, njoints, given = 0; + + CHKS((s->cutter_comp_side != CUTTER_COMP::OFF), + _("Cannot use %s with cutter radius compensation on"), name); + CHP(kins_context(s, &vctx)); + ctx = (KinematicsUserContext *)vctx; + njoints = kinematicsUserGetNumJoints(ctx); + p = kinematicsUserParams(ctx); + CHP(current_joints(s, ctx, joints)); + + for (a = 0; a < 9; a++) { + const int angular = (a >= 3 && a <= 5); + home[a] = angular ? USER_TO_PROGRAM_ANG(parameters[base + a]) + : USER_TO_PROGRAM_LEN(parameters[base + a]); + given += flags[a]; + } + + if (given) { + CHP(slide_joints(name, s, ctx, p, njoints, flags, words, joints)); + CHP(ptp_joint_move(name, G_0, block, s, ctx, joints)); } else { - 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); + CHKS((!p), _("%s: the kinematics module gives no joint mapping"), name); + for (a = 0; a < 9; a++) { flags[a] = p->joints_of_axis[a] != 0; } } - 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; + CHP(slide_joints(name, s, ctx, p, njoints, flags, home, joints)); + return ptp_joint_move(name, G_0, block, s, ctx, joints); } diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index 7b9a3df8df6..64e826be063 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -377,18 +377,22 @@ public: int p, int q, const double *now, double *joints, const char *name); int tool_vector_ends(block_pointer block, setup_pointer settings, double *a, double *b, double *c); int convert_ptp_joints(int code, int move, block_pointer block, setup_pointer settings); + int convert_home_slides(int code, block_pointer block, setup_pointer settings); int slide_joints(const char *name, setup_pointer settings, void *ctx, const struct kins_params *params, int njoints, const int flags[9], const double words[9], double *joints); int slide_words(const char *name, setup_pointer settings, void *ctx, const struct kins_params *params, int njoints, int undeclared, const int flags[9], const double words[9], double *joints); int machine_frame_joints(const char *name, setup_pointer settings, void *ctx, const int flags[9], const double words[9], double *joints); + int ptp_joint_move(const char *name, int move, block_pointer block, setup_pointer settings, + void *ctx, const double *joints); int ptp_seconds(block_pointer block, setup_pointer settings, double x, double y, double z, double a, double b, double c, double u, double v, double w, double *seconds); int kins_load(setup_pointer settings); int kins_context(setup_pointer settings, void **ctx); int kins_here(setup_pointer settings, void **ctx); + bool kins_machine_frame(setup_pointer settings); int tool_offset_point(setup_pointer settings, const EmcPose *offset, const double *standing, EmcPose *point, bool *known); void kins_set_tool(void *ctx, const EmcPose *offset); diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index d16a14bbadb..b39536012e2 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -1098,6 +1098,9 @@ int Interp::init() // INI file m98/m99 subprogram default setting _setup.disable_fanuc_style_sub = inifile.findBoolV("DISABLE_FANUC_STYLE_SUB", "RS274NGC", false); logDebug("init: DISABLE_FANUC_STYLE_SUB = %d", _setup.disable_fanuc_style_sub); + + // G53, G28, G30, G28.1 and G30.1 refused while the kinematics is not the identity + _setup.machine_moves_need_machine_frame = inifile.findBoolV("MACHINE_MOVES_NEED_MACHINE_FRAME", "RS274NGC", false); } } diff --git a/tests/ptp-machine-frame/README b/tests/ptp-machine-frame/README index c7e76a61ea1..b5a51121880 100644 --- a/tests/ptp-machine-frame/README +++ b/tests/ptp-machine-frame/README @@ -12,5 +12,8 @@ The test checks that G13.1 and G49 select the machine frame type rather than the identity, that G53.5 puts the carriage at a machine frame point, the letters not given holding their machine coordinate and not their joint, from the machine frame type and from under the tilted tip -kinematics alike, and that a plain move after a machine frame move starts -from the right place. +kinematics alike, that G28.5 goes to a position stored on the machine +frame, through a waypoint when given one, that MACHINE_MOVES_NEED_MACHINE_FRAME +refuses G53, G28, G30 and the stores under the tip and under the plain +identity, and that a plain move after a machine frame move starts from the +right place. diff --git a/tests/ptp-machine-frame/test-ui.py b/tests/ptp-machine-frame/test-ui.py index 28021cc33f5..5b816f4116a 100755 --- a/tests/ptp-machine-frame/test-ui.py +++ b/tests/ptp-machine-frame/test-ui.py @@ -74,6 +74,17 @@ def carriage(x, y, z, b=0.0, cc=0.0): j1 = y / math.sin(SLANT) return [x - j1 * math.cos(SLANT), j1, z, b, cc] +def refused(*cmds): + for cmd in cmds: + 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) + def expect_type(what, want): k = kins_type() print("%-34s kins-type %d" % (what, k)) @@ -119,6 +130,27 @@ def expect_type(what, want): expect("G53.5 X20 Y10 Z-5 under the tip", j, carriage(20, 10, -5, 30)) j = mdi("G53.5 G0 B0") expect("G53.5 B0 is the joint", j, carriage(20, 10, -5, 0)) +# and G53 under the tip is refused: the config wants machine moves on the +# machine frame type, and the plain identity is not that type either +refused("G53 G0 X0", "G28", "G30", "G28.1", "G30.1") +mdi("G12.1 P2") +refused("G53 G0 X0", "G28.1") +mdi("G13.1") +drain() + +# --- G28.5 goes to a machine frame position stored on the machine frame ---- +# with no words every letter goes, B to its stored zero included; with words +# the machine passes through that machine frame point and only those +# letters go on to the stored position. Under G43.4 a program Z is the +# tool length above the carriage, the offset the interpreter carries, so the +# waypoint's Z is 10 + 50 +mdi("G49", "G53 G0 X20 Y10 Z0 B0 C0", "G28.1") +mdi("G43.4 H1", "G0 B30", "G0 X0 Y0 Z10") +j = mdi("G28.5") +expect("G28.5 with the head tilted", j, carriage(20, 10, 0, 0)) +mdi("G0 X0 Y0 Z10") +j = mdi("G28.5 X40") +expect("G28.5 X40, through X then to X only", j, carriage(20, 0, 10 + TOOL, 0)) mdi("G49", "G0 B0", "G53 G0 X0 Y0 Z0") drain() diff --git a/tests/ptp-machine-frame/test.ini b/tests/ptp-machine-frame/test.ini index 20053c9a2db..1976b63c410 100644 --- a/tests/ptp-machine-frame/test.ini +++ b/tests/ptp-machine-frame/test.ini @@ -8,6 +8,8 @@ DISPLAY = ./test-ui.py [RS274NGC] RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 PARAMETER_FILE = sim.var +# G53, G28, G30 and the stores allowed on the machine frame type only +MACHINE_MOVES_NEED_MACHINE_FRAME = 1 [KINS] KINEMATICS = slantkins diff --git a/tests/twp-native/README b/tests/twp-native/README index 868da2c4f14..a381fbc8fea 100644 --- a/tests/twp-native/README +++ b/tests/twp-native/README @@ -9,5 +9,14 @@ asked for in the plane; a move along plane X goes along plane X in the world; G68.3 reads the plane back off the oriented tool; G69 cancels. G53.6 standing near the end of C's travel reaches the plane by the nearest pose inside it, never running C past its limit. Q1 lets the table take part. The point-to-point moves are checked too: G53.4 G0 to a program point, G53.5 and G53.7 G0 to a slide position with -the head tilted, G53.4 G1 taking the time the straight move would in G94 and -in G93, and what they refuse. +the head tilted, G28.5 and G30.5 to a slide position stored on the identity +kinematics, through a slide waypoint when given one, G53.4 G1 taking the +time the straight move would in G94 and in G93, and what they refuse. The +config sets MACHINE_MOVES_NEED_MACHINE_FRAME, so G53, G28, G30, G28.1 and G30.1 +are refused under TCP and accepted on the identity kinematics. + +The path of a move is read from a sampler on the servo thread, every cycle +of the joint commands and the world position, logged by halsampler to +samples.log while the test runs: a status snapshot per task cycle misses the +turn of a move that stops and comes back. The log is left behind when a +check fails. diff --git a/tests/twp-native/sim.hal b/tests/twp-native/sim.hal index e92c60eb526..792895c2bed 100644 --- a/tests/twp-native/sim.hal +++ b/tests/twp-native/sim.hal @@ -4,6 +4,11 @@ loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JO addf motion-command-handler servo-thread addf motion-controller servo-thread +# every servo cycle of the joints and the world position, logged for the +# test to read the path of a move, not a task-cycle snapshot of it +loadrt sampler depth=4000 cfg=fffffffff +addf sampler.0 servo-thread + net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb @@ -11,6 +16,17 @@ net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb +net J0 => sampler.0.pin.0 +net J1 => sampler.0.pin.1 +net J2 => sampler.0.pin.2 +net J3 => sampler.0.pin.3 +net J4 => sampler.0.pin.4 +net J5 => sampler.0.pin.5 +net Xcmd axis.x.pos-cmd => sampler.0.pin.6 +net Ycmd axis.y.pos-cmd => sampler.0.pin.7 +net Zcmd axis.z.pos-cmd => sampler.0.pin.8 +loadusr halsampler -t samples.log + net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/twp-native/test-ui.py b/tests/twp-native/test-ui.py index aed1f65744c..670c1e6d75b 100755 --- a/tests/twp-native/test-ui.py +++ b/tests/twp-native/test-ui.py @@ -67,29 +67,47 @@ def mdi(*cmds): 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 +# the path of a move is read from the sampler log, every servo cycle of the +# joint commands and the world position, which halsampler writes to +# samples.log as it runs; status would give a snapshot per task cycle, which +# misses the turn of a move that stops and comes back +LOG = "samples.log" + +def log_samples(): + with open(LOG) as f: + lines = f.read().split("\n") + out = [] + for line in lines[:-1]: # the last piece is a line still being written + v = line.split() + if len(v) != 1 + JOINTS + 3: + continue # an "overrun" line, counted from the pin at the end + v = [float(x) for x in v[1:]] + out.append((v[:JOINTS], v[JOINTS:])) + return out + +# run one command and return the settled joints and the log of the way there: +# the log is block buffered, so it is read until it has caught up with the +# machine at rest def sampled(cmd): + n0 = len(log_samples()) c.mdi(cmd) - 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 + deadline = time.time() + 10 + while True: + samples = log_samples() + if len(samples) > n0 and close(samples[-1][0], end, 2e-6): + break + if time.time() > deadline: + error("%s: the sampler log did not catch up with the machine" % cmd) + break + time.sleep(0.02) + return end, samples[n0:] # a point-to-point move runs every joint on a straight line in joint space, # all together: the fraction of the way each moving joint has gone is the -# same for all of them at every sample, never goes back, and reaches one +# same for all of them at every sample, never goes back, and reaches one; +# the log prints six decimals, which on a short move is 1e-5 of the way def joint_line(what, samples, start, end): moving = [i for i in range(JOINTS) if abs(end[i] - start[i]) > 1e-6] if not moving: @@ -99,7 +117,7 @@ def joint_line(what, samples, start, end): 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: + if max(abs(x - f) for x in fs) > 1e-5: error("%s: joints out of step at sample %d of %d: %s" % (what, n, len(samples), fs)) return if f < last - 1e-6: @@ -492,6 +510,44 @@ def pose_near(pairs, b_now, c_now, limited): if abs(after[2]) > 1e-6 or abs(after[SECONDARY]) > 1e-6: error("G53.5 Z0 B0 did not put joints 2 and 4 at zero") +# --- G28.5 and G30.5 ------------------------------------------------------ +# the stored positions are saved on the identity kinematics, where the world +# is the slides: G28.1 at X10 Y20 Z-5 with the head straight, G30.1 at the +# same slides with the head at B10 +mdi("G12.1 P0", "G0 X10 Y20 Z-5 A0 B0 C0", "G28.1", "G0 B10", "G30.1", + "G0 X0 Y0 Z0 B0", "G12.1 P1") +# with the head tilted under TCP, G28.5 alone takes every slide to the +# stored position, on one joint-space move +before = mdi("G0 X0 Y0 Z0 A0 B-30 C0") +after, samples = sampled("G28.5") +show("G28.5", after) +drain() +joint_line("G28.5", samples, before, after) +if not close(after, [10, 20, -5, 0, 0, 0], 1e-6): + error("G28.5 put the joints at %s, not at the stored slides" % (after,)) +# an axis word is a slide to pass through first, and then only that letter +# goes to its stored value: Z3 on the way, then Z-5, the rest untouched +before = mdi("G0 X0 Y0 Z0 A0 B-30 C0") +after, samples = sampled("G30.5 Z3") +show("G30.5 Z3", after) +drain() +if abs(after[2] + 5) > 1e-6: + error("G30.5 Z3 left joint 2 at %.6f, not at the stored -5" % after[2]) +for j in (0, 1, 3, 4, 5): + if abs(after[j] - before[j]) > 1e-6: + error("G30.5 Z3 moved joint %d from %.9f to %.9f" % (j, before[j], after[j])) +# the first move ends on Z3 and the second starts there inside the same +# servo cycle, so the turn falls between two samples: joint 2 decelerates +# into it at 700 mm/s^2, which over one 1 ms cycle is 3.5e-4 short of it, +# and it never goes past it +peak = max(j[2] for j, p in samples) +if peak > 3 + 1e-6 or peak < 3 - 1e-3: + error("G30.5 Z3 did not pass through the Z3 slide (joint 2 peaked at %.6f)" % peak) +after = mdi("G30.5") +if not close(after, [10, 20, -5, 0, 10, 0], 1e-6): + error("G30.5 put the joints at %s, not at the stored slides with B10" % (after,)) +mdi("G0 X0 Y0 Z0 A0 B0 C0") + # a point-to-point feed takes the time the straight move would: 10 mm at # F600 is one second, and F30 in G93 is two def timed(cmd): @@ -525,10 +581,14 @@ def timed(cmd): error("a point-to-point feed at G93 F30 took %.3f s, not about two" % took) drain() -# what the point-to-point codes refuse +# what the point-to-point codes refuse, and what MACHINE_MOVES_NEED_MACHINE_FRAME +# refuses while the kinematics is not the identity: G53, G28, G30 and the +# stores; the slide forms are the way to the machine's positions from here 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"): + "G53.5 G0 J0=1", "G53.5 G0", "G91 G53.5 G0 X1", "G53.4 G1 F0 X1", + "G91 G28.5 X1", "G30.5 G1 X1", + "G53 G0 X0", "G28", "G30 Z1", "G28.1", "G30.1"): c.mdi(cmd) c.wait_complete(30) m = e.poll() @@ -538,6 +598,11 @@ def timed(cmd): print("refused as expected:", m[1]) c.mode(linuxcnc.MODE_MDI) mdi("G90 G94 G0 X0 Y0 Z0 A0 B0 C0") +# and on the identity kinematics the same lines are accepted +mdi("G12.1 P0", "G53 G0 X0", "G28", "G30 Z1", "G28.1", "G30.1", "G12.1 P1") +m = e.poll() +if m: + error("on the identity kinematics %s" % (m[1],)) # --- a plane refuses what would move the ground under it --------------- # each refusal is an interpreter error, and the abort that follows cancels @@ -599,5 +664,10 @@ def timed(cmd): except OSError: pass +overruns = int(hal.get_value("sampler.0.overruns")) +if overruns: + error("the sampler lost %d samples" % overruns) +if not errors: + os.unlink(LOG) print("Exiting with %d errors" % errors) sys.exit(1 if errors else 0) diff --git a/tests/twp-native/test.ini b/tests/twp-native/test.ini index a3a6bcf9e1a..32a317fb972 100644 --- a/tests/twp-native/test.ini +++ b/tests/twp-native/test.ini @@ -8,6 +8,8 @@ DISPLAY = ./test-ui.py [RS274NGC] RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 PARAMETER_FILE = sim.var +# G53, G28, G30 and the stores refused off the identity: the slide forms are the test's +MACHINE_MOVES_NEED_MACHINE_FRAME = 1 [KINS] KINEMATICS = xyzacb_trsrn diff --git a/tests/twp-native/test.sh b/tests/twp-native/test.sh index 765cf14fed6..d27cc469eb5 100755 --- a/tests/twp-native/test.sh +++ b/tests/twp-native/test.sh @@ -1,4 +1,4 @@ #!/bin/bash -e # a failed run leaves the var file behind, and it carries offsets -rm -f sim.var sim.var.bak +rm -f sim.var sim.var.bak samples.log linuxcnc -r test.ini From 593499205bdf7ef654e5c8dc36409b20e2674f49 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 19 Sep 2026 13:17:01 +1000 Subject: [PATCH 63/77] genserkins, pumakins: declare the arm kinematics the machine frame A robot's working transform reports the flange in the base frame, ABB's tool0 and KUKA's $NULLFRAME: the tool is applied on top by the interpreter, not by the maths, so the type G43.4 switches to is also the machine with the tool left out, the frame G13.1 and G49 cancel to and G53.5 moves in. pumakins and genserkins declare that one type both primary and machine. G13.1 and G49 leave a robot on its arm kinematics, where a robot controller leaves it, and the identity type, joints as axes, is reached by number; G53.5 moves the flange along a base axis and holds the rest, the orientation included, interpolated in joint space, and G28.5 and G30.5 with it. A machine frame type does not read the tool offset in the parameter block, so a module whose working transform does read it declares a type without it. The machine frame is described as the frame with the tool left out rather than with the orientation left out, since on a robot the rotary letters are the flange angles the module reports, not joints; the G13.1, G49 and G43.4 descriptions say machine frame where they said identity. tests/ptp-robot checks that G53.5 moves the flange from the arm kinematics and from the identity alike, and that G13.1, G43.4 and G49 all leave the robot on the arm kinematics, in place of the refusal it checked before. --- .../axis/vismach/puma/remap_subs/428remap.ngc | 2 +- docs/src/gcode/g-code.adoc | 80 +++++++++------- docs/src/motion/kinematics-conventions.adoc | 31 ++++--- docs/src/motion/switchkins.adoc | 33 ++++--- src/emc/kinematics/genserfuncs.c | 4 + src/emc/kinematics/kinematics.h | 25 ++--- src/emc/kinematics/kins_module.h | 9 +- src/emc/kinematics/pumakins.c | 6 +- src/emc/rs274ngc/interp_workplane.cc | 4 +- tests/ptp-robot/README | 10 +- tests/ptp-robot/test-ui.py | 91 +++++++++++++++---- 11 files changed, 194 insertions(+), 101 deletions(-) diff --git a/configs/sim/axis/vismach/puma/remap_subs/428remap.ngc b/configs/sim/axis/vismach/puma/remap_subs/428remap.ngc index d2d0f3c8d39..b71caa08b0d 100644 --- a/configs/sim/axis/vismach/puma/remap_subs/428remap.ngc +++ b/configs/sim/axis/vismach/puma/remap_subs/428remap.ngc @@ -3,7 +3,7 @@ o<428remap>sub # = 0 G12.1 P# ; select kinstype 0, syncs interp and motion - ; (G13.1 is identity, which is type 1 on this module) + ; (G13.1 is the machine frame, this type on a robot; identity is type 1) o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #]] (debug,M428: Wrong kinematics type) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index c3d8d24cb52..bcb61aa24d2 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -952,9 +952,11 @@ G13.1 'G12.1' selects one of the kinematics provided by a switchable kinematics module; the 'P' word is the kinematics number. 'G13.1' cancels back to -identity kinematics, where joints and coordinates are the same thing. -Which number that is is declared by the module itself, so 'G13.1' means -the same kinematics whatever order a module lists its types in. +the machine frame kinematics: the identity kinematics, where joints and +coordinates are the same thing, on a machine whose slides line up with its +frame, and the arm kinematics with no tool on a robot. Which number that is +is declared by the module itself, so 'G13.1' means the same kinematics +whatever order a module lists its types in. [WARNING] Deprecation notice: selecting the kinematics by writing the @@ -970,12 +972,12 @@ planned in one kinematics and executed in another. Because of that, both codes stop any blending that was in progress, in the same way 'G4' does. The kinematics module decides what each number means, and declares -which one is identity. See the `switchkins` section of the kins(9) man -page for the modules that support switching and the order in which they +which one is the machine frame. See the `switchkins` section of the kins(9) +man page for the modules that support switching and the order in which they list their kinematics. A machine whose kinematics module is not switchable rejects the change, and 'G13.1' is an error on a switchable -module that declares no identity kinematics; such a module can still be -driven by number with 'G12.1'. +module that declares no identity or machine frame kinematics; such a module +can still be driven by number with 'G12.1'. Selecting a kinematics does not move the machine. It changes how joint positions and coordinate positions map onto each other, so the position @@ -996,14 +998,14 @@ G12.1 P# (put back whatever the caller was using) Nothing cancels the selection on its own. It survives the end of the program and an abort, so that the kinematics keeps matching what the position readout shows. End a program with 'G13.1' if it should leave the -machine in identity kinematics. +machine in the machine frame kinematics. .G12.1, G13.1 Example [source,ngc] ---- G12.1 P1 (switch to kinematics 1) G0 X0 Y0 -G13.1 (back to identity kinematics) +G13.1 (back to the machine frame kinematics) ---- It is an error if: @@ -1012,7 +1014,7 @@ It is an error if: * The 'P' word is negative. * The 'P' word names a kinematics the module does not provide. * A 'P' word is used with 'G13.1'. -* 'G13.1' is used and the module declares no identity kinematics. +* 'G13.1' is used and the module declares no identity or machine frame kinematics. [[gcode:g17-g19.1]] == G17 - G19.1 Plane Select(((G17 - G19.1 Plane Select))) @@ -1076,7 +1078,7 @@ G28 Z2.5 (rapid to Z2.5 then to Z location specified in #5163) The 'absolute' position is the machine coordinate system of the kinematics in force, see <>: on a kinematics that is not the identity it is that kinematics' world, not the slides, and the stored position is a world -position too. <> is the form that moves the slides. +position too. <> is the machine frame form. It is an error if : @@ -1223,7 +1225,8 @@ with its frame, and the move is a <> there, rapid, with no offset, tool length, rotation or work plane applied. The values are in the machine units of the INI file, as for 'G28'. The stored position has to be a machine frame position: store it with 'G28.1' or 'G30.1' -while the identity kinematics is in force, where the world is that frame. +while the machine frame kinematics is in force, what 'G13.1' selects, where +the world is that frame. * 'G28.5' - moves every letter the machine has to the stored position. * 'G28.5 axes' - moves the letters named by 'axes' to the given machine @@ -1811,9 +1814,9 @@ declares its working transform, so the program runs with tool length compensation in the module's working kinematics. The switch happens first and the offset applies after it, as if the two had been written on consecutive lines. 'G49' is the matching cancel: it clears the -offset and switches back to identity kinematics, as long as the offset -in effect is still 'G43.4''s and no 'G12.1' or 'G13.1' has selected a -kinematics since. +offset and switches back to the machine frame kinematics, what 'G13.1' +selects, as long as the offset in effect is still 'G43.4''s and no 'G12.1' +or 'G13.1' has selected a kinematics since. The H word, the offset itself and the parameters it lands in are 'G43''s. Which kinematics is the working one is declared by the module @@ -1909,11 +1912,13 @@ It is an error if: * '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. +'G49' also switches a switchable kinematics module back to its machine +frame kinematics, what 'G13.1' selects, when it cancels a 'G43.4' or +'G43.5', undoing the switch that code made. It leaves a kinematics selected +by 'G12.1' or 'G13.1' alone, as it does after a plain 'G43', and a module +that declares no identity or machine frame kinematics gets the plain +cancel. On a robot the arm kinematics is the machine frame, so there the +cancel switches nothing. It is OK to program using the same offset already in use. It is also OK to program using no tool length offset if none is currently being @@ -2118,19 +2123,23 @@ one more layer of interpretation from the destination than the one before: without leaving its coordinate system and without the trajectory planner trying to hold the tool tip on a line the joints cannot follow at speed. * 'G53.5' takes the same axis letters as a position in the machine frame - with the orientation left out: 'X', 'Y' and 'Z' are the carriage, the - point the rotary joints do not move, in machine coordinates, and the - rotary letters are the rotary joints themselves. On a machine whose slides - line up with its frame, which every shipped module is, that is the slides: - each letter names the joints the module's identity mapping gives it, which - the `coordinates=` parameter and the `[TRAJ]COORDINATES` line describe, - and the two joints of a gantry take one value together. A module whose + with the tool left out: 'X', 'Y' and 'Z' are the point the tool hangs + from, the pivot of a head or the flange of a robot, in machine + coordinates, and the rotary letters are the orientation as the machine + frame kinematics reports it, on a mill the rotary joints themselves. On a + machine whose slides line up with its frame that is the slides: each + letter names the joints the module's identity mapping gives it, which the + `coordinates=` parameter and the `[TRAJ]COORDINATES` line describe, and + the two joints of a gantry take one value together. A module whose carriage does not line up, a slanted slide or a composite Y on a mill-turn, declares a machine frame type of its own (see the <> chapter) and 'G53.5' goes through it, so that 'Y10' is machine Y whichever slides make it, and - a letter not given holds its machine coordinate rather than a joint. - Values are in program units, so 'G20' scales them like any other axis + a letter not given holds its machine coordinate rather than a joint. On a + robot the machine frame is the arm kinematics with no tool, the flange in + the base frame, so 'Z10' puts the flange at base Z 10 with its orientation + held and 'A', 'B' and 'C' are the flange angles the module reports; the + move is still interpolated in joint space. Values are in program units, so 'G20' scales them like any other axis word. No offset, tool length, rotation or work plane applies. On a mill whose joint 2 carries the Z slide, 'G53.5 G0 Z0' puts that slide at zero whatever the head is doing, where 'G53 G0 Z0' puts the tool tip at machine @@ -2141,10 +2150,10 @@ one more layer of interpretation from the destination than the one before: + An axis letter carries a unit class and a joint does not, so where the machine frame is the joints 'G53.5' is refused on a machine whose letters -name joints of the other kind. A 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. +name joints of the other kind: a module that declares no kinematics types +and answers X with a rotary joint, which turns in degrees, has the whole +code refused rather than that one letter. Which joints turn is what +`[JOINT_n] TYPE` declares. * 'G53.7' takes joint values, one word per joint, and works on every machine: 'J2=-5' sends joint 2 to -5. The number after 'J' is the joint number, the @@ -2152,8 +2161,9 @@ declares. 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 one value. This is the form that names a joint outright, a robot's + joints by number, and the only form on a machine whose letters do not name + the joints they look like and whose module declares no machine frame. With 'G0' the speed comes from the joint limits in the INI file, `[JOINT_n] MAX_VELOCITY` and `MAX_ACCELERATION`, scaled so that no joint diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 7d4fa9834fe..3904c5e0d4c 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -73,17 +73,22 @@ the zero pose or a switch would jump the reported position; a module without one keeps it too, so that `G53.5` machine frame positions and `G53` positions coincide with the rotaries at zero. -The machine frame is the frame with the orientation left out: XYZ is the -pivot, the point the rotary joints do not move, in machine coordinates, and -the rotary letters are the rotary joints as they are. On a machine whose -slides line up with its frame that is the identity type, and it is what -`G13.1` and `G49` select and `G53.5` moves in. A machine whose carriage does -not line up, a slanted slide, a composite Y made by two slides, an offset -pivot, declares a machine frame type of its own next to its identity type, -and its identity type then keeps the plain meaning, joints are axes, which a -consumer uses to skip the maths. The zero-pose rule holds for the machine -frame type as it does for the identity: the working transform and the machine -frame type agree with the rotaries at zero. +The machine frame is the frame with the tool left out: XYZ is the point the +tool hangs from, the pivot of a head or the flange of a robot, in machine +coordinates, and the rotary letters are the orientation as the machine frame +type reports it, the rotary joints on a mill, the flange angles on a robot. +It is what `G13.1` and `G49` select and `G53.5` moves in. On a machine whose +slides line up with its frame it is the identity type. A machine whose +carriage does not line up, a slanted slide, a composite Y made by two slides, +an offset pivot, declares a machine frame type of its own next to its +identity type, and its identity type then keeps the plain meaning, joints are +axes, which a consumer uses to skip the maths. A robot's working transform +reports the flange in the base frame, the tool applied on top by the +interpreter and not by the maths, so the one type is both the working +transform and the machine frame, what ABB calls `tool0` and KUKA +`$NULLFRAME`. The zero-pose rule holds for the machine frame type as it does +for the identity: the working transform and the machine frame type agree with +the rotaries at zero. Orientations are measured against the machine frame, and there are two of them. A module reports the tool frame and the work frame separately, each in machine @@ -559,7 +564,9 @@ the last answer after a switch. A type also says what it IS: `identity` marks the no-transform type, joints are axes; `primary` the working transform `G43.4` and `G43.5` switch to; `machine` the machine frame type `G13.1` and `G49` cancel to and `G53.5` moves in, which a module leaves unset when its -identity type is that frame (see the Switchable Kinematics chapter). A module with +identity type is that frame and sets on its working transform when that +reports the flange with no tool, as the robot modules do (see the Switchable +Kinematics chapter). A module with several types has one geometry table and one ops table per type, registered with `switchkinsRegisterOps()`; a module with one type describes itself in a `kins_module` and links `kins_single.c`. diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 56d82baa261..aefdec57aac 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -228,7 +228,7 @@ 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 +to the machine frame kinstype, what 'G13.1' selects. See the G-code documentation for 'G43.4', 'G43.5' and 'G49', and for 'G12.1' and 'G13.1', for the full description. === M-code commands @@ -568,8 +568,8 @@ kinematics.h: . *KINSTYPE_IDENTITY* no transform: the joints are the world . *KINSTYPE_PRIMARY* the module's working transform -. *KINSTYPE_MACHINE* the machine frame: the pivot in machine coordinates, - the rotaries as joints +. *KINSTYPE_MACHINE* the machine frame: the tool left out, XYZ the pivot + or the flange in machine coordinates A kinstype registered with switchkinsRegisterOps() carries its flag in the ops table itself, as the 'identity', 'primary' or 'machine' field; a @@ -585,15 +585,24 @@ kinstype declared KINSTYPE_MACHINE, 'G53.5', 'G28.5' and 'G30.5' move in its world, and 'G43.4' switches to the kinstype declared KINSTYPE_PRIMARY, whatever their numbers, so a module whose kinematics are not in the conventional order still gets working spellings. The -machine frame type is the machine with the orientation left out, XYZ the -pivot in machine coordinates and the rotary letters the rotary joints; -on a machine whose slides line up with its frame that is the identity -type, so a module that declares no machine frame kinstype has its -identity kinstype stand in, and every shipped module is of that kind. A -module whose carriage does not line up, a slanted slide or a composite Y -on a mill-turn, declares its machine frame kinstype separately and keeps -KINSTYPE_IDENTITY for a kinstype whose joints really are the axes, since -motion and the planner skip the maths on that flag alone. At most one +machine frame kinstype is the machine with the tool left out, XYZ the +pivot or the flange in machine coordinates and the rotary letters the +orientation as that kinstype reports it; on a machine whose slides line +up with its frame that is the identity kinstype, so a module that +declares no machine frame kinstype has its identity kinstype stand in, +and every shipped machine tool module is of that kind. A robot's +working transform reports the flange in the base frame, the tool applied +on top by the interpreter and not by the maths, so pumakins and +genserkins declare that one kinstype both primary and machine: 'G13.1' +and 'G49' leave a robot on its arm kinematics, as ABB's `tool0` and +KUKA's `$NULLFRAME` do, and its identity kinstype is reached by number. +A machine frame kinstype does not read the tool offset in the parameter +block; a module whose working transform does declares a kinstype without +it. A module whose carriage does not line up, a slanted slide or a +composite Y on a mill-turn, declares its machine frame kinstype +separately and keeps KINSTYPE_IDENTITY for a kinstype whose joints +really are the axes, since motion and the planner skip the maths on that +flag alone. At most one kinstype may be declared identity, at most one primary and at most one machine frame, and declaring a kinstype the module does not provide fails the module load. A module that declares nothing keeps working diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index c9a140a7ab6..0e39a16ca11 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -689,11 +689,15 @@ static int genser_inverse(const kins_params *p, kins_scratch *s, return GO_RESULT_ERROR; } +/* the world is the last link's frame in the base frame with no tool in the + maths, the machine frame of a robot as well as its working transform, so + the one type is both */ const kins_ops GENSER_OPS = { .forward = genser_forward, .inverse = genser_inverse, .jacobian = genser_jacobian, .primary = 1, + .machine = 1, }; /* diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index 5642ab92c3d..5dbc9719a5b 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -120,20 +120,23 @@ extern KINEMATICS_TYPE kinematicsType(void); ** numeric-only and G13.1 refuses to guess. ** ** The machine frame type is the one whose world is the machine frame -** with the orientation left out: XYZ is the pivot, the point the rotary -** joints do not move, in machine coordinates, and the rotary letters are -** the rotary joints as they are. On a machine whose slides line up with -** its frame that is the identity type, and where a module declares no -** machine frame type its identity type stands in. A module whose -** carriage does not line up, a slanted slide or an offset pivot, declares -** its machine frame type separately and keeps identity for a type whose -** joints really are the axes, since a consumer skips the maths on that -** flag alone. +** with the tool left out: XYZ is the point the tool hangs from, the pivot +** of a head or the flange of a robot, in machine coordinates, and the +** rotary letters are the orientation as that type reports it, the rotary +** joints on a mill, the flange angles on a robot. On a machine whose +** slides line up with its frame that is the identity type, and where a +** module declares no machine frame type its identity type stands in. A +** module whose carriage does not line up, a slanted slide or an offset +** pivot, declares its machine frame type separately and keeps identity +** for a type whose joints really are the axes, since a consumer skips +** the maths on that flag alone. A robot's working transform is its +** flange in the base frame with no tool in the maths, so it is the +** machine frame type as well and carries both flags. */ #define KINSTYPE_IDENTITY 0x1 /* no transform: the joints are the world */ #define KINSTYPE_PRIMARY 0x2 /* the module's working transform */ -#define KINSTYPE_MACHINE 0x4 /* the machine frame: the pivot in machine - coordinates, the rotaries as joints */ +#define KINSTYPE_MACHINE 0x4 /* the machine frame: the tool left out, XYZ + the pivot or flange in machine coordinates */ /* flags of a kinematics type, or -1 for a type the module does not ** provide (and for every type on a machine with plain kinematics) */ diff --git a/src/emc/kinematics/kins_module.h b/src/emc/kinematics/kins_module.h index 414b91196ee..314dd9d32b4 100644 --- a/src/emc/kinematics/kins_module.h +++ b/src/emc/kinematics/kins_module.h @@ -326,9 +326,12 @@ typedef int (*kins_jacobian_fn)(const kins_params *p, const double *joint, with the last answer after a switch. identity says joints are axes, which a consumer may use to skip the maths altogether. primary says this is the module's working transform, the type G43.4 switches to. machine says - this is the machine frame type, the pivot in machine coordinates with the - rotaries as joints, which G13.1 and G49 select and G53.5 moves in; a module - that leaves it unset on every type has its identity type stand in. */ + this is the machine frame type, the tool left out, the pivot or the flange + in machine coordinates, which G13.1 and G49 select and G53.5 moves in; a + module that leaves it unset on every type has its identity type stand in. + A machine frame type does not read the tool offset in the parameter block, + so a working transform that leaves the tool out anyway, a robot's flange, + carries primary and machine both. */ typedef struct kins_ops { kins_forward_fn forward; kins_inverse_fn inverse; diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index cbed57f8285..3788afbf8e4 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -413,7 +413,10 @@ static int puma_inverse(const kins_params *p, kins_scratch *s, // is the shared identity one. The maths is the ISO 9787 flange frame, so // the tool axis it produces runs holder towards tip, the opposite of the // convention; the declared half turn puts it right. No closed form -// Jacobian: the shared code differences the inverse. +// Jacobian: the shared code differences the inverse. The world is the +// flange in the base frame with no tool in the maths, D6 being geometry, +// which is the machine frame of a robot as well as its working transform, +// so the one type is both. static const kins_ops puma_ops = { .forward = puma_forward, .inverse = puma_inverse, @@ -422,6 +425,7 @@ static const kins_ops puma_ops = { .tool = puma_tool_frame, .native = &TOOL_FRAME_FLANGE, .primary = 1, + .machine = 1, }; int switchkinsSetup(kparms* kp, diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index fa3e2bbb452..ea10c04d68f 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -1060,8 +1060,8 @@ bool Interp::kins_machine_frame(setup_pointer s) } // The axis letters read as machine frame coordinates, the words the module's -// machine frame type answers to: the pivot in machine coordinates, the -// rotaries as joints, in program units. The joints are read in on the type +// machine frame type answers to: the pivot or the flange in machine +// coordinates, the orientation as that type reports it, in program units. The joints are read in on the type // in force and come out as the joints of the machine frame point, the letters // not given standing where they are. Where the machine frame type is a plain // identity the letters name joints, and a letter carries a unit class where diff --git a/tests/ptp-robot/README b/tests/ptp-robot/README index 8fda2b1b350..72b4454251e 100644 --- a/tests/ptp-robot/README +++ b/tests/ptp-robot/README @@ -1,6 +1,8 @@ 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. +pumakins declares its arm kinematics the machine frame as well as the +working transform, since its world is the flange in the base frame with +no tool, so G53.5 moves the flange along a base axis and holds the rest, +from the arm kinematics and from the identity alike, and G13.1, G43.4 +and G49 all leave the robot on the arm kinematics. G53.7 names joints +by number, and G53.4 takes a Cartesian destination. diff --git a/tests/ptp-robot/test-ui.py b/tests/ptp-robot/test-ui.py index c013a42f2ff..375de49eaf2 100755 --- a/tests/ptp-robot/test-ui.py +++ b/tests/ptp-robot/test-ui.py @@ -1,7 +1,8 @@ #!/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. +# the letter form of the point-to-point move is the machine frame, the flange +# in the base frame with no tool, and the joint form names the joints. +import hal import linuxcnc import sys import time @@ -41,19 +42,6 @@ def 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) @@ -90,11 +78,74 @@ def refused(cmd, expect): 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") +# the letter form is the machine frame: on a robot the arm kinematics with +# no tool, the flange in the base frame, so a letter moves that coordinate +# of the flange and every other one holds, the orientation included +def flange(): + s.poll() + return list(s.position[:6]) + +def kins_type(): + return int(hal.get_value("motion.kins-type")) + +def check_held(what, before, after, moved): + for i, name in enumerate("XYZABC"): + want = before[i] + moved.get(name, 0.0) + off = after[i] - want + if i >= 3: + # the flange angles come back in (-180, 180] + off = (off + 180.0) % 360.0 - 180.0 + if abs(off) > 1e-3: + error("%s left %s at %.4f, not %.4f" % (what, name, after[i], want)) + +mdi("G53.7 G0 J0=0 J1=-30 J2=40 J3=0 J4=50 J5=0") +before = flange() +after_joints = mdi("G53.5 G0 Z%.6f" % (before[2] + 50)) +after = flange() +print("G53.5 G0 Z+50 %s" % " ".join("%.4f" % v for v in after)) +drain() +check_held("G53.5 Z", before, after, {"Z": 50}) +if abs(after_joints[0]) > 1e-6: + error("G53.5 Z turned the waist to %.6f" % after_joints[0]) + +before = flange() +mdi("G53.5 G0 X%.6f A%.6f" % (before[0] - 40, before[3] + 15)) +after = flange() +print("G53.5 G0 X-40 A+15 %s" % " ".join("%.4f" % v for v in after)) +drain() +check_held("G53.5 X A", before, after, {"X": -40, "A": 15}) + +# the same move asked from the identity kinematics lands the flange at the +# same place: the letters are the machine frame whatever type is in force, +# and G13.1 comes back to the arm kinematics, which is that frame +mdi("G53.7 G0 J0=0 J1=-30 J2=40 J3=0 J4=50 J5=0") +before = flange() +mdi("G12.1 P1") +if kins_type() != 1: + error("G12.1 P1 left kins-type %d" % kins_type()) +mdi("G53.5 G0 Z%.6f" % (before[2] + 50)) +mdi("G13.1") +if kins_type() != 0: + error("G13.1 left kins-type %d, the arm kinematics is type 0" % kins_type()) +after = flange() +print("G53.5 G0 Z+50 from the identity %s" % " ".join("%.4f" % v for v in after)) +drain() +check_held("G53.5 Z from the identity", before, after, {"Z": 50}) + +# G43.4 and G49 leave a robot on its arm kinematics: there is no other +# frame to come back to +mdi("G43.4 H1") +if kins_type() != 0: + error("G43.4 left kins-type %d" % kins_type()) +mdi("G49") +if kins_type() != 0: + error("G49 left kins-type %d" % kins_type()) +drain() + +# a letter of the wrong unit class is not a joint any more: A is the flange +# roll, in degrees, and Z is a length, so neither is refused +mdi("G53.5 G0 A0") +drain() # and the code that takes a Cartesian target still works: the point the # robot is standing on is reachable by definition, so ask for it From 1afcd434330621cd900670c8594f993143de6bc8 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 19 Sep 2026 18:53:54 +1000 Subject: [PATCH 64/77] tests: what the limits have to mean on a kinematics that is not the identity Two tests that fail today and define item 6 of the multiaxis plan: travel is the joint limits, on every move and every kinematics type, and the [AXIS_L] box is the machine frame envelope, the carriage, not the tool tip. tests/kins-limits/swing: 5axiskins with a 400 mm pivot, the head laid over to B90 by a joint move, then G0 C180 with the tool tip held. The carriage draws a half circle of radius 400 to keep the tip still, and the planner runs C at C's own limits, so the slides are asked for 1396 mm/s^2 against their 800 and would be asked for 419 mm/s against their 200. The joints follow their commands through limit3 at 20 percent over each joint's own limits, the way a drive or a step generator follows only as fast as it can, so the command a joint cannot follow is a real following error: joint 1 trips it 84 cycles in. The test asserts no following error, the endpoint reached, and every joint's commanded velocity and acceleration within its own INI limit at every servo cycle, read from a sampler. The slowest joint has to set the pace; how fast the tip goes is not asserted. tests/kins-limits/box: joints reaching 1000, the box at 500, a 250 pivot and a 50 tool with the head at B90 so that the tip sits 300 from the carriage. Under the tool centre point kinematics a move with the tip past the box and the carriage inside it must be accepted, one with the tip inside and the carriage past the box refused, G53.5 to a carriage past the box refused and to one inside it accepted whatever the tip does; a carriage past the joint limit is refused whatever the box says. Today the box is checked on the world of the kinematics in force, the tip, and never on a joint-target move, so three of the five go the wrong way. --- tests/kins-limits/box/README | 12 +++ tests/kins-limits/box/checkresult | 3 + tests/kins-limits/box/sim.hal | 16 +++ tests/kins-limits/box/test-ui.py | 117 +++++++++++++++++++++ tests/kins-limits/box/test.ini | 132 ++++++++++++++++++++++++ tests/kins-limits/box/test.sh | 4 + tests/kins-limits/box/tool.tbl | 1 + tests/kins-limits/swing/README | 15 +++ tests/kins-limits/swing/checkresult | 3 + tests/kins-limits/swing/sim.hal | 58 +++++++++++ tests/kins-limits/swing/test-ui.py | 153 ++++++++++++++++++++++++++++ tests/kins-limits/swing/test.ini | 144 ++++++++++++++++++++++++++ tests/kins-limits/swing/test.sh | 4 + tests/kins-limits/swing/tool.tbl | 1 + 14 files changed, 663 insertions(+) create mode 100644 tests/kins-limits/box/README create mode 100755 tests/kins-limits/box/checkresult create mode 100644 tests/kins-limits/box/sim.hal create mode 100755 tests/kins-limits/box/test-ui.py create mode 100644 tests/kins-limits/box/test.ini create mode 100755 tests/kins-limits/box/test.sh create mode 100644 tests/kins-limits/box/tool.tbl create mode 100644 tests/kins-limits/swing/README create mode 100755 tests/kins-limits/swing/checkresult create mode 100644 tests/kins-limits/swing/sim.hal create mode 100755 tests/kins-limits/swing/test-ui.py create mode 100644 tests/kins-limits/swing/test.ini create mode 100755 tests/kins-limits/swing/test.sh create mode 100644 tests/kins-limits/swing/tool.tbl diff --git a/tests/kins-limits/box/README b/tests/kins-limits/box/README new file mode 100644 index 00000000000..e00646f7457 --- /dev/null +++ b/tests/kins-limits/box/README @@ -0,0 +1,12 @@ +The [AXIS_L] box is the machine frame envelope: the carriage, not the +tool tip. + +5axiskins with the joints reaching 1000 and the box at 500, a 250 pivot and +a 50 tool, the head laid over to B90 so that the tip sits 300 from the +carriage in the XY plane. Under the tool centre point kinematics: a move +whose tip is past the box with the carriage inside it is accepted; a move +whose tip is inside with the carriage past the box is refused; G53.5 to a +carriage position past the box is refused, and to one inside it with the +tip past the box is accepted. A carriage past the joint limit is refused +whatever the box says. Nothing is asserted about the identity kinematics, +where the box and the joints describe the same thing. diff --git a/tests/kins-limits/box/checkresult b/tests/kins-limits/box/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/kins-limits/box/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/kins-limits/box/sim.hal b/tests/kins-limits/box/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/kins-limits/box/sim.hal @@ -0,0 +1,16 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/kins-limits/box/test-ui.py b/tests/kins-limits/box/test-ui.py new file mode 100755 index 00000000000..c43aa594a6b --- /dev/null +++ b/tests/kins-limits/box/test-ui.py @@ -0,0 +1,117 @@ +#!/usr/bin/env python3 +# The [AXIS_L] box is the machine frame envelope, the carriage and not the +# tool tip: see README. +import linuxcnc +import sys +import time + +JOINTS = 6 + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def joints(): + s.poll() + return [s.joint_position[i] for i in range(JOINTS)] + +def drain(): + said = [] + while True: + m = e.poll() + if not m: + return said + said.append(m) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + now = joints() + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.05) + error("timed out waiting for the move") + return last + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(60) + return settled() + +def accepted(cmd, carriage_x): + drain() + now = mdi(cmd) + said = [m for m in drain() if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR)] + if said: + error("%s was refused: %s" % (cmd, said[0][1].strip())) + elif abs(now[0] - carriage_x) > 1e-3: + error("%s left the carriage at X %.3f, not %.3f" % (cmd, now[0], carriage_x)) + else: + print("accepted as expected: %-24s carriage X %.3f" % (cmd, now[0])) + c.mode(linuxcnc.MODE_MDI) + c.wait_complete(30) + +def refused(cmd, needle): + drain() + before = joints() + c.mdi(cmd) + c.wait_complete(30) + m = e.poll() + if not m or m[0] not in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + settled() + error("%s was accepted" % cmd) + elif needle not in m[1]: + error("%s said %r, nothing about %r" % (cmd, m[1].strip(), needle)) + else: + print("refused as expected: %s" % m[1].strip()) + drain() + settled() + if max(abs(a - b) for a, b in zip(joints(), before)) > 1e-6: + error("%s moved the joints" % cmd) + c.mode(linuxcnc.MODE_MDI) + c.wait_complete(30) + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() + +# the tool centre point kinematics with the tool on, the head laid over by +# a joint move: the tip is now 300 from the carriage along +X at C0 +mdi("G12.1 P0") +mdi("G43 H1") +mdi("G53.7 G0 J0=0 J1=0 J2=0 J3=90 J4=0 J5=0") +j = joints() +s.poll() +print("carriage X %.3f, tip X %.3f" % (j[0], s.position[0])) +if abs(s.position[0] - j[0] - 300) > 1e-3: + error("the tip is %.3f from the carriage, not 300" % (s.position[0] - j[0])) + +# the tip past the box, the carriage inside it: accepted +accepted("G0 X600", 300) +# the tip inside the box, the carriage past it: refused +refused("G0 X-300", "limit") +# the carriage past the joint limit: refused whatever the box says +refused("G0 X-800", "limit") +# G53.5 names the carriage: past the box refused, inside it accepted even +# with the tip past the box +refused("G53.5 G0 X-600", "limit") +accepted("G53.5 G0 X400", 400) + +mdi("G53.7 G0 J0=0 J3=0") +mdi("G49") +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/kins-limits/box/test.ini b/tests/kins-limits/box/test.ini new file mode 100644 index 00000000000..48dd3792291 --- /dev/null +++ b/tests/kins-limits/box/test.ini @@ -0,0 +1,132 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# switchkins-type 0 is 5axiskins, the tool centre point; 1 is identity +# the joints reach further than the [AXIS_L] box, so the box is a +# restriction the config imposes and not the travel +KINEMATICS = 5axiskins coordinates=xyzbcw +JOINTS = 6 + +[HAL] +HALFILE = sim.hal + +[TRAJ] +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 200 +MAX_LINEAR_VELOCITY = 346 +MAX_LINEAR_ACCELERATION = 800 +DEFAULT_LINEAR_ACCELERATION = 800 +MAX_ANGULAR_VELOCITY = 360 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Y] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_B] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[JOINT_0] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = LINEAR +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/kins-limits/box/test.sh b/tests/kins-limits/box/test.sh new file mode 100755 index 00000000000..765cf14fed6 --- /dev/null +++ b/tests/kins-limits/box/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/kins-limits/box/tool.tbl b/tests/kins-limits/box/tool.tbl new file mode 100644 index 00000000000..b558277601f --- /dev/null +++ b/tests/kins-limits/box/tool.tbl @@ -0,0 +1 @@ +T1 P1 D6.0 Z50 ; diff --git a/tests/kins-limits/swing/README b/tests/kins-limits/swing/README new file mode 100644 index 00000000000..a9d17309b6d --- /dev/null +++ b/tests/kins-limits/swing/README @@ -0,0 +1,15 @@ +A turn of the head under the tool centre point must not run a slide +past its own limits. + +5axiskins with a 400 mm pivot, the head laid over to B90 by a joint move, +then a plain G0 C180 with the tool tip held: the tip does not move, the +carriage swings through a half circle of radius 400 to keep it still. The +joints follow their commands through limit3, 20 percent over each joint's +own velocity and acceleration limits, the way a drive or a step generator +follows only as fast as it can; a command inside the limits is followed +exactly, one well over them falls behind and motion trips the following +error. The test asserts that the move ends on its endpoint with no +following error, and that no joint's commanded velocity or acceleration +went over the joint's INI limit, sampled every servo cycle. The slowest +joint has to set the pace of the move; how slow the tip goes is not +asserted. diff --git a/tests/kins-limits/swing/checkresult b/tests/kins-limits/swing/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/kins-limits/swing/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/kins-limits/swing/sim.hal b/tests/kins-limits/swing/sim.hal new file mode 100644 index 00000000000..8b447ed8103 --- /dev/null +++ b/tests/kins-limits/swing/sim.hal @@ -0,0 +1,58 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +# each joint follows its command the way a drive or a step generator does, +# only as fast as it can: 20 percent over the joint's own limits, so a +# command inside the limits is followed exactly and one well over them falls +# behind and trips the following error +loadrt limit3 count=6 + +# the joint commands and their velocities every servo cycle, for the report +loadrt sampler depth=4000 cfg=ffffffffffff + +addf motion-command-handler servo-thread +addf motion-controller servo-thread +addf limit3.0 servo-thread +addf limit3.1 servo-thread +addf limit3.2 servo-thread +addf limit3.3 servo-thread +addf limit3.4 servo-thread +addf limit3.5 servo-thread +addf sampler.0 servo-thread + +setp limit3.0.maxv 240 +setp limit3.0.maxa 960 +setp limit3.1.maxv 240 +setp limit3.1.maxa 960 +setp limit3.2.maxv 240 +setp limit3.2.maxa 960 +setp limit3.3.maxv 72 +setp limit3.3.maxa 240 +setp limit3.4.maxv 72 +setp limit3.4.maxa 240 +setp limit3.5.maxv 240 +setp limit3.5.maxa 960 + +net J0cmd joint.0.motor-pos-cmd => limit3.0.in sampler.0.pin.0 +net J1cmd joint.1.motor-pos-cmd => limit3.1.in sampler.0.pin.1 +net J2cmd joint.2.motor-pos-cmd => limit3.2.in sampler.0.pin.2 +net J3cmd joint.3.motor-pos-cmd => limit3.3.in sampler.0.pin.3 +net J4cmd joint.4.motor-pos-cmd => limit3.4.in sampler.0.pin.4 +net J5cmd joint.5.motor-pos-cmd => limit3.5.in sampler.0.pin.5 +net J0fb limit3.0.out => joint.0.motor-pos-fb +net J1fb limit3.1.out => joint.1.motor-pos-fb +net J2fb limit3.2.out => joint.2.motor-pos-fb +net J3fb limit3.3.out => joint.3.motor-pos-fb +net J4fb limit3.4.out => joint.4.motor-pos-fb +net J5fb limit3.5.out => joint.5.motor-pos-fb +net J0vel joint.0.vel-cmd => sampler.0.pin.6 +net J1vel joint.1.vel-cmd => sampler.0.pin.7 +net J2vel joint.2.vel-cmd => sampler.0.pin.8 +net J3vel joint.3.vel-cmd => sampler.0.pin.9 +net J4vel joint.4.vel-cmd => sampler.0.pin.10 +net J5vel joint.5.vel-cmd => sampler.0.pin.11 +loadusr halsampler -t samples.log + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/kins-limits/swing/test-ui.py b/tests/kins-limits/swing/test-ui.py new file mode 100755 index 00000000000..fea6005fe38 --- /dev/null +++ b/tests/kins-limits/swing/test-ui.py @@ -0,0 +1,153 @@ +#!/usr/bin/env python3 +# A turn of the head under the tool centre point must not run a slide past +# its own limits: see README. +import hal +import linuxcnc +import os +import sys +import time + +JOINTS = 6 +LOG = "samples.log" +SERVO = 0.001 + +ini = linuxcnc.ini("test.ini") +VEL = [float(ini.find("JOINT_%d" % j, "MAX_VELOCITY")) for j in range(JOINTS)] +ACC = [float(ini.find("JOINT_%d" % j, "MAX_ACCELERATION")) for j in range(JOINTS)] + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def joints(): + s.poll() + return [s.joint_position[i] for i in range(JOINTS)] + +def drain(): + said = [] + while True: + m = e.poll() + if not m: + return said + said.append(m) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + now = joints() + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.05) + error("timed out waiting for the move") + return last + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(60) + return settled() + +def log_samples(): + with open(LOG) as f: + lines = f.read().split("\n") + out = [] + for line in lines[:-1]: + v = line.split() + if len(v) != 1 + 2 * JOINTS: + continue + v = [float(x) for x in v[1:]] + out.append((v[:JOINTS], v[JOINTS:])) + return out + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() + +# the tool centre point kinematics, the head laid over by a joint move so +# that getting there cannot swing anything; the carriage at X-400 puts the +# tip at X0, the centre of the half circle it is about to draw +mdi("G12.1 P0") +mdi("G53.7 G0 J0=-400 J1=0 J2=0 J3=90 J4=0 J5=0") +start = joints() +s.poll() +tip = list(s.position[:3]) +print("start joints %s, tip %s" % (" ".join("%.3f" % v for v in start), + " ".join("%.3f" % v for v in tip))) +drain() +# the log is block buffered: wait until it has caught up with the machine +# at rest before marking where the swing starts in it +deadline = time.time() + 10 +while True: + samples = log_samples() + if samples and max(abs(a - b) for a, b in zip(samples[-1][0], start)) < 2e-6: + break + if time.time() > deadline: + error("the sampler log did not catch up with the machine") + break + time.sleep(0.02) +n0 = len(samples) + +# the swing: the tip holds, C turns half a revolution, the carriage follows +# a half circle of radius 400 to keep the tip where it is +c.mdi("G0 C180") +c.wait_complete(60) +end = settled() +said = drain() +time.sleep(0.5) +samples = log_samples()[n0:] + +for m in said: + print("channel:", m) +faults = [m for m in said if "following error" in m[1]] +if faults: + error("the swing tripped a following error: %s" % faults[0][1].strip()) +s.poll() +if s.task_state != linuxcnc.STATE_ON: + error("the machine is not on after the swing (task state %d)" % s.task_state) +if abs(end[4] - 180) > 1e-3: + error("C ended at %.4f, not 180" % end[4]) +s.poll() +after = list(s.position[:3]) +if max(abs(a - b) for a, b in zip(after, tip)) > 1e-3: + error("the tip moved from %s to %s" % (tip, after)) + +# the commanded velocity and acceleration of every joint against its own +# INI limit, every servo cycle; a joint over its limit is what the drive +# could not follow. A fault freezes the command in one cycle, which is +# not an acceleration the planner asked for: the samples stop at the last +# moving one +last = max((k for k in range(len(samples)) if any(abs(v) > 0 for v in samples[k][1])), default=-1) +samples = samples[:last + 1] +print("%d samples through the swing" % len(samples)) +for j in range(JOINTS): + vpeak = max(abs(v[j]) for p, v in samples) if samples else 0.0 + apeak = 0.0 + for k in range(1, len(samples)): + apeak = max(apeak, abs(samples[k][1][j] - samples[k - 1][1][j]) / SERVO) + print("joint %d: velocity peak %8.3f of %8.3f (%.2fx), acceleration peak %9.2f of %9.2f (%.2fx)" + % (j, vpeak, VEL[j], vpeak / VEL[j], apeak, ACC[j], apeak / ACC[j])) + if vpeak > VEL[j] * 1.001: + error("joint %d was commanded at %.3f, over its limit of %.3f" % (j, vpeak, VEL[j])) + if apeak > ACC[j] * 1.01: + error("joint %d was commanded at %.2f, over its acceleration limit of %.2f" % (j, apeak, ACC[j])) + +overruns = int(hal.get_value("sampler.0.overruns")) +if overruns: + error("the sampler lost %d samples" % overruns) +if not errors: + os.unlink(LOG) +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/kins-limits/swing/test.ini b/tests/kins-limits/swing/test.ini new file mode 100644 index 00000000000..8eac90e4fdf --- /dev/null +++ b/tests/kins-limits/swing/test.ini @@ -0,0 +1,144 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# switchkins-type 0 is 5axiskins, the tool centre point; 1 is identity +# the pivot is long so that a turn of the head swings the slides fast +KINEMATICS = 5axiskins coordinates=xyzbcw +JOINTS = 6 + +[HAL] +HALFILE = sim.hal +HALCMD = setp 5axiskins.pivot-length 400 + +[TRAJ] +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 200 +MAX_LINEAR_VELOCITY = 346 +MAX_LINEAR_ACCELERATION = 800 +DEFAULT_LINEAR_ACCELERATION = 800 +MAX_ANGULAR_VELOCITY = 360 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Y] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_B] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[JOINT_0] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/kins-limits/swing/test.sh b/tests/kins-limits/swing/test.sh new file mode 100755 index 00000000000..d27cc469eb5 --- /dev/null +++ b/tests/kins-limits/swing/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak samples.log +linuxcnc -r test.ini diff --git a/tests/kins-limits/swing/tool.tbl b/tests/kins-limits/swing/tool.tbl new file mode 100644 index 00000000000..d793e2d60ed --- /dev/null +++ b/tests/kins-limits/swing/tool.tbl @@ -0,0 +1 @@ +T1 P1 D0.0 Z12.5 ; From 06382dd1045182f33930888c4cb8327a75fc501b Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 19 Sep 2026 19:07:51 +1000 Subject: [PATCH 65/77] motion: read the [AXIS_L] box on the machine frame, whatever kinematics is in force The position limits in [AXIS_L] were checked on the world of the kinematics in force, so under a tool centre point kinematics they boxed the tool tip, which moves with the tool length and the head, and a joint-target move never saw them at all. A box on the tip refuses moves the machine can make and lets through moves it cannot, which is why switchkins.adoc told configurations to setp ini.L.min_limit at every switch and the puma sims shipped M128 to M130 to do it. The box is now the machine frame envelope: the carriage, or a robot's flange, in machine coordinates, the frame that does not move when a head tilts or a tool is loaded. kinematicsMachineFrame() runs the forward of the type flagged KINSTYPE_MACHINE on a set of joints without switching to it, and motion's inRange() takes every endpoint through it after the inverse, Cartesian and joint interpolated moves alike, so G0 and G53.5 to the same carriage position get the same answer. A module that declares no machine frame type keeps the box on the world of the type in force, as before; motion links the new entry point weakly so an older module still loads. The travel itself is the joint limits, checked on every move as they were. The puma M-codes keep swapping the velocity and acceleration limits per type and leave the box alone, and M128 restores max_velocity rather than a pin that never existed. tests/kins-limits/box goes green: under the tool centre point a move with the tip past the box and the carriage inside is accepted, one with the tip inside and the carriage past the box is refused, and G53.5 gets the box too. --- configs/sim/axis/vismach/puma/mcodes/M128 | 7 +- configs/sim/axis/vismach/puma/mcodes/M129 | 9 +- configs/sim/axis/vismach/puma/mcodes/M130 | 9 +- configs/sim/axis/vismach/puma/puma.ini | 2 +- configs/sim/axis/vismach/puma/puma560.ini | 2 +- configs/sim/axis/vismach/puma/puma560_uvw.ini | 2 +- configs/sim/axis/vismach/puma/puma_cube.ini | 2 +- docs/src/config/ini-config.adoc | 5 + docs/src/motion/switchkins.adoc | 37 +++++--- src/emc/kinematics/kinematics.h | 8 ++ src/emc/kinematics/kins_single.c | 9 ++ src/emc/kinematics/switchkins.c | 15 +++ src/emc/motion/command.c | 95 +++++++++++++------ 13 files changed, 143 insertions(+), 59 deletions(-) diff --git a/configs/sim/axis/vismach/puma/mcodes/M128 b/configs/sim/axis/vismach/puma/mcodes/M128 index 26375100367..145f76e0015 100755 --- a/configs/sim/axis/vismach/puma/mcodes/M128 +++ b/configs/sim/axis/vismach/puma/mcodes/M128 @@ -9,12 +9,11 @@ emc_init -ini $::env(INI_FILE_NAME) -quick package require Hal parse_ini $::env(INI_FILE_NAME) -# RESTORE INI axis limits +# RESTORE INI axis velocity and acceleration limits (the position +# limits are never changed: a box on the machine frame, the flange) foreach l $coords { set L [string toupper $l] - catch {hal setp ini.$l.min_limit [set ::AXIS_[set L](MIN_LIMIT)]} - catch {hal setp ini.$l.max_limit [set ::AXIS_[set L](MAX_LIMIT)]} - catch {hal setp ini.$l.min_velocity [set ::AXIS_[set L](MAX_VELOCITY)]} + catch {hal setp ini.$l.max_velocity [set ::AXIS_[set L](MAX_VELOCITY)]} catch {hal setp ini.$l.max_acceleration [set ::AXIS_[set L](MAX_ACCELERATION)]} } diff --git a/configs/sim/axis/vismach/puma/mcodes/M129 b/configs/sim/axis/vismach/puma/mcodes/M129 index f9f9a2d6b84..7fe3c4e7e19 100755 --- a/configs/sim/axis/vismach/puma/mcodes/M129 +++ b/configs/sim/axis/vismach/puma/mcodes/M129 @@ -1,5 +1,5 @@ #!/usr/bin/tclsh -# switch kinematics and change axis limits to equiv joint limits +# switch kinematics and change axis velocity limits to equiv joint limits set kinstype 1 ;# 1: IDENTITY kins set switchkins_pin 3 ;# agree with inifile set max_joints 6 ;# agree with inifile @@ -10,11 +10,12 @@ emc_init -ini $::env(INI_FILE_NAME) -quick package require Hal parse_ini $::env(INI_FILE_NAME) -# SET axis limits for IDENTITY kins using INI joint limits +# SET axis velocity and acceleration limits for IDENTITY kins using +# INI joint limits. The position limits stay: they are a box on the +# machine frame, the flange, which motion reads through the arm +# kinematics whatever type is in force for {set j 0} {$j < $max_joints} {incr j} { set l [lindex $coords $j] - catch {hal setp ini.$l.min_limit [set ::JOINT_[set j](MIN_LIMIT)]} - catch {hal setp ini.$l.max_limit [set ::JOINT_[set j](MAX_LIMIT)]} catch {hal setp ini.$l.max_velocity [set ::JOINT_[set j](MAX_VELOCITY)]} catch {hal setp ini.$l.max_acceleration [set ::JOINT_[set j](MAX_ACCELERATION)]} } diff --git a/configs/sim/axis/vismach/puma/mcodes/M130 b/configs/sim/axis/vismach/puma/mcodes/M130 index 8051f2a2237..dcdede0fa04 100755 --- a/configs/sim/axis/vismach/puma/mcodes/M130 +++ b/configs/sim/axis/vismach/puma/mcodes/M130 @@ -1,5 +1,5 @@ #!/usr/bin/tclsh -# switch kinematics and change axis limits +# switch kinematics and change axis velocity limits set kinstype 2 ;# 2: TYPE 2 kins set switchkins_pin 3 ;# agree with inifile set max_joints 6 ;# agree with inifile @@ -10,11 +10,12 @@ emc_init -ini $::env(INI_FILE_NAME) -quick package require Hal parse_ini $::env(INI_FILE_NAME) -# SET axis limits for IDENTITY kins using INI joint limits +# SET axis velocity and acceleration limits for IDENTITY kins using +# INI joint limits. The position limits stay: they are a box on the +# machine frame, the flange, which motion reads through the arm +# kinematics whatever type is in force for {set j 0} {$j < $max_joints} {incr j} { set l [lindex $coords $j] - catch {hal setp ini.$l.min_limit [set ::JOINT_[set j](MIN_LIMIT)]} - catch {hal setp ini.$l.max_limit [set ::JOINT_[set j](MAX_LIMIT)]} catch {hal setp ini.$l.max_velocity [set ::JOINT_[set j](MAX_VELOCITY)]} catch {hal setp ini.$l.max_acceleration [set ::JOINT_[set j](MAX_ACCELERATION)]} } diff --git a/configs/sim/axis/vismach/puma/puma.ini b/configs/sim/axis/vismach/puma/puma.ini index 99c0fa33112..f40fb5849c9 100644 --- a/configs/sim/axis/vismach/puma/puma.ini +++ b/configs/sim/axis/vismach/puma/puma.ini @@ -33,7 +33,7 @@ RS274NGC_STARTUP_CODE = G21 G10L2P0 x450 y100 z-495 a-180 (debug, ini: startup o MDI_COMMAND = M428 MDI_COMMAND = M429 MDI_COMMAND = M430 -# MDI-COMMANDS 03,04,05 ALTER limits when switching +# MDI-COMMANDS 03,04,05 ALTER velocity and acceleration limits when switching MDI_COMMAND = M128 MDI_COMMAND = M129 MDI_COMMAND = M130 diff --git a/configs/sim/axis/vismach/puma/puma560.ini b/configs/sim/axis/vismach/puma/puma560.ini index 046f5d118d5..3d181caff47 100644 --- a/configs/sim/axis/vismach/puma/puma560.ini +++ b/configs/sim/axis/vismach/puma/puma560.ini @@ -36,7 +36,7 @@ PARAMETER_FILE = puma560.var MDI_COMMAND = M428 MDI_COMMAND = M429 MDI_COMMAND = M430 -# MDI-COMMANDS 03,04,05 ALTER limits when switching +# MDI-COMMANDS 03,04,05 ALTER velocity and acceleration limits when switching MDI_COMMAND = M128 MDI_COMMAND = M129 MDI_COMMAND = M130 diff --git a/configs/sim/axis/vismach/puma/puma560_uvw.ini b/configs/sim/axis/vismach/puma/puma560_uvw.ini index a6b9f8544e9..b4d84d0e71c 100644 --- a/configs/sim/axis/vismach/puma/puma560_uvw.ini +++ b/configs/sim/axis/vismach/puma/puma560_uvw.ini @@ -35,7 +35,7 @@ PARAMETER_FILE = puma560.var MDI_COMMAND = M428 MDI_COMMAND = M429 MDI_COMMAND = M430 -# MDI-COMMANDS 03,04,05 ALTER limits when switching +# MDI-COMMANDS 03,04,05 ALTER velocity and acceleration limits when switching MDI_COMMAND = M128 MDI_COMMAND = M129 MDI_COMMAND = M130 diff --git a/configs/sim/axis/vismach/puma/puma_cube.ini b/configs/sim/axis/vismach/puma/puma_cube.ini index 8397ce3ea7a..2742f6d1059 100644 --- a/configs/sim/axis/vismach/puma/puma_cube.ini +++ b/configs/sim/axis/vismach/puma/puma_cube.ini @@ -113,7 +113,7 @@ POSTGUI_HALFILE = puma_postgui.hal MDI_COMMAND = M428 MDI_COMMAND = M429 MDI_COMMAND = M430 -# MDI-COMMANDS 03,04,05 ALTER limits when switching +# MDI-COMMANDS 03,04,05 ALTER velocity and acceleration limits when switching MDI_COMMAND = M128 MDI_COMMAND = M129 MDI_COMMAND = M130 diff --git a/docs/src/config/ini-config.adoc b/docs/src/config/ini-config.adoc index 04c8b551cae..26a60ad570e 100644 --- a/docs/src/config/ini-config.adoc +++ b/docs/src/config/ini-config.adoc @@ -1048,6 +1048,11 @@ The __ specifies one of: X Y Z A B C U V W When this limit is exceeded, the controller aborts axis motion. The axis must be homed before MAX_LIMIT is in force. For a rotary axis (A,B,C typ) with unlimited rotation having no `MAX_LIMIT` for that axis in the `[AXIS_``]` section a value of 1e99 is used. ++ +`MIN_LIMIT` and `MAX_LIMIT` together are a box in the machine frame: the carriage, or the flange of a robot, in machine coordinates, the frame that does not move when a head tilts or a tool is loaded. +On a kinematics module that declares its machine frame type (see the <> chapter) every move is checked against the box where its joints put the machine frame, whatever kinematics type is in force: a tool centre point move whose tip lies past the box with the carriage inside it is accepted, and a 'G53.5' to a carriage position past the box is refused. +The travel itself is the `[JOINT_N]` limits; the box is a restriction the configuration adds inside it. +A module that declares no machine frame type keeps the box on the world of the kinematics in force. * `WRAPPED_ROTARY = 1` - (bool) When this is set to 1 for an ANGULAR axis the axis will move 0-359.999 degrees. Positive Numbers will move the axis in a positive direction and negative numbers will move the axis in the negative direction. It is ignored on a `LINEAR` axis. diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index aefdec57aac..13e2033a452 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -300,9 +300,21 @@ MAX_VELOCITY = MIN_ACCELERATION = ---- -The INI file limits specified apply to the type 0 default -kinematics type that is activated at startup. These limits may -*not* be applicable when switching to alternative kinematics. +The position limits, `MIN_LIMIT` and `MAX_LIMIT`, are a box in the +machine frame, the world of the kinstype the module declares +KINSTYPE_MACHINE (its identity kinstype where it declares none): +the carriage in machine coordinates, or a robot's flange in its base +frame. Every move is checked against that box where its joints put +the machine frame, whichever kinstype is in force, so the box means +the same thing after a switch as before it and needs no resetting. +The travel is the `[JOINT_N]` limits, checked on every move as well; +the box is a restriction inside it. A module that declares no +kinstype flags keeps the box on the world of the kinstype in force, +as before. + +The velocity and acceleration limits specified apply to the type 0 +default kinematics type that is activated at startup. These limits +may *not* be applicable when switching to alternative kinematics. However, since an interpreter-motion synchronization is required when switching kinematics, INI-HAL pins can be used to setup limits for a pending kinematics type. @@ -340,7 +352,7 @@ kinematics (trivkins). See the kins man page for more information ($ man kins). A user-provided M-code can alter any or all of the axis coordinate -limits prior to changing the motion.switchkins-type pin and +velocity and acceleration limits prior to changing the motion.switchkins-type pin and synchronizing the interpreter and motionparts of LinuxCNC. As an example, a bash script invoking halcmd can be 'hardcoded' to set any number of HAL pins: @@ -366,24 +378,25 @@ with a complex (non-identity) kinematics (type0) after homing. The system is configured so that it can be switched to identity kinematics (type1) in order to manipulate individual joints using the conventional letters from the set 'XYZABCUVW'. The INI file -settings ([AXIS_L]) are *not* applicable when operating with -identity (type1) kinematics. To address this use case, the user -M-code scripts can be designed as follows: +velocity and acceleration settings ([AXIS_L]) are *not* applicable +when operating with identity (type1) kinematics. To address this +use case, the user M-code scripts can be designed as follows: *M129* (Switch to identity type1) . read and parse INI file -. HAL: setp the INI-HAL limit pins for each axis letter ([AXIS_L]) - according to the 'identity-referenced' joint number INI file - setting ([JOINT_N]) +. HAL: setp the INI-HAL velocity and acceleration pins for each axis + letter ([AXIS_L]) according to the 'identity-referenced' joint + number INI file setting ([JOINT_N]) . HAL: `setp motion.switchkins-type 1` . MDI: execute a syncing G-code (M66E0L0) *M128* (restore robot default kinematics type 0) . read and parse INI file -. HAL: setp the INI-HAL limit pins for each axis letter ([AXIS_L]) - according to the appropriate INI file setting ([AXIS_L]) +. HAL: setp the INI-HAL velocity and acceleration pins for each axis + letter ([AXIS_L]) according to the appropriate INI file setting + ([AXIS_L]) . HAL: `setp motion.switchkins-type 0` . MDI: execute a syncing G-code (M66E0L0) diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index 5dbc9719a5b..60e2ddb2275 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -142,6 +142,14 @@ extern KINEMATICS_TYPE kinematicsType(void); ** provide (and for every type on a machine with plain kinematics) */ extern int kinematicsTypeFlags(int ktype); +/* The joints placed in the machine frame: the forward of the type +** flagged KINSTYPE_MACHINE, whatever type is in force, so that a limit +** on the frame can be read for a move under any type. pos carries the +** caller's estimate in for an iterative forward. Returns 0, -1 where +** no type declares the machine frame (a consumer then falls back on the +** world of the type in force), or -2 where the forward fails. */ +extern int kinematicsMachineFrame(const double *joint, struct EmcPose *pos); + /* 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 diff --git a/src/emc/kinematics/kins_single.c b/src/emc/kinematics/kins_single.c index 2a0b0c8dc0a..2135525eaf7 100644 --- a/src/emc/kinematics/kins_single.c +++ b/src/emc/kinematics/kins_single.c @@ -147,6 +147,14 @@ int kinematicsTypeFlags(int ktype) return -1; } +// and so no machine frame type either +int kinematicsMachineFrame(const double *joint, EmcPose *pos) +{ + (void)joint; + (void)pos; + return -1; +} + // The module's description, for a copy of it loaded outside RT. A module // with one type does not depend on its parameters for its shape, so this // is the table as declared. @@ -171,4 +179,5 @@ EXPORT_SYMBOL(kinematicsSetTool); EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsTypeFlags); +EXPORT_SYMBOL(kinematicsMachineFrame); EXPORT_SYMBOL(kinsDescribe); diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index dfbc1360629..1131529564b 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -461,6 +461,20 @@ int kinematicsTypeFlags(int ktype) return ktype_flags[ktype]; } // kinematicsTypeFlags() +int kinematicsMachineFrame(const double *joint, EmcPose *pos) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + KINEMATICS_INVERSE_FLAGS iflags = 0; + int k; + + for (k = 0; k < kins_count; k++) { + int f = kinematicsTypeFlags(k); + if (f < 0 || !(f & KINSTYPE_MACHINE)) { continue; } + return call_forward(k, joint, pos, &fflags, &iflags) == 0 ? 0 : -2; + } + return -1; +} // kinematicsMachineFrame() + int switchkinsRegisterOps(int ktype, const kins_ops *ops) { if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { @@ -569,6 +583,7 @@ EXPORT_SYMBOL(switchkinsRegisterFrames); EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); EXPORT_SYMBOL(switchkinsDeclare); EXPORT_SYMBOL(kinematicsTypeFlags); +EXPORT_SYMBOL(kinematicsMachineFrame); EXPORT_SYMBOL(switchkinsRegisterJacobian); EXPORT_SYMBOL(switchkinsRegisterOps); EXPORT_SYMBOL(switchkinsInit); diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index d94d8ee0686..265358c845a 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -74,6 +74,8 @@ // module written before the call exports no such symbol, and the weak // reference leaves it NULL rather than refusing to load motion #pragma weak kinematicsSetTool +// old modules export no kinematicsMachineFrame; keep it optional +#pragma weak kinematicsMachineFrame #define ABS(x) (((x) < 0) ? -(x) : (x)) @@ -270,47 +272,66 @@ void apply_spindle_limits(spindle_status_t *s){ } -/* inRange() returns non-zero if the position lies within the joint - limits, or 0 if not. It also reports an error for each joint limit - violation. It's possible to get more than one violation per move. */ -STATIC int inRange(EmcPose pos, int id, char *move_type) +/* The [AXIS_L] box is the machine frame envelope: the carriage, or the + flange, in machine coordinates, the frame that does not move when the + head tilts or a tool is loaded. Where the module names its machine + frame type, the endpoint's joints go through that type's forward and + the box is read there, whatever type is in force; a module that names + none keeps the box on the world of the type in force, as before. + Returns non-zero when the endpoint is inside the box, and reports each + letter outside it. */ +STATIC int box_ok(const double *joint_pos, const EmcPose *pos, int id, const char *move_type) { - double joint_pos[EMCMOT_MAX_JOINTS]; - int joint_num, axis_num; - emcmot_joint_t *joint; - int in_range = 1; + EmcPose frame = *pos; /* the estimate in, and the fallback */ int failing_axes[EMCMOT_MAX_AXIS]; double targets[EMCMOT_MAX_AXIS]; const char axis_letters[] = "XYZABCUVW"; + int axis_num, in_box = 1; if (EMCMOT_MAX_AXIS != 9) { rtapi_print_msg(RTAPI_MSG_ERR, "BUG: %s(): invalid number of axes defined", __func__); - } else { - targets[0] = pos.tran.x; - targets[1] = pos.tran.y; - targets[2] = pos.tran.z; - targets[3] = pos.a; - targets[4] = pos.b; - targets[5] = pos.c; - targets[6] = pos.u; - targets[7] = pos.v; - targets[8] = pos.w; - axis_check_constraints(targets, failing_axes); - for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num += 1) { - if (failing_axes[axis_num] == -1) { - reportError(_("%s move on line %d would exceed %c's %s limit"), - move_type, id, axis_letters[axis_num], _("negative")); - in_range = 0; - } - if (failing_axes[axis_num] == 1) { - reportError(_("%s move on line %d would exceed %c's %s limit"), - move_type, id, axis_letters[axis_num], _("positive")); - in_range = 0; - } + return 1; + } + if (kinematicsMachineFrame && kinematicsMachineFrame(joint_pos, &frame) == -2) { + reportError(_("%s move on line %d cannot be placed in the machine frame"), + move_type, id); + return 0; + } + targets[0] = frame.tran.x; + targets[1] = frame.tran.y; + targets[2] = frame.tran.z; + targets[3] = frame.a; + targets[4] = frame.b; + targets[5] = frame.c; + targets[6] = frame.u; + targets[7] = frame.v; + targets[8] = frame.w; + axis_check_constraints(targets, failing_axes); + for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num += 1) { + if (failing_axes[axis_num] == -1) { + reportError(_("%s move on line %d would exceed %c's %s limit"), + move_type, id, axis_letters[axis_num], _("negative")); + in_box = 0; + } + if (failing_axes[axis_num] == 1) { + reportError(_("%s move on line %d would exceed %c's %s limit"), + move_type, id, axis_letters[axis_num], _("positive")); + in_box = 0; } } + return in_box; +} - /* Now, check that the endpoint puts the joints within their limits too */ +/* inRange() returns non-zero if the position lies within the joint + limits and the [AXIS_L] box, or 0 if not. It also reports an error for + each limit violation. It's possible to get more than one violation per + move. */ +STATIC int inRange(EmcPose pos, int id, char *move_type) +{ + double joint_pos[EMCMOT_MAX_JOINTS]; + int joint_num; + emcmot_joint_t *joint; + int in_range = 1; /* start the inverse from where the queue leaves the joints */ queue_end_joints(joint_pos); @@ -324,6 +345,10 @@ STATIC int inRange(EmcPose pos, int id, char *move_type) return 0; } + /* the box, read where the joints put the machine frame */ + if (!box_ok(joint_pos, &pos, id, move_type)) { in_range = 0; } + + /* and the joints within their limits */ for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { /* point to joint data */ joint = &joints[joint_num]; @@ -1247,6 +1272,14 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) SET_MOTION_ERROR_FLAG(1); break; } + /* the joints named still have to keep the machine frame in the box */ + if (!box_ok(target, &end, emcmotCommand->id, "Joint interpolated")) { + reportError(_("invalid params in joint interpolated move")); + emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; + tpAbort(&emcmotInternal->coord_tp); + SET_MOTION_ERROR_FLAG(1); + break; + } } else { if (!inRange(end, emcmotCommand->id, "Joint interpolated")) { reportError(_("invalid params in joint interpolated move")); From 4cd4a55a4878b0d1065377535524069dd48d667f Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 19 Sep 2026 21:08:24 +1000 Subject: [PATCH 66/77] canon: cap every segment at what its joints can follow A segment is planned in world coordinates against the [AXIS_L] velocity and acceleration limits, which say nothing about the joints once the kinematics is not the identity: a turn of a tilted head under the tool centre point swings the carriage through a circle the head's own limit never mentions, and the carriage trips its following error. Every segment is now also sampled through the kinematics module's Jacobian in canon, ahead of motion, on the kinematics type and the tool offset the program is under, and its velocity and acceleration are lowered to what the slowest joint can give within its own [JOINT_n] limits. motion_planning/segment_cap.cc does the reading: at each sample the rate of every joint per unit of path parameter along the local tangent; between samples how that rate changes, charged against the acceleration budget at the capped velocity (what a carriage on a circle feels as centripetal, at most half the budget, the velocity lowered where it would take more); from three samples in a row how much the rate can bulge between two of them, so a peak falling between samples is covered. Lines are sampled from canon.endPoint to the target, traverse, feed, probe, rigid tap and the chained segments of the naive cam alike, through getStraightVelocity() and getStraightAcceleration(); arcs are sampled on the posemath circle ARC_FEED builds, in ARC_FEED itself. Canon owns its non-RT context: the module and joint count from [KINS] in the INI, the type tracked through SELECT_KINS_TYPE and dropped to the machine's whenever the interpreter synchs, the tool from the offset canon applies, the joints at the start of a segment kept from the last one while they still explain the point, else the machine's, else the inverse iterated to a fixed point. The identity, as a module or as the type in force, loads nothing. A module that runs only in realtime caps nothing, as before. Where the module cannot answer at a sample, or a joint would have to move at any speed at all (a pole), an operator message says so and the segment goes through capped rather than dropped, since a dropped segment sends the next one along another path. tests/kins-limits/swing goes green, and gains a second phase: the tip draws a full circle at a feed it could keep on its own while C turns another revolution, so the carriage rides the small circle and the big swing together. kinslimits prints the same cap for a move after its own per-sample listing. emcJointGetMaxVelocity() and emcJointGetMaxAcceleration() read back what iniJoint() set. The switchkins chapter's limit section says what the velocity and acceleration limits now mean and when a switch still wants them reset; the [AXIS_L] entry in the INI chapter points to it. --- docs/src/config/ini-config.adoc | 4 + docs/src/motion/kinematics-conventions.adoc | 4 +- docs/src/motion/switchkins.adoc | 28 +- src/emc/motion_planning/Submakefile | 4 +- src/emc/motion_planning/kinslimits.cc | 31 +- src/emc/motion_planning/segment_cap.cc | 175 ++++++++++ src/emc/motion_planning/segment_cap.hh | 75 +++++ src/emc/nml_intf/emc.hh | 2 + src/emc/task/Submakefile | 2 +- src/emc/task/emccanon.cc | 352 +++++++++++++++++++- src/emc/task/taskintf.cc | 18 + tests/kins-limits/swing/test-ui.py | 115 ++++--- 12 files changed, 747 insertions(+), 63 deletions(-) create mode 100644 src/emc/motion_planning/segment_cap.cc create mode 100644 src/emc/motion_planning/segment_cap.hh diff --git a/docs/src/config/ini-config.adoc b/docs/src/config/ini-config.adoc index 26a60ad570e..8c37cb75b21 100644 --- a/docs/src/config/ini-config.adoc +++ b/docs/src/config/ini-config.adoc @@ -1037,6 +1037,10 @@ The __ specifies one of: X Y Z A B C U V W The AXIS GUI shows the axis in the units of its type; other GUIs may not. * `MAX_VELOCITY = 1.2` - (real) Maximum velocity for this axis in <> per second. * `MAX_ACCELERATION = 20.0` - (real) Maximum acceleration for this axis in machine units per second squared. ++ +`MAX_VELOCITY` and `MAX_ACCELERATION` bound the letter in the world of the kinematics in force. +The joints are bound on their own: every move is capped at what its joints can follow within their `[JOINT_N]` limits, read through the kinematics module's Jacobian along the move, so on a kinematics that is not the identity a move runs at the pace of its slowest joint whatever these say. +See the <> chapter, INI file limit settings. * `MAX_JERK = 0.0` - (real) Maximum jerk for this axis in machine units per second cubed. Used when S-curve trajectory planning is enabled. When set to 0 (default), no per-axis jerk limiting is applied. diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 3904c5e0d4c..1fe501ba5db 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -595,7 +595,9 @@ names. Its tool is the caller's where the caller gives one through `kinematicsUserSetTool()`, since a planner knows what a segment runs under better than the machine does; otherwise it is motion's, from motion's own offset pins, and the loader says once when the module's tool pin disagrees with -them. `kinslimits` is built on it. +them. `kinslimits` is built on it, and so is the cap canon puts on every +segment's velocity and acceleration from the joint limits through the Jacobian +(see the Switchable Kinematics chapter, INI file limit settings). A module that does not provide the form keeps working as it did. It just cannot be evaluated outside realtime, which the loader reports. diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 13e2033a452..f58a76411e4 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -312,12 +312,28 @@ the box is a restriction inside it. A module that declares no kinstype flags keeps the box on the world of the kinstype in force, as before. -The velocity and acceleration limits specified apply to the type 0 -default kinematics type that is activated at startup. These limits -may *not* be applicable when switching to alternative kinematics. -However, since an interpreter-motion synchronization is required -when switching kinematics, INI-HAL pins can be used to setup -limits for a pending kinematics type. +The velocity and acceleration limits, `MAX_VELOCITY` and +`MAX_ACCELERATION`, bound the coordinate letter in the world of the +kinstype in force. What the joints can follow is bound separately: +every segment is sampled through the module's Jacobian ahead of +motion, on the kinstype and the tool offset the program is under, and +its velocity and acceleration are lowered to what the slowest joint +can give within its own `[JOINT_N]` limits, the share a carriage on a +circle feels as centripetal included. A turn of a tilted head under +the tool centre point, which swings the carriage through a circle the +head's own limit never mentions, runs at the carriage's pace. A +module that can only be evaluated in realtime (one without the +parameter block form, see <>) +caps nothing; on it the `[AXIS_L]` limits are all there is. + +So a switch does not need the velocity and acceleration limits reset +to keep the joints within their limits. It may still want them reset +where a letter names a joint of another kind after the switch: a +robot's identity kinstype puts joint 0, a rotary, on X, and the +`[AXIS_X]` limit in mm/s says nothing useful about it. Since an +interpreter-motion synchronization is required when switching +kinematics, INI-HAL pins can be used to set up limits for a pending +kinematics type. [NOTE] INI-HAL pins are typically not recognized during a G-code program diff --git a/src/emc/motion_planning/Submakefile b/src/emc/motion_planning/Submakefile index 3a8ccdf737a..88944bbea76 100644 --- a/src/emc/motion_planning/Submakefile +++ b/src/emc/motion_planning/Submakefile @@ -6,6 +6,7 @@ INCLUDES += emc/kinematics_userspace LIBKINSLIMITS_CXXSRCS := $(addprefix emc/motion_planning/, \ jacobian.cc \ joint_limits.cc \ + segment_cap.cc \ ) # kins_util.c is the shared kinematics code the modules link; the loader @@ -42,7 +43,8 @@ USERSRCS += $(KINSLIMITS_SRCS) TARGETS += ../bin/kinslimits -MOTION_PLANNING_HH := emc/motion_planning/jacobian.hh emc/motion_planning/joint_limits.hh +MOTION_PLANNING_HH := emc/motion_planning/jacobian.hh emc/motion_planning/joint_limits.hh \ + emc/motion_planning/segment_cap.hh $(patsubst emc/motion_planning/%,../include/%,$(MOTION_PLANNING_HH)): ../include/%.hh: emc/motion_planning/%.hh cp $^ $@ HEADERS += $(patsubst emc/motion_planning/%,../include/%,$(MOTION_PLANNING_HH)) diff --git a/src/emc/motion_planning/kinslimits.cc b/src/emc/motion_planning/kinslimits.cc index 1b750be6776..04ca010defc 100644 --- a/src/emc/motion_planning/kinslimits.cc +++ b/src/emc/motion_planning/kinslimits.cc @@ -6,8 +6,8 @@ * * The tool attaches to a running HAL instance, loads the kinematics * module through the non-RT interface, samples the move, and reports - * the most restrictive cap found along it. The sampling loop here is - * the same one the trajectory planner uses to cap a segment. + * the most restrictive cap found along it, then the cap canon puts on + * the segment through segmentCap(), which is the one the planner sees. * * Example (in a terminal with a running config, or under halrun): * @@ -36,6 +36,7 @@ #include #include "jacobian.hh" #include "joint_limits.hh" +#include "segment_cap.hh" using namespace motion_planning; @@ -263,6 +264,32 @@ int main(int argc, char **argv) min_vel, at_vel, min_vel_s, min_acc, at_acc, min_jerk, at_jerk); printf("worst cond : %.3f\n", max_cond); + /* the cap canon reads for the same move, along the same tangent, with + the change of the joint rates along the move charged as well */ + { + struct { EmcPose start, end; } line = { start, end }; + SegmentCapLimits lim; + SegmentCap cap; + double joints[KINEMATICS_USER_MAX_JOINTS] = {0}; + lim.joints = num_joints; + for (int j = 0; j < KINEMATICS_USER_MAX_JOINTS; j++) { + lim.vel[j] = j < num_joints ? vel_v[j] : SEGMENT_CAP_NONE; + lim.acc[j] = j < num_joints ? acc_v[j] : SEGMENT_CAP_NONE; + } + auto pose_at = [](double f, EmcPose *p, void *arg) { + const EmcPose *se = (const EmcPose *)arg; + for (int ax = 0; ax < 9; ax++) { + set_pose_axis(p, ax, pose_axis(se[0], ax) + f * (pose_axis(se[1], ax) - pose_axis(se[0], ax))); + } + }; + if (segmentCap(ctx, &lim, target, samples, pose_at, &line, joints, &cap) == 0) { + printf("canon's cap : vel %.3f (joint %d at s=%.3f), acc %.1f (joint %d), %d of %d samples unanswered\n", + cap.vel, cap.vel_joint, cap.vel_at, cap.acc, cap.acc_joint, cap.unanswered, cap.samples); + } else { + printf("canon's cap : none, no sample could be evaluated\n"); + } + } + kinematicsUserFree(ctx); hal_exit(comp_id); return 0; diff --git a/src/emc/motion_planning/segment_cap.cc b/src/emc/motion_planning/segment_cap.cc new file mode 100644 index 00000000000..5c14539226c --- /dev/null +++ b/src/emc/motion_planning/segment_cap.cc @@ -0,0 +1,175 @@ +/******************************************************************** + * Description: segment_cap.cc + * The velocity and acceleration cap a segment gets from the joint + * limits through the kinematics module's Jacobian. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2026 All rights reserved. + ********************************************************************/ + +#include "segment_cap.hh" +#include + +namespace motion_planning { + +static double pose_axis(const EmcPose &p, int ax) +{ + switch (ax) { + case AXIS_X: return p.tran.x; + case AXIS_Y: return p.tran.y; + case AXIS_Z: return p.tran.z; + case AXIS_A: return p.a; + case AXIS_B: return p.b; + case AXIS_C: return p.c; + case AXIS_U: return p.u; + case AXIS_V: return p.v; + default: return p.w; + } +} + +int segmentCap(KinematicsUserContext *ctx, const SegmentCapLimits *lim, + double length, int samples, SegmentPoseFn pose_at, void *arg, + double *joints, SegmentCap *out) +{ + const double tiny = 1e-12; + /* the rate of every joint per unit of path parameter at each sample, + the most that rate changes per unit of parameter on either side of + it, and the most it can bulge past the samples on either side */ + static double g[SEGMENT_CAP_MAX_SAMPLES][KINEMATICS_USER_MAX_JOINTS]; + static double h[SEGMENT_CAP_MAX_SAMPLES][KINEMATICS_USER_MAX_JOINTS]; + static double bulge[SEGMENT_CAP_MAX_SAMPLES][KINEMATICS_USER_MAX_JOINTS]; + static double s[SEGMENT_CAP_MAX_SAMPLES]; + static bool ok[SEGMENT_CAP_MAX_SAMPLES]; + int K = samples < 2 ? 2 : samples; + int njoints; + int k, j, a, prev, answered = 0; + + out->vel = SEGMENT_CAP_NONE; + out->acc = SEGMENT_CAP_NONE; + out->vel_joint = -1; + out->acc_joint = -1; + out->vel_at = 0.0; + out->samples = 0; + out->unanswered = 0; + if (!ctx || !lim || !pose_at || !joints || length <= 0.0) { return -1; } + if (K > SEGMENT_CAP_MAX_SAMPLES) { K = SEGMENT_CAP_MAX_SAMPLES; } + njoints = lim->joints; + if (njoints > KINEMATICS_USER_MAX_JOINTS) { njoints = KINEMATICS_USER_MAX_JOINTS; } + out->samples = K; + + for (k = 0; k < K; k++) { + double f = (double)k / (double)(K - 1); + /* the tangent by a central difference of the geometry, exact for + a line and a small fraction of a sample step for an arc */ + double df = 1.0 / (8.0 * (K - 1)); + double fp = f + df > 1.0 ? 1.0 : f + df; + double fm = f - df < 0.0 ? 0.0 : f - df; + EmcPose p, pp, pm; + double t[AXIS_COUNT]; + double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]; + + s[k] = f * length; + ok[k] = false; + pose_at(f, &p, arg); + pose_at(fp, &pp, arg); + pose_at(fm, &pm, arg); + for (a = 0; a < AXIS_COUNT; a++) { + t[a] = (pose_axis(pp, a) - pose_axis(pm, a)) / ((fp - fm) * length); + } + /* the joints at the sample, seeded from the sample before so the + whole segment stays on one solution branch, then the Jacobian + on that branch */ + if (kinematicsUserInverse(ctx, &p, joints) != 0 + || kinematicsUserJacobian(ctx, &p, J) != 0) { + out->unanswered++; + continue; + } + for (j = 0; j < njoints; j++) { + g[k][j] = 0.0; + h[k][j] = 0.0; + bulge[k][j] = 0.0; + for (a = 0; a < AXIS_COUNT; a++) { g[k][j] += J[j][a] * t[a]; } + } + ok[k] = true; + answered++; + } + if (!answered) { return -1; } + + /* how far the rate can rise between two samples: the parabola + through three in a row bulges by an eighth of its second + difference over its middle, and a peak between samples is + covered by charging that to all three */ + for (k = 1; k + 1 < K; k++) { + if (!ok[k - 1] || !ok[k] || !ok[k + 1]) { continue; } + for (j = 0; j < njoints; j++) { + double b = fabs(g[k + 1][j] - 2.0 * g[k][j] + g[k - 1][j]) / 8.0; + if (b > bulge[k][j]) { bulge[k][j] = b; } + if (b > bulge[k - 1][j]) { bulge[k - 1][j] = b; } + if (b > bulge[k + 1][j]) { bulge[k + 1][j] = b; } + } + } + + /* the joint velocity limits */ + for (k = 0; k < K; k++) { + if (!ok[k]) { continue; } + for (j = 0; j < njoints; j++) { + double rate = fabs(g[k][j]) + bulge[k][j]; + if (rate > tiny && lim->vel[j] / rate < out->vel) { + out->vel = lim->vel[j] / rate; + out->vel_joint = j; + out->vel_at = s[k] / length; + } + } + } + + /* how the rates change along the segment, read between neighbours + and charged to both */ + for (prev = -1, k = 0; k < K; k++) { + if (!ok[k]) { continue; } + if (prev >= 0) { + for (j = 0; j < njoints; j++) { + double dh = fabs(g[k][j] - g[prev][j]) / (s[k] - s[prev]); + if (dh > h[k][j]) { h[k][j] = dh; } + if (dh > h[prev][j]) { h[prev][j] = dh; } + } + } + prev = k; + } + + /* the changing rate at the capped velocity takes at most half a + joint's acceleration budget */ + for (k = 0; k < K; k++) { + if (!ok[k]) { continue; } + for (j = 0; j < njoints; j++) { + if (h[k][j] > tiny) { + double cap = sqrt(lim->acc[j] / (2.0 * h[k][j])); + if (cap < out->vel) { + out->vel = cap; + out->vel_joint = j; + out->vel_at = s[k] / length; + } + } + } + } + + /* the joint acceleration limits, less what the changing rate takes */ + for (k = 0; k < K; k++) { + if (!ok[k]) { continue; } + for (j = 0; j < njoints; j++) { + double rate = fabs(g[k][j]) + bulge[k][j]; + if (rate > tiny) { + double cap = (lim->acc[j] - h[k][j] * out->vel * out->vel) / rate; + if (cap < out->acc) { + out->acc = cap; + out->acc_joint = j; + } + } + } + } + return 0; +} // segmentCap() + +} // namespace motion_planning diff --git a/src/emc/motion_planning/segment_cap.hh b/src/emc/motion_planning/segment_cap.hh new file mode 100644 index 00000000000..80826bcb900 --- /dev/null +++ b/src/emc/motion_planning/segment_cap.hh @@ -0,0 +1,75 @@ +/******************************************************************** + * Description: segment_cap.hh + * The velocity and acceleration a segment can be run at without + * asking any joint for more than its own limits, read off the + * kinematics module's Jacobian along the segment. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2026 All rights reserved. + ********************************************************************/ +#ifndef SEGMENT_CAP_HH +#define SEGMENT_CAP_HH + +#include +#include + +namespace motion_planning { + +/* The joint limits the cap is read against, in the units motion commands + the joints in: [JOINT_n] MAX_VELOCITY and MAX_ACCELERATION. */ +struct SegmentCapLimits { + int joints; + double vel[KINEMATICS_USER_MAX_JOINTS]; + double acc[KINEMATICS_USER_MAX_JOINTS]; +}; + +/* No joint binds. */ +#define SEGMENT_CAP_NONE 1e9 + +/* The most a segment was sampled at. */ +#define SEGMENT_CAP_MAX_SAMPLES 129 + +/* What the joints allow along the segment: the fastest the path parameter + may advance, and the hardest it may accelerate, per second. */ +struct SegmentCap { + double vel; + double acc; + int vel_joint; /* the joint setting each, -1 for none */ + int acc_joint; + double vel_at; /* the fraction of the segment the velocity cap is read at */ + int samples; /* points the segment was sampled at */ + int unanswered; /* of them, the ones the module could not invert or differentiate */ +}; + +/* The pose at a fraction of the segment, 0 the start and 1 the end, in + the units the module takes, which are motion's. A line interpolates, + an arc walks its circle. */ +typedef void (*SegmentPoseFn)(double fraction, EmcPose *pose, void *arg); + +/* Sample the segment at `samples` points, the ends among them, and read + at each the rate every joint moves at per unit of path parameter: the + module's Jacobian along the local tangent. Between neighbouring samples + read how that rate changes with the parameter, and from three in a row + how much it can bulge between two of them, which is added to the rate so + that a peak falling between samples is covered. A joint's velocity + limit then caps the path velocity, and its acceleration limit caps the + path acceleration once the share the changing rate takes at the capped + velocity (what a carriage on a circle feels as centripetal) is set + aside: at most half the joint's budget goes to it, and the velocity is + lowered where it would take more. + + length is the segment's extent in the path parameter, in the units the + caps come back in. joints seeds the inverse at the start and comes back + holding the joints at the end, so the next segment starts on the same + solution branch. Returns 0, or -1 where no sample could be evaluated, + the caps then NONE. */ +int segmentCap(KinematicsUserContext *ctx, const SegmentCapLimits *lim, + double length, int samples, SegmentPoseFn pose_at, void *arg, + double *joints, SegmentCap *out); + +} // namespace motion_planning + +#endif // SEGMENT_CAP_HH diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index ca7b7f6eb07..e43e813906e 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -316,6 +316,8 @@ extern int emcJointSetHomingParams(int joint, double home, double offset, double extern int emcJointUpdateHomingParams(int joint, double home, double offset, int sequence); extern int emcJointSetMaxVelocity(int joint, double vel); extern int emcJointSetMaxAcceleration(int joint, double acc); +extern double emcJointGetMaxVelocity(int joint); +extern double emcJointGetMaxAcceleration(int joint); extern int emcJointInit(int joint); extern int emcJointHalt(int joint); diff --git a/src/emc/task/Submakefile b/src/emc/task/Submakefile index 44ea2ba8ac5..4776597f323 100644 --- a/src/emc/task/Submakefile +++ b/src/emc/task/Submakefile @@ -32,7 +32,7 @@ USERSRCS += $(MILLTASKSRCS) ../bin/milltask: $(call TOOBJS, $(MILLTASKSRCS)) ../lib/librs274.so.0 ../lib/liblinuxcnc.a \ ../lib/libnml.so.0 ../lib/liblinuxcncini.so.1 ../lib/libposemath.so.0 \ ../lib/liblinuxcnchal.so.0 ../lib/libpyplugin.so.0 \ - ../lib/libtooldata.so.0 + ../lib/libtooldata.so.0 ../lib/libkinslimits.so.0 $(ECHO) Linking $(notdir $@) diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 7d8fa5f3722..1d30d17c6f0 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -65,6 +65,11 @@ #include "tooldata/tooldata.hh" #include #include +#include // getpid() +#include +#include +#include +#include //#define EMCCANON_DEBUG @@ -482,6 +487,13 @@ static double toExtVel(double vel) { static double toExtAcc(double acc) { return toExtVel(acc); } +static double fromExtVel(double vel) { + if (!canon.cartesian_move && canon.angular_move) { + return FROM_EXT_ANG(vel); + } + return FROM_EXT_LEN(vel); +} + static void send_g5x_msg(int index) { flush_segments(); @@ -847,6 +859,306 @@ static double getStraightJerk(double x, double y, double z, return 0.0; // a move to nowhere } +//---------------------------------------------------------------------- +// The kinematics ahead of motion: what the joints allow a segment. +// +// A segment is planned in world coordinates against the [AXIS_L] +// velocity and acceleration limits, which say nothing about the joints +// once the kinematics is not the identity: a turn of a tilted head under +// the tool centre point swings a carriage through a circle the head's +// own limit never mentions. So every segment is also sampled through +// the module's Jacobian here, on the kinematics type and the tool offset +// the program is under (canon runs ahead of motion, so neither is what +// the machine has reached), and its velocity and acceleration are +// lowered to what the slowest joint can follow. The module is the one +// motion runs, loaded through the non-RT loader in kinematics_userspace/ +// the first time a segment asks. A module that runs only in realtime, +// and a kinematics type that is the identity, cap nothing. +//---------------------------------------------------------------------- + +static struct { + KinematicsUserContext *ctx; + int comp_id; + bool tried; // loading was attempted, once + int type; // the kinematics type the program is in, -1 for the machine's + double joints[EMCMOT_MAX_JOINTS]; // the joints at canon.endPoint, as far as canon knows + bool in_arc; // ARC_FEED caps the arc itself; the straight helpers stand back + bool debug; // EMCCANON_KINS_DEBUG in the environment: say what every segment got + motion_planning::SegmentCapLimits limits; + // the last straight segment's answer, since its velocity and its + // acceleration are asked for separately + struct { + bool valid; + EmcPose start, end; + bool capped; + double vel, acc; + } memo; +} kins = { NULL, -1, false, -1, {}, false, false, {}, { false, {}, {}, false, 0.0, 0.0 } }; + +// the machine's own module, once +static bool kins_load(void) +{ + char name[HAL_NAME_LEN + 1]; + int joints, j; + + if (kins.tried) { return kins.ctx != NULL; } + kins.tried = true; + linuxcnc::IniFile ini(emc_inifile); + if (!ini) { return false; } + auto module = ini.findString("KINEMATICS", "KINS"); + joints = ini.findIntV("JOINTS", "KINS", 0); + if (!module || joints < 1) { return false; } + snprintf(name, sizeof(name), "canon.%d", (int)getpid()); + kins.comp_id = hal_init(name); + if (kins.comp_id < 0) { return false; } + kins.ctx = kinematicsUserInitString(module->c_str(), joints, kins.comp_id, name); + hal_ready(kins.comp_id); + if (kins.ctx && kinematicsUserIsRtOnly(kins.ctx)) { + kinematicsUserFree(kins.ctx); + kins.ctx = NULL; + } + if (!kins.ctx) { + hal_exit(kins.comp_id); + kins.comp_id = -1; + return false; + } + // the joint limits as the INI gives them; one left out binds nothing + kins.limits.joints = kinematicsUserGetNumJoints(kins.ctx); + for (j = 0; j < KINEMATICS_USER_MAX_JOINTS; j++) { + double vel = j < kins.limits.joints ? emcJointGetMaxVelocity(j) : 0.0; + double acc = j < kins.limits.joints ? emcJointGetMaxAcceleration(j) : 0.0; + kins.limits.vel[j] = vel > 0.0 ? vel : SEGMENT_CAP_NONE; + kins.limits.acc[j] = acc > 0.0 ? acc : SEGMENT_CAP_NONE; + } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { kins.joints[j] = 0.0; } + kins.debug = getenv("EMCCANON_KINS_DEBUG") != NULL; + if (kins.debug) { + fprintf(stderr, "canon kins: %s, %d joints, limits", module->c_str(), kins.limits.joints); + for (j = 0; j < kins.limits.joints; j++) { + fprintf(stderr, " %g/%g", kins.limits.vel[j], kins.limits.acc[j]); + } + fprintf(stderr, "\n"); + } + return true; +} + +// the module on the type and the tool offset the program is under, or +// NULL where there is nothing to cap with +static KinematicsUserContext *kins_here(void) +{ + int type = kins.type >= 0 ? kins.type : GET_EXTERNAL_KINS_TYPE(); + int flags = GET_EXTERNAL_KINS_TYPE_FLAGS(type); + EmcPose tool; + + // the identity, whether the module as a whole is one or the type in + // force is, needs no module loaded to know the joints are the axes + if (GET_EXTERNAL_KINEMATICS_IDENTITY()) { return NULL; } + if (flags >= 0 && (flags & KINSTYPE_IDENTITY)) { return NULL; } + if (!kins_load()) { return NULL; } + if (kinematicsUserSetType(kins.ctx, type) != 0) { return NULL; } + if (kinematicsUserIsIdentity(kins.ctx)) { return NULL; } + tool = to_ext_pose(canon.toolOffset.tran.x, canon.toolOffset.tran.y, canon.toolOffset.tran.z, + canon.toolOffset.a, canon.toolOffset.b, canon.toolOffset.c, + canon.toolOffset.u, canon.toolOffset.v, canon.toolOffset.w); + kinematicsUserSetTool(kins.ctx, &tool); + return kins.ctx; +} + +// whether two machine points are the same, to a hair either way +static bool kins_same(const EmcPose *a, const EmcPose *b) +{ + const double tol = 1e-6; + + return fabs(a->tran.x - b->tran.x) < tol && fabs(a->tran.y - b->tran.y) < tol + && fabs(a->tran.z - b->tran.z) < tol + && fabs(a->a - b->a) < tol && fabs(a->b - b->b) < tol && fabs(a->c - b->c) < tol + && fabs(a->u - b->u) < tol && fabs(a->v - b->v) < tol && fabs(a->w - b->w) < tol; +} + +// the joints at the start of a segment, as far as canon can know ahead of +// motion: the ones it holds while they still explain the point, since a +// point does not name one joint set, else the joints the machine stands +// in if those do, else the inverse iterated to a fixed point +static void kins_seed(KinematicsUserContext *ctx, const EmcPose *start) +{ + double standing[EMCMOT_MAX_JOINTS] = {0}; + EmcPose seeded = *start; + int i, n, pass; + + if (kinematicsUserForward(ctx, kins.joints, &seeded) == 0 && kins_same(&seeded, start)) { return; } + n = GET_EXTERNAL_JOINT_POSITIONS(standing, EMCMOT_MAX_JOINTS); + if (n > 0) { + seeded = *start; + if (kinematicsUserForward(ctx, standing, &seeded) == 0 && kins_same(&seeded, start)) { + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { kins.joints[i] = standing[i]; } + return; + } + } + for (pass = 0; pass < 8; pass++) { + double prev[EMCMOT_MAX_JOINTS], worst = 0.0; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { prev[i] = kins.joints[i]; } + if (kinematicsUserInverse(ctx, start, kins.joints) != 0) { return; } + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { worst = fmax(worst, fabs(kins.joints[i] - prev[i])); } + if (worst < 1e-9) { break; } + } +} + +// how many points to sample a segment at: enough that a turn of the +// rotaries, a stretch of travel on a machine whose Jacobian changes with +// position, or a sweep of arc is seen every few degrees or centimetres +static int kins_samples(const EmcPose *start, const EmcPose *end, double sweep_deg) +{ + double rot = fmax(fabs(end->a - start->a), fmax(fabs(end->b - start->b), fabs(end->c - start->c))); + double lin = sqrt(pow(end->tran.x - start->tran.x, 2) + pow(end->tran.y - start->tran.y, 2) + + pow(end->tran.z - start->tran.z, 2)); + int n = 3 + (int)ceil(rot / 10.0) + (int)ceil(lin / 50.0) + (int)ceil(sweep_deg / 10.0); + + return n > SEGMENT_CAP_MAX_SAMPLES ? SEGMENT_CAP_MAX_SAMPLES : n; +} + +// what the joints allow along a segment, in the machine's units per second +// of the path parameter canon plans it with; false where nothing binds +static bool kins_cap(KinematicsUserContext *ctx, const EmcPose *start, const EmcPose *end, + double length, int samples, motion_planning::SegmentPoseFn pose_at, void *arg, + double *vel, double *acc) +{ + motion_planning::SegmentCap cap; + + kins_seed(ctx, start); + if (motion_planning::segmentCap(ctx, &kins.limits, length, samples, pose_at, arg, kins.joints, &cap) != 0) { + CANON_ERROR("the kinematics cannot reach the move to X%.3f Y%.3f Z%.3f A%.3f B%.3f C%.3f, the joints are not checked along it", + end->tran.x, end->tran.y, end->tran.z, end->a, end->b, end->c); + return false; + } + if (cap.unanswered) { + CANON_ERROR("the kinematics cannot reach %d of %d points on the move to X%.3f Y%.3f Z%.3f A%.3f B%.3f C%.3f, the joints are not checked there", + cap.unanswered, cap.samples, end->tran.x, end->tran.y, end->tran.z, end->a, end->b, end->c); + } + // a pole: some joint would have to move at any speed at all for the + // path to advance, and the move crawls rather than being dropped, + // since a dropped segment would send the next one along another path + if (cap.vel_joint >= 0 && cap.vel < 1e-3) { + CANON_ERROR("joint %d cannot follow the move to X%.3f Y%.3f Z%.3f A%.3f B%.3f C%.3f near %.0f%% of its length: the kinematics is singular there and the move crawls", + cap.vel_joint, end->tran.x, end->tran.y, end->tran.z, end->a, end->b, end->c, 100.0 * cap.vel_at); + } + if (kins.debug) { + fprintf(stderr, "canon kins: to X%.3f Y%.3f Z%.3f A%.3f B%.3f C%.3f length %.3f in %d samples: vel %.3f (joint %d at %.2f) acc %.3f (joint %d), %d unanswered; seed", + end->tran.x, end->tran.y, end->tran.z, end->a, end->b, end->c, length, cap.samples, + cap.vel, cap.vel_joint, cap.vel_at, cap.acc, cap.acc_joint, cap.unanswered); + for (int j = 0; j < kins.limits.joints; j++) { fprintf(stderr, " %.3f", kins.joints[j]); } + fprintf(stderr, "\n"); + } + *vel = cap.vel; + *acc = cap.acc; + return cap.vel_joint >= 0 || cap.acc_joint >= 0; +} + +struct KinsLine { + EmcPose start, end; +}; + +static void kins_line_pose(double f, EmcPose *pose, void *arg) +{ + const KinsLine *line = (const KinsLine *)arg; + + pose->tran.x = line->start.tran.x + f * (line->end.tran.x - line->start.tran.x); + pose->tran.y = line->start.tran.y + f * (line->end.tran.y - line->start.tran.y); + pose->tran.z = line->start.tran.z + f * (line->end.tran.z - line->start.tran.z); + pose->a = line->start.a + f * (line->end.a - line->start.a); + pose->b = line->start.b + f * (line->end.b - line->start.b); + pose->c = line->start.c + f * (line->end.c - line->start.c); + pose->u = line->start.u + f * (line->end.u - line->start.u); + pose->v = line->start.v + f * (line->end.v - line->start.v); + pose->w = line->start.w + f * (line->end.w - line->start.w); +} + +// the cap on a straight segment from canon.endPoint, in the machine's +// units per second of the parameter getStraightVelocity() plans it with, +// the length along the axes getStraightSpan() measures +static bool kins_straight_cap(double x, double y, double z, + double a, double b, double c, + double u, double v, double w, + double *vel, double *acc) +{ + KinematicsUserContext *ctx; + KinsLine line; + double length; + + if (kins.in_arc) { return false; } + line.start = to_ext_pose(canon.endPoint); + line.end = to_ext_pose(x, y, z, a, b, c, u, v, w); + if (kins.memo.valid && kins_same(&kins.memo.start, &line.start) && kins_same(&kins.memo.end, &line.end)) { + *vel = kins.memo.vel; + *acc = kins.memo.acc; + return kins.memo.capped; + } + ctx = kins_here(); + if (!ctx) { return false; } + // the length getStraightVelocity() plans the segment along + StraightSpan span = getStraightSpan(x, y, z, a, b, c, u, v, w); + if (!span.moving) { + return false; + } + length = axisKindsMeasuredAngular(kinds, span.measured) ? + TO_EXT_ANG(span.length) : TO_EXT_LEN(span.length); + kins.memo.valid = true; + kins.memo.start = line.start; + kins.memo.end = line.end; + kins.memo.capped = kins_cap(ctx, &line.start, &line.end, length, + kins_samples(&line.start, &line.end, 0.0), + kins_line_pose, &line, &kins.memo.vel, &kins.memo.acc); + *vel = kins.memo.vel; + *acc = kins.memo.acc; + return kins.memo.capped; +} + +struct KinsArc { + PmCircle circle; // in canon's units, as ARC_FEED builds it + bool line; // no turn at all: the chord + EmcPose start, end; +}; + +static void kins_arc_pose(double f, EmcPose *pose, void *arg) +{ + const KinsArc *arc = (const KinsArc *)arg; + PmCartesian p; + + kins_line_pose(f, pose, (void *)&arc->start); + if (arc->line) { return; } + pmCirclePoint(&arc->circle, f * arc->circle.angle, &p); + pose->tran.x = TO_EXT_LEN(p.x); + pose->tran.y = TO_EXT_LEN(p.y); + pose->tran.z = TO_EXT_LEN(p.z); +} + +// the cap on an arc from canon.endPoint, in the machine's units per +// second of the XYZ length ARC_FEED plans it with +static bool kins_arc_cap(const PM_CARTESIAN ¢er, const PM_CARTESIAN &normal, + const PM_CARTESIAN &end_xyz, const CANON_POSITION &endpt, + int rotation, double length, double full_angle, + double *vel, double *acc) +{ + KinematicsUserContext *ctx = kins_here(); + KinsArc arc; + + if (!ctx || length <= 0.0) { return false; } + arc.start = to_ext_pose(canon.endPoint); + arc.end = to_ext_pose(endpt); + arc.line = rotation == 0; + if (!arc.line) { + PmCartesian s = { canon.endPoint.x, canon.endPoint.y, canon.endPoint.z }; + PmCartesian e = { end_xyz.x, end_xyz.y, end_xyz.z }; + PmCartesian c = { center.x, center.y, center.z }; + PmCartesian n = { normal.x, normal.y, normal.z }; + if (pmCircleInit(&arc.circle, &s, &e, &c, &n, rotation > 0 ? rotation - 1 : rotation) != 0) { + return false; + } + } + return kins_cap(ctx, &arc.start, &arc.end, TO_EXT_LEN(length), + kins_samples(&arc.start, &arc.end, fabs(full_angle) * 180.0 / M_PI), + kins_arc_pose, &arc, vel, acc); +} + /** * Get the limiting acceleration for a displacement from the current position to the given position. * returns a single acceleration that is the minimum of all axis accelerations. @@ -876,6 +1188,14 @@ static AccelData getStraightAcceleration(double x, double y, double z, if (out.tmax > 0.0) { out.acc = out.dtot / out.tmax; } + // and what the joints allow, through the kinematics + { + double kvel, kacc; + if (out.acc > 0.0 && kins_straight_cap(x, y, z, a, b, c, u, v, w, &kvel, &kacc) + && fromExtVel(kacc) < out.acc) { + out.acc = fromExtVel(kacc); + } + } if(debug_velacc) printf("cartesian %d ang %d acc %g\n", canon.cartesian_move, canon.angular_move, out.acc); return out; @@ -920,6 +1240,14 @@ static VelData getStraightVelocity(double x, double y, double z, } else { out.vel = canon.linearFeedRate; } + // and what the joints allow, through the kinematics + { + double kvel, kacc; + if (out.vel > 0.0 && kins_straight_cap(x, y, z, a, b, c, u, v, w, &kvel, &kacc) + && fromExtVel(kvel) < out.vel) { + out.vel = fromExtVel(kvel); + } + } if(debug_velacc) printf("cartesian %d ang %d vel %g\n", canon.cartesian_move, canon.angular_move, out.vel); return out; @@ -1108,6 +1436,10 @@ void SELECT_KINS_TYPE(int switchkins_type) { flush_segments(); + // the segments from here on run under it + kins.type = switchkins_type; + kins.memo.valid = false; + auto selectKinsMsg = std::make_unique(); selectKinsMsg->switchkins_type = switchkins_type; @@ -2807,6 +3139,7 @@ void ARC_FEED(int line_number, // Find the equivalent maximum velocity for a linear displacement // This accounts for speed restrictions due to helical and other axes + kins.in_arc = true; VelData veldata = getStraightVelocity(endpt); // Compute spiral length, first by the minimum circular arc length @@ -2839,6 +3172,7 @@ void ARC_FEED(int line_number, // Use "straight" acceleration measure to compute acceleration bounds due // to non-circular components (helical axis, other axes) AccelData accdata = getStraightAcceleration(endpt); + kins.in_arc = false; double tt_max_motion = accdata.tmax; double tt_max_spiral = spiral_length / a_max_axes; @@ -2848,6 +3182,16 @@ void ARC_FEED(int line_number, // circle plane and helical axis will still be within limits double a_max = total_xyz_length / tt_max; + // and what the joints allow along the arc, through the kinematics + { + double kvel, kacc; + if (kins_arc_cap(center_cart, normal_cart, end_cart, endpt, rotation, + total_xyz_length, full_angle, &kvel, &kacc)) { + v_max = std::min(v_max, FROM_EXT_LEN(kvel)); + a_max = std::min(a_max, FROM_EXT_LEN(kacc)); + } + } + // Limit velocity by maximum double vel = std::min(canon.linearFeedRate, v_max); canon_debug("current F = %f\n",canon.linearFeedRate); @@ -3065,6 +3409,7 @@ static void use_tool_length_offset(const EmcPose& offset, const EmcPose *point) canon.toolOffset.u = FROM_PROG_AX(6, offset.u); canon.toolOffset.v = FROM_PROG_AX(7, offset.v); canon.toolOffset.w = FROM_PROG_AX(8, offset.w); + kins.memo.valid = false; /* append it to interp list so it gets updated at the right time, not at read-ahead time */ @@ -3613,6 +3958,8 @@ void INIT_CANON() canon.linearFeedRate = 0.0; canon.angularFeedRate = 0.0; ZERO_EMC_POSE(canon.toolOffset); + kins.type = -1; + kins.memo.valid = false; { std::string err; @@ -4070,7 +4417,10 @@ int GET_EXTERNAL_KINS_TYPE() // motion publishes the kinematics it is actually running, which is // not necessarily the one G-code last asked for: an abort can drop a // queued switch, and the motion.switchkins-type pin can select one - // without the interpreter seeing it + // without the interpreter seeing it. The interpreter asks when it + // synchs to the machine; canon's own idea of the type goes with it + kins.type = -1; + kins.memo.valid = false; return emcStatus->motion.traj.switchkins_type; } diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 1b709fb0292..756ecb223b4 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -585,6 +585,24 @@ int emcAxisSetLockingJoint(int axis, int joint) return retval; } +double emcJointGetMaxVelocity(int joint) +{ + if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) { + return 0; + } + + return JointConfig[joint].MaxVel; +} + +double emcJointGetMaxAcceleration(int joint) +{ + if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) { + return 0; + } + + return JointConfig[joint].MaxAccel; +} + double emcAxisGetMaxVelocity(int axis) { if (axis < 0 || axis >= EMCMOT_MAX_AXIS) { diff --git a/tests/kins-limits/swing/test-ui.py b/tests/kins-limits/swing/test-ui.py index fea6005fe38..44d45ab8e52 100755 --- a/tests/kins-limits/swing/test-ui.py +++ b/tests/kins-limits/swing/test-ui.py @@ -87,62 +87,75 @@ def log_samples(): print("start joints %s, tip %s" % (" ".join("%.3f" % v for v in start), " ".join("%.3f" % v for v in tip))) drain() -# the log is block buffered: wait until it has caught up with the machine -# at rest before marking where the swing starts in it -deadline = time.time() + 10 -while True: - samples = log_samples() - if samples and max(abs(a - b) for a, b in zip(samples[-1][0], start)) < 2e-6: - break - if time.time() > deadline: - error("the sampler log did not catch up with the machine") - break - time.sleep(0.02) -n0 = len(samples) +def catch_up(where): + # the log is block buffered: wait until it has caught up with the + # machine at rest before marking where the next move starts in it + deadline = time.time() + 10 + while True: + samples = log_samples() + if samples and max(abs(a - b) for a, b in zip(samples[-1][0], where)) < 2e-6: + return len(samples) + if time.time() > deadline: + error("the sampler log did not catch up with the machine") + return len(samples) + time.sleep(0.02) + +def swing(cmd, n0): + c.mdi(cmd) + c.wait_complete(60) + end = settled() + said = drain() + time.sleep(0.5) + samples = log_samples()[n0:] + for m in said: + print("channel:", m) + faults = [m for m in said if "following error" in m[1]] + if faults: + error("%s tripped a following error: %s" % (cmd, faults[0][1].strip())) + s.poll() + if s.task_state != linuxcnc.STATE_ON: + error("the machine is not on after %s (task state %d)" % (cmd, s.task_state)) + # the commanded velocity and acceleration of every joint against its + # own INI limit, every servo cycle; a joint over its limit is what the + # drive could not follow. A fault freezes the command in one cycle, + # which is not an acceleration the planner asked for: the samples stop + # at the last moving one + last = max((k for k in range(len(samples)) if any(abs(v) > 0 for v in samples[k][1])), default=-1) + samples = samples[:last + 1] + print("%d samples through %s" % (len(samples), cmd)) + for j in range(JOINTS): + vpeak = max(abs(v[j]) for p, v in samples) if samples else 0.0 + apeak = 0.0 + for k in range(1, len(samples)): + apeak = max(apeak, abs(samples[k][1][j] - samples[k - 1][1][j]) / SERVO) + print("joint %d: velocity peak %8.3f of %8.3f (%.2fx), acceleration peak %9.2f of %9.2f (%.2fx)" + % (j, vpeak, VEL[j], vpeak / VEL[j], apeak, ACC[j], apeak / ACC[j])) + if vpeak > VEL[j] * 1.001: + error("joint %d was commanded at %.3f, over its limit of %.3f" % (j, vpeak, VEL[j])) + if apeak > ACC[j] * 1.01: + error("joint %d was commanded at %.2f, over its acceleration limit of %.2f" % (j, apeak, ACC[j])) + return end + +def tip_held(what): + s.poll() + after = list(s.position[:3]) + if max(abs(a - b) for a, b in zip(after, tip)) > 1e-3: + error("the tip moved from %s to %s through %s" % (tip, after, what)) # the swing: the tip holds, C turns half a revolution, the carriage follows # a half circle of radius 400 to keep the tip where it is -c.mdi("G0 C180") -c.wait_complete(60) -end = settled() -said = drain() -time.sleep(0.5) -samples = log_samples()[n0:] - -for m in said: - print("channel:", m) -faults = [m for m in said if "following error" in m[1]] -if faults: - error("the swing tripped a following error: %s" % faults[0][1].strip()) -s.poll() -if s.task_state != linuxcnc.STATE_ON: - error("the machine is not on after the swing (task state %d)" % s.task_state) +end = swing("G0 C180", catch_up(start)) if abs(end[4] - 180) > 1e-3: error("C ended at %.4f, not 180" % end[4]) -s.poll() -after = list(s.position[:3]) -if max(abs(a - b) for a, b in zip(after, tip)) > 1e-3: - error("the tip moved from %s to %s" % (tip, after)) - -# the commanded velocity and acceleration of every joint against its own -# INI limit, every servo cycle; a joint over its limit is what the drive -# could not follow. A fault freezes the command in one cycle, which is -# not an acceleration the planner asked for: the samples stop at the last -# moving one -last = max((k for k in range(len(samples)) if any(abs(v) > 0 for v in samples[k][1])), default=-1) -samples = samples[:last + 1] -print("%d samples through the swing" % len(samples)) -for j in range(JOINTS): - vpeak = max(abs(v[j]) for p, v in samples) if samples else 0.0 - apeak = 0.0 - for k in range(1, len(samples)): - apeak = max(apeak, abs(samples[k][1][j] - samples[k - 1][1][j]) / SERVO) - print("joint %d: velocity peak %8.3f of %8.3f (%.2fx), acceleration peak %9.2f of %9.2f (%.2fx)" - % (j, vpeak, VEL[j], vpeak / VEL[j], apeak, ACC[j], apeak / ACC[j])) - if vpeak > VEL[j] * 1.001: - error("joint %d was commanded at %.3f, over its limit of %.3f" % (j, vpeak, VEL[j])) - if apeak > ACC[j] * 1.01: - error("joint %d was commanded at %.2f, over its acceleration limit of %.2f" % (j, apeak, ACC[j])) +tip_held("G0 C180") + +# the same on an arc: the tip draws a full circle of radius 100 at a feed +# it could keep on its own while C turns another revolution, so the +# carriage rides the small circle and the big swing together +end = swing("G17 G2 X0 Y0 I100 J0 C540 F5000", catch_up(end)) +if abs(end[4] - 540) > 1e-3: + error("C ended at %.4f, not 540" % end[4]) +tip_held("the arc") overruns = int(hal.get_value("sampler.0.overruns")) if overruns: From 9c86f65396a8819077ca92385f1490ce6315aee5 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 19 Sep 2026 21:39:56 +1000 Subject: [PATCH 67/77] motion: cap a world jog at what its joints can follow A world jog is planned per axis against the [AXIS_L] limits, which say nothing about the joints once the kinematics is not the identity: a jog of C under the tool centre point with the head laid over swings the carriage through a circle, and the carriage trips its following error a fraction of a degree in. The segment cap in canon cannot help, a jog never passes canon. Each servo cycle in teleop mode, before the axis planners run, teleop_joint_cap() reads the module's Jacobian at the joints the machine stands in (weak, so an old module without one keeps today's behaviour, and nothing is read on the identity) and, for the jog the planners were asked for, the rate every joint would move at per unit of it and how that rate has changed since the last cycle, what a carriage on a circle feels as centripetal. The planners are then capped so that no joint is asked for more than its own limits: the velocity by the joint velocity limits, read a stopping distance ahead since the rate keeps growing while the planners slow down, and by the acceleration budget the changing rate takes at that velocity, at most half of it; the acceleration by what the budget leaves. Every active axis scales by the same factor, so a jog of two axes keeps its direction. The jog's own request is kept apart from the planner's caps in the axis, teleop_vel_req and teleop_acc_req, and axis_teleop_request() and axis_teleop_cap() are the two calls the cap needs from axis.c. tests/kins-limits/jog is the swing test's machine in teleop mode: a continuous jog of C at the axis's full rate stopped past 150 degrees, an incremental jog back by 90, an incremental jog of X with the head still laid over, every joint following through limit3 at 20 percent over its limits; red before this (joint 1 asked for 1396 mm/s^2 against 800, following error at C 0.4), green with it, the carriage held at its limits and the jog of C running faster where the carriage rate is small and slowing as it grows. The switchkins and INI chapters say that a jog is bounded the same way as a move. --- docs/src/config/ini-config.adoc | 2 +- docs/src/motion/switchkins.adoc | 7 +- src/emc/motion/axis.c | 36 ++++++ src/emc/motion/axis.h | 2 + src/emc/motion/control.c | 117 ++++++++++++++++++ tests/kins-limits/jog/README | 17 +++ tests/kins-limits/jog/checkresult | 3 + tests/kins-limits/jog/sim.hal | 58 +++++++++ tests/kins-limits/jog/test-ui.py | 189 ++++++++++++++++++++++++++++++ tests/kins-limits/jog/test.ini | 144 +++++++++++++++++++++++ tests/kins-limits/jog/test.sh | 4 + tests/kins-limits/jog/tool.tbl | 1 + 12 files changed, 578 insertions(+), 2 deletions(-) create mode 100644 tests/kins-limits/jog/README create mode 100755 tests/kins-limits/jog/checkresult create mode 100644 tests/kins-limits/jog/sim.hal create mode 100755 tests/kins-limits/jog/test-ui.py create mode 100644 tests/kins-limits/jog/test.ini create mode 100755 tests/kins-limits/jog/test.sh create mode 100644 tests/kins-limits/jog/tool.tbl diff --git a/docs/src/config/ini-config.adoc b/docs/src/config/ini-config.adoc index 8c37cb75b21..61ae1c1cf6a 100644 --- a/docs/src/config/ini-config.adoc +++ b/docs/src/config/ini-config.adoc @@ -1039,7 +1039,7 @@ The __ specifies one of: X Y Z A B C U V W * `MAX_ACCELERATION = 20.0` - (real) Maximum acceleration for this axis in machine units per second squared. + `MAX_VELOCITY` and `MAX_ACCELERATION` bound the letter in the world of the kinematics in force. -The joints are bound on their own: every move is capped at what its joints can follow within their `[JOINT_N]` limits, read through the kinematics module's Jacobian along the move, so on a kinematics that is not the identity a move runs at the pace of its slowest joint whatever these say. +The joints are bound on their own: every move is capped at what its joints can follow within their `[JOINT_N]` limits, read through the kinematics module's Jacobian along the move, and a world jog the same way every servo cycle, so on a kinematics that is not the identity a move or a jog runs at the pace of its slowest joint whatever these say. See the <> chapter, INI file limit settings. * `MAX_JERK = 0.0` - (real) Maximum jerk for this axis in machine units per second cubed. Used when S-curve trajectory planning is enabled. diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index f58a76411e4..9907fb2d479 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -324,7 +324,12 @@ the tool centre point, which swings the carriage through a circle the head's own limit never mentions, runs at the carriage's pace. A module that can only be evaluated in realtime (one without the parameter block form, see <>) -caps nothing; on it the `[AXIS_L]` limits are all there is. +caps nothing; on it the `[AXIS_L]` limits are all there is. A world +jog is bounded the same way, in motion, every servo cycle: the Jacobian +at the joints the machine stands in caps the jog's planners so that no +joint is asked for more than its limits, and a jog of C under the tool +centre point with the head laid over runs at the pace of the carriage +it swings. So a switch does not need the velocity and acceleration limits reset to keep the joints within their limits. It may still want them reset diff --git a/src/emc/motion/axis.c b/src/emc/motion/axis.c index 9e1908a6aa4..16c39cf91a8 100644 --- a/src/emc/motion/axis.c +++ b/src/emc/motion/axis.c @@ -15,6 +15,8 @@ typedef struct { double acc_limit; /* upper limit of axis accel */ double jerk_limit; /* upper limit of axis jerk */ simple_tp_t teleop_tp; /* planner for teleop mode motion */ + double teleop_vel_req; /* the speed the jog asked for */ + double teleop_acc_req; /* and the acceleration it may use */ int old_ajog_counts; /* prior value, used for deltas */ int kb_ajog_active; /* non-zero during a keyboard jog */ @@ -279,6 +281,8 @@ void axis_jog_cont(int axis_num, double vel, long servo_period) axis->teleop_tp.max_vel = fabs(vel); axis->teleop_tp.max_acc = axis->acc_limit; + axis->teleop_vel_req = axis->teleop_tp.max_vel; + axis->teleop_acc_req = axis->teleop_tp.max_acc; axis->kb_ajog_active = 1; axis->teleop_tp.enable = 1; } @@ -301,6 +305,8 @@ void axis_jog_incr(int axis_num, double offset, double vel, long servo_period) axis->teleop_tp.pos_cmd = tmp1; axis->teleop_tp.max_vel = fabs(vel); axis->teleop_tp.max_acc = axis->acc_limit; + axis->teleop_vel_req = axis->teleop_tp.max_vel; + axis->teleop_acc_req = axis->teleop_tp.max_acc; axis->kb_ajog_active = 1; axis->teleop_tp.enable = 1; } @@ -322,6 +328,8 @@ void axis_jog_abs(int axis_num, double offset, double vel) axis->teleop_tp.pos_cmd = tmp1; axis->teleop_tp.max_vel = fabs(vel); axis->teleop_tp.max_acc = axis->acc_limit; + axis->teleop_vel_req = axis->teleop_tp.max_vel; + axis->teleop_acc_req = axis->teleop_tp.max_acc; axis->kb_ajog_active = 1; axis->teleop_tp.enable = 1; } @@ -431,6 +439,8 @@ void axis_handle_jogwheels(bool motion_teleop_flag, bool motion_enable_flag, boo axis->teleop_tp.pos_cmd = pos; axis->teleop_tp.max_vel = axis->vel_limit; axis->teleop_tp.max_acc = aaccel_limit; + axis->teleop_vel_req = axis->teleop_tp.max_vel; + axis->teleop_acc_req = axis->teleop_tp.max_acc; axis->wheel_ajog_active = 1; axis->teleop_tp.enable = 1; } @@ -674,6 +684,32 @@ static int update_teleop_with_check(int axis_num, simple_tp_t *the_tp, double se return 0; } +// Whether the axis's teleop planner has somewhere to go this cycle, and +// which way, how fast and how hard the jog asked it to: what the cap on +// the joints reads before the planners run +int axis_teleop_request(int axis_num, double *dir, double *vel, double *acc) +{ + emcmot_axis_t *axis = &axis_array[axis_num]; + double togo = axis->teleop_tp.pos_cmd - axis->teleop_tp.curr_pos; + + if (!axis->teleop_tp.enable) { return 0; } + if (fabs(togo) < TINY_DP(axis->teleop_tp.max_acc, 0.001) && axis->teleop_tp.curr_vel == 0.0) { return 0; } + *dir = togo > 0.0 ? 1.0 : togo < 0.0 ? -1.0 : axis->teleop_tp.curr_vel > 0.0 ? 1.0 : -1.0; + *vel = axis->teleop_vel_req; + *acc = axis->teleop_acc_req; + return 1; +} + +// The most the planner may do this cycle: what the jog asked, or less +// where the joints cannot follow that +void axis_teleop_cap(int axis_num, double vel, double acc) +{ + emcmot_axis_t *axis = &axis_array[axis_num]; + + axis->teleop_tp.max_vel = vel < axis->teleop_vel_req ? vel : axis->teleop_vel_req; + axis->teleop_tp.max_acc = acc < axis->teleop_acc_req ? acc : axis->teleop_acc_req; +} + int axis_calc_motion(double servo_period) { int axis_num; diff --git a/src/emc/motion/axis.h b/src/emc/motion/axis.h index 5d61f0b13f0..bc00504570d 100644 --- a/src/emc/motion/axis.h +++ b/src/emc/motion/axis.h @@ -51,6 +51,8 @@ void axis_apply_ext_offsets_to_carte_pos(int extfactor, double *pcmd_p[]); int axis_update_coord_with_bound(double *pcmd_p[], double servo_period); +int axis_teleop_request(int axis_num, double *dir, double *vel, double *acc); +void axis_teleop_cap(int axis_num, double vel, double acc); int axis_calc_motion(double servo_period); diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 35275c93144..8638ae9b71d 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -46,6 +46,10 @@ static int switchkins_type = 0; KINEMATICS_FORWARD_FLAGS fflags = 0; KINEMATICS_INVERSE_FLAGS iflags = 0; +// old modules export no kinematicsJacobian; a jog on them is bounded by +// the axis limits alone, as before +#pragma weak kinematicsJacobian + /*! \todo FIXME - debugging - uncomment the following line to log changes in JOINT_FLAG and MOTION_FLAG */ // #define WATCH_FLAGS 1 @@ -1341,6 +1345,118 @@ static void handle_jjogwheels(void) first_pass = 0; } +/* A world jog is planned per axis against the [AXIS_L] limits, which say + nothing about the joints once the kinematics is not the identity: a jog + of C under the tool centre point with the head laid over swings the + carriage through a circle. Each cycle, before the teleop planners run, + the module's Jacobian at the joints the machine stands in says how fast + every joint would move per unit of the jog the planners were asked for, + and how that rate has changed since the last cycle (what a carriage on a + circle feels as centripetal), and the planners are capped so that no + joint is asked for more than its own limits: the velocity by the joint + velocity limits, read a stopping distance ahead since the rate keeps + growing while the planners slow down, and by the acceleration budget + the changing rate takes at that velocity (at most half); the + acceleration by what the budget leaves. All active axes scale together + so the jog keeps its direction. Nothing is done on the identity, or on + a module without a Jacobian. */ +static void teleop_joint_cap(double period) +{ + static double jac_prev[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + static int have_prev = 0; + static double acc_scale_prev = 1.0; + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + double joint_pos[EMCMOT_MAX_JOINTS]; + double dir[EMCMOT_MAX_AXIS], vreq[EMCMOT_MAX_AXIS], areq[EMCMOT_MAX_AXIS]; + double rhat[EMCMOT_MAX_AXIS], ahat[EMCMOT_MAX_AXIS]; + int active[EMCMOT_MAX_AXIS]; + double rnorm = 0.0, anorm = 0.0, speed = 0.0, ahead, vcap = 1e99, acap = 1e99; + double vel_scale, acc_scale; + const double tiny = 1e-12; + int flags, any = 0, a, j; + + if (!kinematicsJacobian) { have_prev = 0; return; } + flags = emcmotStatus->switchkins_flags[emcmotStatus->switchkins_type]; + if (flags >= 0 ? (flags & KINSTYPE_IDENTITY) != 0 + : emcmotConfig->kinType == KINEMATICS_IDENTITY) { + have_prev = 0; + return; + } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + active[a] = axis_teleop_request(a, &dir[a], &vreq[a], &areq[a]); + if (active[a]) { + double v = axis_get_teleop_vel_cmd(a); + any = 1; + rnorm += vreq[a] * vreq[a]; + anorm += areq[a] * areq[a]; + speed += v * v; + } + } + if (!any) { have_prev = 0; return; } + rnorm = sqrt(rnorm); + anorm = sqrt(anorm); + speed = sqrt(speed); + if (rnorm < tiny || anorm < tiny) { have_prev = 0; return; } + for (j = 0; j < NO_OF_KINS_JOINTS; j++) { joint_pos[j] = joints[j].pos_cmd; } + if (kinematicsJacobian(joint_pos, &emcmotStatus->carte_pos_cmd, jac, &iflags) != 0) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + if (active[a]) { axis_teleop_cap(a, vreq[a], areq[a]); } + } + have_prev = 0; + return; + } + /* the direction the jog was asked in, and the one the planners + accelerate in, each at its own limit */ + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + rhat[a] = active[a] ? dir[a] * vreq[a] / rnorm : 0.0; + ahat[a] = active[a] ? dir[a] * areq[a] / anorm : 0.0; + } + /* the path the planners need to stop from the speed they are at, + at the acceleration they were left last cycle */ + ahead = speed * speed / (2.0 * anorm * acc_scale_prev); + for (j = 0; j < NO_OF_KINS_JOINTS; j++) { + double rate = 0.0, change = 0.0, cap; + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + rate += jac[j][a] * rhat[a]; + if (have_prev) { change += (jac[j][a] - jac_prev[j][a]) * rhat[a]; } + } + rate = fabs(rate); + /* how the rate changes per unit of path, from the last cycle */ + change = (have_prev && speed > tiny) ? fabs(change) / (speed * period) : 0.0; + if (rate > tiny) { + cap = joints[j].vel_limit / (rate + change * ahead); + if (cap < vcap) { vcap = cap; } + } + if (change > tiny) { + cap = sqrt(joints[j].acc_limit / (2.0 * change)); + if (cap < vcap) { vcap = cap; } + } + joint_pos[j] = change; /* kept for the acceleration pass */ + } + for (j = 0; j < NO_OF_KINS_JOINTS; j++) { + double arate = 0.0, cap; + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { arate += jac[j][a] * ahat[a]; } + arate = fabs(arate); + if (arate > tiny) { + cap = (joints[j].acc_limit - joint_pos[j] * vcap * vcap) / arate; + if (cap < acap) { acap = cap; } + } + } + vel_scale = vcap / rnorm; + acc_scale = acap / anorm; + if (vel_scale > 1.0) { vel_scale = 1.0; } + if (acc_scale > 1.0) { acc_scale = 1.0; } + if (acc_scale < 1e-6) { acc_scale = 1e-6; } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + if (active[a]) { axis_teleop_cap(a, vel_scale * vreq[a], acc_scale * areq[a]); } + } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac_prev[j][a] = jac[j][a]; } + } + have_prev = 1; + acc_scale_prev = acc_scale; +} // teleop_joint_cap() + static void get_pos_cmds(long period) { int joint_num, result; @@ -1647,6 +1763,7 @@ static void get_pos_cmds(long period) break; case EMCMOT_MOTION_TELEOP: + teleop_joint_cap(servo_period); ext_offset_teleop_limit = axis_calc_motion(servo_period); if (!ext_offset_teleop_limit) { ext_offset_coord_limit = 0; //in case was set in prior coord motion diff --git a/tests/kins-limits/jog/README b/tests/kins-limits/jog/README new file mode 100644 index 00000000000..2f2e00310c9 --- /dev/null +++ b/tests/kins-limits/jog/README @@ -0,0 +1,17 @@ +A world jog under the tool centre point must not run a slide past its +own limits. + +The swing test's machine: 5axiskins with a 400 mm pivot, the head laid +over to B90 by a joint move, the joints following their commands through +limit3 at 20 percent over each joint's own limits, the way a drive or a +step generator follows only as fast as it can. Then, in teleop mode, a +continuous jog of C at the axis's full rate, stopped by the test past +150 degrees, an incremental jog of C back by 90, and an incremental jog +of X with the head still laid over. The tip holds through the jogs of +C while the carriage swings on a circle of radius 400; nothing about the +[AXIS_C] limits says how fast that is. The test asserts that no jog +trips a following error, that the incremental jogs land where they were +sent, and that no joint's commanded velocity or acceleration went over +the joint's INI limit, sampled every servo cycle. The pace of the jog +is not asserted: it is whatever the slowest joint allows, and it changes +along the swing. diff --git a/tests/kins-limits/jog/checkresult b/tests/kins-limits/jog/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/kins-limits/jog/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/kins-limits/jog/sim.hal b/tests/kins-limits/jog/sim.hal new file mode 100644 index 00000000000..8b447ed8103 --- /dev/null +++ b/tests/kins-limits/jog/sim.hal @@ -0,0 +1,58 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +# each joint follows its command the way a drive or a step generator does, +# only as fast as it can: 20 percent over the joint's own limits, so a +# command inside the limits is followed exactly and one well over them falls +# behind and trips the following error +loadrt limit3 count=6 + +# the joint commands and their velocities every servo cycle, for the report +loadrt sampler depth=4000 cfg=ffffffffffff + +addf motion-command-handler servo-thread +addf motion-controller servo-thread +addf limit3.0 servo-thread +addf limit3.1 servo-thread +addf limit3.2 servo-thread +addf limit3.3 servo-thread +addf limit3.4 servo-thread +addf limit3.5 servo-thread +addf sampler.0 servo-thread + +setp limit3.0.maxv 240 +setp limit3.0.maxa 960 +setp limit3.1.maxv 240 +setp limit3.1.maxa 960 +setp limit3.2.maxv 240 +setp limit3.2.maxa 960 +setp limit3.3.maxv 72 +setp limit3.3.maxa 240 +setp limit3.4.maxv 72 +setp limit3.4.maxa 240 +setp limit3.5.maxv 240 +setp limit3.5.maxa 960 + +net J0cmd joint.0.motor-pos-cmd => limit3.0.in sampler.0.pin.0 +net J1cmd joint.1.motor-pos-cmd => limit3.1.in sampler.0.pin.1 +net J2cmd joint.2.motor-pos-cmd => limit3.2.in sampler.0.pin.2 +net J3cmd joint.3.motor-pos-cmd => limit3.3.in sampler.0.pin.3 +net J4cmd joint.4.motor-pos-cmd => limit3.4.in sampler.0.pin.4 +net J5cmd joint.5.motor-pos-cmd => limit3.5.in sampler.0.pin.5 +net J0fb limit3.0.out => joint.0.motor-pos-fb +net J1fb limit3.1.out => joint.1.motor-pos-fb +net J2fb limit3.2.out => joint.2.motor-pos-fb +net J3fb limit3.3.out => joint.3.motor-pos-fb +net J4fb limit3.4.out => joint.4.motor-pos-fb +net J5fb limit3.5.out => joint.5.motor-pos-fb +net J0vel joint.0.vel-cmd => sampler.0.pin.6 +net J1vel joint.1.vel-cmd => sampler.0.pin.7 +net J2vel joint.2.vel-cmd => sampler.0.pin.8 +net J3vel joint.3.vel-cmd => sampler.0.pin.9 +net J4vel joint.4.vel-cmd => sampler.0.pin.10 +net J5vel joint.5.vel-cmd => sampler.0.pin.11 +loadusr halsampler -t samples.log + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/kins-limits/jog/test-ui.py b/tests/kins-limits/jog/test-ui.py new file mode 100755 index 00000000000..55aae72ee14 --- /dev/null +++ b/tests/kins-limits/jog/test-ui.py @@ -0,0 +1,189 @@ +#!/usr/bin/env python3 +# A world jog under the tool centre point must not run a slide past its +# own limits: see README. +import hal +import linuxcnc +import os +import sys +import time + +JOINTS = 6 +LOG = "samples.log" +SERVO = 0.001 +C = 5 + +ini = linuxcnc.ini("test.ini") +VEL = [float(ini.find("JOINT_%d" % j, "MAX_VELOCITY")) for j in range(JOINTS)] +ACC = [float(ini.find("JOINT_%d" % j, "MAX_ACCELERATION")) for j in range(JOINTS)] + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def joints(): + s.poll() + return [s.joint_position[i] for i in range(JOINTS)] + +def drain(): + said = [] + while True: + m = e.poll() + if not m: + return said + said.append(m) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + now = joints() + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.05) + error("timed out waiting for the move") + return last + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(60) + return settled() + +def log_samples(): + with open(LOG) as f: + lines = f.read().split("\n") + out = [] + for line in lines[:-1]: + v = line.split() + if len(v) != 1 + 2 * JOINTS: + continue + v = [float(x) for x in v[1:]] + out.append((v[:JOINTS], v[JOINTS:])) + return out + +def catch_up(where): + # the log is block buffered: wait until it has caught up with the + # machine at rest before marking where the next move starts in it + deadline = time.time() + 10 + while True: + samples = log_samples() + if samples and max(abs(a - b) for a, b in zip(samples[-1][0], where)) < 2e-6: + return len(samples) + if time.time() > deadline: + error("the sampler log did not catch up with the machine") + return len(samples) + time.sleep(0.02) + +def checked(what, n0): + end = settled() + said = drain() + time.sleep(0.5) + samples = log_samples()[n0:] + for m in said: + print("channel:", m) + faults = [m for m in said if "following error" in m[1]] + if faults: + error("%s tripped a following error: %s" % (what, faults[0][1].strip())) + s.poll() + if s.task_state != linuxcnc.STATE_ON: + error("the machine is not on after %s (task state %d)" % (what, s.task_state)) + # the commanded velocity and acceleration of every joint against its + # own INI limit, every servo cycle; a joint over its limit is what the + # drive could not follow. A fault freezes the command in one cycle, + # which is not an acceleration the planner asked for: the samples stop + # at the last moving one + last = max((k for k in range(len(samples)) if any(abs(v) > 0 for v in samples[k][1])), default=-1) + samples = samples[:last + 1] + print("%d samples through %s" % (len(samples), what)) + for j in range(JOINTS): + vpeak = max(abs(v[j]) for p, v in samples) if samples else 0.0 + apeak = 0.0 + for k in range(1, len(samples)): + apeak = max(apeak, abs(samples[k][1][j] - samples[k - 1][1][j]) / SERVO) + print("joint %d: velocity peak %8.3f of %8.3f (%.2fx), acceleration peak %9.2f of %9.2f (%.2fx)" + % (j, vpeak, VEL[j], vpeak / VEL[j], apeak, ACC[j], apeak / ACC[j])) + if vpeak > VEL[j] * 1.001: + error("joint %d was commanded at %.3f, over its limit of %.3f" % (j, vpeak, VEL[j])) + if apeak > ACC[j] * 1.01: + error("joint %d was commanded at %.2f, over its acceleration limit of %.2f" % (j, apeak, ACC[j])) + return end + +def tip_held(what): + s.poll() + after = list(s.position[:3]) + if max(abs(a - b) for a, b in zip(after, tip)) > 1e-3: + error("the tip moved from %s to %s through %s" % (tip, after, what)) + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() + +# the tool centre point kinematics, the head laid over by a joint move so +# that getting there cannot swing anything; the carriage at X-400 puts the +# tip at X0, the centre of the circle a jog of C is about to swing it on +mdi("G12.1 P0") +mdi("G53.7 G0 J0=-400 J1=0 J2=0 J3=90 J4=0 J5=0") +start = joints() +s.poll() +tip = list(s.position[:3]) +print("start joints %s, tip %s" % (" ".join("%.3f" % v for v in start), + " ".join("%.3f" % v for v in tip))) +drain() + +# world jogs: teleop mode +c.mode(linuxcnc.MODE_MANUAL) +c.wait_complete(30) +c.teleop_enable(1) +c.wait_complete(30) + +# a continuous jog of C at the axis's full rate, the tip held: stopped by +# the test once C is past 150, so the planner ramps up, cruises, ramps down +n0 = catch_up(start) +c.jog(linuxcnc.JOG_CONTINUOUS, 0, C, 60) +deadline = time.time() + 60 +while time.time() < deadline: + s.poll() + if s.position[C] > 150: + break + time.sleep(0.01) +c.jog(linuxcnc.JOG_STOP, 0, C) +end = checked("the continuous jog of C", n0) +if end[4] < 150: + error("C stopped at %.3f, before 150" % end[4]) +tip_held("the continuous jog of C") + +# an incremental jog back by 90 at the same rate: the planner knows its +# endpoint, the cap still holds +n0 = catch_up(end) +back_from = end[4] +c.jog(linuxcnc.JOG_INCREMENT, 0, C, -60, 90) +end = checked("the incremental jog of C", n0) +if abs(end[4] - (back_from - 90)) > 1e-3: + error("C ended at %.3f, not %.3f" % (end[4], back_from - 90)) +tip_held("the incremental jog of C") + +# a jog of X with the head still laid over: the Jacobian does not change +# along it, the carriage simply runs at the jog rate +n0 = catch_up(end) +c.jog(linuxcnc.JOG_INCREMENT, 0, 0, 200, 100) +end = checked("the incremental jog of X", n0) + +overruns = int(hal.get_value("sampler.0.overruns")) +if overruns: + error("the sampler lost %d samples" % overruns) +if not errors: + os.unlink(LOG) +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/kins-limits/jog/test.ini b/tests/kins-limits/jog/test.ini new file mode 100644 index 00000000000..8eac90e4fdf --- /dev/null +++ b/tests/kins-limits/jog/test.ini @@ -0,0 +1,144 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# switchkins-type 0 is 5axiskins, the tool centre point; 1 is identity +# the pivot is long so that a turn of the head swings the slides fast +KINEMATICS = 5axiskins coordinates=xyzbcw +JOINTS = 6 + +[HAL] +HALFILE = sim.hal +HALCMD = setp 5axiskins.pivot-length 400 + +[TRAJ] +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 200 +MAX_LINEAR_VELOCITY = 346 +MAX_LINEAR_ACCELERATION = 800 +DEFAULT_LINEAR_ACCELERATION = 800 +MAX_ANGULAR_VELOCITY = 360 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Y] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_B] +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[JOINT_0] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = LINEAR +FERROR = 1.0 +MIN_FERROR = 0.5 +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/kins-limits/jog/test.sh b/tests/kins-limits/jog/test.sh new file mode 100755 index 00000000000..d27cc469eb5 --- /dev/null +++ b/tests/kins-limits/jog/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries offsets +rm -f sim.var sim.var.bak samples.log +linuxcnc -r test.ini diff --git a/tests/kins-limits/jog/tool.tbl b/tests/kins-limits/jog/tool.tbl new file mode 100644 index 00000000000..d793e2d60ed --- /dev/null +++ b/tests/kins-limits/jog/tool.tbl @@ -0,0 +1 @@ +T1 P1 D0.0 Z12.5 ; From 99e42384a990e4dfed1575cb46424fbe8dd6c06f Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 19 Sep 2026 22:31:30 +1000 Subject: [PATCH 68/77] docs, qtvcp, hal_glib: list every code the kinematics work added The quick reference, the qtvcp MDI help and the modal group tables of hal_glib knew none of G12.1, G13.1, G28.5, G30.5, G43.4, G43.5, G53.1 to G53.7, G68.2 to G68.4 and G69. One sweep for all of them, the same three places G28.2 was added to. docs/src/gcode.html.in gets a row per code, linked to its section; the point-to-point and orientation codes sit under Motion, the two tool length forms under Tool Length Offset, the plane, its cancel and the kinematics selection under Other Modal Codes, the machine frame forms of G28 and G30 next to them. lib/python/qtvcp/lib/mdi_text.py gets a title, a word list and a description for each, condensed from the G-code chapter, so the qtvcp G-code help lists and explains them. lib/python/common/hal_glib.py gets the codes in their modal groups and a gcode-group9-changed signal for the tilted work plane, which stat.gcodes reports in its own slot; group 8 also gains G43.2, which was missing. The allocated G-code tables of the remap chapter get the same codes, and row 33, which listed G30 and G30.1 in G33's place, now lists G33 and G33.1. The G28.5 heading and its cross-reference from G30 said slide position; it is the machine frame position, as the section itself says. --- docs/src/gcode.html.in | 12 ++ docs/src/gcode/g-code.adoc | 6 +- docs/src/remap/remap.adoc | 18 +-- lib/python/common/hal_glib.py | 15 +- lib/python/qtvcp/lib/mdi_text.py | 265 +++++++++++++++++++++++++++++++ 5 files changed, 299 insertions(+), 17 deletions(-) diff --git a/docs/src/gcode.html.in b/docs/src/gcode.html.in index 02353bbcf36..b573eafa3be 100644 --- a/docs/src/gcode.html.in +++ b/docs/src/gcode.html.in @@ -65,6 +65,11 @@ tr.head td, tr.head th { background: black; color: white; } G33K ($)Spindle Synchronized Motion G33.1K ($)Rigid Tapping G80 Cancel Canned Cycle + G53.4Point-to-Point Move, Program Coordinates + G53.5Point-to-Point Move, Machine Frame + G53.7J<n>=Point-to-Point Move, Joint Values + G53.1, G53.3, G53.6(P) (Q)Orient the Tool to the Work Plane + G53.2(P) (Q)Solve the Tool Orientation, No Motion Canned cycles(X Y Z or U V W apply to canned cycles, depending on active plane) G70Q (X) (Z) (D) (E) (P)Lathe finishing cycle @@ -100,6 +105,8 @@ tr.head td, tr.head th { background: black; color: white; } G43 H Tool Length Offset G43.1 Dynamic Tool Length Offset G43.2 H Apply additional Tool Length Offset + G43.4 (H) Tool Length Offset on Primary Kinematics + G43.5 (H) Tool Length Offset, Tool Axis as a Vector G49 Cancel Tool Length Compensation Stopping @@ -136,6 +143,10 @@ tr.head td, tr.head th { background: black; color: white; } M52P0 (off) or P1 (on)Adaptive Feed Control M53P0 (off) or P1 (on)Feed Stop Control G54-G59.3Select Coordinate System + G68.2, G68.4P Q I J K RTilted Work Plane + G68.3(R)Tilted Work Plane from the Tool + G69Cancel Tilted Work Plane + G12.1, G13.1PSelect Kinematics Flow-control Codes o subSubroutines, sub/endsub call @@ -167,6 +178,7 @@ tr.head td, tr.head th { background: black; color: white; } G28, G28.1Go/Set Predefined Position G28.2PHome from G-code G30, G30.1Go/Set Predefined Position + G28.5, G30.5Go to Predefined Machine Frame Position G53Move in Machine Coordinates G52, G92Coordinate System Offset G92.1, G92.2Reset G92 Offsets diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index bcb61aa24d2..37c4974a133 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -74,7 +74,7 @@ as the 'L number', and so on for any other letter. |<> |Plane Select |<> |Set Units of Measure |<> |Go to Predefined Position -|<> |Go to Predefined Slide Position +|<> |Go to Predefined Machine Frame Position |<> |Home from G-code |<> |Go to Predefined Position |<> |Spindle Synchronized Motion @@ -1199,7 +1199,7 @@ G30 Z2.5 (rapid to Z2.5 then to the Z location specified in #5183) The 'absolute' position is the machine coordinate system of the kinematics in force, see <>; <> is the form that moves -the slides. +in the machine frame. It is an error if : @@ -1209,7 +1209,7 @@ It is an error if : 'G30' and for 'G30.1'. [[gcode:g28.5]] -== G28.5, G30.5 Go to Predefined Slide Position(((G28.5 Go to Predefined Slide Position))) +== G28.5, G30.5 Go to Predefined Machine Frame Position(((G28.5 Go to Predefined Machine Frame Position))) [source,ngc] ---- diff --git a/docs/src/remap/remap.adoc b/docs/src/remap/remap.adoc index 75e2a0980fa..02c225dda19 100644 --- a/docs/src/remap/remap.adoc +++ b/docs/src/remap/remap.adoc @@ -1829,8 +1829,8 @@ All the listed G-codes are already defined in the current implementation of Linu |# |Gxx |Gxx.1 |Gxx.2 |Gxx.3 |Gxx.4 |Gxx.5 |Gxx.6 |Gxx.7 |Gxx.8 |Gxx.9 |10 |G10 | | | | | | | | | |11 | | | | | | | | | | -|12 | | | | | | | | | | -|13 | | | | | | | | | | +|12 | |G12.1 | | | | | | | | +|13 | |G13.1 | | | | | | | | |14 | | | | | | | | | | |15 | | | | | | | | | | |16 | | | | | | | | | | @@ -1851,7 +1851,7 @@ All the listed G-codes are already defined in the current implementation of Linu |25 | | | | | | | | | | |26 | | | | | | | | | | |27 | | | | | | | | | | -|28 |G28 |G28.1 |G28.2 | | | | | | | +|28 |G28 |G28.1 |G28.2 | | |G28.5 | | | | |29 | | | | | | | | | | |=== @@ -1859,10 +1859,10 @@ All the listed G-codes are already defined in the current implementation of Linu [width="90%",align="center",options="header,strong,unbreakable",cols="1*2^em,10*1. +""" + +G13_1 = """G13.1 Select Machine Frame Kinematics +Cancels back to the kinematics the module declares +the machine frame: the identity on a machine whose +slides line up with its frame, the arm kinematics +with no tool on a robot. Which number that is, the +module declares, so G13.1 means the same whatever +order the module lists its types in. A queue +synchronisation point like G12.1. +""" + G17 = """G17 Plane Select G17 = XY Plane """ @@ -492,6 +564,28 @@ def gcode_descriptions(gcode): or if Pn names a joint the machine does not have. """ +G28_5 = """G28.5 Go to Predefined Machine Frame Position +G28.5 + +G28.5 is to G28 what G53.5 is to G53: the position +stored in #5161-#5169 is read in the machine frame +with the orientation left out, the carriage or the +flange in machine coordinates, and the move is a +point-to-point move there, rapid, with no offset, +tool length, rotation or work plane applied. With +axes, those letters go to the given machine frame +position first, then to their stored positions. +Store the position with G28.1 while the machine +frame kinematics is in force (G13.1). +""" + +G30_5 = """G30.5 Go to Predefined Machine Frame Position +G30.5 + +As G28.5, with the position stored in #5181-#5189 +by G30.1. +""" + G30 = """G30 Go to Predefined Position G30 uses the values stored in parameters 5181-5189 as the X Y Z A B C U V W final point to move to. @@ -698,6 +792,37 @@ def gcode_descriptions(gcode): location. """ +G43_4 = """G43.4 Tool Length Offset on Primary Kinematics +G43.4 Hn +n = tool number, Hn is optional + +G43 together with a switch to the kinematics the +module declares its working transform, tool centre +point control, so the program runs with tool length +compensation in that kinematics. The switch happens +first, the offset applies after it. G49 cancels the +offset and switches back to the machine frame +kinematics. Rejected on a switchable module that +declares no primary kinematics; a plain G43 on a +machine that does not switch. +""" + +G43_5 = """G43.5 Tool Length Offset with the Tool Axis as a Vector +G43.5 Hn +G0 X- Y- Z- I- J- K- +G1 X- Y- Z- I- J- K- F- + +G43.4 with one thing more: while it is in effect a +G0 or G1 line may give the tool axis direction as a +vector I J K, from the tip towards the holder, in +place of rotary words, and the interpreter works +out where the rotaries have to go. The pose nearest +where the rotaries stand is taken. G2 and G3 keep +I J K as the arc centre. It is an error to give +I J K with a rotary word, or a zero vector, or on +the identity kinematics. +""" + G49 = """G49 Cancel Tool Length Compensation """ @@ -719,6 +844,101 @@ def gcode_descriptions(gcode): line if one is currently active. """ +G53_1 = """G53.1 Orient the Tool to the Work Plane +G53.1 + +Turns the rotaries until the tool axis is normal to +the active tilted work plane (G68.2). The linear +joints stay where they are and the tip swings with +them: a point-to-point move. P picks among the +poses that reach the plane, nearest first: P0 (the +default) the nearest, P1 and P2 the secondary +rotary positive or negative. Q0 (the default) holds +the joints that carry the work, Q1 frees them. An +error with no plane active, or on the identity +kinematics. +""" + +G53_2 = """G53.2 Solve the Tool Orientation without Moving +G53.2 + +Solves the same pose as G53.1 and moves nothing. +The pose is published on #<_orient_x> to +#<_orient_c> and on #5071 to #5079, in program +units in the plane, with #<_orient_valid> (#5080) +set to 1, so the program can reach it with a move +of its own, for instance a single G0 naming X Y Z +and the rotary words together. P and Q as for +G53.1. +""" + +G53_3 = """G53.3 Orient the Tool and Move in the Work Plane +G53.3 X- Y- Z- + +Turns the rotaries as G53.1 does and takes the tool +to X Y Z, given in the plane, in one point-to-point +move. A word left out keeps the present value. P +and Q as for G53.1. +""" + +G53_4 = """G53.4 Point-to-Point Move in Program Coordinates +G53.4 G0 +G53.4 G1 F- + +A point-to-point move defines its two ends and +leaves the path to the joints: the inverse +kinematics runs once at the destination and every +joint travels from where it is to where it must be, +all together, the slowest setting the pace. The +tool does not follow a straight line. G53.4 takes +the destination in program coordinates, through +the offsets and the work plane like any other +move: the move to cross a singularity or turn a +head right round without leaving the coordinate +system. Non-modal, on a line with G0 or G1 in +force; with G1 it takes the time the straight move +would at F. +""" + +G53_5 = """G53.5 Point-to-Point Move in the Machine Frame +G53.5 G0 +G53.5 G1 F- + +A point-to-point move (see G53.4) to a position in +the machine frame with the tool left out: X Y Z the +point the tool hangs from, the pivot of a head or +the flange of a robot, in machine coordinates, the +rotary letters the orientation as the machine frame +kinematics reports it, on a mill the rotary joints. +Program units, no offset, tool length, rotation or +work plane. A letter not given holds its machine +frame coordinate. The form for parking and tool +change positions, whatever the head is doing. +Absolute distance mode only. +""" + +G53_6 = """G53.6 Orient the Tool, Holding the Tool Centre Point +G53.6 + +Turns the rotaries until the tool is normal to the +active work plane, as G53.1, keeping the tool +centre point where it is: a Cartesian move of the +rotary words, the kinematics compensating the +linear joints all along. P and Q as for G53.1. +""" + +G53_7 = """G53.7 Point-to-Point Move to Joint Values +G53.7 G0 J= ... +G53.7 G1 F- J= ... + +A point-to-point move (see G53.4) to joint values, +one word per joint: J2=-5 sends joint 2 to -5 in +the joint's own units, nothing converted, no offset +of any kind. A joint not named holds. The form that +works on every machine, a robot included. Absolute +distance mode only; the J= word is read only here. +""" + G54 = """G54 Select Coordinate System G54 = select coordinate system 1 """ @@ -788,6 +1008,51 @@ def gcode_descriptions(gcode): linear move. """ +G68_2 = """G68.2 Tilted Work Plane +G68.2 X- Y- Z- I- J- K- (Euler angles) +G68.2 P1 X- Y- Z- I- J- K- (angles about fixed axes) +G68.2 P2 Q0..Q3 X- Y- Z- (three points, one per block) +G68.2 P3 Q1 X- Y- Z- I- J- K- (two vectors: origin and +X,) +G68.2 P3 Q2 I- J- K- (then +Z, the normal) + +Defines a plane on top of the active coordinate +system, with its own cancel (G69), so the blocks +that follow are programmed in the tilted plane +while G54 itself is untouched. X Y Z is the plane's +origin, the rest of the words its rotation, both in +the coordinate system active when the plane is +defined. P selects the form, numbered as Fanuc +numbers them; Q names the axis order of the angle +forms (Q313 and Q123 the defaults) or the block of +the point and vector forms. R turns the plane about +its own Z after everything else. The plane does not +move the tool: G53.1, G53.3 or G53.6 orient it. +""" + +G68_3 = """G68.3 Tilted Work Plane from the Tool Direction +G68.3 X- Y- Z- + +Defines a plane whose Z is the tool axis as the +rotary joints stand now, with the origin at X Y Z +and R turning it about that Z. An error where the +kinematics cannot be evaluated by the interpreter, +is the identity, or reports no tool frame. +""" + +G68_4 = """G68.4 Tilted Work Plane on the Active Plane +G68.4 (any G68.2 form) + +Takes any G68.2 form and composes it onto the plane +already active, so the new plane is given in the +coordinates of the old one. An error with no plane +active. +""" + +G69 = """G69 Cancel Tilted Work Plane +Cancels the plane. So does the end of the program, +M2 or M30, and an abort. +""" + G73 = """G73 Drilling Cycle with Chip Breaking G73 X Y Z R Q L R = retract position along the Z axis From 0608a55580e2106386128ee0c15363f8bd0df28e Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 21 Sep 2026 21:39:07 +1000 Subject: [PATCH 69/77] kinematics_user: a Jacobian entry at joints the caller holds kinematicsUserJacobian() runs the module's inverse at the pose before differentiating, to be on the caller's solution branch. The segment sampler in canon has just run that inverse itself at every sample, so each sample paid for it twice. kinematicsUserJacobianAt() takes the joints the caller holds and differentiates there; the old entry inverts and then calls it. segmentCap() uses the new one, and its answers are unchanged. --- .../kinematics_userspace/kinematics_user.c | 20 +++++++++++++++++-- .../kinematics_userspace/kinematics_user.h | 13 ++++++++++++ src/emc/motion_planning/segment_cap.cc | 2 +- 3 files changed, 32 insertions(+), 3 deletions(-) diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index 976087839c1..2fe6cb4bbb2 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -441,9 +441,7 @@ int kinematicsUserJacobian(KinematicsUserContext* ctx, { KINEMATICS_INVERSE_FLAGS iflags = 0; KINEMATICS_FORWARD_FLAGS fflags = 0; - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; double j[EMCMOT_MAX_JOINTS]; - int r, a; if (!ctx || !ctx->initialized || !world || !J) return -1; if (ctx->rt_only) return -1; @@ -455,6 +453,24 @@ int kinematicsUserJacobian(KinematicsUserContext* ctx, world, j, &iflags, &fflags) != 0) { return -1; } + return kinematicsUserJacobianAt(ctx, j, world, J); +} + +int kinematicsUserJacobianAt(KinematicsUserContext* ctx, + const double* joints, + const EmcPose* world, + double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]) +{ + KINEMATICS_INVERSE_FLAGS iflags = 0; + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + double j[EMCMOT_MAX_JOINTS] = {0}; + int r, a; + + if (!ctx || !ctx->initialized || !joints || !world || !J) return -1; + if (ctx->rt_only) return -1; + + refresh(ctx); + for (r = 0; r < KINEMATICS_USER_MAX_JOINTS; r++) j[r] = joints[r]; if (kinsOpsJacobian(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, j, world, jac, &iflags) != 0) { return -1; diff --git a/src/emc/kinematics_userspace/kinematics_user.h b/src/emc/kinematics_userspace/kinematics_user.h index 231d0b20929..5b27f145ee8 100644 --- a/src/emc/kinematics_userspace/kinematics_user.h +++ b/src/emc/kinematics_userspace/kinematics_user.h @@ -142,6 +142,19 @@ int kinematicsUserJacobian(KinematicsUserContext* ctx, const EmcPose* world, double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]); +/** + * The same at joints the caller already holds for the pose, from its own + * kinematicsUserInverse(): the derivative is taken there and no inverse is + * run first. What a caller sampling a path wants, having just inverted + * each sample. + * + * @return 0 on success, -1 on failure + */ +int kinematicsUserJacobianAt(KinematicsUserContext* ctx, + const double* joints, + const EmcPose* world, + double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]); + /** * The parameter block as it stands, refreshed from HAL first. For * reporting; the block belongs to the context. diff --git a/src/emc/motion_planning/segment_cap.cc b/src/emc/motion_planning/segment_cap.cc index 5c14539226c..ec4cf00faf0 100644 --- a/src/emc/motion_planning/segment_cap.cc +++ b/src/emc/motion_planning/segment_cap.cc @@ -83,7 +83,7 @@ int segmentCap(KinematicsUserContext *ctx, const SegmentCapLimits *lim, whole segment stays on one solution branch, then the Jacobian on that branch */ if (kinematicsUserInverse(ctx, &p, joints) != 0 - || kinematicsUserJacobian(ctx, &p, J) != 0) { + || kinematicsUserJacobianAt(ctx, joints, &p, J) != 0) { out->unanswered++; continue; } From 7cdb591a535f30dd8bfff591a3e3fdce680ea084 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 23 Sep 2026 21:32:53 +1000 Subject: [PATCH 70/77] The stack's own conversions follow [AXIS_] TYPE The code this stack adds on top of the interpreter and canon still took A B C for angles and X Y Z U V W for lengths: the machine pose and back (interp_workplane.cc), the tool offset handed to the kinematics, the point G43.4 and G49 re-anchor on, the words of G53.5, G53.7, G28.5 and G30.5, and the check that a letter on the identity kinematics names a joint of its own kind. They go by type now, as the rest does since "Honor [AXIS_] TYPE in the interpreter and canon". Canon's segment sampling counts the turn of every angular axis, not of A B C. --- src/emc/rs274ngc/interp_convert.cc | 12 +++---- src/emc/rs274ngc/interp_workplane.cc | 48 +++++++++++++--------------- src/emc/task/emccanon.cc | 7 +++- 3 files changed, 34 insertions(+), 33 deletions(-) diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 3a683e6c4b2..8346e7b98cd 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -6799,12 +6799,12 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu 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); + in_program.a = USER_TO_PROGRAM_AX(3, point.a); + in_program.b = USER_TO_PROGRAM_AX(4, point.b); + in_program.c = USER_TO_PROGRAM_AX(5, point.c); + in_program.u = USER_TO_PROGRAM_AX(6, point.u); + in_program.v = USER_TO_PROGRAM_AX(7, point.v); + in_program.w = USER_TO_PROGRAM_AX(8, point.w); USE_TOOL_LENGTH_OFFSET(tool_offset, in_program); } else { double dx, dy, dz; diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index ea10c04d68f..ea288083a4e 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -511,12 +511,12 @@ void Interp::kins_set_tool(void *vctx, const EmcPose *offset) 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); + tool.a = PROGRAM_TO_USER_AX(3, offset->a); + tool.b = PROGRAM_TO_USER_AX(4, offset->b); + tool.c = PROGRAM_TO_USER_AX(5, offset->c); + tool.u = PROGRAM_TO_USER_AX(6, offset->u); + tool.v = PROGRAM_TO_USER_AX(7, offset->v); + tool.w = PROGRAM_TO_USER_AX(8, offset->w); kinematicsUserSetTool((KinematicsUserContext *)vctx, &tool); } @@ -542,12 +542,12 @@ void Interp::current_machine_pose(setup_pointer s, EmcPose *pose) 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]); + pose->a = PROGRAM_TO_USER_AX(3, abs_pos[3]); + pose->b = PROGRAM_TO_USER_AX(4, abs_pos[4]); + pose->c = PROGRAM_TO_USER_AX(5, abs_pos[5]); + pose->u = PROGRAM_TO_USER_AX(6, abs_pos[6]); + pose->v = PROGRAM_TO_USER_AX(7, abs_pos[7]); + pose->w = PROGRAM_TO_USER_AX(8, abs_pos[8]); } // and back: a machine pose as program coordinates, through the chain @@ -558,12 +558,12 @@ void Interp::machine_pose_to_program(setup_pointer s, const EmcPose *pose, doubl 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; + prog[3] = USER_TO_PROGRAM_AX(3, pose->a) - s->tool_offset.a - s->AA_origin_offset - s->AA_axis_offset; + prog[4] = USER_TO_PROGRAM_AX(4, pose->b) - s->tool_offset.b - s->BB_origin_offset - s->BB_axis_offset; + prog[5] = USER_TO_PROGRAM_AX(5, pose->c) - s->tool_offset.c - s->CC_origin_offset - s->CC_axis_offset; + prog[6] = USER_TO_PROGRAM_AX(6, pose->u) - s->tool_offset.u - s->u_origin_offset - s->u_axis_offset; + prog[7] = USER_TO_PROGRAM_AX(7, pose->v) - s->tool_offset.v - s->v_origin_offset - s->v_axis_offset; + prog[8] = USER_TO_PROGRAM_AX(8, 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 @@ -1103,7 +1103,7 @@ int Interp::slide_words(const char *name, setup_pointer s, void *vctx, const str if (undeclared || kinematicsUserIsIdentity(ctx)) { CHKS((!p), _("%s: the kinematics module gives no joint mapping"), name); for (a = 0; a < 9; a++) { - const int angular = (a >= 3 && a <= 5); + const int angular = axisKindsAngular(s->axis_kinds, a); for (j = 0; j < njoints; j++) { int turns; if (!(p->joints_of_axis[a] & (1 << j))) { continue; } @@ -1120,12 +1120,11 @@ int Interp::slide_words(const char *name, setup_pointer s, void *vctx, const str return machine_frame_joints(name, s, ctx, flags, words, joints); } for (a = 0; a < 9; a++) { - const int angular = (a >= 3 && a <= 5); double value; if (!flags[a]) { continue; } CHKS((p->joints_of_axis[a] == 0), _("%s: %c is not a joint of this kinematics"), name, letters[a]); - value = angular ? PROGRAM_TO_USER_ANG(words[a]) : PROGRAM_TO_USER_LEN(words[a]); + value = PROGRAM_TO_USER_AX(a, words[a]); for (j = 0; j < njoints; j++) { if (p->joints_of_axis[a] & (1 << j)) { joints[j] = value; } } @@ -1153,9 +1152,8 @@ int Interp::machine_frame_joints(const char *name, setup_pointer s, void *vctx, coord[3] = &pose.a; coord[4] = &pose.b; coord[5] = &pose.c; coord[6] = &pose.u; coord[7] = &pose.v; coord[8] = &pose.w; for (a = 0; a < 9; a++) { - const int angular = (a >= 3 && a <= 5); if (!flags[a]) { continue; } - *coord[a] = angular ? PROGRAM_TO_USER_ANG(words[a]) : PROGRAM_TO_USER_LEN(words[a]); + *coord[a] = PROGRAM_TO_USER_AX(a, words[a]); } for (pass = 0; pass < 8; pass++) { double prev[EMCMOT_MAX_JOINTS], worst = 0.0; @@ -1300,9 +1298,7 @@ int Interp::convert_home_slides(int code, block_pointer block, setup_pointer s) CHP(current_joints(s, ctx, joints)); for (a = 0; a < 9; a++) { - const int angular = (a >= 3 && a <= 5); - home[a] = angular ? USER_TO_PROGRAM_ANG(parameters[base + a]) - : USER_TO_PROGRAM_LEN(parameters[base + a]); + home[a] = USER_TO_PROGRAM_AX(a, parameters[base + a]); given += flags[a]; } diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 1d30d17c6f0..5b2f807ebbc 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -1008,7 +1008,12 @@ static void kins_seed(KinematicsUserContext *ctx, const EmcPose *start) // position, or a sweep of arc is seen every few degrees or centimetres static int kins_samples(const EmcPose *start, const EmcPose *end, double sweep_deg) { - double rot = fmax(fabs(end->a - start->a), fmax(fabs(end->b - start->b), fabs(end->c - start->c))); + const double turn[6] = {end->a - start->a, end->b - start->b, end->c - start->c, + end->u - start->u, end->v - start->v, end->w - start->w}; + double rot = 0.0; + for (int n = 0; n < 6; n++) { + if (AXIS_ANG(n + 3)) { rot = fmax(rot, fabs(turn[n])); } + } double lin = sqrt(pow(end->tran.x - start->tran.x, 2) + pow(end->tran.y - start->tran.y, 2) + pow(end->tran.z - start->tran.z, 2)); int n = 3 + (int)ceil(rot / 10.0) + (int)ceil(lin / 50.0) + (int)ceil(sweep_deg / 10.0); From 658faf975ac680beb4cca637eecf12edc3e2d570 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 23 Sep 2026 21:42:39 +1000 Subject: [PATCH 71/77] Kinematics types name the axes that orient the tool G43.5 wrote the pose it solved into A B C and refused A B C words beside I J K, whatever axes the machine orients with. A kinematics type now names them: kins_ops.orient gives the two letters, first rotation then second, and left NULL they are the letters the joint map gives the two joints that turn the frames, which is right for every module in the tree. kinematicsUserOrientAxes() answers for the active type, with head or table for each, from the frames: two heads or two tables in the order toolFrameOrientJoints() gives, a table and a head the table first. G43.5 writes the solution into those two letters and refuses a word of either beside I J K, and G53.3 and G53.6 turn those two letters where they turned A B C and left U V W behind; a type that orients otherwise, a robot's three pose angles, keeps A B C. The interpreter exposes them as #<_kins_orient_1>, #<_kins_orient_2> (axis numbers, 0 X to 8 W, -1 for none) and #<_kins_orient_1_head>, #<_kins_orient_2_head>, and refuses a module whose type orients with an axis [AXIS_] TYPE makes LINEAR when it first loads it. --- docs/src/gcode/g-code.adoc | 9 +- docs/src/gcode/overview.adoc | 13 ++ docs/src/motion/kinematics-conventions.adoc | 8 +- src/emc/kinematics/kins_module.h | 7 +- .../kinematics_userspace/kinematics_user.c | 67 +++++++++ .../kinematics_userspace/kinematics_user.h | 13 ++ src/emc/rs274ngc/interp_convert.cc | 3 +- src/emc/rs274ngc/interp_namedparams.cc | 28 ++++ src/emc/rs274ngc/interp_workplane.cc | 113 ++++++++++++--- src/emc/rs274ngc/rs274ngc_interp.hh | 4 +- tests/kins-orient-linear/README | 4 + tests/kins-orient-linear/checkresult | 3 + tests/kins-orient-linear/sim.hal | 16 +++ tests/kins-orient-linear/test-ui.py | 39 ++++++ tests/kins-orient-linear/test.ini | 131 ++++++++++++++++++ tests/kins-orient-linear/test.sh | 3 + tests/kins-orient-linear/tool.tbl | 1 + tests/kins-switch/test-ui.py | 46 ++++++ tests/remap/introspect/expected | 4 +- 19 files changed, 486 insertions(+), 26 deletions(-) create mode 100644 tests/kins-orient-linear/README create mode 100755 tests/kins-orient-linear/checkresult create mode 100644 tests/kins-orient-linear/sim.hal create mode 100755 tests/kins-orient-linear/test-ui.py create mode 100644 tests/kins-orient-linear/test.ini create mode 100755 tests/kins-orient-linear/test.sh create mode 100644 tests/kins-orient-linear/tool.tbl diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 37c4974a133..00f64345876 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -1884,8 +1884,10 @@ a rotary swings the long way round only where the short way runs out of travel. 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 +The pose the interpreter finds goes into the move as ordinary words of +the two axes the kinematics type orients the tool with, the ones +'#<_kins_orient_1>' and '#<_kins_orient_2>' name (A B C for a type that +orients with three angles, a robot's wrist), in program coordinates, so the rotary offsets of the coordinate system apply, and the move interpolates like any other move with rotary words: the rotaries turn together with the linear axes between the poses of consecutive lines. The vector itself is not interpolated @@ -1899,7 +1901,8 @@ 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, +* 'I J K' are given together with a word of an orienting axis on the same line, +* the kinematics type orients the tool with an axis that is not rotary, * the vector is zero, * the kinematics selected is the identity one, or the module supplies no tool frame or cannot be evaluated by the interpreter, diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index 24fdd07724f..5f78ff9296e 100644 --- a/docs/src/gcode/overview.adoc +++ b/docs/src/gcode/overview.adoc @@ -512,6 +512,19 @@ can be added easily without changes to the source code. 'P' number of the last 'G12.1', or 0 after 'G13.1' or when no kinematics has been selected. See <>. +* '#<_kins_orient_1>', '#<_kins_orient_2>' - The axes the kinematics type + in force orients the tool with, as axis numbers, 0 for X to 8 for W: the + first rotation, the one whose axis stays put, then the second. A head + and a table give the table first. The type's module declares the letters, + or they are those its joint map gives the joints that turn. -1 where the + type does not orient with two rotaries, the identity type or a robot + among them, or the interpreter cannot evaluate the kinematics. An axis + named here must be 'ANGULAR' in the INI file; the interpreter refuses a + module whose type orients with a 'LINEAR' axis when it first loads it. + +* '#<_kins_orient_1_head>', '#<_kins_orient_2_head>' - 1 where that axis + turns the head, 0 where it turns the table, -1 as above. + * '#<_orient_valid>', '#<_orient_x>', '#<_orient_y>', '#<_orient_z>', '#<_orient_a>', '#<_orient_b>', '#<_orient_c>' - The pose 'G53.2' last solved, in program units in the tilted work plane. '#<_orient_valid>' is diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 1fe501ba5db..01a50e01a92 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -566,7 +566,13 @@ the no-transform type, joints are axes; `primary` the working transform `G49` cancel to and `G53.5` moves in, which a module leaves unset when its identity type is that frame and sets on its working transform when that reports the flange with no tool, as the robot modules do (see the Switchable -Kinematics chapter). A module with +Kinematics chapter). A type may also name the two axis letters that orient +the tool, first rotation then second, in `orient`: "AC" for instance. Left +out, they are the letters the joint map gives the joints that turn the frames, +which is right for a module whose forward puts each joint in its own letter; +a module that writes an orienting joint into another letter declares them. +`G43.5` writes its solution into these letters, and the interpreter reads them +back as `#<_kins_orient_1>` and `#<_kins_orient_2>`. 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`. diff --git a/src/emc/kinematics/kins_module.h b/src/emc/kinematics/kins_module.h index 314dd9d32b4..313150623e5 100644 --- a/src/emc/kinematics/kins_module.h +++ b/src/emc/kinematics/kins_module.h @@ -331,7 +331,11 @@ typedef int (*kins_jacobian_fn)(const kins_params *p, const double *joint, module that leaves it unset on every type has its identity type stand in. A machine frame type does not read the tool offset in the parameter block, so a working transform that leaves the tool out anyway, a robot's flange, - carries primary and machine both. */ + carries primary and machine both. orient names the two axis letters + that orient the tool, first rotation then second, "AC" for instance; left + NULL they are the letters the joint map gives the joints that turn the + frames, which is right for a module whose forward puts each joint in its + own letter, and a module that puts one elsewhere declares them. */ typedef struct kins_ops { kins_forward_fn forward; kins_inverse_fn inverse; @@ -343,6 +347,7 @@ typedef struct kins_ops { int identity; /* joints are axes */ int primary; /* the working transform */ int machine; /* the machine frame */ + const char *orient; /* orienting letters, NULL: from the joint map */ } kins_ops; /* A module described for a caller outside RT: its table, its joint diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index 2fe6cb4bbb2..720d7e7d801 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -25,6 +25,7 @@ ********************************************************************/ #include "kinematics_user.h" +#include #include #include #include @@ -641,6 +642,72 @@ int kinematicsUserOrientJoints(KinematicsUserContext* ctx, const double* seed, return r; } +// the letter of joint j: the principal one, else any that maps to it +static int joint_letter(const kins_params *p, int j) +{ + int a; + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + if (p->joint_of_axis[a] == j) { return a; } + } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + if (p->joints_of_axis[a] & (1u << j)) { return a; } + } + return -1; +} + +int kinematicsUserOrientAxes(KinematicsUserContext* ctx, const double* seed, + int axes[2], int head[2]) +{ + static const char letters[] = "XYZABCUVW"; + const kins_ops *ops; + double j[EMCMOT_MAX_JOINTS]; + unsigned int table_mask = 0, head_mask = 0; + int joints[2] = {-1, -1}; + int i, r = -1; + + if (!axes || !head) return -1; + axes[0] = axes[1] = head[0] = head[1] = -1; + if (!ctx || !ctx->initialized || ctx->rt_only || !seed) return -1; + ops = ctx->info.ops[ctx->ktype]; + if (!ops->tool || !ops->work) return -1; + refresh(ctx); + pad_joints(ctx, seed, j); + frame_ctx = ctx; + if (toolFrameWorkJoints(frame_work, ctx->num_joints, j, &table_mask) == 0 + && toolFrameWorkJoints(frame_tool, ctx->num_joints, j, &head_mask) == 0 + && !(table_mask & head_mask)) { + int tables = __builtin_popcount(table_mask), heads = __builtin_popcount(head_mask); + if (heads == 2 && tables == 0) { + r = toolFrameOrientJoints(frame_tool, ctx->num_joints, j, &joints[0], &joints[1]); + head[0] = head[1] = 1; + } else if (tables == 2 && heads == 0) { + r = toolFrameOrientJoints(frame_work, ctx->num_joints, j, &joints[0], &joints[1]); + head[0] = head[1] = 0; + } else if (tables == 1 && heads == 1) { + joints[0] = __builtin_ctz(table_mask); + joints[1] = __builtin_ctz(head_mask); + head[0] = 0; + head[1] = 1; + r = 0; + } + } + frame_ctx = NULL; + for (i = 0; i < 2 && r == 0; i++) { + if (ops->orient && ops->orient[0] && ops->orient[1]) { + const char *at = strchr(letters, toupper((unsigned char)ops->orient[i])); + axes[i] = at ? (int)(at - letters) : -1; + } else { + axes[i] = joint_letter(&ctx->params, joints[i]); + } + if (axes[i] < 0) { r = -1; } + } + if (r != 0) { + axes[0] = axes[1] = head[0] = head[1] = -1; + } + return r; +} + KinematicsUserContext* kinematicsUserInitString(const char* kinematics, int num_joints, int comp_id, diff --git a/src/emc/kinematics_userspace/kinematics_user.h b/src/emc/kinematics_userspace/kinematics_user.h index 5b27f145ee8..8fd05be3b05 100644 --- a/src/emc/kinematics_userspace/kinematics_user.h +++ b/src/emc/kinematics_userspace/kinematics_user.h @@ -257,6 +257,19 @@ int kinematicsUserWorkJoints(KinematicsUserContext* ctx, const double* seed, int kinematicsUserOrientJoints(KinematicsUserContext* ctx, const double* seed, int* primary, int* secondary); +/** + * The axes that orient the tool on the active type, as axis numbers, 0 X + * to 8 W: the first rotation then the second, and whether each turns the + * head (1) or the table (0). Two heads or two tables are ordered as + * kinematicsUserOrientJoints() orders them, the one whose axis stays put + * first; a table and a head, the table first. The letters are the type's + * declared orient, else those the joint map gives the joints. Returns 0, + * or -1 with all four set to -1 where the type turns other than two + * rotaries between tool and work, or a letter cannot be found. + */ +int kinematicsUserOrientAxes(KinematicsUserContext* ctx, const double* seed, + int axes[2], int head[2]); + /** * kinematicsUserInitSparm() from the value of [KINS] KINEMATICS as the * HAL file hands it to loadrt: the module name first, then any of diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 8346e7b98cd..65c82c313fc 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -5714,7 +5714,8 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 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)); + double *rotary_end[6] = {&AA_end, &BB_end, &CC_end, &u_end, &v_end, &w_end}; + CHP(tool_vector_ends(block, settings, rotary_end)); } if (move == G_1) { diff --git a/src/emc/rs274ngc/interp_namedparams.cc b/src/emc/rs274ngc/interp_namedparams.cc index cb830d8542b..7d09de70385 100644 --- a/src/emc/rs274ngc/interp_namedparams.cc +++ b/src/emc/rs274ngc/interp_namedparams.cc @@ -59,6 +59,10 @@ enum predefined_named_parameters { NP_LINE, NP_MOTION_MODE, NP_KINS_TYPE, + NP_KINS_ORIENT_1, + NP_KINS_ORIENT_2, + NP_KINS_ORIENT_1_HEAD, + NP_KINS_ORIENT_2_HEAD, NP_ORIENT_VALID, NP_ORIENT_X, NP_ORIENT_Y, @@ -553,6 +557,22 @@ int Interp::lookup_named_param(const char *nameBuf, *value = _setup.kins_type; break; + case NP_KINS_ORIENT_1: // _kins_orient_1 and kin: the axes that orient the tool + case NP_KINS_ORIENT_2: + case NP_KINS_ORIENT_1_HEAD: + case NP_KINS_ORIENT_2_HEAD: + { + int axes[2], head[2]; + kins_orient(&_setup, axes, head); + switch (cmd) { + case NP_KINS_ORIENT_1: *value = axes[0]; break; + case NP_KINS_ORIENT_2: *value = axes[1]; break; + case NP_KINS_ORIENT_1_HEAD: *value = head[0]; break; + default: *value = head[1]; break; + } + } + break; + case NP_ORIENT_VALID: // _orient_valid: G53.2 has solved a pose *value = _setup.orient_valid; break; @@ -922,6 +942,14 @@ int Interp::init_named_parameters() // kinematics selected by G12.1 P- / G13.1, 0 when none has been selected init_readonly_param("_kins_type", NP_KINS_TYPE, PA_USE_LOOKUP); + // the axes that orient the tool on that type, 0 X to 8 W, first rotation + // then second, and whether each turns the head (1) or the table (0); -1 + // where the type does not orient with two rotaries + init_readonly_param("_kins_orient_1", NP_KINS_ORIENT_1, PA_USE_LOOKUP); + init_readonly_param("_kins_orient_2", NP_KINS_ORIENT_2, PA_USE_LOOKUP); + init_readonly_param("_kins_orient_1_head", NP_KINS_ORIENT_1_HEAD, PA_USE_LOOKUP); + init_readonly_param("_kins_orient_2_head", NP_KINS_ORIENT_2_HEAD, PA_USE_LOOKUP); + // 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); diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index ea288083a4e..51a4d791d2f 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -479,10 +479,49 @@ int Interp::kins_load(setup_pointer s) CHKS((!ctx), _("kinematics module %s cannot be loaded here"), s->kins_module); s->kins_ctx = ctx; for (int i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = 0.0; } + CHP(kins_check_orient(s)); } return INTERP_OK; } +// Every type the module evaluates here orients the tool with angular axes: +// a type whose rotaries are letters the INI file makes LINEAR would have +// G43.5 and G68.2 write degrees into a length. +int Interp::kins_check_orient(setup_pointer s) +{ + static const char letters[] = "XYZABCUVW"; + KinematicsUserContext *ctx = KINS_CTX(s); + double zero[EMCMOT_MAX_JOINTS] = {0}; + int t, i, axes[2], head[2]; + + for (t = 0; t < kinematicsUserGetNumTypes(ctx); t++) { + if (kinematicsUserSetType(ctx, t) != 0) { continue; } + if (kinematicsUserOrientAxes(ctx, zero, axes, head) != 0) { continue; } + for (i = 0; i < 2; i++) { + if (axisKindsAngular(s->axis_kinds, axes[i])) { continue; } + kins_release(s); + ERS(_("kinematics type %d of %s orients the tool with %c, which [AXIS_%c] TYPE makes LINEAR"), + t, s->kins_module, letters[axes[i]], letters[axes[i]]); + } + } + return INTERP_OK; +} + +// The axes that orient the tool on the kinematics type the program is in, +// at the joints it stands in: axis numbers 0 X to 8 W, first rotation then +// second, and 1 for a head, 0 for a table. All -1 where the type turns +// other than two rotaries, or no module can be evaluated here. +void Interp::kins_orient(setup_pointer s, int axes[2], int head[2]) +{ + void *vctx; + double now[EMCMOT_MAX_JOINTS]; + + axes[0] = axes[1] = head[0] = head[1] = -1; + if (kins_context(s, &vctx) != INTERP_OK) { return; } + if (current_joints(s, (KinematicsUserContext *)vctx, now) != INTERP_OK) { return; } + kinematicsUserOrientAxes((KinematicsUserContext *)vctx, now, axes, head); +} + // the loaded module, on the kinematics type the program is in int Interp::kins_context(setup_pointer s, void **out) { @@ -770,6 +809,22 @@ int Interp::convert_work_plane_from_tool(block_pointer block, setup_pointer s) return work_plane_set(s, G_68_3, origin, rotation); } +// The rotary letters a solution for the kinematics type in force is +// written into, as axis numbers: the two it orients the tool with, else the +// A B C of the pose, a robot's wrist. Returns how many. +static int orienting_letters(KinematicsUserContext *ctx, const double *now, int orienting[3]) +{ + int axes[2], head[2]; + + orienting[0] = 3; + orienting[1] = 4; + orienting[2] = 5; + if (kinematicsUserOrientAxes(ctx, now, axes, head) != 0) { return 3; } + orienting[0] = axes[0]; + orienting[1] = axes[1]; + return 2; +} + // 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 @@ -826,6 +881,18 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) return INTERP_OK; } + // the rotaries the solution turns, in the letters the type orients + // with; every other axis stays where it is + double rot[6] = {s->AA_current, s->BB_current, s->CC_current, + s->u_current, s->v_current, s->w_current}; + { + int orienting[3]; + int count = orienting_letters(ctx, now, orienting); + for (i = 0; i < count; i++) { + if (orienting[i] >= 3) { rot[orienting[i] - 3] = end_prog[orienting[i]]; } + } + } + write_canon_state_tag(block, s); if (code == G_53_1) { // the rotaries alone: the linear joints are where they are, since @@ -841,28 +908,28 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) } 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); + rot[0], rot[1], rot[2], rot[3], rot[4], rot[5]); } else { double x = block->x_flag ? block->x_number : s->current_x; double y = block->y_flag ? block->y_number : s->current_y; double z = block->z_flag ? block->z_number : s->current_z; JOINT_TRAVERSE(block->line_number, NULL, 0, x, y, z, - end_prog[3], end_prog[4], end_prog[5], - s->u_current, s->v_current, s->w_current); + rot[0], rot[1], rot[2], rot[3], rot[4], rot[5]); 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]; + // the joints went where the solver put them, every letter with them + for (i = 0; i < 6; i++) { rot[i] = end_prog[3 + i]; } } + s->AA_current = rot[0]; + s->BB_current = rot[1]; + s->CC_current = rot[2]; + s->u_current = rot[3]; + s->v_current = rot[4]; + s->w_current = rot[5]; return INTERP_OK; } @@ -981,19 +1048,21 @@ int Interp::orient_solve(setup_pointer s, void *vctx, const PmCartesian *axis, c // 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) +// coordinates of the axes the kinematics type orients with (A B C U V W +// ends, 0 A to 5 W), so a rotary offset is right by construction. +int Interp::tool_vector_ends(block_pointer block, setup_pointer s, double *rotary_end[6]) { + static const char letters[] = "XYZABCUVW"; void *vctx; KinematicsUserContext *ctx; double now[EMCMOT_MAX_JOINTS], sol[EMCMOT_MAX_JOINTS]; double v[3], prog[9]; + const bool rotary_flag[6] = {block->a_flag, block->b_flag, block->c_flag, + block->u_flag, block->v_flag, block->w_flag}; PmCartesian axis; EmcPose pose; int i; - 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; @@ -1003,6 +1072,16 @@ int Interp::tool_vector_ends(block_pointer block, setup_pointer s, double *a, do CHKS((kinematicsUserIsIdentity(ctx)), _("G43.5: a tool vector needs a kinematics type that describes the machine; select it with G12.1 first")); CHP(current_joints(s, ctx, now)); + int orienting[3]; + int count = orienting_letters(ctx, now, orienting); + CHKS((orienting[0] < 3 || orienting[1] < 3), + _("G43.5: kinematics type %d orients the tool with %c and %c, not rotary axes"), + s->kins_type, letters[orienting[0]], letters[orienting[1]]); + for (i = 0; i < count; i++) { + CHKS((rotary_flag[orienting[i] - 3]), + _("G43.5: a tool vector and a %c word on one line give the orientation twice"), + letters[orienting[i]]); + } if (block->g_modes[GM_MODAL_0] == G_53) { axis.x = v[0]; axis.y = v[1]; @@ -1019,9 +1098,9 @@ int Interp::tool_vector_ends(block_pointer block, setup_pointer s, double *a, do _("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]; + for (i = 0; i < count; i++) { + *rotary_end[orienting[i] - 3] = prog[orienting[i]]; + } return INTERP_OK; } diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index 64e826be063..b9e9f26d6db 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -375,7 +375,9 @@ public: 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 tool_vector_ends(block_pointer block, setup_pointer settings, double *rotary_end[6]); + int kins_check_orient(setup_pointer s); + void kins_orient(setup_pointer s, int axes[2], int head[2]); int convert_ptp_joints(int code, int move, block_pointer block, setup_pointer settings); int convert_home_slides(int code, block_pointer block, setup_pointer settings); int slide_joints(const char *name, setup_pointer settings, void *ctx, const struct kins_params *params, diff --git a/tests/kins-orient-linear/README b/tests/kins-orient-linear/README new file mode 100644 index 00000000000..674867e3a64 --- /dev/null +++ b/tests/kins-orient-linear/README @@ -0,0 +1,4 @@ +A kinematics type that orients the tool with an axis the INI file makes +LINEAR is refused: 5axiskins turns the tool with B and C, and here +[AXIS_B] TYPE is LINEAR. The interpreter refuses when it first loads the +module, the G43.5 vector below, with a message naming the type and B. diff --git a/tests/kins-orient-linear/checkresult b/tests/kins-orient-linear/checkresult new file mode 100755 index 00000000000..42526aa5579 --- /dev/null +++ b/tests/kins-orient-linear/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# test-ui.py prints PASS once the refusal names B +grep -q "^PASS$" "$1" diff --git a/tests/kins-orient-linear/sim.hal b/tests/kins-orient-linear/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/kins-orient-linear/sim.hal @@ -0,0 +1,16 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/kins-orient-linear/test-ui.py b/tests/kins-orient-linear/test-ui.py new file mode 100755 index 00000000000..3d72ea20b34 --- /dev/null +++ b/tests/kins-orient-linear/test-ui.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python3 +# 5axiskins orients with B and C; [AXIS_B] TYPE = LINEAR: see README. + +import linuxcnc +import os +import sys + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.home(-1) +c.wait_complete() +c.mode(linuxcnc.MODE_MDI) +c.wait_complete() + +said = [] +for cmd in ("G12.1 P0", "G43.5 H1", "G0 X0 K1"): + c.mdi(cmd) + c.wait_complete(30) + while True: + m = e.poll() + if not m: + break + said.append(m) + +errors = [m[1] for m in said if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR)] +print("errors: %s" % errors) +ok = any("orients the tool with B, which [AXIS_B] TYPE makes LINEAR" in m for m in errors) +c.state(linuxcnc.STATE_ESTOP) +for f in ("sim.var", "sim.var.bak"): + try: + os.unlink(f) + except OSError: + pass +print("PASS" if ok else "FAIL: no refusal naming B") +sys.exit(0 if ok else 1) diff --git a/tests/kins-orient-linear/test.ini b/tests/kins-orient-linear/test.ini new file mode 100644 index 00000000000..0ea12355fb5 --- /dev/null +++ b/tests/kins-orient-linear/test.ini @@ -0,0 +1,131 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# switchkins-type 0 is 5axiskins, 1 is identity, 2 is the userk template +KINEMATICS = 5axiskins coordinates=xyzbcw +JOINTS = 6 + +[HAL] +HALFILE = sim.hal + +[TRAJ] +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 200 +MAX_LINEAR_VELOCITY = 346 +MAX_LINEAR_ACCELERATION = 800 +DEFAULT_LINEAR_ACCELERATION = 800 +MAX_ANGULAR_VELOCITY = 360 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Y] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_B] +TYPE = LINEAR +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_W] +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[JOINT_0] +TYPE = LINEAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +MIN_LIMIT = -120 +MAX_LIMIT = 120 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = LINEAR +MIN_LIMIT = -100 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/kins-orient-linear/test.sh b/tests/kins-orient-linear/test.sh new file mode 100755 index 00000000000..079fb121f31 --- /dev/null +++ b/tests/kins-orient-linear/test.sh @@ -0,0 +1,3 @@ +#!/bin/bash -e +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/kins-orient-linear/tool.tbl b/tests/kins-orient-linear/tool.tbl new file mode 100644 index 00000000000..d793e2d60ed --- /dev/null +++ b/tests/kins-orient-linear/tool.tbl @@ -0,0 +1 @@ +T1 P1 D0.0 Z12.5 ; diff --git a/tests/kins-switch/test-ui.py b/tests/kins-switch/test-ui.py index b8f540e8908..999244bd120 100755 --- a/tests/kins-switch/test-ui.py +++ b/tests/kins-switch/test-ui.py @@ -297,6 +297,52 @@ def refused(cmd, needle): mdi("G49") refused("G0 X0 K1", "K word with no") +# ---- the axes that orient the tool --------------------------------------- +# +# 5axiskins turns the tool with a head: C with its axis fixed, B carried by +# it. The interpreter finds them from the tool frame and names them by the +# joint map, as axis numbers; the identity type orients with nothing. Under +# G43.5 a word of an axis that does not orient, W here, goes with a vector. + +def param(name): + drain() + c.mdi("(debug,#<%s>)" % name) + c.wait_complete(30) + deadline = time.time() + 5 + while time.time() < deadline: + m = e.poll() + if not m: + time.sleep(0.01) + continue + if m[0] == linuxcnc.OPERATOR_DISPLAY: + return float(m[1]) + error("#<%s> gave no value" % name) + return None + +ORIENT = ("_kins_orient_1", "_kins_orient_2", "_kins_orient_1_head", "_kins_orient_2_head") +errors_before = errors +mdi("G12.1 P0") +got = [param(n) for n in ORIENT] +if got != [5, 4, 1, 1]: + error("on the 5-axis type the orienting axes read %s, not C then B, both heads" % (got,)) +mdi("G13.1") +got = [param(n) for n in ORIENT] +if got != [-1, -1, -1, -1]: + error("on the identity type the orienting axes read %s, not none" % (got,)) +mdi("G12.1 P0") +mdi("G0 X0 Y0 Z0 B0 C0 W0") +mdi("G43.5 H1") +with_w = mdi("G0 X0 Y0 Z0 K1 W2") +if abs(with_w[5] - 2) > 1e-6: + error("a vector with a W word left W at %.4f, not 2" % with_w[5]) +mdi("G0 W0") +mdi("G49") +said = drain() +if any(m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR) for m in said): + error("the orienting axes section reported %s" % (said,)) +elif errors == errors_before: + print("the orienting axes are C then B, heads, and W goes along with a vector") + # ---- a negative kinematics number is refused ----------------------------- drain() diff --git a/tests/remap/introspect/expected b/tests/remap/introspect/expected index 206bc4d96af..37173c3fa16 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', '_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'] +globals: ['_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_orient_1', '_kins_orient_1_head', '_kins_orient_2', '_kins_orient_2_head', '_kins_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_orient_1', '_kins_orient_1_head', '_kins_orient_2', '_kins_orient_2_head', '_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) From d13e2a66d67eb6403e11f1489afacca8e0208eaa Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 23 Sep 2026 23:46:58 +1000 Subject: [PATCH 72/77] switchkins: a module may declare several primary types A machine with two tool heads, a saw blade and a waterjet say, has a working transform per head and selects the head with G12.1. The loader now accepts any number of types flagged primary; identity and machine frame stay unique, since G13.1 and G49 resolve a single answer from them. G43.4 and G43.5 stay on the type in force when it is primary, and from any other type switch to the lowest numbered primary, which is the single primary on every module that declares one. tests/ptp-machine-frame gains a second head on slantkins (type 3, the tip again, primary) and checks both rules. --- docs/src/gcode/g-code.adoc | 5 ++++- docs/src/motion/kinematics-conventions.adoc | 5 +++-- docs/src/motion/switchkins.adoc | 10 +++++++--- src/emc/kinematics/kins_module.h | 3 ++- src/emc/kinematics/switchkins.c | 14 +++++--------- src/emc/rs274ngc/interp_convert.cc | 6 +++++- tests/ptp-machine-frame/README | 6 ++++-- tests/ptp-machine-frame/slantkins.comp | 11 +++++++++++ tests/ptp-machine-frame/test-ui.py | 11 +++++++++++ 9 files changed, 52 insertions(+), 19 deletions(-) diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 00f64345876..befcae3c686 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -1822,7 +1822,10 @@ The H word, the offset itself and the parameters it lands in are 'G43''s. Which kinematics is the working one is declared by the module (KINSTYPE_PRIMARY, see the <> chapter), the number is not the answer; a module that -declares none rejects 'G43.4' at read time. +declares none rejects 'G43.4' at read time. A module with one working +kinematics per tool head declares each one primary: 'G43.4' stays on the +one selected with 'G12.1' and, from any other kinematics, switches to the +lowest numbered. The switch is a queue synchronisation point like '<>': when 'G43.4' or 'G49' changes the diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 01a50e01a92..901f9bb01fd 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -561,8 +561,9 @@ 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, joints are axes; `primary` the working transform -`G43.4` and `G43.5` switch to; `machine` the machine frame type `G13.1` and +the no-transform type, joints are axes; `primary` a working transform +`G43.4` and `G43.5` switch to, which a module with one per tool head sets +on each, `G12.1` choosing the head; `machine` the machine frame type `G13.1` and `G49` cancel to and `G53.5` moves in, which a module leaves unset when its identity type is that frame and sets on its working transform when that reports the flange with no tool, as the robot modules do (see the Switchable diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 9907fb2d479..cca9b92c08d 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -618,7 +618,11 @@ G-code reads these declarations: 'G13.1' and 'G49' cancel to the kinstype declared KINSTYPE_MACHINE, 'G53.5', 'G28.5' and 'G30.5' move in its world, and 'G43.4' switches to the kinstype declared KINSTYPE_PRIMARY, whatever their numbers, so a module whose kinematics -are not in the conventional order still gets working spellings. The +are not in the conventional order still gets working spellings. A +machine with two tool heads, a saw blade and a waterjet say, declares +one primary kinstype per head and selects the head with 'G12.1 P-': +'G43.4' stays on the kinstype in force when it is primary, and from any +other switches to the lowest numbered primary. The machine frame kinstype is the machine with the tool left out, XYZ the pivot or the flange in machine coordinates and the rotary letters the orientation as that kinstype reports it; on a machine whose slides line @@ -637,8 +641,8 @@ composite Y on a mill-turn, declares its machine frame kinstype separately and keeps KINSTYPE_IDENTITY for a kinstype whose joints really are the axes, since motion and the planner skip the maths on that flag alone. At most one -kinstype may be declared identity, at most one primary and at most one -machine frame, and declaring a kinstype the module does not provide +kinstype may be declared identity and at most one machine frame, any +number primary, and declaring a kinstype the module does not provide fails the module load. A module that declares nothing keeps working exactly as before for 'G12.1 P-' and 'G49', but 'G13.1' and 'G43.4' are an error, since the numbers of the identity and primary kinematics diff --git a/src/emc/kinematics/kins_module.h b/src/emc/kinematics/kins_module.h index 313150623e5..24ca503e33f 100644 --- a/src/emc/kinematics/kins_module.h +++ b/src/emc/kinematics/kins_module.h @@ -325,7 +325,8 @@ typedef int (*kins_jacobian_fn)(const kins_params *p, const double *joint, forward starts from the pose it is handed, so the shared code seeds it with the last answer after a switch. identity says joints are axes, which a consumer may use to skip the maths altogether. primary says this is - the module's working transform, the type G43.4 switches to. machine says + a working transform, the type G43.4 switches to; a module with one per + tool head flags each, G12.1 choosing the head. machine says this is the machine frame type, the tool left out, the pivot or the flange in machine coordinates, which G13.1 and G49 select and G53.5 moves in; a module that leaves it unset on every type has its identity type stand in. diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index 1131529564b..817f68103d4 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -635,7 +635,7 @@ int switchkinsInit(const int comp_id, const char* coordinates) { int i; - int identities, primaries, machines; + int identities, machines; int res = 0; char* emsg = "other"; @@ -663,13 +663,13 @@ int switchkinsInit(const int comp_id, } if (!kins_count) { emsg = "no switchkins-types provided"; goto error; } - // declarations must name provided types, and each flag is unique: - // G13.1 and G49 resolve the machine frame type from the flags and - // G43.4 the primary, so two answers is a load error. A module that + // declarations must name provided types, and identity and machine + // are unique: G13.1 and G49 resolve the machine frame type from the + // flags, so two answers is a load error. Several types may be + // primary, one per tool head, G12.1 choosing among them. A module that // names no machine frame type has its identity type stand in, which // is the truth on every machine whose slides line up with its frame. identities = 0; - primaries = 0; machines = 0; for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { if (ktype_flags[i] & KINSTYPE_MACHINE) { machines++; } @@ -686,7 +686,6 @@ int switchkinsInit(const int comp_id, identities++; if (!machines) { ktype_flags[i] |= KINSTYPE_MACHINE; } } - if (ktype_flags[i] & KINSTYPE_PRIMARY) { primaries++; } rtapi_print("switchkins-type %d declared:%s%s%s\n", i, (ktype_flags[i] & KINSTYPE_IDENTITY) ? " identity" : "", (ktype_flags[i] & KINSTYPE_PRIMARY) ? " primary" : "", @@ -695,9 +694,6 @@ int switchkinsInit(const int comp_id, if (identities > 1) { emsg = "more than one identity switchkins-type declared"; goto error; } - if (primaries > 1) { - emsg = "more than one primary switchkins-type declared"; goto error; - } if (machines > 1) { emsg = "more than one machine frame switchkins-type declared"; goto error; } diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 65c82c313fc..61b86f4f9f4 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -6597,7 +6597,7 @@ static int kins_type_info_available() return 0; } -// the type carrying a KINSTYPE_ flag, or -1 when the module declares none; +// the lowest type carrying a KINSTYPE_ flag, or -1 when the module declares none; // -1 for a type is "no information", and it matches every flag, so it must // be excluded before the bit test int flagged_kins_type(int flag) @@ -6667,10 +6667,14 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu (_("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); + int now = GET_EXTERNAL_KINS_TYPE_FLAGS(settings->kins_type); // G43.4 is G43 on the module's working transform: switch first, then // apply the offset, as if the switch line had run and drained. With // no kinematics attached there is nothing to switch to. G43.5 is // the same, and the lines after it may give the tool axis as I J K. + // A module with a working transform per tool head flags each one + // primary: the one in force stays, from any other type the lowest. + if (now >= 0 && (now & KINSTYPE_PRIMARY)) { primary = settings->kins_type; } CHKS(primary < 0 && kins_type_info_available(), NCE_NO_PRIMARY_KINEMATICS_TYPE); if (primary >= 0 && primary != settings->kins_type) { // the switch keeps the joints and moves the point, so the point diff --git a/tests/ptp-machine-frame/README b/tests/ptp-machine-frame/README index b5a51121880..ced057462de 100644 --- a/tests/ptp-machine-frame/README +++ b/tests/ptp-machine-frame/README @@ -6,10 +6,12 @@ template builds: XYZBC on five joints, joint 1 a slide slanted 30 degrees in the XY plane, so machine X and Y are made by joints 0 and 1 together. Type 0 is the carriage in machine coordinates, declared the machine frame type and not an identity; type 1 the tool tip with the head tilted about Y -and the tool length from the tool table; type 2 a plain identity. +and the tool length from the tool table; type 2 a plain identity; type 3 +the tip again, a second head, declared primary as well. The test checks that G13.1 and G49 select the machine frame type rather -than the identity, that G53.5 puts the carriage at a machine frame point, +than the identity, that G43.4 stays on a primary type selected with G12.1 +and from any other goes to the lowest numbered primary, that G53.5 puts the carriage at a machine frame point, the letters not given holding their machine coordinate and not their joint, from the machine frame type and from under the tilted tip kinematics alike, that G28.5 goes to a position stored on the machine diff --git a/tests/ptp-machine-frame/slantkins.comp b/tests/ptp-machine-frame/slantkins.comp index db9d694a9bb..8be16a89e2b 100644 --- a/tests/ptp-machine-frame/slantkins.comp +++ b/tests/ptp-machine-frame/slantkins.comp @@ -15,6 +15,9 @@ by B and the tool length from the tool table along the tool axis, folded so that tip and carriage agree at B zero. type2 is a plain identity, the joints as the axes, declared identity. + +type3 is type1 again, declared primary as well, the way a machine with a +second tool head declares a working transform for each. """; pin out si32 dummy=0 "one pin, which halcompile requires"; option period no; @@ -132,6 +135,13 @@ static const kins_ops tip_ops = { .primary = 1, }; +// a second head: G43.4 stays on it once G12.1 has selected it +static const kins_ops second_head_ops = { + .forward = tip_forward, + .inverse = tip_inverse, + .primary = 1, +}; + int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, KF* kfwd0, KF* kfwd1, KF* kfwd2, @@ -152,6 +162,7 @@ int switchkinsSetup(kparms* kp, switchkinsRegisterOps(0, &frame_ops); switchkinsRegisterOps(1, &tip_ops); switchkinsRegisterOps(2, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(3, &second_head_ops); return 0; } diff --git a/tests/ptp-machine-frame/test-ui.py b/tests/ptp-machine-frame/test-ui.py index 5b816f4116a..386c6c9cde3 100755 --- a/tests/ptp-machine-frame/test-ui.py +++ b/tests/ptp-machine-frame/test-ui.py @@ -102,6 +102,17 @@ def expect_type(what, want): expect_type("G49", 0) mdi("G12.1 P1", "G13.1") expect_type("G12.1 P1 then G13.1", 0) + +# --- two primary types: G43.4 keeps the head G12.1 selected ------------------ +mdi("G12.1 P3", "G43.4 H1") +expect_type("G12.1 P3 then G43.4, the second head", 3) +mdi("G49") +expect_type("G49 from the second head", 0) +mdi("G12.1 P2", "G43.4 H1") +expect_type("G43.4 from the identity, the lowest", 1) +mdi("G12.1 P3", "G43.4 H1") +expect_type("G43.4 again after G12.1 P3", 3) +mdi("G49") drain() # --- G53.5 is the machine frame, not the slides ------------------------------ From 956192e2f14d49f4cca0a2b5115f29c21b9096be Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 24 Sep 2026 00:11:33 +1000 Subject: [PATCH 73/77] G53.2 publishes the rotaries in the order the kinematics orients with #<_orient_a> #<_orient_b> #<_orient_c> become #<_orient_rot1> to #<_orient_rot3>: the angles of the axes #<_kins_orient_1> to #<_kins_orient_3> name, in that order, so a program reads them the same way whatever letters the machine gives its rotaries, as Fanuc and Siemens number the rotaries first and second. #5071-#5076 carry X Y Z and the three rotaries, #5077-#5079 are written 0. kinematicsUserOrientAxes() answers up to three axes. A type with three rotaries between tool and work, xyzacb_trsrn's A table and B C head, gives all three, tables first, each group in joint order, or the three letters its orient declares; a robot arm, whose joints all turn the tool, gives the A B C of its pose. #<_kins_orient_3> and #<_kins_orient_3_head> are new; a two rotary type reads -1 there, as before. G43.5 and the LINEAR check cover the third axis. --- .../bridgemill/g532-fused-orient-move.ngc | 2 +- docs/src/gcode/g-code.adoc | 12 ++-- docs/src/gcode/overview.adoc | 42 ++++++++----- docs/src/motion/kinematics-conventions.adoc | 17 ++--- lib/python/qtvcp/lib/mdi_text.py | 6 +- src/emc/kinematics/kins_module.h | 5 +- .../kinematics_userspace/kinematics_user.c | 36 ++++++++++- .../kinematics_userspace/kinematics_user.h | 12 ++-- src/emc/rs274ngc/interp_array.cc | 2 +- src/emc/rs274ngc/interp_internal.hh | 4 +- src/emc/rs274ngc/interp_namedparams.cc | 35 ++++++----- src/emc/rs274ngc/interp_workplane.cc | 63 ++++++++++--------- src/emc/rs274ngc/rs274ngc_interp.hh | 2 +- tests/kins-switch/test-ui.py | 7 ++- tests/ptp-robot/test-ui.py | 19 ++++++ tests/remap/introspect/expected | 4 +- tests/twp-native/test-ui.py | 34 +++++++--- 17 files changed, 200 insertions(+), 102 deletions(-) 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 index bd193299758..3439a71fae7 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/g532-fused-orient-move.ngc +++ b/configs/sim/axis/vismach/5axis/bridgemill/g532-fused-orient-move.ngc @@ -36,7 +36,7 @@ o210 else 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 + g0 x0 y0 z[#+#] b#<_orient_rot2> c#<_orient_rot1> ; one TCP turn and travel, C orients first g1 z[#+#] g0 z[#+#] # = [#+1] diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index befcae3c686..eede03fc747 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -2013,10 +2013,14 @@ differ in what happens then: * '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 + parameters '#<_orient_x>', '#<_orient_y>', '#<_orient_z>', + '#<_orient_rot1>', '#<_orient_rot2>' and '#<_orient_rot3>', and on the + numbered parameters '#5071' to '#5076' in the same order, in program + units in the plane, and sets '#<_orient_valid>' and '#5080' to 1. The + rotaries come in the order the kinematics orients the tool with, the + axes '#<_kins_orient_1>' to '#<_kins_orient_3>' name, so a program reads + the angles the same way whatever letters the machine gives them; + '#<_orient_rot3>' is 0 on a machine that orients with two rotaries. The program can then reach the pose with a move of its own making, for instance a single 'G0' that names 'X Y Z' and the rotary words together, so the turn and the travel are one Cartesian move under the TCP kinematics. This is what Heidenhain diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index 5f78ff9296e..03d0dd7cc99 100644 --- a/docs/src/gcode/overview.adoc +++ b/docs/src/gcode/overview.adoc @@ -304,9 +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. +* '5071-5076' - Pose last solved by <> for X, Y, Z + and the first, second and third orienting rotary, in program units in + the tilted work plane. Same values as `#<_orient_x>` and kin. Read-only, + volatile. +* '5077-5079' - Written 0 by 'G53.2'. Read-only, volatile. * '5080' - 'G53.2' result: 1 once a 'G53.2' has solved a pose, 0 before. Same as `#<_orient_valid>`. Read-only, volatile. * '5081-5089' - Tool length offset currently applied to motion for X, Y, @@ -512,24 +514,32 @@ can be added easily without changes to the source code. 'P' number of the last 'G12.1', or 0 after 'G13.1' or when no kinematics has been selected. See <>. -* '#<_kins_orient_1>', '#<_kins_orient_2>' - The axes the kinematics type - in force orients the tool with, as axis numbers, 0 for X to 8 for W: the - first rotation, the one whose axis stays put, then the second. A head - and a table give the table first. The type's module declares the letters, - or they are those its joint map gives the joints that turn. -1 where the - type does not orient with two rotaries, the identity type or a robot - among them, or the interpreter cannot evaluate the kinematics. An axis +* '#<_kins_orient_1>', '#<_kins_orient_2>', '#<_kins_orient_3>' - The + axes the kinematics type in force orients the tool with, as axis numbers, + 0 for X to 8 for W: the first rotation, the one whose axis stays put, then + the second. A head and a table give the table first. The type's module + declares the letters, or they are those its joint map gives the joints + that turn. '#<_kins_orient_3>' is -1 on those. A type with three + rotaries, a table and a two axis head say, gives all three, the tables + first; a robot arm, whose joints all turn the tool, reads 3, 4 and 5, the + A B C of its pose. All three are -1 where the type orients otherwise, the identity + type among them, or the interpreter cannot evaluate the kinematics. An axis named here must be 'ANGULAR' in the INI file; the interpreter refuses a module whose type orients with a 'LINEAR' axis when it first loads it. -* '#<_kins_orient_1_head>', '#<_kins_orient_2_head>' - 1 where that axis - turns the head, 0 where it turns the table, -1 as above. +* '#<_kins_orient_1_head>', '#<_kins_orient_2_head>', + '#<_kins_orient_3_head>' - 1 where that axis turns the head, 0 where it + turns the table, -1 as above. * '#<_orient_valid>', '#<_orient_x>', '#<_orient_y>', '#<_orient_z>', - '#<_orient_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 + '#<_orient_rot1>', '#<_orient_rot2>', '#<_orient_rot3>' - The pose 'G53.2' + last solved, in program units in the tilted work plane: X Y Z, then the + angles of the axes '#<_kins_orient_1>' to '#<_kins_orient_3>' name, in + that order, 0 for a third the machine does not have. This is how Fanuc + and Siemens number the rotaries of a five axis machine, first and second, + whatever their letters. '#<_orient_valid>' is 1 once a 'G53.2' has run; + reading the others before that is an error. The same values are on the + numbered parameters '#5071' to '#5076' and '#5080'. See <>. * '#<_plane>' - returns the value designating the current plane: diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 901f9bb01fd..3d162f27d21 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -567,13 +567,16 @@ on each, `G12.1` choosing the head; `machine` the machine frame type `G13.1` and `G49` cancel to and `G53.5` moves in, which a module leaves unset when its identity type is that frame and sets on its working transform when that reports the flange with no tool, as the robot modules do (see the Switchable -Kinematics chapter). A type may also name the two axis letters that orient -the tool, first rotation then second, in `orient`: "AC" for instance. Left -out, they are the letters the joint map gives the joints that turn the frames, -which is right for a module whose forward puts each joint in its own letter; -a module that writes an orienting joint into another letter declares them. -`G43.5` writes its solution into these letters, and the interpreter reads them -back as `#<_kins_orient_1>` and `#<_kins_orient_2>`. A module with +Kinematics chapter). A type may also name the axis letters that orient +the tool, first rotation then second, in `orient`: "AC" for instance, or three +letters, tables first, for a type with three rotaries. Left out, they are the +letters the joint map gives the joints that turn the frames, which is right +for a module whose forward puts each joint in its own letter; a module that +writes an orienting joint into another letter declares them. A robot arm, all +of whose joints turn the tool, orients with the A B C of its pose. `G43.5` +writes its solution into these letters, the interpreter reads them back as +`#<_kins_orient_1>` to `#<_kins_orient_3>`, and `G53.2` publishes the angles +it solves in the same order as `#<_orient_rot1>` to `#<_orient_rot3>`. A module with several types has one geometry table and one ops table per type, registered with `switchkinsRegisterOps()`; a module with one type describes itself in a `kins_module` and links `kins_single.c`. diff --git a/lib/python/qtvcp/lib/mdi_text.py b/lib/python/qtvcp/lib/mdi_text.py index 506a91da7ee..da3c2e4965c 100644 --- a/lib/python/qtvcp/lib/mdi_text.py +++ b/lib/python/qtvcp/lib/mdi_text.py @@ -863,8 +863,10 @@ def gcode_descriptions(gcode): G53.2 Solves the same pose as G53.1 and moves nothing. -The pose is published on #<_orient_x> to -#<_orient_c> and on #5071 to #5079, in program +The pose is published on #<_orient_x> #<_orient_y> +#<_orient_z> and the rotaries in the order the +kinematics orients with, #<_orient_rot1> to +#<_orient_rot3>, and on #5071 to #5076, in program units in the plane, with #<_orient_valid> (#5080) set to 1, so the program can reach it with a move of its own, for instance a single G0 naming X Y Z diff --git a/src/emc/kinematics/kins_module.h b/src/emc/kinematics/kins_module.h index 24ca503e33f..977081c8af3 100644 --- a/src/emc/kinematics/kins_module.h +++ b/src/emc/kinematics/kins_module.h @@ -332,8 +332,9 @@ typedef int (*kins_jacobian_fn)(const kins_params *p, const double *joint, module that leaves it unset on every type has its identity type stand in. A machine frame type does not read the tool offset in the parameter block, so a working transform that leaves the tool out anyway, a robot's flange, - carries primary and machine both. orient names the two axis letters - that orient the tool, first rotation then second, "AC" for instance; left + carries primary and machine both. orient names the axis letters that + orient the tool, first rotation then second, "AC" for instance, or three, + tables first, on a type with three rotaries; left NULL they are the letters the joint map gives the joints that turn the frames, which is right for a module whose forward puts each joint in its own letter, and a module that puts one elsewhere declares them. */ diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index 720d7e7d801..b6166d30d8b 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -657,7 +657,7 @@ static int joint_letter(const kins_params *p, int j) } int kinematicsUserOrientAxes(KinematicsUserContext* ctx, const double* seed, - int axes[2], int head[2]) + int axes[3], int head[3]) { static const char letters[] = "XYZABCUVW"; const kins_ops *ops; @@ -667,7 +667,7 @@ int kinematicsUserOrientAxes(KinematicsUserContext* ctx, const double* seed, int i, r = -1; if (!axes || !head) return -1; - axes[0] = axes[1] = head[0] = head[1] = -1; + for (i = 0; i < 3; i++) { axes[i] = head[i] = -1; } if (!ctx || !ctx->initialized || ctx->rt_only || !seed) return -1; ops = ctx->info.ops[ctx->ktype]; if (!ops->tool || !ops->work) return -1; @@ -690,6 +690,35 @@ int kinematicsUserOrientAxes(KinematicsUserContext* ctx, const double* seed, head[0] = 0; head[1] = 1; r = 0; + } else if (tables + heads == 3) { + // three rotaries: the tables first, each group in joint order + int n = 0, jn; + for (jn = 0; jn < ctx->num_joints; jn++) { + if (table_mask & (1u << jn)) { axes[n] = jn; head[n++] = 0; } + } + for (jn = 0; jn < ctx->num_joints; jn++) { + if (head_mask & (1u << jn)) { axes[n] = jn; head[n++] = 1; } + } + frame_ctx = NULL; + for (n = 0; n < 3; n++) { + if (ops->orient && strlen(ops->orient) >= 3) { + const char *at = strchr(letters, toupper((unsigned char)ops->orient[n])); + axes[n] = at ? (int)(at - letters) : -1; + } else { + axes[n] = joint_letter(&ctx->params, axes[n]); + } + } + if (axes[0] < 0 || axes[1] < 0 || axes[2] < 0) { + for (i = 0; i < 3; i++) { axes[i] = head[i] = -1; } + return -1; + } + return 3; + } else if (heads > 3 && tables == 0) { + // an arm: the wrist is one of many joints that turn the tool, + // and the pose gives the orientation as A B C + for (i = 0; i < 3; i++) { axes[i] = 3 + i; head[i] = 1; } + frame_ctx = NULL; + return 3; } } frame_ctx = NULL; @@ -704,8 +733,9 @@ int kinematicsUserOrientAxes(KinematicsUserContext* ctx, const double* seed, } if (r != 0) { axes[0] = axes[1] = head[0] = head[1] = -1; + return -1; } - return r; + return 2; } KinematicsUserContext* kinematicsUserInitString(const char* kinematics, diff --git a/src/emc/kinematics_userspace/kinematics_user.h b/src/emc/kinematics_userspace/kinematics_user.h index 8fd05be3b05..b2847832f41 100644 --- a/src/emc/kinematics_userspace/kinematics_user.h +++ b/src/emc/kinematics_userspace/kinematics_user.h @@ -263,12 +263,16 @@ int kinematicsUserOrientJoints(KinematicsUserContext* ctx, const double* seed, * head (1) or the table (0). Two heads or two tables are ordered as * kinematicsUserOrientJoints() orders them, the one whose axis stays put * first; a table and a head, the table first. The letters are the type's - * declared orient, else those the joint map gives the joints. Returns 0, - * or -1 with all four set to -1 where the type turns other than two - * rotaries between tool and work, or a letter cannot be found. + * declared orient, else those the joint map gives the joints. Three + * rotaries come tables first, each group in joint order. A type whose + * tool is turned by more than three joints and no table, a robot arm, + * orients with the A B C of its pose, all three head. Returns how many + * axes, 2 or 3, the unused entries -1; or -1 with every entry -1 where the + * type turns other rotaries between tool and work, or a letter cannot be + * found. */ int kinematicsUserOrientAxes(KinematicsUserContext* ctx, const double* seed, - int axes[2], int head[2]); + int axes[3], int head[3]); /** * kinematicsUserInitSparm() from the value of [KINS] KINEMATICS as the diff --git a/src/emc/rs274ngc/interp_array.cc b/src/emc/rs274ngc/interp_array.cc index be6f9d1b433..50698306322 100644 --- a/src/emc/rs274ngc/interp_array.cc +++ b/src/emc/rs274ngc/interp_array.cc @@ -228,7 +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 + 5071, 5072, 5073, 5074, 5075, 5076, 5077, 5078, 5079, 5080, // G53.2 pose X Y Z, rotaries 1 2 3, 0 0 0, valid 5400, // tool toolno 5401, // tool x offset 5402, // tool y offset diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index 45a97736cb7..8065b72c481 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -777,9 +777,9 @@ struct setup 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 + // the pose G53.2 last solved, in program words, for #<_orient_x> and kin bool orient_valid; - double orient_pose[6]; // x y z a b c + double orient_pose[6]; // x y z, the rotaries in orient order // the kinematics, for G68.3 and the orientation moves: loaded on first // use through the non-realtime loader, on a HAL component of our own void *kins_ctx; // KinematicsUserContext diff --git a/src/emc/rs274ngc/interp_namedparams.cc b/src/emc/rs274ngc/interp_namedparams.cc index 7d09de70385..2c7dd6cfcbf 100644 --- a/src/emc/rs274ngc/interp_namedparams.cc +++ b/src/emc/rs274ngc/interp_namedparams.cc @@ -61,15 +61,17 @@ enum predefined_named_parameters { NP_KINS_TYPE, NP_KINS_ORIENT_1, NP_KINS_ORIENT_2, + NP_KINS_ORIENT_3, NP_KINS_ORIENT_1_HEAD, NP_KINS_ORIENT_2_HEAD, + NP_KINS_ORIENT_3_HEAD, NP_ORIENT_VALID, NP_ORIENT_X, NP_ORIENT_Y, NP_ORIENT_Z, - NP_ORIENT_A, - NP_ORIENT_B, - NP_ORIENT_C, + NP_ORIENT_ROT1, + NP_ORIENT_ROT2, + NP_ORIENT_ROT3, NP_PLANE, NP_CCOMP, NP_METRIC, @@ -559,16 +561,17 @@ int Interp::lookup_named_param(const char *nameBuf, case NP_KINS_ORIENT_1: // _kins_orient_1 and kin: the axes that orient the tool case NP_KINS_ORIENT_2: + case NP_KINS_ORIENT_3: case NP_KINS_ORIENT_1_HEAD: case NP_KINS_ORIENT_2_HEAD: + case NP_KINS_ORIENT_3_HEAD: { - int axes[2], head[2]; + int axes[3], head[3]; kins_orient(&_setup, axes, head); - switch (cmd) { - case NP_KINS_ORIENT_1: *value = axes[0]; break; - case NP_KINS_ORIENT_2: *value = axes[1]; break; - case NP_KINS_ORIENT_1_HEAD: *value = head[0]; break; - default: *value = head[1]; break; + if (cmd <= NP_KINS_ORIENT_3) { + *value = axes[cmd - NP_KINS_ORIENT_1]; + } else { + *value = head[cmd - NP_KINS_ORIENT_1_HEAD]; } } break; @@ -580,9 +583,9 @@ int Interp::lookup_named_param(const char *nameBuf, 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: + case NP_ORIENT_ROT1: + case NP_ORIENT_ROT2: + case NP_ORIENT_ROT3: if (!_setup.orient_valid) { ERS(_("no G53.2 has solved an orientation yet")); } @@ -947,17 +950,19 @@ int Interp::init_named_parameters() // where the type does not orient with two rotaries init_readonly_param("_kins_orient_1", NP_KINS_ORIENT_1, PA_USE_LOOKUP); init_readonly_param("_kins_orient_2", NP_KINS_ORIENT_2, PA_USE_LOOKUP); + init_readonly_param("_kins_orient_3", NP_KINS_ORIENT_3, PA_USE_LOOKUP); init_readonly_param("_kins_orient_1_head", NP_KINS_ORIENT_1_HEAD, PA_USE_LOOKUP); init_readonly_param("_kins_orient_2_head", NP_KINS_ORIENT_2_HEAD, PA_USE_LOOKUP); + init_readonly_param("_kins_orient_3_head", NP_KINS_ORIENT_3_HEAD, PA_USE_LOOKUP); // the pose G53.2 last solved: 1.0 once one has been, and its words init_readonly_param("_orient_valid", NP_ORIENT_VALID, PA_USE_LOOKUP); init_readonly_param("_orient_x", NP_ORIENT_X, PA_USE_LOOKUP); init_readonly_param("_orient_y", NP_ORIENT_Y, PA_USE_LOOKUP); init_readonly_param("_orient_z", NP_ORIENT_Z, PA_USE_LOOKUP); - init_readonly_param("_orient_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); + init_readonly_param("_orient_rot1", NP_ORIENT_ROT1, PA_USE_LOOKUP); + init_readonly_param("_orient_rot2", NP_ORIENT_ROT2, PA_USE_LOOKUP); + init_readonly_param("_orient_rot3", NP_ORIENT_ROT3, PA_USE_LOOKUP); // G17/18/19/17.1/18.1/19.1 -> return 170/180/190/171/181/191 init_readonly_param("_plane", NP_PLANE, PA_USE_LOOKUP); diff --git a/src/emc/rs274ngc/interp_workplane.cc b/src/emc/rs274ngc/interp_workplane.cc index 51a4d791d2f..de54fa610fe 100644 --- a/src/emc/rs274ngc/interp_workplane.cc +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -492,12 +492,12 @@ int Interp::kins_check_orient(setup_pointer s) static const char letters[] = "XYZABCUVW"; KinematicsUserContext *ctx = KINS_CTX(s); double zero[EMCMOT_MAX_JOINTS] = {0}; - int t, i, axes[2], head[2]; + int t, i, n, axes[3], head[3]; for (t = 0; t < kinematicsUserGetNumTypes(ctx); t++) { if (kinematicsUserSetType(ctx, t) != 0) { continue; } - if (kinematicsUserOrientAxes(ctx, zero, axes, head) != 0) { continue; } - for (i = 0; i < 2; i++) { + n = kinematicsUserOrientAxes(ctx, zero, axes, head); + for (i = 0; i < n; i++) { if (axisKindsAngular(s->axis_kinds, axes[i])) { continue; } kins_release(s); ERS(_("kinematics type %d of %s orients the tool with %c, which [AXIS_%c] TYPE makes LINEAR"), @@ -509,14 +509,15 @@ int Interp::kins_check_orient(setup_pointer s) // The axes that orient the tool on the kinematics type the program is in, // at the joints it stands in: axis numbers 0 X to 8 W, first rotation then -// second, and 1 for a head, 0 for a table. All -1 where the type turns -// other than two rotaries, or no module can be evaluated here. -void Interp::kins_orient(setup_pointer s, int axes[2], int head[2]) +// second, a robot's wrist A B C, and 1 for a head, 0 for a table. All -1 +// where the type turns other rotaries, or no module can be evaluated here. +void Interp::kins_orient(setup_pointer s, int axes[3], int head[3]) { void *vctx; double now[EMCMOT_MAX_JOINTS]; + int i; - axes[0] = axes[1] = head[0] = head[1] = -1; + for (i = 0; i < 3; i++) { axes[i] = head[i] = -1; } if (kins_context(s, &vctx) != INTERP_OK) { return; } if (current_joints(s, (KinematicsUserContext *)vctx, now) != INTERP_OK) { return; } kinematicsUserOrientAxes((KinematicsUserContext *)vctx, now, axes, head); @@ -810,19 +811,18 @@ int Interp::convert_work_plane_from_tool(block_pointer block, setup_pointer s) } // The rotary letters a solution for the kinematics type in force is -// written into, as axis numbers: the two it orients the tool with, else the -// A B C of the pose, a robot's wrist. Returns how many. +// written into, as axis numbers: those it orients the tool with, else the +// A B C of the pose. Returns how many. static int orienting_letters(KinematicsUserContext *ctx, const double *now, int orienting[3]) { - int axes[2], head[2]; + int head[3]; + int n = kinematicsUserOrientAxes(ctx, now, orienting, head); + if (n > 0) { return n; } orienting[0] = 3; orienting[1] = 4; orienting[2] = 5; - if (kinematicsUserOrientAxes(ctx, now, axes, head) != 0) { return 3; } - orienting[0] = axes[0]; - orienting[1] = axes[1]; - return 2; + return 3; } // G53.1, G53.2, G53.3 and G53.6: the rotaries to the plane's normal. G53.1 @@ -868,15 +868,22 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { s->kins_seed[i] = sol[i]; } machine_pose_to_program(s, &end_pose, end_prog); + int orienting[3]; + int count = orienting_letters(ctx, now, orienting); + 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]; } + // STAY: solve only, nothing moves. X Y Z and the rotaries in the + // order the type orients with go to the named parameters + // #<_orient_x> and kin and to #5071-#5080, for the program to use + // in a move of its own making, the way Heidenhain's STAY fills + // Q120-122. The machine state does not change. + for (i = 0; i < 3; i++) { s->orient_pose[i] = end_prog[i]; } + for (i = 0; i < 3; i++) { + s->orient_pose[3 + i] = (i < count) ? end_prog[orienting[i]] : 0.0; + } s->orient_valid = true; - for (i = 0; i < 9; i++) { s->parameters[5071 + i] = end_prog[i]; } + for (i = 0; i < 6; i++) { s->parameters[5071 + i] = s->orient_pose[i]; } + for (i = 6; i < 9; i++) { s->parameters[5071 + i] = 0.0; } s->parameters[5080] = 1.0; return INTERP_OK; } @@ -885,12 +892,8 @@ int Interp::convert_orient_tool(int code, block_pointer block, setup_pointer s) // with; every other axis stays where it is double rot[6] = {s->AA_current, s->BB_current, s->CC_current, s->u_current, s->v_current, s->w_current}; - { - int orienting[3]; - int count = orienting_letters(ctx, now, orienting); - for (i = 0; i < count; i++) { - if (orienting[i] >= 3) { rot[orienting[i] - 3] = end_prog[orienting[i]]; } - } + for (i = 0; i < count; i++) { + if (orienting[i] >= 3) { rot[orienting[i] - 3] = end_prog[orienting[i]]; } } write_canon_state_tag(block, s); @@ -1074,10 +1077,10 @@ int Interp::tool_vector_ends(block_pointer block, setup_pointer s, double *rotar CHP(current_joints(s, ctx, now)); int orienting[3]; int count = orienting_letters(ctx, now, orienting); - CHKS((orienting[0] < 3 || orienting[1] < 3), - _("G43.5: kinematics type %d orients the tool with %c and %c, not rotary axes"), - s->kins_type, letters[orienting[0]], letters[orienting[1]]); for (i = 0; i < count; i++) { + CHKS((orienting[i] < 3), + _("G43.5: kinematics type %d orients the tool with %c, not a rotary axis"), + s->kins_type, letters[orienting[i]]); CHKS((rotary_flag[orienting[i] - 3]), _("G43.5: a tool vector and a %c word on one line give the orientation twice"), letters[orienting[i]]); diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index b9e9f26d6db..d09357d50b1 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -377,7 +377,7 @@ public: int p, int q, const double *now, double *joints, const char *name); int tool_vector_ends(block_pointer block, setup_pointer settings, double *rotary_end[6]); int kins_check_orient(setup_pointer s); - void kins_orient(setup_pointer s, int axes[2], int head[2]); + void kins_orient(setup_pointer s, int axes[3], int head[3]); int convert_ptp_joints(int code, int move, block_pointer block, setup_pointer settings); int convert_home_slides(int code, block_pointer block, setup_pointer settings); int slide_joints(const char *name, setup_pointer settings, void *ctx, const struct kins_params *params, diff --git a/tests/kins-switch/test-ui.py b/tests/kins-switch/test-ui.py index 999244bd120..6847c9bb756 100755 --- a/tests/kins-switch/test-ui.py +++ b/tests/kins-switch/test-ui.py @@ -319,15 +319,16 @@ def param(name): error("#<%s> gave no value" % name) return None -ORIENT = ("_kins_orient_1", "_kins_orient_2", "_kins_orient_1_head", "_kins_orient_2_head") +ORIENT = ("_kins_orient_1", "_kins_orient_2", "_kins_orient_3", + "_kins_orient_1_head", "_kins_orient_2_head", "_kins_orient_3_head") errors_before = errors mdi("G12.1 P0") got = [param(n) for n in ORIENT] -if got != [5, 4, 1, 1]: +if got != [5, 4, -1, 1, 1, -1]: error("on the 5-axis type the orienting axes read %s, not C then B, both heads" % (got,)) mdi("G13.1") got = [param(n) for n in ORIENT] -if got != [-1, -1, -1, -1]: +if got != [-1, -1, -1, -1, -1, -1]: error("on the identity type the orienting axes read %s, not none" % (got,)) mdi("G12.1 P0") mdi("G0 X0 Y0 Z0 B0 C0 W0") diff --git a/tests/ptp-robot/test-ui.py b/tests/ptp-robot/test-ui.py index 375de49eaf2..4c46e26de99 100755 --- a/tests/ptp-robot/test-ui.py +++ b/tests/ptp-robot/test-ui.py @@ -162,5 +162,24 @@ def check_held(what, before, after, moved): % (["%.4f" % v for v in s.position[:3]], ["%.4f" % v for v in here])) print("G53.4 back to the same point %s" % " ".join("%.4f" % v for v in back)) +# the wrist turns the tool with three joints: the arm kinematics orients +# with the A B C of its pose, all three head +mdi("G13.1") +drain() +c.mdi("(DEBUG,#<_kins_orient_1> #<_kins_orient_2> #<_kins_orient_3>" + " #<_kins_orient_1_head> #<_kins_orient_2_head> #<_kins_orient_3_head>)") +c.wait_complete(30) +said = None +deadline = time.time() + 5 +while said is None and time.time() < deadline: + m = e.poll() + if not m: + time.sleep(0.01) + elif m[0] == linuxcnc.OPERATOR_DISPLAY: + said = [float(v) for v in m[1].split()] +if said != [3, 4, 5, 1, 1, 1]: + error("the arm kinematics orients with %s, not A B C, heads" % (said,)) +print("orienting axes %s" % said) + print("Exiting with %d errors" % errors) sys.exit(1 if errors else 0) diff --git a/tests/remap/introspect/expected b/tests/remap/introspect/expected index 37173c3fa16..f7dfe04cee5 100644 --- a/tests/remap/introspect/expected +++ b/tests/remap/introspect/expected @@ -29,8 +29,8 @@ speed= 3000.0 global parameter set in test.ngc: 47.11 parameter set via test.ini: 3.14159 locals: ['a_new_local'] -globals: ['_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_orient_1', '_kins_orient_1_head', '_kins_orient_2', '_kins_orient_2_head', '_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_orient_1', '_kins_orient_1_head', '_kins_orient_2', '_kins_orient_2_head', '_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'] +globals: ['_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_orient_1', '_kins_orient_1_head', '_kins_orient_2', '_kins_orient_2_head', '_kins_orient_3', '_kins_orient_3_head', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_orient_rot1', '_orient_rot2', '_orient_rot3', '_orient_valid', '_orient_x', '_orient_y', '_orient_z', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] +params(): ['a_new_local', '_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_kins_orient_1', '_kins_orient_1_head', '_kins_orient_2', '_kins_orient_2_head', '_kins_orient_3', '_kins_orient_3_head', '_kins_type', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_orient_rot1', '_orient_rot2', '_orient_rot3', '_orient_valid', '_orient_x', '_orient_y', '_orient_z', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo'] 14 N..... MESSAGE(" after introspect: return value=2.718280 call_level= 0.000000") 15 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) 16 N..... SET_XY_ROTATION(0.0000) diff --git a/tests/twp-native/test-ui.py b/tests/twp-native/test-ui.py index 670c1e6d75b..bcf5489e19d 100755 --- a/tests/twp-native/test-ui.py +++ b/tests/twp-native/test-ui.py @@ -306,14 +306,15 @@ def in_plane(): 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 +# read the pose back through the parameters: the rotaries come in the +# order the type orients with, the A table, then B and C, and a move to +# them is a move to the present B and C, with the table held before3 = mdi("G0 B0 C0") -after3, samples = sampled("G0 B#<_orient_b> C#<_orient_c>") -show("G0 to #<_orient_b/c>", after3) +after3, samples = sampled("G0 B#<_orient_rot2> C#<_orient_rot3>") +show("G0 to #<_orient_rot2/rot3>", after3) drain() if abs(wrap(after3[SECONDARY] - stay[SECONDARY])) > 1e-3 or abs(wrap(after3[PRIMARY] - stay[PRIMARY])) > 1e-3: - error("#<_orient_b> #<_orient_c> held (%.4f, %.4f), G53.6 had reached (%.4f, %.4f)" + error("#<_orient_rot2> #<_orient_rot3> held (%.4f, %.4f), G53.6 had reached (%.4f, %.4f)" % (after3[SECONDARY], after3[PRIMARY], stay[SECONDARY], stay[PRIMARY])) if not close(tool_axis(after3), list(R2[:, 2]), 1e-6): error("the tool axis at the pose G53.2 published is not the plane normal") @@ -331,11 +332,26 @@ def in_plane(): 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>") +# the axes the type orients with, and the first of them, the table, where +# it stands; the numbered parameters past the rotaries read 0 +c.mdi("(DEBUG,#<_kins_orient_1> #<_kins_orient_2> #<_kins_orient_3>" + " #<_kins_orient_1_head> #<_kins_orient_2_head> #<_kins_orient_3_head>" + " #<_orient_rot1> #5074 #5077 #5078 #5079)") c.wait_complete(30) -m = e.poll() -if m and m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): - error("#<_orient_a> after G53.2: %s" % m[1]) +deadline = time.time() + 5 +said = None +while said is None and time.time() < deadline: + m = e.poll() + if not m: + time.sleep(0.01) + elif m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reading the orienting axes after G53.2: %s" % m[1]) + break + elif m[0] == linuxcnc.OPERATOR_DISPLAY: + said = [float(v) for v in m[1].split()] +want = [3, 4, 5, 0, 1, 1, stay[TABLE], stay[TABLE], 0, 0, 0] +if said is None or len(said) != len(want) or not close(said, want, 1e-3): + error("the orienting axes, rot1, #5074 and #5077-#5079 read %s, not %s" % (said, want)) drain() # --- the orientation stays inside the rotary travel -------------------- From 58ea0a1b032c8aea26cc0579511ecdad0774182b Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 24 Sep 2026 15:41:26 +1000 Subject: [PATCH 74/77] twinspindlekins: a mill-turn whose two spindles are two primary types A head on X Y Z tilts the tool on B between a main spindle turning its part on C and a sub spindle facing it along Z and turning its own part on W, an ANGULAR axis. Type 0 works in the frame of the main spindle's part and orients the tool with C and B, type 1 in the frame of the sub spindle's part, the main one turned half a turn about X so its Z too points at the tool, and orients it with W and B; both are primary, so G12.1 picks the spindle and a program reads the same on either part. Type 2 is the identity and the machine frame. Written as pure functions of the parameter block, with work and tool frames and the orienting letters declared, so the interpreter solves the tilted work plane on either spindle. The joints read the gauge line with B at 0 and the tool length goes along the tool, as on 5axiskins. tests/kins-params checks both working types against realtime. --- docs/src/man/man9/kins.9.adoc | 34 ++++ docs/src/motion/switchkins.adoc | 5 + src/Makefile | 10 + src/emc/kinematics/twinspindlekins.c | 278 +++++++++++++++++++++++++++ tests/kins-params/test.sh | 12 ++ 5 files changed, 339 insertions(+) create mode 100644 src/emc/kinematics/twinspindlekins.c diff --git a/docs/src/man/man9/kins.9.adoc b/docs/src/man/man9/kins.9.adoc index 01cef58cc73..78902a6f2a6 100644 --- a/docs/src/man/man9/kins.9.adoc +++ b/docs/src/man/man9/kins.9.adoc @@ -44,6 +44,8 @@ kins, genhexkins, genserkins, maxkins, pentakins, pumakins, rotatekins, scarakin *loadrt 5axiskins* +*loadrt twinspindlekins* + == DESCRIPTION Rather than exporting HAL pins and functions, these components provide @@ -426,6 +428,38 @@ expected by it (XYZBCW `->` joints 0..5) one rather than replacing it. Put a given length in one column or the other, not both. +=== twinspindlekins - mill-turn with two spindles + +XYZBCW -- a head on X Y Z tilts the tool on B, about Y, between a main +spindle turning its part on C and a sub spindle facing it along Z and +turning its own part on W. Declare W `TYPE = ANGULAR` in its `[AXIS_W]` +section; C and W may be `WRAPPED_ROTARY`. Coordinates XYZBCW are required; +A, U and V may be added with the coordinates parameter and map one to one +to their joints. + +Type 0 works in the frame of the part in the main spindle, and orients the +tool with C and B. Type 1 works in the frame of the part in the sub spindle, +which is the main spindle's turned half a turn about X, so its Z too points +out of the face towards the tool, and orients the tool with W and B. Both +are primary: G12.1 selects the spindle, and a program runs the same on +either part. Type 2 is the identity and the machine frame. The rotary +directions are the conventional ones. + +With B at 0 the tool points along -Z and the slides read the gauge line; the +tool length is applied along the tool, as on 5axiskins. The sim config +configs/sim/axis/vismach/twinspindle drills one subroutine's holes into +both parts. + +*twinspindlekins.pivot-length*:: + Distance from the B rotation point to the tool holder gauge line. + +*twinspindlekins.tool-length*:: + Tool length, applied along the tool. Motion hands the module the offset + in effect (G43, G49), so the pin needs no connection. + +*twinspindlekins.spindle-distance*:: + Distance along Z from the main spindle face to the sub spindle face. + == SEE ALSO For additional information, see following subsections of the section 'Advanced Topics' of the LinuxCNC documentation: diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index cca9b92c08d..a22620953c6 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -50,6 +50,7 @@ The following kinematics modules support switchable kinematics: . *xyzab_tdr_kins* (type0:identity type1:tcp) . *xyzacb_trsrn* (type0:identity type1:tcp type2:tool) . *xyzbca_trsrn* (type0:identity type1:tcp type2:tool) +. *twinspindlekins* (type0:main spindle type1:sub spindle type2:identity) Every module listed above uses its own kinematics for type0 and identity kinematics for type1. Each accepts the module string @@ -72,6 +73,9 @@ type0 is then reached without running the failing kinematics again. The 'sparm' setting exchanges type0 and type1, so any G-code or HAL logic that selects a kinematics type by number must match. +twinspindlekins has no 'sparm': its type0 and type1 are both working +kinematics, one per spindle, and type2 is the identity. + Kinematics that solve the forward direction iteratively, genhexkins among them, need a pose to start from. While another type is running they take the estimate the caller supplies, which motion seeds from @@ -463,6 +467,7 @@ configs/sim/axis/vismach/ . . 5axis/table-dual-rotary/xyzab-tdr.ini (xyzab_tdr_kins) . 5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini (xyzacb_trsrn) . 5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini (xyzbca_trsrn) +. twinspindle/twinspindle.ini (twinspindlekins) == User kinematics provisions diff --git a/src/Makefile b/src/Makefile index ae4fbe8c88c..9aa359ac67d 100644 --- a/src/Makefile +++ b/src/Makefile @@ -1281,6 +1281,15 @@ obj-m += 5axiskins.o 5axiskins-objs += emc/kinematics/switchkins_main.o 5axiskins-objs += emc/kinematics/switchkins_setup.o 5axiskins-objs += $(USERKFUNCS) + +obj-m += twinspindlekins.o +twinspindlekins-objs := emc/kinematics/twinspindlekins.o +twinspindlekins-objs += libposemath/_posemath.o +twinspindlekins-objs += $(MATHSTUB) +twinspindlekins-objs += emc/kinematics/kins_util.o +twinspindlekins-objs += emc/kinematics/switchkins.o +twinspindlekins-objs += emc/kinematics/switchkins_main.o +twinspindlekins-objs += emc/kinematics/switchkins_setup.o #---------------------------------------------------------------- obj-$(CONFIG_MOTMOD) += motmod.o @@ -1445,6 +1454,7 @@ endif ../rtlib/homemod$(MODULE_EXT): $(addprefix objects/rt,$(homemod-objs)) ../rtlib/trivkins$(MODULE_EXT): $(addprefix objects/rt,$(trivkins-objs)) ../rtlib/5axiskins$(MODULE_EXT): $(addprefix objects/rt,$(5axiskins-objs)) +../rtlib/twinspindlekins$(MODULE_EXT): $(addprefix objects/rt,$(twinspindlekins-objs)) ../rtlib/maxkins$(MODULE_EXT): $(addprefix objects/rt,$(maxkins-objs)) ../rtlib/rotatekins$(MODULE_EXT): $(addprefix objects/rt,$(rotatekins-objs)) ../rtlib/tripodkins$(MODULE_EXT): $(addprefix objects/rt,$(tripodkins-objs)) diff --git a/src/emc/kinematics/twinspindlekins.c b/src/emc/kinematics/twinspindlekins.c new file mode 100644 index 00000000000..97019c25305 --- /dev/null +++ b/src/emc/kinematics/twinspindlekins.c @@ -0,0 +1,278 @@ +/******************************************************************** +* Description: twinspindlekins.c +* kinematics for a mill-turn with a tilting head and two spindles +* +* License: GPL Version 2 +* +* Notes: +* 1) The head carries the tool on X Y Z and tilts it on B, about Y, from +* a pivot pivot-length above the gauge line. With B at 0 the tool +* points along -Z, tool axis +Z, and the slides read the gauge line. +* The pose is the tip plus the tool length along the part's Z, which +* G43 takes off again, so a tilt moves the tip by the length along +* the tool axis less the length along Z the offset stands for. +* 2) The main spindle turns the work about machine Z on C, its face in +* the plane Z 0. The sub spindle faces it from spindle-distance along +* +Z and turns its own work on W, which the INI makes ANGULAR. +* 3) Type 0 works on the part in the main spindle and type 1 on the part +* in the sub spindle, each in that part's own frame: X Y Z of the part +* and B, C or W the orientation. Both are primary: G12.1 picks the +* spindle. Type 2 is the identity and stands in for the machine frame. +* 4) The sub spindle part frame is the main spindle's turned half a turn +* about X: X stays, Y and Z reverse, so Z still points out of the face +* towards the tool. Moved to the other spindle, a program reads the +* same. +* 5) The rotary directions are the conventional ones: C and W turn the +* work the way that turns the tool positively about the part's Z. +* 6) Coordinates XYZBCW are required, A U V may be added with the +* coordinates parameter and map one to one to their joints. +********************************************************************/ + +#define REQUIRED_COORDINATES "XYZBCW" + +#include +#include +#include +#include + +#include + +static const kins_param_desc twin_params[] = { + { "pivot-length", KINS_PARAM_FLOAT, KINS_IN, 0, 150.0 }, + { "tool-length", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + { "spindle-distance", KINS_PARAM_FLOAT, KINS_IN, 0, 500.0 }, +}; +enum { P_PIVOT_LENGTH, P_TOOL_LENGTH, P_SPINDLE_DISTANCE }; + +#define JX (p->joint_of_axis[0]) +#define JY (p->joint_of_axis[1]) +#define JZ (p->joint_of_axis[2]) +#define JA (p->joint_of_axis[3]) +#define JB (p->joint_of_axis[4]) +#define JC (p->joint_of_axis[5]) +#define JU (p->joint_of_axis[6]) +#define JV (p->joint_of_axis[7]) +#define JW (p->joint_of_axis[8]) + +// the tool tip in the machine frame, from the slides and B (note 1) +static PmCartesian tip_from_joints(const kins_params *p, const double *joints) +{ + const double P = p->geometry[P_PIVOT_LENGTH]; + const double L = P + p->tool.tran.z; + const double b = joints[JB]*TO_RAD; + PmCartesian t; + + t.x = joints[JX] - L*sin(b); + t.y = joints[JY]; + t.z = joints[JZ] + P - L*cos(b); + return t; +} // tip_from_joints() + +// the part frame of each spindle, the machine seen from the part: machine +// = O + R part, with R = Rz(-C) for the main spindle and Rx(180) Rz(-W) for +// the sub spindle (notes 2, 4, 5) +static PmCartesian part_from_machine(const kins_params *p, int sub, + double angle, PmCartesian m) +{ + const double s = sin(angle*TO_RAD), c = cos(angle*TO_RAD); + PmCartesian q, r; + + q = m; + if (sub) { + q.y = -m.y; + q.z = p->geometry[P_SPINDLE_DISTANCE] - m.z; + } + r.x = c*q.x - s*q.y; + r.y = s*q.x + c*q.y; + r.z = q.z; + return r; +} // part_from_machine() + +static PmCartesian machine_from_part(const kins_params *p, int sub, + double angle, PmCartesian r) +{ + const double s = sin(angle*TO_RAD), c = cos(angle*TO_RAD); + PmCartesian q, m; + + q.x = c*r.x + s*r.y; + q.y = -s*r.x + c*r.y; + q.z = r.z; + m = q; + if (sub) { + m.y = -q.y; + m.z = p->geometry[P_SPINDLE_DISTANCE] - q.z; + } + return m; +} // machine_from_part() + +static int twin_forward(const kins_params *p, int sub, const double *joints, + EmcPose *pos) +{ + const double angle = sub ? joints[JW] : joints[JC]; + + pos->tran = part_from_machine(p, sub, angle, tip_from_joints(p, joints)); + pos->tran.z += p->tool.tran.z; + pos->b = joints[JB]; + pos->c = joints[JC]; + pos->w = joints[JW]; + + // optional letters (specify with coordinates module parameter) + pos->a = (JA != -1)? joints[JA] : 0; + pos->u = (JU != -1)? joints[JU] : 0; + pos->v = (JV != -1)? joints[JV] : 0; + + return 0; +} // twin_forward() + +static int twin_inverse(const kins_params *p, int sub, const EmcPose *pos, + double *joints) +{ + const double T = p->tool.tran.z; + const double L = p->geometry[P_PIVOT_LENGTH] + T; + const double b = pos->b*TO_RAD; + PmCartesian tip = pos->tran; + PmCartesian t; + EmcPose P; // computed position + + tip.z -= T; + t = machine_from_part(p, sub, sub ? pos->w : pos->c, tip); + P.tran.x = t.x + L*sin(b); + P.tran.y = t.y; + P.tran.z = t.z - p->geometry[P_PIVOT_LENGTH] + L*cos(b); + P.b = pos->b; + P.c = pos->c; + P.w = pos->w; + + // optional letters (specify with coordinates module parameter) + P.a = (JA != -1)? pos->a : 0; + P.u = (JU != -1)? pos->u : 0; + P.v = (JV != -1)? pos->v : 0; + + return kinsPoseToMappedJoints(p, &P, joints); +} // twin_inverse() + +static int main_forward(const kins_params *p, kins_scratch *s, + const double *joints, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)s; (void)fflags; (void)iflags; + return twin_forward(p, 0, joints, pos); +} + +static int main_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)s; (void)iflags; (void)fflags; + return twin_inverse(p, 0, pos, joints); +} + +static int sub_forward(const kins_params *p, kins_scratch *s, + const double *joints, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)s; (void)fflags; (void)iflags; + return twin_forward(p, 1, joints, pos); +} + +static int sub_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)s; (void)iflags; (void)fflags; + return twin_inverse(p, 1, pos, joints); +} + +// the tool frame, Ry(B), is the same on both spindles +static int twin_tool_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + const double sb = sin(joints[JB]*TO_RAD), cb = cos(joints[JB]*TO_RAD); + + rot->x.x = cb; rot->y.x = 0; rot->z.x = sb; + rot->x.y = 0; rot->y.y = 1; rot->z.y = 0; + rot->x.z = -sb; rot->y.z = 0; rot->z.z = cb; + + return 0; +} // twin_tool_frame() + +// the work frames, the R of part_from_machine(), its columns the part axes +static int main_work_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + const double s = sin(joints[JC]*TO_RAD), c = cos(joints[JC]*TO_RAD); + + rot->x.x = c; rot->y.x = s; rot->z.x = 0; + rot->x.y = -s; rot->y.y = c; rot->z.y = 0; + rot->x.z = 0; rot->y.z = 0; rot->z.z = 1; + + return 0; +} // main_work_frame() + +static int sub_work_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + const double s = sin(joints[JW]*TO_RAD), c = cos(joints[JW]*TO_RAD); + + rot->x.x = c; rot->y.x = s; rot->z.x = 0; + rot->x.y = s; rot->y.y = -c; rot->z.y = 0; + rot->x.z = 0; rot->y.z = 0; rot->z.z = -1; + + return 0; +} // sub_work_frame() + +// each spindle orients with its own rotary, the table first, and B; the +// joint map gives the same letters, declared so the table reads +static const kins_ops main_ops = { + .forward = main_forward, + .inverse = main_inverse, + .work = main_work_frame, + .tool = twin_tool_frame, + .native = &TOOL_FRAME_SPINDLE, + .primary = 1, + .orient = "CB", +}; + +static const kins_ops sub_ops = { + .forward = sub_forward, + .inverse = sub_inverse, + .work = sub_work_frame, + .tool = twin_tool_frame, + .native = &TOOL_FRAME_SPINDLE, + .primary = 1, + .orient = "WB", +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "twinspindlekins"; // !!! must agree with filename + kp->halprefix = "twinspindlekins"; // hal pin names + kp->required_coordinates = REQUIRED_COORDINATES; + kp->allow_duplicates = 1; + kp->max_joints = EMCMOT_MAX_JOINTS; + kp->params = twin_params; + kp->nparams = sizeof(twin_params)/sizeof(twin_params[0]); + + switchkinsRegisterOps(0, &main_ops); + switchkinsRegisterOps(1, &sub_ops); + switchkinsRegisterOps(2, &KINS_IDENTITY_OPS); + + return 0; +} // switchkinsSetup() diff --git a/tests/kins-params/test.sh b/tests/kins-params/test.sh index e429bf5029c..168189d2f8b 100755 --- a/tests/kins-params/test.sh +++ b/tests/kins-params/test.sh @@ -79,6 +79,18 @@ run "maxkins" \ 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 +# a table and a head, two primaries: one rotary turns the tool, so no pair +run "twinspindlekins coordinates=XYZBCW" \ + "setp twinspindlekins.pivot-length 120 +setp twinspindlekins.tool-length 30 +setp twinspindlekins.spindle-distance 400" \ + "joints=6 jnt=10,20,30,15,25,35 orient=-" +run "twinspindlekins coordinates=XYZBCW" \ + "setp twinspindlekins.pivot-length 120 +setp twinspindlekins.tool-length 30 +setp twinspindlekins.spindle-distance 400" \ + "joints=6 jnt=10,20,30,15,25,35 orient=-" 1 + run "xyzac-trt-kins coordinates=XYZAC" \ "setp xyzac-trt-kins.y-offset 3 setp xyzac-trt-kins.z-offset 11 From 5848844d1c623ba108dab292ac4d98ecacc4b36f Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 24 Sep 2026 15:41:26 +1000 Subject: [PATCH 75/77] sim: twinspindle, one subroutine drilling the parts in both spindles A vismach sim of twinspindlekins, the spindle line across the screen, with a demo program that drills a ring of tilted holes into the part in the main spindle and then into the part in the sub spindle through one subroutine. It names no rotary: G53.3 turns the ones the type in force orients with, and the report reads them as #<_kins_orient_1> and #<_kins_orient_2>, C and B on one spindle, W and B on the other, with their angles as #<_orient_rot1> and #<_orient_rot2>. tests/twinspindle checks the orienting axes of each type, that the same tilted hole solves and is reached on either spindle, the tip on the hole and the tool along its axis, and that the demo program runs. --- configs/sim/axis/vismach/twinspindle/README | 33 ++++ .../twinspindle/twin-spindle-holes.ngc | 64 ++++++ .../axis/vismach/twinspindle/twinspindle.hal | 15 ++ .../axis/vismach/twinspindle/twinspindle.ini | 160 +++++++++++++++ .../axis/vismach/twinspindle/twinspindle.tbl | 1 + .../vismach/twinspindle/twinspindlegui.py | 102 ++++++++++ tests/twinspindle/README | 12 ++ tests/twinspindle/checkresult | 3 + tests/twinspindle/sim.hal | 16 ++ tests/twinspindle/test-ui.py | 182 ++++++++++++++++++ tests/twinspindle/test.ini | 139 +++++++++++++ tests/twinspindle/test.sh | 4 + tests/twinspindle/tool.tbl | 1 + 13 files changed, 732 insertions(+) create mode 100644 configs/sim/axis/vismach/twinspindle/README create mode 100644 configs/sim/axis/vismach/twinspindle/twin-spindle-holes.ngc create mode 100644 configs/sim/axis/vismach/twinspindle/twinspindle.hal create mode 100644 configs/sim/axis/vismach/twinspindle/twinspindle.ini create mode 100644 configs/sim/axis/vismach/twinspindle/twinspindle.tbl create mode 100755 configs/sim/axis/vismach/twinspindle/twinspindlegui.py create mode 100644 tests/twinspindle/README create mode 100755 tests/twinspindle/checkresult create mode 100644 tests/twinspindle/sim.hal create mode 100755 tests/twinspindle/test-ui.py create mode 100644 tests/twinspindle/test.ini create mode 100755 tests/twinspindle/test.sh create mode 100644 tests/twinspindle/tool.tbl diff --git a/configs/sim/axis/vismach/twinspindle/README b/configs/sim/axis/vismach/twinspindle/README new file mode 100644 index 00000000000..b6dad63183b --- /dev/null +++ b/configs/sim/axis/vismach/twinspindle/README @@ -0,0 +1,33 @@ +A mill-turn with a tilting head between two spindles, on twinspindlekins. + +The head carries the tool on X Y Z and tilts it on B, about Y. The main +spindle turns its part on C; the sub spindle faces it along Z, 500 away, +and turns its own part on W. W is an angle, so [AXIS_W] declares it +TYPE = ANGULAR, and both spindles are WRAPPED_ROTARY. The vismach window +draws the spindle line across the screen as on a lathe, X up. + +Kinematics type 0 works on the part in the main spindle and orients the +tool with C and B; type 1 works on the part in the sub spindle and orients +it with W and B. Each part's frame has its Z out of its own spindle's face, +so a program reads the same on either part. Type 2 is the identity, the +machine frame, which G13.1 selects. + +Demo program: + + twin-spindle-holes.ngc -- one subroutine drills a ring of six holes, + each tilted 20 degrees out from the part axis, + into the face of the part the kinematics type + in force works on; the program runs it on the + main spindle (G12.1 P0, G54) and then on the + sub spindle (G12.1 P1, G55). The subroutine + names no rotary: G53.3 turns the ones the type + orients with, and the report on the terminal + reads them by number, #<_kins_orient_1> and + #<_kins_orient_2>, 5 and 4 (C and B) on the + main spindle, 8 and 4 (W and B) on the sub + spindle, with their angles #<_orient_rot1> and + #<_orient_rot2>. + +The tool table provides tool 1, length 50. The drawing hands its pivot +length and spindle distance to the kinematics (twinspindle.hal), so the +two agree. diff --git a/configs/sim/axis/vismach/twinspindle/twin-spindle-holes.ngc b/configs/sim/axis/vismach/twinspindle/twin-spindle-holes.ngc new file mode 100644 index 00000000000..86b120350c1 --- /dev/null +++ b/configs/sim/axis/vismach/twinspindle/twin-spindle-holes.ngc @@ -0,0 +1,64 @@ +; twin-spindle-holes.ngc - one subroutine drills the same ring of tilted +; holes into the part in the main spindle and into the part in the sub +; spindle. +; +; Kinematics type 0 works on the main spindle's part and orients the tool +; with C and B; type 1 works on the sub spindle's part and orients it with +; W and B. The subroutine names neither: G53.3 turns whatever rotaries the +; type in force orients with, and the report reads them by number, +; #<_kins_orient_1> and #<_kins_orient_2>, with their angles in the same +; order, #<_orient_rot1> and #<_orient_rot2>. Each part's frame has its Z +; out of its own spindle, so both faces are at Z60 and the offsets read +; the same. + +o sub + # = 6 ; holes + # = 30 ; hole circle radius on the face + # = 20 ; tilt of each hole out from the part axis + # = 0 + o101 while [# lt #] + # = [# * 360 / #] + ; the hole's plane: origin on the face, X along the hole circle, + ; Z the hole axis + g68.2 p3 q1 x[#*cos[#]] y[#*sin[#]] z0 i[-sin[#]] j[cos[#]] k0 + g68.2 p3 q2 i[sin[#]*cos[#]] j[sin[#]*sin[#]] k[cos[#]] + g53.2 ; solve the pose, report it + (print, hole #: axis #<_kins_orient_1> to #<_orient_rot1>, axis #<_kins_orient_2> to #<_orient_rot2>) + g53.3 x0 y0 z5 ; orient and go, 5 above the hole + g1 z-10 + g0 z5 + # = [# + 1] + o101 endwhile + g0 z40 + g69 +o endsub + +; from one part to the other in the machine frame, the identity type: +; up, clear of both parts, and the head pointing down +o sub + g13.1 + g53 g0 x300 + g53 g0 z50 b90 +o endsub + +g21 g90 g94 g17 g40 +t1 m6 +g43 +f300 + +g10 l2 p1 x0 y0 z60 c0 w0 ; G54, the main spindle's part +g10 l2 p2 x0 y0 z60 c0 w0 ; G55, the sub spindle's part + +o call +g12.1 p0 ; the main spindle's part +g54 +o call + +o call +g12.1 p1 ; the sub spindle's part +g55 +o call + +o call +g54 +m2 diff --git a/configs/sim/axis/vismach/twinspindle/twinspindle.hal b/configs/sim/axis/vismach/twinspindle/twinspindle.hal new file mode 100644 index 00000000000..16044ff4de1 --- /dev/null +++ b/configs/sim/axis/vismach/twinspindle/twinspindle.hal @@ -0,0 +1,15 @@ +loadusr -W ./twinspindlegui.py + +net :jx joint.0.pos-fb twinspindlegui.jx +net :jy joint.1.pos-fb twinspindlegui.jy +net :jz joint.2.pos-fb twinspindlegui.jz +net :jb joint.3.pos-fb twinspindlegui.jb +net :jc joint.4.pos-fb twinspindlegui.jc +net :jw joint.5.pos-fb twinspindlegui.jw + +# the drawing and the kinematics share the geometry +net :pivot-len twinspindlegui.pivot_len twinspindlekins.pivot-length +net :spindle-distance twinspindlegui.spindle_distance twinspindlekins.spindle-distance + +net :tool-len motion.tooloffset.z twinspindlegui.tool_length +net :tool-diam halui.tool.diameter twinspindlegui.tool_diam diff --git a/configs/sim/axis/vismach/twinspindle/twinspindle.ini b/configs/sim/axis/vismach/twinspindle/twinspindle.ini new file mode 100644 index 00000000000..438e0a85fd4 --- /dev/null +++ b/configs/sim/axis/vismach/twinspindle/twinspindle.ini @@ -0,0 +1,160 @@ +[EMC] +VERSION = 1.1 +MACHINE = Sim-twinspindlekins (XYZBCW mill-turn, two spindles) + DEBUG = 0 + +[DISPLAY] +# X Y Z are already the tip in the frame of the part the type works on, so +# the plot takes them as they are: a rotary in GEOMETRY would turn them +# again, and W, a translation there, would draw as a move along Z + GEOMETRY = XYZ + DISPLAY = axis + OPEN_FILE = ./twin-spindle-holes.ngc + INCREMENTS = 10 mm, 1 mm, .1 mm + JOG_AXES = XYZ + CYCLE_TIME = 0.100 + POSITION_OFFSET = RELATIVE +POSITION_FEEDBACK = ACTUAL +MAX_FEED_OVERRIDE = 2.0 + PROGRAM_PREFIX = ../../nc_files/ + INTRO_GRAPHIC = linuxcnc.gif + INTRO_TIME = 2 + TOOL_EDITOR = tooledit z diam + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G80 G90 G94 G97 + PARAMETER_FILE = twinspindle.var + +[EMCMOT] + EMCMOT = motmod +COMM_TIMEOUT = 1.0 +SERVO_PERIOD = 1000000 + +[TASK] + TASK = milltask +CYCLE_TIME = 0.010 + +[HAL] + HALUI = halui +HALFILE = LIB:basic_sim.tcl +HALFILE = twinspindle.hal + +[TRAJ] + COORDINATES = XYZBCW + LINEAR_UNITS = mm + ANGULAR_UNITS = degree +DEFAULT_LINEAR_VELOCITY = 50.0 + MAX_LINEAR_VELOCITY = 200.0 +MAX_LINEAR_ACCELERATION = 800.0 +DEFAULT_LINEAR_ACCELERATION = 800.0 + MAX_ANGULAR_VELOCITY = 360.0 + +[EMCIO] +TOOL_TABLE = twinspindle.tbl + +[KINS] +# type 0: the part in the main spindle, C and B orient +# type 1: the part in the sub spindle, W and B orient +# type 2: identity, the machine frame +KINEMATICS = twinspindlekins + JOINTS = 6 + +[AXIS_X] + MIN_LIMIT = -400 + MAX_LIMIT = 500 + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Y] + MIN_LIMIT = -300 + MAX_LIMIT = 300 + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Z] + MIN_LIMIT = -300 + MAX_LIMIT = 800 + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_B] + MIN_LIMIT = -30 + MAX_LIMIT = 210 + MAX_VELOCITY = 90 +MAX_ACCELERATION = 400 + +# the main spindle +[AXIS_C] + WRAPPED_ROTARY = 1 + MIN_LIMIT = -1e9 + MAX_LIMIT = 1e9 + MAX_VELOCITY = 180 +MAX_ACCELERATION = 720 + +# the sub spindle: an angle, so ANGULAR, however the letter reads +[AXIS_W] + TYPE = ANGULAR + WRAPPED_ROTARY = 1 + MIN_LIMIT = -1e9 + MAX_LIMIT = 1e9 + MAX_VELOCITY = 180 +MAX_ACCELERATION = 720 + +# the joints home to the head over the middle, pointing down at the +# spindle line, clear of both parts +[JOINT_0] + TYPE = LINEAR + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + MIN_LIMIT = -400 + MAX_LIMIT = 500 + HOME = 300 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_1] + TYPE = LINEAR + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + MIN_LIMIT = -300 + MAX_LIMIT = 300 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_2] + TYPE = LINEAR + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + MIN_LIMIT = -300 + MAX_LIMIT = 800 + HOME = 50 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_3] + TYPE = ANGULAR + MAX_VELOCITY = 90 +MAX_ACCELERATION = 400 + MIN_LIMIT = -30 + MAX_LIMIT = 210 + HOME = 90 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_4] + TYPE = ANGULAR + MAX_VELOCITY = 180 +MAX_ACCELERATION = 720 + MIN_LIMIT = -1e9 + MAX_LIMIT = 1e9 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_5] + TYPE = ANGULAR + MAX_VELOCITY = 180 +MAX_ACCELERATION = 720 + MIN_LIMIT = -1e9 + MAX_LIMIT = 1e9 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 diff --git a/configs/sim/axis/vismach/twinspindle/twinspindle.tbl b/configs/sim/axis/vismach/twinspindle/twinspindle.tbl new file mode 100644 index 00000000000..569cf6b70b2 --- /dev/null +++ b/configs/sim/axis/vismach/twinspindle/twinspindle.tbl @@ -0,0 +1 @@ +T1 P1 Z50 D10 ;drill diff --git a/configs/sim/axis/vismach/twinspindle/twinspindlegui.py b/configs/sim/axis/vismach/twinspindle/twinspindlegui.py new file mode 100755 index 00000000000..eb128e95cea --- /dev/null +++ b/configs/sim/axis/vismach/twinspindle/twinspindlegui.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python3 +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. + +# The twinspindlekins machine: a B tilting head on X Y Z between a main +# spindle (C) and a sub spindle (W) that face each other along Z. The model +# is built in machine coordinates and turned at the end so that Z, the +# spindle line, runs across the screen as on a lathe, X up. +# +# pivot_len and spindle_distance are handed to the kinematics by +# twinspindle.hal, so the drawing and the maths agree; they can be set at +# invocation, example: twinspindlegui.py pivot_len=200 + +from vismach import * +import hal +import sys + +pivot_len = 150 # gauge line to the B axis +spindle_distance = 500 # main spindle face to sub spindle face +lat, lon = -80, 0 # the starting view, from the front + +for setting in sys.argv[1:]: + exec(setting) + +c = hal.component("twinspindlegui") +for pin in ("jx", "jy", "jz", "jb", "jc", "jw", "tool_length", "tool_diam"): + c.newpin(pin, hal.Type.REAL, hal.Dir.IN) +c.newpin("pivot_len", hal.Type.REAL, hal.Dir.OUT) +c.newpin("spindle_distance", hal.Type.REAL, hal.Dir.OUT) +c["pivot_len"] = pivot_len +c["spindle_distance"] = spindle_distance +c.ready() + +class HalToolCylinder(CylinderZ): + def __init__(self, comp, *args): + CylinderZ.__init__(self, *args) + self.comp = comp + + def coords(self): + r = 5 # default if hal pin not set + if c.tool_diam > 0: r = c.tool_diam/2 + return -self.comp.tool_length, r, 0, r + +# a chuck and a part, face at z 0, the part standing on +z; the flat on +# the part shows the spindle turning +def spindle(work=None): + part = [CylinderZ(0, 50, 60, 50), + Color([1, 0.2, 0.2, 1], [Box(40, -8, 0, 52, 8, 60)])] + if work: + part.append(Translate([work], 0, 0, 60)) + return Collection([Color([0.6, 0.6, 0.7, 1], [CylinderZ(-100, 90, 0, 90)]), + Color([0.9, 0.8, 0.5, 1], part)]) + +# the main spindle turns the work by -C: its frame is Rz(-C) +work = Capture() +main_spindle = HalRotate([spindle(work)], c, "jc", -1, 0, 0, 1) +main_spindle = Collection([main_spindle, + Box(-160, -160, -400, 160, 160, -100)]) + +# the sub spindle is the same, turned by -W, then half a turn about X to +# face the main spindle from spindle_distance +sub_spindle = HalRotate([spindle()], c, "jw", -1, 0, 0, 1) +sub_spindle = Collection([sub_spindle, + Box(-160, -160, -400, 160, 160, -100)]) +sub_spindle = Rotate([sub_spindle], 180, 1, 0, 0) +sub_spindle = HalTranslate([sub_spindle], c, "spindle_distance", 0, 0, 1) + +# the head, gauge line at z 0: the tool below it, the spindle motor above +# up to the B axis at pivot_len, which is then put at the origin +tooltip = Capture() +head = Collection([HalTranslate([tooltip], c, "tool_length", 0, 0, -1), + Color([0.9, 0.9, 0.2, 1], [HalToolCylinder(c)]), + CylinderZ(0, 30, 40, 45), + CylinderZ(40, 45, pivot_len - 45, 45), + Translate([CylinderY(-55, 55, 55, 55)], 0, 0, pivot_len)]) +head = Translate([head], 0, 0, -pivot_len) +head = HalRotate([head], c, "jb", 1, 0, 1, 0) + +# the ram carries the B axis from the side, along Y, clear of the head +# whichever way it tilts; the slides read the gauge line at B 0, pivot_len +# below the B axis +head = Collection([head, + Box(-50, 60, -50, 50, 700, 50)]) +head = Translate([head], 0, 0, pivot_len) +head = HalTranslate([head], c, "jx", 1, 0, 0) +head = HalTranslate([head], c, "jy", 0, 1, 0) +head = HalTranslate([head], c, "jz", 0, 0, 1) + +bed = Box(-420, -300, -450, -400, 900, 1000) + +model = Collection([bed, main_spindle, sub_spindle, head]) +# Z across the screen, X up +model = Rotate([model], -90, 0, 1, 0) + +main(model, tooltip, work, size=900, lat=lat, lon=lon) diff --git a/tests/twinspindle/README b/tests/twinspindle/README new file mode 100644 index 00000000000..c6e5486836b --- /dev/null +++ b/tests/twinspindle/README @@ -0,0 +1,12 @@ +twinspindlekins: a B tilting head between a main spindle on C and a sub +spindle on W, W ANGULAR, each spindle a primary kinematics type in the +frame of its own part, and an identity type. + +The test checks that each type names its own rotaries on +#<_kins_orient_1> to #<_kins_orient_3> and their head flags, C then B on +the main spindle, W then B on the sub spindle, nothing on the identity; +that G53.2 and G53.3 on the same tilted hole give the spindle angle and a +B on either side of 180 as #<_orient_rot1> and #<_orient_rot2>, and put +the tip on the hole with the tool along its axis, worked out from the +joints; and that the sim config's demo program drills both parts through +one subroutine and parks. diff --git a/tests/twinspindle/checkresult b/tests/twinspindle/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/twinspindle/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/twinspindle/sim.hal b/tests/twinspindle/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/twinspindle/sim.hal @@ -0,0 +1,16 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/twinspindle/test-ui.py b/tests/twinspindle/test-ui.py new file mode 100755 index 00000000000..9bc2348cde2 --- /dev/null +++ b/tests/twinspindle/test-ui.py @@ -0,0 +1,182 @@ +#!/usr/bin/env python3 +# twinspindlekins: two primary types, one per spindle, orienting with +# different rotaries: see README. + +import linuxcnc +import math +import os +import sys +import time + +JOINTS = 6 +PIVOT = 150.0 # twinspindlekins.pivot-length default +DISTANCE = 500.0 # twinspindlekins.spindle-distance default +TOOL = 50.0 # T1 in tool.tbl +FACE = 60.0 # G54 and G55 Z, the part faces +PROGRAM = os.path.abspath("../../configs/sim/axis/vismach/twinspindle/twin-spindle-holes.ngc") + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.home(-1) +c.wait_complete() +c.mode(linuxcnc.MODE_MDI) +c.wait_complete() + +errors = 0 + +def error(msg): + global errors + errors += 1 + print("*** ERROR " + msg) + +def drain(): + while True: + m = e.poll() + if not m: + return + print("channel:", m) + if m[0] in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): + error("reported: %s" % m[1]) + +def settled(): + deadline = time.time() + 60 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(JOINTS)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.05) + error("timed out waiting for the move") + return last + +def mdi(*cmds): + for cmd in cmds: + c.mdi(cmd) + c.wait_complete(60) + return settled() + +def param(name): + drain() + c.mdi("(debug,#<%s>)" % name) + c.wait_complete(30) + deadline = time.time() + 5 + while time.time() < deadline: + m = e.poll() + if not m: + time.sleep(0.01) + continue + if m[0] == linuxcnc.OPERATOR_DISPLAY: + return float(m[1]) + error("#<%s> gave no value" % name) + return None + +def turn(a, b): + # a and b the same angle, whole turns apart + return abs((a - b + 180.0) % 360.0 - 180.0) < 1e-6 + +# the module's maths, written out again: the tip and the tool axis, tip +# towards holder, in the frame of the part on spindle `sub`. The slides +# carry the B axis PIVOT above the gauge line, the tip hangs PIVOT + TOOL +# below it along the tool axis. +def part_pose(j, sub): + L = PIVOT + TOOL + b = math.radians(j[3]) + m = [j[0] - L*math.sin(b), j[1], j[2] + PIVOT - L*math.cos(b)] + u = [math.sin(b), 0.0, math.cos(b)] + if sub: + m = [m[0], -m[1], DISTANCE - m[2]] + u = [u[0], -u[1], -u[2]] + a = math.radians(j[5] if sub else j[4]) + rz = lambda v: [math.cos(a)*v[0] - math.sin(a)*v[1], + math.sin(a)*v[0] + math.cos(a)*v[1], v[2]] + return rz(m), rz(u) + +ORIENT = ("_kins_orient_1", "_kins_orient_2", "_kins_orient_3", + "_kins_orient_1_head", "_kins_orient_2_head", "_kins_orient_3_head") + +mdi("T1 M6", "G43", + "G10 L2 P1 X0 Y0 Z%g C0 W0" % FACE, + "G10 L2 P2 X0 Y0 Z%g C0 W0" % FACE) + +# ---- each type names its own rotaries -------------------------------------- + +for ktype, want, what in ((0, [5, 4, -1, 0, 1, -1], "C then B"), + (1, [8, 4, -1, 0, 1, -1], "W then B"), + (2, [-1] * 6, "nothing")): + mdi("G12.1 P%d" % ktype) + got = [param(n) for n in ORIENT] + if got != want: + error("type %d orients with %s, not %s: table then head" % (ktype, got, what)) + +# ---- the same hole on either spindle --------------------------------------- +# +# A hole 30 out on the face at 60 degrees, tilted 20 degrees outwards. On +# the main spindle C turns it under the head and B leans the tool; on the +# sub spindle, whose part faces the other way, W turns it and B leans the +# tool past 180. + +A, T, R = 60.0, 20.0, 30.0 +n = [math.sin(math.radians(T))*math.cos(math.radians(A)), + math.sin(math.radians(T))*math.sin(math.radians(A)), + math.cos(math.radians(T))] +hole = [R*math.cos(math.radians(A)), R*math.sin(math.radians(A)), 0.0] +PLANE = ("G68.2 P3 Q1 X%.9f Y%.9f Z0 I%.9f J%.9f K0" + % (hole[0], hole[1], -math.sin(math.radians(A)), math.cos(math.radians(A))), + "G68.2 P3 Q2 I%.9f J%.9f K%.9f" % tuple(n)) + +for ktype, offset, joint, b in ((0, "G54", 4, T), (1, "G55", 5, 180.0 - T)): + sub = ktype == 1 + mdi("G13.1", "G53 G0 X300", "G53 G0 Z50 B90") + mdi("G12.1 P%d" % ktype, offset, *PLANE) + mdi("G53.2") + rot1, rot2 = param("_orient_rot1"), param("_orient_rot2") + print("type %d: rot1 %.6f rot2 %.6f" % (ktype, rot1, rot2)) + if not (turn(rot1, A) and abs(rot2 - b) < 1e-6): + error("type %d: G53.2 gave rot1 %s rot2 %s, not %s and %s" % (ktype, rot1, rot2, A, b)) + j = mdi("G53.3 X0 Y0 Z5") + if not (turn(j[joint], A) and abs(j[3] - b) < 1e-6): + error("type %d: G53.3 left joint %d at %s and B at %s" % (ktype, joint, j[joint], j[3])) + tip, axis = part_pose(j, sub) + want = [hole[i] + 5*n[i] + (FACE if i == 2 else 0.0) for i in range(3)] + if max(abs(tip[i] - want[i]) for i in range(3)) > 1e-6: + error("type %d: the tip is at %s in the part, not %s" % (ktype, tip, want)) + if max(abs(axis[i] - n[i]) for i in range(3)) > 1e-9: + error("type %d: the tool points along %s in the part, not %s" % (ktype, axis, n)) + mdi("G69") +drain() + +# ---- the demo program, both spindles, one subroutine ----------------------- + +mdi("G13.1", "G54") +c.mode(linuxcnc.MODE_AUTO) +c.wait_complete() +c.program_open(PROGRAM) +c.auto(linuxcnc.AUTO_RUN, 0) +deadline = time.time() + 300 +time.sleep(0.5) +while time.time() < deadline: + s.poll() + if s.interp_state == linuxcnc.INTERP_IDLE and s.queue == 0 and s.inpos: + break + time.sleep(0.1) +else: + error("the demo program did not finish") +drain() +j = settled() +if max(abs(j[i] - w) for i, w in ((0, 300.0), (2, 50.0), (3, 90.0))) > 1e-6: + error("the demo program ended at %s, not parked" % (j,)) + +c.state(linuxcnc.STATE_ESTOP) +for f in ("sim.var", "sim.var.bak"): + try: + os.unlink(f) + except OSError: + pass +print("Exiting with %d errors" % errors) +sys.exit(errors != 0) diff --git a/tests/twinspindle/test.ini b/tests/twinspindle/test.ini new file mode 100644 index 00000000000..890edb69759 --- /dev/null +++ b/tests/twinspindle/test.ini @@ -0,0 +1,139 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G80 G90 G94 G97 +PARAMETER_FILE = sim.var + +[KINS] +KINEMATICS = twinspindlekins +JOINTS = 6 + +[HAL] +HALFILE = sim.hal + +[TRAJ] +COORDINATES = XYZBCW +LINEAR_UNITS = mm +ANGULAR_UNITS = degree +DEFAULT_LINEAR_VELOCITY = 50 +MAX_LINEAR_VELOCITY = 200 +MAX_LINEAR_ACCELERATION = 800 +DEFAULT_LINEAR_ACCELERATION = 800 +MAX_ANGULAR_VELOCITY = 360 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] + MIN_LIMIT = -400 + MAX_LIMIT = 500 + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Y] + MIN_LIMIT = -300 + MAX_LIMIT = 300 + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Z] + MIN_LIMIT = -300 + MAX_LIMIT = 800 + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_B] + MIN_LIMIT = -30 + MAX_LIMIT = 210 + MAX_VELOCITY = 90 +MAX_ACCELERATION = 400 + +# the main spindle +[AXIS_C] + WRAPPED_ROTARY = 1 + MIN_LIMIT = -1e9 + MAX_LIMIT = 1e9 + MAX_VELOCITY = 180 +MAX_ACCELERATION = 720 + +# the sub spindle: an angle, so ANGULAR, however the letter reads +[AXIS_W] + TYPE = ANGULAR + WRAPPED_ROTARY = 1 + MIN_LIMIT = -1e9 + MAX_LIMIT = 1e9 + MAX_VELOCITY = 180 +MAX_ACCELERATION = 720 + +# the joints home to the head over the middle, pointing down at the +# spindle line, clear of both parts +[JOINT_0] + TYPE = LINEAR + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + MIN_LIMIT = -400 + MAX_LIMIT = 500 + HOME = 300 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_1] + TYPE = LINEAR + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + MIN_LIMIT = -300 + MAX_LIMIT = 300 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_2] + TYPE = LINEAR + MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + MIN_LIMIT = -300 + MAX_LIMIT = 800 + HOME = 50 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_3] + TYPE = ANGULAR + MAX_VELOCITY = 90 +MAX_ACCELERATION = 400 + MIN_LIMIT = -30 + MAX_LIMIT = 210 + HOME = 90 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_4] + TYPE = ANGULAR + MAX_VELOCITY = 180 +MAX_ACCELERATION = 720 + MIN_LIMIT = -1e9 + MAX_LIMIT = 1e9 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 + +[JOINT_5] + TYPE = ANGULAR + MAX_VELOCITY = 180 +MAX_ACCELERATION = 720 + MIN_LIMIT = -1e9 + MAX_LIMIT = 1e9 + HOME_SEARCH_VEL = 0 + HOME_SEQUENCE = 0 diff --git a/tests/twinspindle/test.sh b/tests/twinspindle/test.sh new file mode 100755 index 00000000000..f494f0fc1fb --- /dev/null +++ b/tests/twinspindle/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +# a failed run leaves the var file behind, and it carries the offsets +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/twinspindle/tool.tbl b/tests/twinspindle/tool.tbl new file mode 100644 index 00000000000..569cf6b70b2 --- /dev/null +++ b/tests/twinspindle/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z50 D10 ;drill From 1a264aaeaa142bee8903fbff12b07b7ecd1343f7 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 26 Sep 2026 22:35:50 +1000 Subject: [PATCH 76/77] genserkins: build the Jacobian from the DH frames compute_jfwd() converts every link to a quaternion pose twice and builds the Jacobian through generic matrix calls: 0.9 us of the 1.2 us that genser_jacobian() takes. tpnext asks for up to about 60 Jacobians per move near a singular pose, and genser_inverse() takes one per iteration. genser_jfwd_frames() chains the DH frames as rotations and origins and takes each column as z_i x (o_n - o_i), z_i. It gives what compute_jfwd() gives to within 3e-10 relative over 20000 random poses, in 0.12 us. genser_jacobian() and genser_inverse() use it; genser_kin_jac_inv() and genser_kin_jac_fwd() keep compute_jfwd(). With the puma560 genser config, adding a move near the wrist singular pose falls from 153 to 71 us on average and from 207 to 93 us at most; other moves fall from 36 to 22 us. --- src/emc/kinematics/genserfuncs.c | 78 +++++++++++++++++++++++++------- 1 file changed, 61 insertions(+), 17 deletions(-) diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index 0e39a16ca11..e037e2be6e4 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -333,6 +333,65 @@ int genser_kin_jac_fwd(void *kins, return GO_RESULT_OK; } +/* What compute_jfwd() gives, straight from the DH frames in the base + frame: joint i moves along or turns about z_i, the axis of frame i + through its origin o_i, so the last frame's origin moves z_i or + z_i x (o_n - o_i) and turns 0 or z_i. The planner asks for many + Jacobians along each move, and this one skips the quaternions. */ +static void genser_jfwd_frames(const genser_struct *genser, const go_real *jest, + go_matrix *Jfwd) +{ + double R[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; + double o[3] = {0, 0, 0}; + double z[GENSER_MAX_JOINTS][3], oi[GENSER_MAX_JOINTS][3]; + int n = genser->link_num, i, r, c; + + for (i = 0; i < n; i++) { + const go_link *link = &genser->links[i]; + int prismatic = GO_QUANTITY_LENGTH == link->quantity; + double th = prismatic ? link->u.dh.theta : jest[i]; + double d = prismatic ? jest[i] : link->u.dh.d; + double sal = sin(link->u.dh.alpha), cal = cos(link->u.dh.alpha); + double sth = sin(th), cth = cos(th); + // as go_dh_pose_convert(): Rx(alpha) Tx(a) Rz(theta) Tz(d) + double L[3][3] = {{cth, -sth, 0}, + {sth * cal, cth * cal, -sal}, + {sth * sal, cth * sal, cal}}; + double t[3] = {link->u.dh.a, -sal * d, cal * d}; + double N[3][3]; + + for (r = 0; r < 3; r++) { + o[r] += R[r][0] * t[0] + R[r][1] * t[1] + R[r][2] * t[2]; + } + for (r = 0; r < 3; r++) { + for (c = 0; c < 3; c++) { + N[r][c] = R[r][0] * L[0][c] + R[r][1] * L[1][c] + R[r][2] * L[2][c]; + } + } + memcpy(R, N, sizeof(R)); + for (r = 0; r < 3; r++) { + z[i][r] = R[r][2]; + oi[i][r] = o[r]; + } + } + for (i = 0; i < n; i++) { + double v[3] = {o[0] - oi[i][0], o[1] - oi[i][1], o[2] - oi[i][2]}; + if (GO_QUANTITY_LENGTH == genser->links[i].quantity) { + for (r = 0; r < 3; r++) { + Jfwd->el[r][i] = z[i][r]; + Jfwd->el[3 + r][i] = 0; + } + continue; + } + Jfwd->el[0][i] = z[i][1] * v[2] - z[i][2] * v[1]; + Jfwd->el[1][i] = z[i][2] * v[0] - z[i][0] * v[2]; + Jfwd->el[2][i] = z[i][0] * v[1] - z[i][1] * v[0]; + for (r = 0; r < 3; r++) { + Jfwd->el[3 + r][i] = z[i][r]; + } + } +} + /* The Jacobian in the terms of kinematics.h: joints in degrees per pose word in EmcPose units, the derivative of genser_inverse(). @@ -356,8 +415,6 @@ static int genser_jacobian(const kins_params *p, const double *joint, genser_struct *genser = &genser_stg; GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS); GO_MATRIX_DECLARE(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6); - go_pose T_L_0; - go_link linkout[GENSER_MAX_JOINTS] = {}; go_real jest[GENSER_MAX_JOINTS]; double E[3][3]; double sb, cb, sc, cc; @@ -379,14 +436,7 @@ static int genser_jacobian(const kins_params *p, const double *joint, go_matrix_init(Jfwd, Jfwd_stg, 6, genser->link_num); go_matrix_init(Jinv, Jinv_stg, genser->link_num, 6); - for (link = 0; link < genser->link_num; link++) { - retval = go_link_joint_set(&genser->links[link], jest[link], &linkout[link]); - if (GO_RESULT_OK != retval) - return -1; - } - retval = compute_jfwd(linkout, genser->link_num, &Jfwd, &T_L_0); - if (GO_RESULT_OK != retval) - return -1; + genser_jfwd_frames(genser, jest, &Jfwd); retval = compute_jinv(&Jfwd, &Jinv); if (GO_RESULT_OK != retval) return -1; // singular: no finite joint rate follows the pose @@ -530,7 +580,6 @@ static int genser_inverse(const kins_params *p, kins_scratch *s, genser_struct *genser = &genser_stg; GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS); GO_MATRIX_DECLARE(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6); - go_pose T_L_0; go_real dvw[6]; go_real jest[GENSER_MAX_JOINTS]; go_real dj[GENSER_MAX_JOINTS]; @@ -575,12 +624,7 @@ static int genser_inverse(const kins_params *p, kins_scratch *s, for (link = 0; link < genser->link_num; link++) { go_link_joint_set(&genser->links[link], jest[link], &linkout[link]); } - retval = compute_jfwd(linkout, genser->link_num, &Jfwd, &T_L_0); - if (GO_RESULT_OK != retval) { - rtapi_print("ERR kI - compute_jfwd (joints: %f %f %f %f %f %f), (iterations=%d)\n", - joints[0],joints[1],joints[2],joints[3],joints[4],joints[5], genser->iterations); - return retval; - } + genser_jfwd_frames(genser, jest, &Jfwd); retval = compute_jinv(&Jfwd, &Jinv); if (GO_RESULT_OK != retval) { rtapi_print("ERR kI - compute_jinv (joints: %f %f %f %f %f %f), (iterations=%d)\n", From 5abdda64a01626656e9b8da32faaa6377183d7d2 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sun, 27 Sep 2026 00:19:27 +1000 Subject: [PATCH 77/77] tp: hold the joints when an abort stops a joint interpolated segment The abort reset the planner onto its own position, which inside a joint interpolated segment is the chord between the world ends, and forgot the joints; the servo thread then inverted the chord point and the joints jumped there in one cycle (on 5axiskins, pivot 400, Y at 166670 mm/s). The joints the segment stopped on are now handed out as its end, and the planner takes the point the servo thread found from them, so the machine stays where it stopped. --- src/emc/tp/tp.c | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index b197b2c23d6..1e2385ac02b 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -3518,9 +3518,21 @@ STATIC tp_err_t tpHandleAbort(TP_STRUCT * const tp, TC_STRUCT * const tc, if( MOTION_ID_VALID(tp->spindle.waiting_for_index) || MOTION_ID_VALID(tp->spindle.waiting_for_atspeed) || (tc->currentvel == 0.0 && (!nexttc || nexttc->currentvel == 0.0))) { + /* stopped inside a joint interpolated segment: the machine is + where the joints put it, which the servo thread found from them + last cycle, not on the chord this planner reports; the joints + are handed out as an end so that they stay where they are */ + double stop_joints[EMCMOT_MAX_JOINTS] = {0}; + int joint_stop = tc->active && tcGetJointPos(tc, stop_joints) > 0; tpReleaseQueuedPlanners(tp); tcqInit(&tp->queue); tpForgetJoints(tp); + if (joint_stop) { + int i; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { tp->joint_end[i] = stop_joints[i]; } + tp->joint_end_valid = 1; + tp->currentPos = emcmotStatus->carte_pos_cmd; + } tp->goalPos = tp->currentPos; tp->done = 1; tp->depth = tp->activeDepth = 0;