# This is a python remap for LinuxCNC implementing 'Tilted Work Plane' # G68.2, G68.3, G68.4 and related Gcodes G53.1, G53.3, G53.6, G69 # # Copyright ()c) 2023 David Mueller # # This program is free software: you can redistribute it and/or modify # it under the terms of the GNU General Public License as published by # the Free Software Foundation, either version 2 of the License, or # (at your option) any later version. # # This program is distributed in the hope that it will be useful, # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. # # import sys import traceback import numpy as np from math import sin,cos,tan,asin,acos,atan,atan2,sqrt,pi,degrees,radians,fabs from interpreter import * import emccanon from util import lineno, call_pydevd import hal # logging import logging # this name will be printed first on each log message log = logging.getLogger('remap.py; TWP') # we have to setup a handler to be able to set the log level for this module handler = logging.StreamHandler() formatter = logging.Formatter('%(name)s %(levelname)s: %(message)s') handler.setFormatter(formatter) log.addHandler(handler) # Manually force the log level for this module log.setLevel(logging.ERROR) # One of DEBUG, INFO, WARNING, ERROR, CRITICAL # set up parsing of the inifile import os import linuxcnc # get the path for the ini file used to start this config inifile = os.environ.get("INI_FILE_NAME") # instantiate the LinuxCNC ini-parser config = linuxcnc.ini(inifile) ## SPINDLE ROTARY JOINT LETTERS # spindle primary joint joint_letter_primary = config.getstring('TWP', 'PRIMARY', fallback="").capitalize() # spindle secondary joint (ie the one closer to the tool) joint_letter_secondary = config.getstring('TWP', 'SECONDARY', fallback="").capitalize() if not joint_letter_primary in ('A','B','C') or not joint_letter_secondary in ('A','B','C'): log.error("Unable to parse joint letters given in INI [TWP].") elif joint_letter_primary == joint_letter_secondary: log.error("Letters for primary and secondary joints in INI [TWP] must not be the same.") else: # get the MIN/MAX limits of the respective rotary joint letters category = 'AXIS_' + joint_letter_primary primary_min_limit = config.getreal(category, 'MIN_LIMIT', fallback=0.0) primary_max_limit = config.getreal(category, 'MAX_LIMIT', fallback=0.0) log.info('Joint letter for primary is %s with MIN/MAX limits: %s,%s', joint_letter_primary, primary_min_limit, primary_max_limit) category = 'AXIS_' + joint_letter_secondary secondary_min_limit = config.getreal(category, 'MIN_LIMIT', fallback=0.0) secondary_max_limit = config.getreal(category, 'MAX_LIMIT', fallback=0.0) log.info('Joint letter for secondary is %s with MIN/MAX Limits: %s,%s', joint_letter_secondary, secondary_min_limit, secondary_max_limit) ## CONNECTIONS TO THE KINEMATIC COMPONENT # get the name of the kinematic component kins_comp = config.getstring('KINS', 'KINEMATICS', fallback="") # name of the hal pin that represents the nutation-angle kins_nutation_angle = kins_comp + '_kins.nut-angle' # name of the hal pin that represents the pre-rotation kins_pre_rotation = kins_comp + '_kins.pre-rot' # name of the hal pin that represents the primary joint orientation angle kins_primary_rotation = kins_comp + '_kins.primary-angle' # name of the hal pin that represents the secondary joint orientation angle kins_secondary_rotation = kins_comp + '_kins.secondary-angle' ## CONNECTIONS TO THE HELPER COMPONENT twp_comp = 'twp-helper-comp.' twp_is_defined = twp_comp + 'twp-is-defined' twp_is_active = twp_comp + 'twp-is-active' # raise InterpreterException if execute() or read() fail throw_exceptions = 1 ## VALUE INITIALIZATION # we start with the identity matrix (ie the twp is equal to the world coordinates) twp_matrix = np.asmatrix(np.identity(4)) # some g68.2 p-word modes require several calls to enter all the required parameters so we # need a flag that indicates when the twp has been defined and is ready for g53.x # [current p-word, number of calls required, (state of calls required for that p mode added by g68.2)] # note that we use string since boolean True == 1, which gives wrong results if we want # to count the elements that are True because it is counted as integer '1' # eg: twp_flag = [0, 1, 'empty'] twp_flag = [] # we need a place to store the twp-build-parameters if the mode needs more than one call twp_build_params = {} # container to store the current work offset during twp operations current_work_offset_number = 1 saved_work_offset = [0,0,0] # orientation mode refers to the strategy used to choose from the different rotary angles for a given # tool-z vector. The optimization is applied to the primary axis only with mode 0 (shortest path) being # the default. (0=shortest_path , 1=positive_rotation only, 2=negative_rotation only, ) orient_mode = 0 # defines the kinematic model for (world <-> tool) coordinates of the machine at hand # returns 4x4 transformation matrix for given angles and 4x4 input matrix # NOTE: these matrices must be the same as the ones used to derive the kinematic model def kins_tool_transformation(theta_1, theta_2, pre_rot, matrix_in, direction='fwd'): global joint_letter_primary, joint_letter_secondary global kins_nutation_angle T_in = matrix_in ## Define 4x4 transformation for virtual rotation around tool-z to orient tool-x and -y Stc = sin(pre_rot) Ctc = cos(pre_rot) Rtc=np.matrix([[ Ctc, -Stc, 0, 0], [ Stc, Ctc, 0, 0], [ 0 , 0 , 1, 0], [ 0, 0 , 0, 1]]) ## Define 4x4 transformation for the primary joint # get the basic 3x3 rotation matrix (returns array) if joint_letter_primary == 'A': Rp = Rx(theta_1) elif joint_letter_primary == 'B': Rp = Ry(theta_1) elif joint_letter_primary == 'C': Rp = Rz(theta_1) # add fourth column on the right Rp = np.hstack((Rp, [[0],[0],[0]])) # expand to 4x4 array and make into a matrix row_4 = [0,0,0,1] Rp = np.vstack((Rp, row_4)) Rp = np.asmatrix(Rp) ## Define 4x4 transformation matrix for the secondary joint # get the basic 3x3 rotation matrix (returns array) if joint_letter_secondary == 'A': Rs = Rx(theta_2) elif joint_letter_secondary == 'B': Rs = Ry(theta_2) elif joint_letter_secondary == 'C': Rs = Rz(theta_2) # add fourth column on the right Rs = np.hstack((Rs, [[0],[0],[0]])) # expand to 4x4 array and make into a matrix row_4 = [0,0,0,1] Rs = np.vstack((Rs, row_4)) Rs = np.asmatrix(Rs) if (joint_letter_primary, joint_letter_secondary)== ('C', 'B'): # Additional definitions for nutating joint v = radians(hal.get_value(kins_nutation_angle)) Sv = sin(v) Cv = cos(v) Ss = sin(theta_2) Cs = cos(theta_2) r = Cs + Sv*Sv*(1-Cs) s = Cs + Cv*Cv*(1-Cs) t = Sv*Cv*(1-Cs) # define rotation matrix for the secondary spindle joint Rs=np.matrix([[ Cs, -Cv*Ss, Sv*Ss, 0], [ Cv*Ss, r, t, 0], [ -Sv*Ss, t, s, 0], [ 0, 0, 0, 1]]) elif (joint_letter_primary, joint_letter_secondary)== ('C', 'A'): # Additional definitions for nutating joint v = radians(hal.get_value(kins_nutation_angle)) Sv = sin(v) Cv = cos(v) Ss = sin(theta_2) Cs = cos(theta_2) r = Cs + Sv*Sv*(1-Cs) s = Cs + Cv*Cv*(1-Cs) t = Sv*Cv*(1-Cs) # define rotation matrix for the secondary spindle joint Rs=np.matrix([[ r, -Cv*Ss, t, 0], [ Cv*Ss, Cs, -Sv*Ss, 0], [ t, Sv*Ss, s, 0], [ 0, 0, 0, 1]]) else: log.error('No formula for this spindle kinematic (primary, secondary) %s, %s', joint_letter_primary, joint_letter_secondary) # calculate the transformation matrix for the forward tool kinematic matrix_tool_fwd = np.transpose(Rtc)*np.transpose(Rs)*np.transpose(Rp)*T_in # calculate the transformation matrix for the inverse tool kinematic matrix_tool_inv = Rp*Rs*Rtc*T_in if direction == 'fwd': #log.debug("matrix tool fwd: \n", matrix_tool_fwd) #log.debug("inv would have been: \n", matrix_tool_inv) return matrix_tool_fwd elif direction == 'inv': #log.debug("matrix tool inv: \n", matrix_tool_inv) #log.debug("fwd would have been: \n", matrix_tool_fwd) return matrix_tool_inv else: return 0 # returns angle 'tc' required to rotate the x-axis of the tool-coords parallelto the machine-xy plane # for given machine joint position angles. # For G68.3 this is the default tool-x direction # NOTE: this uses formulas derived from the transformation matrix in the inverse tool kinematic def kins_calc_tool_rot_c_for_horizontal_x(self, theta_1, theta_2): global joint_letter_primary, joint_letter_secondary # The idea is that the tool-x vector is parallel to the machine xy-plane when the # z component of the x-direction vector is equal to zero # Mathematically we take the symbolic formula found in row 3, column 1 of the transformation # matrix from the inverse tool-kinematics, equal that to zero and solve for 'tc'. # this makes the x orientation of the tool coords horizontal and the user can set the # rotation from there using g68.3 r global kins_nutation_angle v = radians(hal.get_value(kins_nutation_angle)) Cv = cos(v) Sv = sin(v) Cs = cos(theta_2) Ss = sin(theta_2) Cp = cos(theta_1) Sp = sin(theta_1) if (joint_letter_primary, joint_letter_secondary)== ('C', 'B'): t = Sv*Cv*(1-Cs) tc = atan2((Sv*Ss),t) elif (joint_letter_primary, joint_letter_secondary)== ('C', 'A'): t = Sv*Cv*(1-Cs) tc = atan2(-t,(Sv*Ss)) else: log.error('No formula for this spindle kinematic (primary, secondary) %s, %s', joint_letter_primary, joint_letter_secondary) # note: tool-c rotation is done using a halpin that feeds into the kinematic component and the # vismach model. In contrast to a gcode command where 'c' refers to a physical machine joint) return tc # calculates the secondary joint position for a given tool-vector # secondary being the joint closest to the tool # Note: this uses functions derived from the custom kinematic def kins_calc_secondary(self, tool_z_req): global joint_letter_primary, joint_letter_secondary global secondary_min_limit, secondary_max_limit global kins_nutation_angle epsilon = 0.000001 theta_2_list=[] (Kzx, Kzy, Kzz) = (tool_z_req[0], tool_z_req[1], tool_z_req[2]) if (joint_letter_primary, joint_letter_secondary)== ('C', 'B'): # This kinmatic has infinite results for the vertical tool orientation # so we explicitly define the angles for that specific case if Kzz > 1 - epsilon: return [0] else: v = radians(hal.get_value(kins_nutation_angle)) Sv = sin(v) Cv = cos(v) theta_2 = acos((Kzz - Cv*Cv)/(1 - Cv*Cv)) elif (joint_letter_primary, joint_letter_secondary)== ('C', 'A'): # This kinmatic has infinite results for the vertical tool orientation # so we explicitly define the angles for that specific case if Kzz > 1 - epsilon: return [0] else: v = radians(hal.get_value(kins_nutation_angle)) Sv = sin(v) Cv = cos(v) theta_2 = acos((Kzz - Cv*Cv)/(1 - Cv*Cv)) else: log.error('No formula for this spindle kinematic (primary, secondary) %s', (joint_letter_primary, joint_letter_secondary)) # since we are using acos() we really have two solutions theta_2 and -theta_2 for theta in [theta_2, -theta_2]: log.debug('Checking if result %s is within secondary joint limits of %s and %s.', degrees(theta), secondary_min_limit, secondary_max_limit) if theta > secondary_min_limit and theta < secondary_max_limit: log.debug('Adding %s to valid angles list.', degrees(theta)) theta_2_list.append(theta) log.debug('List of possible secondary angles: %s\n', theta_2_list) return theta_2_list # calculates the primary joint position for a given tool-vector # Note: this uses functions derived from the custom kinematic def kins_calc_primary(self, tool_z_req, theta_2_list): global joint_letter_primary, joint_letter_secondary global primary_min_limit, primary_max_limit global kins_nutation_angle epsilon = 0.000001 theta_1_list=[] (Kzx, Kzy, Kzz) = (tool_z_req[0], tool_z_req[1], tool_z_req[2]) if (joint_letter_primary, joint_letter_secondary)== ('C', 'B'): # This kinmatic has infinite results for the vertical tool orientation # so we explicitly define the angles for that specific case if Kzz > 1 - epsilon: return [0] else: v = radians(hal.get_value(kins_nutation_angle)) Sv = sin(v) Cv = cos(v) for i in range(len(theta_2_list)): theta_2 = theta_2_list[i] Ss = sin(theta_2) Cs = cos(theta_2) t = Sv*Cv*(1-Cs) p = Sv * Ss theta_1 = asin((p*Kzy - t*Kzx)/(t*t + p*p)) elif (joint_letter_primary, joint_letter_secondary)== ('C', 'A'): # This kinmatic has infinite results for the vertical tool orientation # so we explicitly define the angles for that specific case if Kzz > 1 - epsilon: return [0] else: v = radians(hal.get_value(kins_nutation_angle)) Sv = sin(v) Cv = cos(v) for i in range(len(theta_2_list)): theta_2 = theta_2_list[i] Ss = sin(theta_2) Cs = cos(theta_2) t = Sv*Cv*(1-Cs) p = Sv * Ss q = (t*Kzy - p*Kzx)/(t*t + p*p) theta_1 = asin(q) else: log.error('No formula for this spindle kinematic (primary, secondary) %s', (joint_letter_primary, joint_letter_secondary)) # since we are using asin() we really have two solutions theta_1 and pi-theta_2 for theta in [theta_1, transform_to_pipi(pi - theta_1)]: log.debug('Checking if result %s is within secondary joint limits of %s and %s.', degrees(theta), secondary_min_limit, secondary_max_limit) if theta > secondary_min_limit and theta < secondary_max_limit: log.debug('Adding %s to valid angles list.', degrees(theta)) theta_1_list.append(theta) log.debug('List of possible secondary angles: %s\n', theta_2_list) return theta_1_list # this is from 'mika-s.github.io' # transforms a given angle to the interval of [-pi,pi] def transform_to_pipi(input_angle): revolutions = int((input_angle + np.sign(input_angle) * pi) / (2 * pi)) p1 = truncated_remainder(input_angle + np.sign(input_angle) * pi, 2 * pi) p2 = (np.sign(np.sign(input_angle) + 2 * (np.sign(fabs((truncated_remainder(input_angle + pi, 2 * pi)) / (2 * pi))) - 1))) * pi output_angle = p1 - p2 return output_angle # this is from 'mika-s.github.io' # used by 'transform_to_pipi()' def truncated_remainder(dividend, divisor): divided_number = dividend / divisor divided_number = -int(-divided_number) if divided_number < 0 else int(divided_number) remainder = dividend - divisor * divided_number return remainder # returns a list of valid primary/secondary spindle joint positions for a given tool-orientation vector # or 'None','None' if no valid position could be found def kins_calc_jnt_angles(self, tool_z_req): log.debug('tool_z_requested: %s', tool_z_req) # set the tolerance value epsilon = 0.0001 # create np.array so we can easily calculate differences and check elements tool_z_req = np.array([tool_z_req[0], tool_z_req[1], tool_z_req[2]]) # calculate secondary joint values using kinematic specific formula theta_2_pair = kins_calc_secondary(self, tool_z_req) # calculate primary joint values using kinematic specific formula theta_1_pair = kins_calc_primary(self, tool_z_req, theta_2_pair) joint_angles_list = [] # iterate through all the possible combinations of (theta_1 , theta_2) for i in range(len(theta_1_pair)): for j in range(len(theta_2_pair)): # rotate an identity matrix using the custom tool kinematic model and the (theta_1, theta_2) matrix_in = np.asmatrix(np.identity(4)) t_out = kins_tool_transformation(theta_1_pair[i], theta_2_pair[j], 0, matrix_in,'inv') # the resulting tool-z vector for this pair of (theta_1, theta_2) is found in the third column tool_z_would_be = np.array([t_out[0,2], t_out[1,2], t_out[2,2]]) log.debug('tool_z_would_be: %s', tool_z_would_be) # calculate the difference of the respective elements tool_z_diff = tool_z_req - tool_z_would_be # and check if all elements are within [-epsilon,epsilon] match = np.all((tool_z_diff > -epsilon) & (tool_z_diff < epsilon)) log.debug('Is the tool-Z-vector close enough ? %s', match) if match: # check if we already have this particular pair in the list if not (theta_1_pair[i], theta_2_pair[j]) in joint_angles_list: log.debug('Appending (theta_1_pair, theta_2_pair) %s', (degrees(theta_1_pair[i]), degrees(theta_2_pair[j]))) joint_angles_list.append((theta_1_pair[i], theta_2_pair[j])) log.info('Found valid joint angles: %s', joint_angles_list) if joint_angles_list: return joint_angles_list #return joint_angles_list[-1] else: return None, None def calc_shortest_distance(pos, trgt, mode): # calculate the shortest distance in [-180°, 180°] # eg if pos=170° and trgt=-170° then dist will be 20° # If the operator requests positive or negative rotation # we may need to return the long distance instead log.debug('Got (pos, trgt): %s', (pos, trgt)) dist_short = (trgt - pos + 180) % 360 - 180 # calculate short and long distance if dist_short >= 0: # ie dist_long should be negative dist_long = -(360 - dist_short) else: dist_long = 360 + dist_short log.debug('Calculated (dist_short, dist_long): %s', (dist_short, dist_long)) if mode == 1: # positive rotation only, ie we want a positive distance if dist_short >= 0: # ie we want this one dist = dist_short else: # ie we need to go the other way dist = dist_long if mode == 2: # negative rotation only ie we want a positive distance if dist_short >= 0: # ie we need to go the other way dist = dist_long else: # ie we want this one dist = dist_short else: # mode = 0 ie we want the shortest distance either way dist = dist_short log.debug('Distance returned: %s', dist) return dist # this takes a target angle in [-pi,pi] and finds the closest move within [min_limit, max_limit] # from a given position in [min_limit, max_limit], returns the optimized target angle and the distance # from the given position to that target angle def calc_rotary_move_with_joint_limits(position, target, max_limit, min_limit, mode): pos = degrees(position) trgt = degrees(target) log.debug('(Current_pos, target): %s', (pos, trgt)) # calculate the shortest distance from position to target for the strategy given by # the operator (ie shortest (= default), positive rotation only, negative rotation only ) dist = calc_shortest_distance(pos, trgt, mode) # check that the result is within the rotary axis limits defined in the ini file if dist >= 0: # shortest way is in the positive direction if (pos + dist) <= max_limit: # if the limits allow we rotate the joint in the positive sense log.debug('Max_limit OK, target changed to: %s', (pos + dist)) theta = pos + dist else: # if positive limits would be exceeded we need to go the longer wey in the other direction if mode == 0: log.debug('Max_limit reached, target remains: %s', trgt) theta = trgt else: # if the rotation direction was set by the operator then we can not change direction theta = None dist = None else: # shortest way is in the negative direction if (pos + dist) >= min_limit: # if the limits allow we rotate the joint in the negative sense log.debug('Min_limit OK, target changed to: %s', (pos + dist)) theta = pos + dist else: # if negative limits would be exceeded we need to go the longer way int the other direction if mode == 0: log.debug('Min_limit reached, target remains: %s', trgt) theta = trgt else: # if the rotation direction was set by the operator then we can not change direction theta = None dist = None # we also attach the distance for this particular move and mode log.debug('Angle and distance returned: %s, %s', theta, dist) return theta, dist # this takes a list of joint angle pairs in [-pi,pi] and optimizes them for shortest moves # in (min_limit, max_linit) from the current joint positions using the orient_mode set by # the operator: 0=shortest (default), 1=positive rotation only, 2=negative rotation only def calc_angle_pairs_and_distances(self, possible_prim_sec_angle_pairs): global primary_min_limit, primary_max_limit, secondary_min_limit, secondary_max_limit global orient_mode # get the current joint positions prim_pos, sec_pos = get_current_rotary_positions(self) # we want to return a list of angles that are optimized for the orient_mode and the # rotary axes limits as set in the ini file target_dist_list= [] for prim_trgt, sec_trgt in possible_prim_sec_angle_pairs: # primary joint, here we apply the orient mode requested by the operator prim_move, prim_dist = calc_rotary_move_with_joint_limits(prim_pos, prim_trgt, primary_max_limit, primary_min_limit, orient_mode) # secondary joint, here we want the shortest move (although we could also apply a strategy here) sec_move, sec_dist = calc_rotary_move_with_joint_limits(sec_pos, sec_trgt, secondary_max_limit, secondary_min_limit, 0) # if a solution has been found for this particular pair then we add it to the list if not (prim_move == None) and not (sec_move == None): target_dist_list.append(((prim_move, sec_move),(prim_dist, sec_dist))) log.debug('Assembled target_dist_list: %s',target_dist_list) return target_dist_list # find the optimal joint move from current to target positions in the list # for this we look at the primary joint move only # orient_mode is 0=shortest, 1=positive rotation only, 2=negative rotation only # For orient_mode=(1,2): If no move can be found within joint limits we return None def calc_optimal_joint_move(self, possible_prim_sec_angle_pairs): global orient_mode # this returns a list with all moves ((prim_move, sec_move),(prim_dist, sec_dist)) that # will result in correct tool orientation, stay within the rotary axis limits and respect the # orient_mode if set by the operator valid_joint_moves_and_distances = calc_angle_pairs_and_distances(self, possible_prim_sec_angle_pairs) # now we need to pick and return the (primary angle, secondary angle) that results in the # shortest move of the primary joint (theta_1, theta_2) = (None, None) dist = 3600 for trgt_angles, dists in valid_joint_moves_and_distances: if orient_mode == 0 and fabs(dists[0]) < fabs(dist): # shortest move requested (theta_1, theta_2) = trgt_angles dist = dists[0] elif orient_mode == 1 and fabs(dists[0]) < fabs(dist) and dists[0] >= 0: # positive primary rotation only (theta_1, theta_2) = trgt_angles dist = dists[0] elif orient_mode == 2 and fabs(dists[0]) < fabs(dist) and dists[0] <= 0: # negative primary rotation only (theta_1, theta_2) = trgt_angles dist = dists[0] log.debug('Shortest move selected for (orient_mode, theta_1, theta_2): %s', (orient_mode, theta_1, theta_2)) return theta_1, theta_2 # calculates the required pre-rotation around tool-z so the tool-x matches the requested # orientation after rotation of the spindle joints def kins_calc_pre_rot(self, theta_1, theta_2, tool_x_req, tool_z_requested): # tolerance setting for check if tool-x-vector needs to be rotated at all epsilon = 0.00000001 log.info("Tool-x-requested: %s", tool_x_req) # we need to calculate the current tool-x vector with the given rotations using # the transformation matrix from our custom tool kinematic log.debug("joint angles (secondary, primary) in radians given: %s", (theta_2, theta_1)) log.debug("joint angles (secondary, primary) in degrees given: %s", (theta_2*180/pi, theta_1*180/pi)) # run the identity matrix through the tool kinematic transformation in the requested direction # using the given joint angles and pre-rotation zero matrix_in = np.asmatrix(np.identity(4)) t_out = kins_tool_transformation(theta_1, theta_2, 0, matrix_in,'inv') # the tool-x vector for the given machine joint rotations is found directly in the first column tool_x_is = [t_out[0,0], t_out[1,0], t_out[2,0]] log.debug("tool-x after machine rotation would be: %s", tool_x_is) # we calculate the angular difference between the two vectors so we can 'pre-rotate' # around tool-z to get the requested tool-x vector after machine rotation # just to be sure we normalize the two vectors tool_x_is = tool_x_is / np.linalg.norm(tool_x_is) tool_x_req = tool_x_req / np.linalg.norm(tool_x_req) # check if the x-vector is already in the required orientation (ie parallel) log.debug("check if vectors are parallel: %s", np.dot(tool_x_is,tool_x_req)) if np.dot(tool_x_is,tool_x_req) > 1 - epsilon: log.info("Tool x-vector already oriented, setting pre-rotation = 0") # if we are already parallel then we don't need to pre-rotate pre_rot = 0 else: # we can use the cross product to determine the direction we need to rotate cross = np.cross(tool_x_req, tool_x_is) log.debug("cross product (tool_x_req, tool_x_is): %s", cross) log.info("Tool_z_requested: %s", tool_z_requested) pre_rot = np.arccos(np.dot(tool_x_req, tool_x_is)) log.debug('base pre_rot: %s', pre_rot) # To find out which quadrant we need the angle to be in we create a list of them all pre_rot_list = [pre_rot, -pre_rot, 2*pi-pre_rot, -(2*pi-pre_rot)] log.debug('pre_rot_list: %s',pre_rot_list) # then we run all of them through the kinematic model and see which gives us # the requested tool-x-vector for pre_rot in pre_rot_list: zeta = 0.0001 # run the identity matrix through the tool kinematic transformation in the requested direction # using the given joint angles and pre-rotation angle in the list matrix_in = np.asmatrix(np.identity(4)) t_out = kins_tool_transformation(theta_1, theta_2, pre_rot, matrix_in,'inv') # the tool-x vector for the given primary and secondary rotations is found directly in the first column tool_x_would_be = [t_out[0,0], t_out[1,0], t_out[2,0]] log.debug('tool_x_would_be: %s', tool_x_would_be) # calculate the difference of the respective elements tool_x_diff = tool_x_req - tool_x_would_be # and check if all elements are within [-epsilon,epsilon] match = np.all((tool_x_diff > -zeta) & (tool_x_diff < zeta)) log.debug('Is the tool-X-vector close enough ? %s', match) if match: # if we have a match we leave the loop and use this angle break log.info("Pre-rotation calculated [deg]: %s", degrees(pre_rot)) # return pre_rot in radians return pre_rot # transforms a 4x4 input matrix using the current tool transformation matrix # (forward or inverse) using the kinematic model of the machine def kins_calc_tool_transformation(self, matrix_in, theta_1=None, theta_2=None, pre_rot=None, direction='fwd'): global kins_pre_rotation # if no angle values have been passed we get the current joint positions if theta_2 == None or theta_1 == None: # read current spindle rotary angles and convert to radians theta_1, theta_2 = get_current_rotary_positions(self) else: log.debug("got for secondary joint: %s", theta_2) log.debug("got for primary joint: %s", theta_1) # pre-rot is the virtual rotary axis around the tool-z axis to align the tool-x axis # if no pre-rot angle is passed then we use the currently active value if pre_rot == None: pre_rot = hal.get_value(kins_pre_rotation ) log.debug("current pre-rot: %s", pre_rot) else: log.debug("requested pre-rot value [DEG]): %s", degrees(pre_rot)) # run the input matrix through the tool kinematic transformation in the requested direction # using the current joint angles and pre-rotation as requested matrix_out = kins_tool_transformation(theta_1, theta_2, pre_rot, matrix_in, direction) return matrix_out # define the basic rotation matrices, used for euler twp modes def Rx(th): return np.array([[1, 0 , 0 ], [0, cos(th), -sin(th)], [0, sin(th), cos(th)]]) def Ry(th): return np.array([[ cos(th), 0, sin(th)], [ 0 , 1, 0 ], [-sin(th), 0, cos(th)]]) def Rz(th): return np.array([[cos(th), -sin(th), 0], [sin(th), cos(th), 0], [0 , 0 , 1]]) # returns the rotation matrices for given order and angles def twp_calc_euler_rot_matrix(th1, th2, th3, order): log.debug("euler order requested: %s", order) log.debug("angles given (th1, th2 , th3): %s", (th1, th2, th3)) th1 = radians(th1) th2 = radians(th2) th3 = radians(th3) if order == '131': matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Rx(th3)) elif order=='121': matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rx(th3)) elif order=='212': matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Ry(th3)) elif order=='232': matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Ry(th3)) elif order=='323': matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rz(th3)) elif order=='313': matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Rz(th3)) elif order=='123': matrix = np.dot(np.dot(Rx(th1), Ry(th2)), Rz(th3)) elif order=='132': matrix = np.dot(np.dot(Rx(th1), Rz(th2)), Ry(th3)) elif order=='213': matrix = np.dot(np.dot(Ry(th1), Rx(th2)), Rz(th3)) elif order=='231': matrix = np.dot(np.dot(Ry(th1), Rz(th2)), Rx(th3)) elif order=='321': matrix = np.dot(np.dot(Rz(th1), Ry(th2)), Rx(th3)) elif order=='312': matrix = np.dot(np.dot(Rz(th1), Rx(th2)), Ry(th3)) log.debug('euler rotation as matrix: \n %s', matrix) return matrix # The tilted-work-plane is created in identity mode and must NOT be updated after a switch def gui_update_twp(self): global twp_matrix, saved_work_offset # twp origin as vector (in world coords) from current work-offset to the origin of the twp hal.set_p("twp-helper-comp.twp-ox-in",str(twp_matrix[0,3])) hal.set_p("twp-helper-comp.twp-oy-in",str(twp_matrix[1,3])) hal.set_p("twp-helper-comp.twp-oz-in",str(twp_matrix[2,3])) # twp x-vector hal.set_p("twp-helper-comp.twp-xx-in",str(twp_matrix[0,0])) hal.set_p("twp-helper-comp.twp-xy-in",str(twp_matrix[1,0])) hal.set_p("twp-helper-comp.twp-xz-in",str(twp_matrix[2,0])) # twp z-vector hal.set_p("twp-helper-comp.twp-zx-in",str(twp_matrix[0,2])) hal.set_p("twp-helper-comp.twp-zy-in",str(twp_matrix[1,2])) hal.set_p("twp-helper-comp.twp-zz-in",str(twp_matrix[2,2])) # publish the twp offset coordinates in world coordinates (ie identity) [work_offset_x, work_offset_y, work_offset_z] = saved_work_offset log.debug("Setting work_offsets in the simulation: %s", (work_offset_x, work_offset_y, work_offset_z)) # this is used to translate the rotated twp to the correct position # care must be taken that only the work_offsets in identity mode are sent as that is # what the model uses. The visuals for the offsets are created then rotated according to # the rotary joint position and then translated. # The twp has to be rotated out of the machine xy plane using the g68.2 parameters and is then # translated by the offset values of the identity mode. hal.set_p("twp-helper-comp.twp-ox-world-in",str(work_offset_x)) hal.set_p("twp-helper-comp.twp-oy-world-in",str(work_offset_y)) hal.set_p("twp-helper-comp.twp-oz-world-in",str(work_offset_z)) # NOTE: Due to easier abort handling we currently restrict the use of twp to G54 # as LinuxCNC seems to revert to G54 as the default system def get_current_work_offset(self): # get which offset is active (g54=1 .. g59.3=9) active_offset = int(self.params[5220]) current_work_offset_number = active_offset # set the relevant parameter numbers that hold the active offset values # (G54_x: #5221, G55_x:#[5221+20], G56_x:#[5221+40] ....) work_offset_x = (active_offset-1)*20 + 5221 work_offset_y = work_offset_x + 1 work_offset_z = work_offset_x + 2 co_x = self.params[work_offset_x] co_y = self.params[work_offset_y] co_z = self.params[work_offset_z] current_work_offset = [co_x, co_y, co_z] return [current_work_offset_number, current_work_offset] def get_current_rotary_positions(self): global joint_letter_primary, joint_letter_secondary if joint_letter_primary == 'A': theta_1 = radians(self.AA_current) elif joint_letter_primary == 'B': theta_1 = radians(self.BB_current) elif joint_letter_primary == 'C': theta_1 = radians(self.CC_current) log.debug('Current position Primary joint: %s', degrees(theta_1)) # read current spindle rotary angles and convert to radians if joint_letter_secondary == 'A': theta_2 = radians(self.AA_current) elif joint_letter_secondary == 'B': theta_2 = radians(self.BB_current) elif joint_letter_secondary == 'C': theta_2 = radians(self.CC_current) log.debug('Current position Secondary joint: %s', degrees(theta_2)) return theta_1, theta_2 # forms a 4x4 transformation matrix from a given 1x3 point vector [x,y,z] def point_to_matrix(point): # start with a 4x4 identity matrix and add the point vector to the 4th column matrix = np.identity(4) [matrix[0,3], matrix[1,3], matrix[2,3]] = point matrix = np.asmatrix(matrix) return matrix # extracts the point vector form a given 4x4 transformation matrix def matrix_to_point(matrix): point = (matrix[0,3],matrix[1,3],matrix[2,3]) return point def reset_twp_params(self): global pre_rot, twp_matrix, twp_flag, twp_build_params pre_rot = 0 # we must not change tool kins parameters when TOOL kins are active or we get sudden joint position changes # ie don't do this: kins_comp_set_pre_rot(self,0)! twp_flag = [] twp_build_params = {} log.info("Resetting TWP-matrix") twp_matrix = np.asmatrix(np.identity(4)) # Orient the tool to the current twp (with TCP for G53.1 or IDENTITY for G53.6) # (some controllers offer an optional P-word to give preferred rotation directions this is not implemented yet) # Note: To avoid that this python code is run prematurely by the read ahead we need a quebuster at the beginning but # because we need self.execute() to switch the WCS properly this remap needs to be called from # an ngc reamp that contains a quebuster before calling this code # IMPORTANT: # The correct kinematic mode (ie TCP for 53.1 / IDENTITY for G53.6) must be active when this code is called # (ie do it in the ngc remap mentioned above!) def g53x_core(self): global saved_work_offset, twp_matrix, twp_flag, pre_rot global joint_letter_primary, joint_letter_secondary, twp_error_status global orient_mode if self.task == 0: # ignore the preview interpreter yield INTERP_EXECUTE_FINISH return INTERP_OK if not hal.get_value(twp_is_defined): # reset the twp parameters reset_twp_params(self) msg = "G53.x: No TWP defined." log.debug(msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR elif hal.get_value(twp_is_active): # reset the twp parameters reset_twp_params(self) msg = "G53.x: TWP already active" log.debug(msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR # Check if any words have been passed with the respective G53.x command c = self.blocks[self.remap_level] p = c.p_number if c.p_flag else 0 x = c.i_number if c.i_flag else None y = c.j_number if c.j_flag else None z = c.k_number if c.k_flag else None log.debug('G53.x Words passed: (P, X,Y,Z): %s', (p,x,y,z)) if p not in [0,1,2]: # reset the twp parameters reset_twp_params(self) msg = "G53.x : unrecognised P-Word found." log.debug(msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR orient_mode = p # calculate the required rotary joint positions and pre_rotation for the requested tool-orientation try: tool_z_requested = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] # calculate all possible pairs of (primary, secondary) angles so our tool-z vector matches the requested tool-z # angles are returned in [-pi,pi] possible_prim_sec_angle_pairs = kins_calc_jnt_angles(self, tool_z_requested) # An excepton will occur if the requested tool orientation cannot be achieved with the kinematic at hand except Exception as error: log.error('G53.x: Calculation failed, %s', error) possible_prim_sec_angle_pairs = [] if not possible_prim_sec_angle_pairs: # reset the twp parameters reset_twp_params(self) msg = "G53.x ERROR: Requested tool orientation not reachable -> aborting G53.x" log.debug(msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR # this returns one pair of optimized angles in degrees, or (None, None) if no solution could be found theta_1, theta_2 = calc_optimal_joint_move(self, possible_prim_sec_angle_pairs) if theta_1 == None: # reset the twp parameters reset_twp_params(self) msg = ("G53.x ERROR: Requested tool orientation not reachable -> aborting G53.x") log.debug(msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR theta_1 = radians(theta_1) theta_2 = radians(theta_2) # calculate the pre-rotation needed so our tool-x vector matches the requested tool-x vector tool_x_requested = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] pre_rot = kins_calc_pre_rot(self,theta_1, theta_2, tool_x_requested, tool_z_requested) log.debug("Calculated pre-rotation (pre_rot) to match requested tool-x): %s", pre_rot) # mark twp-flag as active twp_flag = [0, 'active'] gui_update_twp(self) # set the pre-rotation value in the kinematic component log.debug("G53.x: setting primary, secondary and pre_rotation angles in kinematic component: %s", (degrees(theta_1), degrees(theta_2), degrees(pre_rot))) hal.set_p(kins_pre_rotation, str(pre_rot)) hal.set_p(kins_primary_rotation, str(degrees(theta_1))) hal.set_p(kins_secondary_rotation, str(degrees(theta_2))) # calculate the work offset in tool-coords P = matrix_to_point(kins_calc_tool_transformation(self, point_to_matrix(saved_work_offset), theta_1, theta_2, pre_rot)) # get the current twp_origin twp_offset = (twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]) # calculate the twp offset in tool-coords Q = matrix_to_point(kins_calc_tool_transformation(self, point_to_matrix(twp_offset), theta_1, theta_2, pre_rot)) log.debug("G53.x: Setting transformed work-offsets for tool-kins in G59, G59.1, G59.2 and G59.3 to: %s ", P) # set the dedicated TWP work offset values (G53, G53.1, G53.2, G53.3) self.execute("G10 L2 P6 X%f Y%f Z%f " % (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]), lineno()) self.execute("G10 L2 P7 X%f Y%f Z%f " % (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]), lineno()) self.execute("G10 L2 P8 X%f Y%f Z%f " % (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]), lineno()) self.execute("G10 L2 P9 X%f Y%f Z%f " % (P[0]+Q[0], P[1]+Q[1], P[2]+Q[2]), lineno()) log.debug("G53.x: Moving (secondary and primary) joints to: %s", (degrees(theta_2), degrees(theta_1))) if (x,y,z) == (None,None,None): # Move rotary joints to align the tool with the requested twp self.execute("G0 %s%f %s%f" % (joint_letter_secondary, degrees(theta_2), joint_letter_primary, degrees(theta_1)), lineno()) # switch to the dedicated TWP work offsets self.execute("G59", lineno()) # activate TOOL kinematics self.execute("M68 E3 Q2") if (x,y,z) != (None,None,None): log.debug('G53.3 called') self.execute("G0 X%s Y%s Z%s %s%f %s%f" % (x, y, z, joint_letter_secondary, degrees(theta_2), joint_letter_primary, degrees(theta_1)), lineno()) # set twp-state to 'active' (2) self.execute("M68 E2 Q2") yield INTERP_EXECUTE_FINISH return INTERP_OK # Cancel an active TWP definition and reset the parameters to zero # Note: To avoid that this python code is run prematurely by the read ahead we need a quebuster at the beginning but # because we need self.execute() to switch the WCS properly this remap needs to be called from # an ngc that contains a quebuster before calling this code def g69_core(self): global twp_flag, saved_work_offset_number, saved_work_offset if self.task == 0: # ignore the preview interpreter yield INTERP_EXECUTE_FINISH return INTERP_OK log.info('G69 called') # reset the twp parameters reset_twp_params(self) gui_update_twp(self) # set twp-state to 'undefined' (0) self.execute("M68 E2 Q0") yield INTERP_EXECUTE_FINISH return INTERP_OK # define a virtual tilted-work-plane (twp) that is perpendicular to the current # tool-orientation def g683(self, **words): global twp_matrix, pre_rot, twp_flag, saved_work_offset_number, saved_work_offset if self.task == 0: # ignore the preview interpreter yield INTERP_EXECUTE_FINISH return INTERP_OK # ! IMPORTANT ! # We need to use 'yield INTERP_EXECUTE_FINISH' here to stop the read ahead # and avoid it executing the rest of the remap ahead of time ## NOTE: No 'self.execute(..)' command can be used after 'yield INTERP_EXECUTE_FINISH' yield INTERP_EXECUTE_FINISH if hal.get_value(twp_is_defined): # reset the twp parameters reset_twp_params(self) msg =("G68.3 ERROR: TWP already defined.") log.debug(msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR # NOTE: Due to easier abort handling we currently restrict the use of twp to G54 # as LinuxCNC seems to revert to G54 as the default system # get which offset is active (g54=1 .. g59.3=9) (n, offsets) = get_current_work_offset(self) if n != 1: # reset the twp parameters reset_twp_params(self) msg = "G68.3 ERROR: Must be in G54 to define TWP." log.debug(msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR c = self.blocks[self.remap_level] # parse the requested origin x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 # parse the requested rotation of tool-x around the origin r = c.r_number if c.r_flag else 0 twp_flag = [0, 1, 'empty'] # one call to define the twp in this mode theta_1, theta_2 = get_current_rotary_positions(self) # calculate tool-prerotation necessary to have tool-x vector in machine xy-plane pre_rot = kins_calc_tool_rot_c_for_horizontal_x(self, theta_1, theta_2 ) log.info("G68.3: Pre-Rotation calculated for x-vector in machine-xy plane [deg]: %s", pre_rot*180/pi) # then we need the tool transformation matrix of the current tool orientation with the # calculated pre-rotation to get the tool-x vector in the machine xy-plane # for this we take the 4x4 identity matrix and pass it through the inverse tool kinematic # transformation using the current rotary joint positions and calculated pre-rotation angle # plus the requested angle of rotation for tool-x from the machine-xy plane start_matrix = np.asmatrix(np.identity(4)) log.info('G68.3: Requested origin rotation [deg]: %s', r) twp_matrix = kins_calc_tool_transformation(self, start_matrix, None, None, pre_rot + radians(r), 'inv') log.debug("G68.3: Tool matrix with x-vector in machine xy-plane: \n%s", twp_matrix) # put the requested origin into the twp_matrix (twp_matrix[0,3], twp_matrix[1,3], twp_matrix[2,3]) = (x, y, z) # update the build state of the twp call twp_flag[2] = 'done' log.info("G68.3: Built twp-transformation-matrix: \n%s", twp_matrix) # collect the currently active work offset values (ie g54, g55 or other) saved_work_offset = offsets saved_work_offset_number = n log.debug("G68.3: Saved work offsets: %s", (n, saved_work_offset)) # set twp-state to 'defined' (1) self.execute("M68 E2 Q1") yield INTERP_EXECUTE_FINISH gui_update_twp(self) return INTERP_OK # definition of a virtual work-plane (twp) using different methods set by the 'p'-word def g682(self, **words): global twp_matrix, pre_rot, twp_flag, twp_build_params, saved_work_offset_number, saved_work_offset if self.task == 0: # ignore the preview interpreter yield INTERP_EXECUTE_FINISH return INTERP_OK # ! IMPORTANT ! # We need to use 'yield INTERP_EXECUTE_FINISH' here to stop the read ahead # and avoid it executing the rest of the remap ahead of time ## NOTE: No 'self.execute(..)' command can be used after 'yield INTERP_EXECUTE_FINISH' yield INTERP_EXECUTE_FINISH if hal.get_value(twp_is_defined): # ie TWP has already been defined # reset the twp parameters reset_twp_params(self) msg = ("G68.2: TWP already defined.") log.debug(msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR # NOTE: Due to easier abort handling we currently restrict the use of twp to G54 # as LinuxCNC seems to revert to G54 as the default system (n, offsets) = get_current_work_offset(self) if n != 1: # reset the twp parameters reset_twp_params(self) msg = "G68.2 ERROR: Must be in G54 to define TWP." log.debug(msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR # collect the currently active work offset values (ie g54, g55 or other) saved_work_offset_number = n saved_work_offset = offsets log.debug("G68.2: Saved work offsets %s", (n, saved_work_offset)) c = self.blocks[self.remap_level] p = c.p_number if c.p_flag else 0 if p == 0: # true euler angles (this is the default mode) twp_flag = [int(p), 1, 'empty'] # one call to define the twp in this mode # parse requested order of rotations (default is '313' ie: ZXZ) q = str(int(c.q_number if c.q_flag else 313)) if q not in ['121','131','212','232','313','323']: # reset the twp parameters reset_twp_params(self) msg = ("G68.2 (P0): No recognised Q-Word found.") log.debug(msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR # parse the requested origin x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 # parse the requested rotation of tool-x around the origin r = c.r_number if c.r_flag else 0 # parse the requested euler rotation angles th1 = c.i_number if c.i_flag else 0 th2 = c.j_number if c.j_flag else 0 th3 = c.k_number if c.k_flag else 0 # build the translation vector of the twp_matrix twp_origin = [[x], [y], [z]] # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') log.debug('G68.2 (P0): Twp_origin_rotation \n%s',twp_origin_rotation) # build the rotation matrix for the requested euler rotation twp_euler_rotation = twp_calc_euler_rot_matrix(th1, th2, th3, q) log.debug('G68.2 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_euler_rotation) # combine rotation and translation and form the 4x4 twp-transformation matrix twp_matrix = np.hstack((twp_rotation, twp_origin)) twp_row_4 = [0,0,0,1] twp_matrix = np.vstack((twp_matrix, twp_row_4)) twp_matrix = np.asmatrix(twp_matrix) # update the build state of the twp call twp_flag[2] = 'done' elif p == 1: # non-true euler angles, eg: 'pitch,roll,yaw' twp_flag = [int(p), 1, 'empty'] # one call to define the twp in this mode # parse requested order of rotations (default is '123' ie: XYZ) q = str(int(c.q_number if c.q_flag else 123)) if q not in ['123','132','213','231','312','321']: # reset the twp parameters reset_twp_params(self) msg = ("G68.2 P1: No recognised Q-Word found.") log.debug(msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR # parse the requested origin x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 # parse the requested rotation of tool-x around the origin r = c.r_number if c.r_flag else 0 # parse the requested euler rotation angles th1 = c.i_number if c.i_flag else 0 th2 = c.j_number if c.j_flag else 0 th3 = c.k_number if c.k_flag else 0 # build the translation vector of the twp_matrix twp_origin = [[x], [y], [z]] # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') log.debug('G68.2 P1: Twp_origin_rotation \n%s',twp_origin_rotation) # build the rotation matrix for the requested euler rotation twp_euler_rotation = twp_calc_euler_rot_matrix(th1, th2, th3, q) log.debug('G68.2 P1: Twp_euler_rotation \n%s',twp_euler_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_euler_rotation) # combine rotation and translation and form the 4x4 twp-transformation matrix twp_matrix = np.hstack((twp_rotation, twp_origin)) twp_row_4 = [0,0,0,1] twp_matrix = np.vstack((twp_matrix, twp_row_4)) twp_matrix = np.asmatrix(twp_matrix) # update the build state of the twp call twp_flag[2] = 'done' elif p == 2: # twp defined py 3 points on the plane # if this is the first call for this mode reset the twp_flag flag if not twp_flag: twp_flag = [int(p), 4 , 'empty', 'empty', 'empty', 'empty'] # four calls needed twp_build_params = {'q0':[], 'q1':[], 'q2':[], 'q3':[]} # Point 1: defines the origin of the twp # Point 2: direction from P1 to P2 defines the positive x direction on the twp (tool-x) # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (tool-z) q = int(c.q_number if c.q_flag else 0) # this mode needs four calls to fill all required parameters if q == 0: # define new origin and rotation x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 # parse the requested rotation of tool-x around the origin r = c.r_number if c.r_flag else 0 twp_build_params['q0'] = [x,y,z,r] twp_flag[2] = 'done' elif q == 1: # define point 1 x1 = c.x_number if c.x_flag else 0 y1 = c.y_number if c.y_flag else 0 z1 = c.z_number if c.z_flag else 0 twp_build_params['q1'] = [x1,y1,z1] twp_flag[3] = 'done' elif q == 2: # define point 2 x2 = c.x_number if c.x_flag else 0 y2 = c.y_number if c.y_flag else 0 z2 = c.z_number if c.z_flag else 0 twp_build_params['q2'] = [x2,y2,z2] twp_flag[4] = 'done' elif q == 3: # define point 3 x3 = c.x_number if c.x_flag else 0 y3 = c.y_number if c.y_flag else 0 z3 = c.z_number if c.z_flag else 0 twp_build_params['q3'] = [x3,y3,z3] twp_flag[5] = 'done' else: # reset the twp parameters reset_twp_params(self) msg = ("G68.2 P2: No recognised Q-Word found.") log.debug(msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR # only start calculations once all the parameters have been passed if twp_flag.count('done') == twp_flag[1]: [x, y, z, r] = twp_build_params['q0'][0:4] # build the translation vector of the twp_matrix twp_origin = [[x], [y], [z]] p1 = twp_build_params['q1'][0:3] p2 = twp_build_params['q2'] p3 = twp_build_params['q3'] log.debug("G68.2 P2: Point 1: %s",p1) log.debug("G68.2 P2: Point 2: %s",p2) log.debug("G68.2 P2: Point 3: %s",p3) # build vectors x:P1->P2 and v2:P1->P3 twp_vect_x = [p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]] log.debug("G68.2 P2: Twp_vect_x: \n%s",twp_vect_x) v2 = [p3[0]-p1[0], p3[1]-p1[1], p3[2]-p1[2]] log.debug("G68.2 P2 (v2): %s",v2) # normalize the two vectors twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) v2 = v2 / np.linalg.norm(v2) # we can use the cross product to calculate the tool-z vector # note: if P3 is on the right side of the vector P1->P2 # then the tool-z will be below the twp (ie tool-z will be downwards) twp_vect_z = np.cross(twp_vect_x , v2) log.debug("G68.2 P2: Twp_vect_z %s",twp_vect_z) # we can use the cross product to calculate the tool-y vector twp_vect_y = np.cross(twp_vect_z, twp_vect_x) log.debug("G68.2 P2: Twp_vect_y %s",twp_vect_y) # build the rotation matrix of the twp_matrix from the calculated tool-vectors # first stack the vectors (lists) and then flip diagonally (transpose) # so the vectors are now vertical twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) twp_vect_rotation = np.transpose(twp_vect_rotation_t) log.debug("G68.2 P2: Built the twp-rotation-matrix: \n%s", twp_vect_rotation) # convert requested origin rotation to radians # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') log.debug('G68.2 P2: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_rotation) # add the origin translation on the right twp_matrix = np.hstack((twp_rotation, twp_origin)) # expand to 4x4 array and make into a matrix twp_row_4 = [0,0,0,1] twp_matrix = np.vstack((twp_matrix, twp_row_4)) twp_matrix = np.asmatrix(twp_matrix) log.debug("G68.2 P2: Built twp-transformation-matrix: \n%s", twp_matrix) elif p == 3: # two vectors (vector 1 defines the tool-x and vector 2 defines the tool-z) q = int(c.q_number if c.q_flag else 0) # if this is the first call for this mode reset the twp_flag flag if not twp_flag: log.info('first call') twp_flag = [int(p), 2 , 'empty', 'empty'] # two calls needed twp_build_params = {'q0':[], 'q1':[]} log.debug('twp_build_params: %s', twp_build_params) if q == 0: # define new origin of the twp x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 # parse the requested rotation of tool-x around the origin r = c.r_number if c.r_flag else 0 # first vector (direction of x in the twp) i = c.i_number if c.i_flag else 0 j = c.j_number if c.j_flag else 0 k = c.k_number if c.k_flag else 0 twp_build_params['q0'] = [x,y,z,i,j,k,r] twp_flag[2] = 'done' elif q == 1: # define second vector (the normal vector of the twp i1 = c.i_number if c.i_flag else 0 j1 = c.j_number if c.j_flag else 0 k1 = c.k_number if c.k_flag else 0 twp_build_params['q1'] = [i1,j1,k1] twp_flag[3] = 'done' else: # reset the twp parameters reset_twp_params(self) msg = ("G68.2 P3: No recognised Q-Word found.") log.debug(msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR # only start calculations once all the parameters have been passed if twp_flag.count('done') == twp_flag[1]: twp_origin = (x ,y, z) = twp_build_params['q0'][0:3] r = twp_build_params['q0'][6] (i, j, k) = twp_build_params['q0'][3:6] (i1, j1, k1) = twp_build_params['q1'] log.debug("(x, y, z): %s", (x, y, z)) log.debug("(i, j, k): %s", (i, j, k)) log.debug("(i1, j1, k1): %s", (i1, j1, k1)) # build unit vector defining tool-x direction twp_vect_x = [i-x, j-y, k-z] twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) twp_vect_z = [i1, j1, k1] twp_vect_z = twp_vect_z / np.linalg.norm(twp_vect_z) orth = np.dot(twp_vect_x, twp_vect_z) log.debug("orth check: %s", orth) # the two vectors must be orthogonal if orth != 0: reset_twp_params(self) msg = ("G68.2 P3: Vectors are not orthogonal.") log.debug(msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR # we can use the cross product to calculate the tool-y vector twp_vect_y = np.cross(twp_vect_z, twp_vect_x) log.debug("G68.2 P3: twp_vect_y %s",twp_vect_y) # build the rotation matrix of the twp_matrix from the calculated tool-vectors # first stack the vectors (lists) and then flip diagonally (transpose) # so the vectors are now vertical twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) twp_vect_rotation = np.transpose(twp_vect_rotation_t) log.debug("G68.2 P3: Built twp-rotation-matrix: \n%s", twp_vect_rotation) # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation try: twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') except Exception as e: log.info('G68.2 P3: twp_origin_rotation failed, %s', e) log.debug('G68.2 P3: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_rotation) # add the origin translation on the right twp_origin = [[x], [y], [z]] twp_matrix = np.hstack((twp_rotation, twp_origin)) # expand to 4x4 array and make into a matrix twp_row_4 = [0,0,0,1] twp_matrix = np.vstack((twp_matrix, twp_row_4)) twp_matrix = np.asmatrix(twp_matrix) log.debug("G68.2 P3: Built twp-transformation-matrix: \n%s", twp_matrix) else: # reset the twp parameters reset_twp_params(self) msg = ("G68.2: No recognised P-Word found.") log.debug(msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR log.debug("G68.2: twp_flag: %s", twp_flag) log.debug("G68.2: calls required: %s", twp_flag.count('done')) log.debug("G68.2: number of calls made: %s", twp_flag.count('done')) if twp_flag.count('done') == twp_flag[1]: log.info('G68.2: requested rotation: %s', radians(r)) log.info("G68.2: twp-tranformation-matrix: \n%s",twp_matrix) twp_origin = [twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]] log.info("G68.2: twp origin: %s", twp_origin) twp_vect_x = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] log.info("G68.2: twp vector-x: %s", twp_vect_x) twp_vect_z = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] log.info("G68.2: twp vector-z: %s", twp_vect_z) # set twp-state to 'defined' (1) self.execute("M68 E2 Q1") yield INTERP_EXECUTE_FINISH gui_update_twp(self) return INTERP_OK # incremental definition of a virtual work-plane (twp) using different methods set by the 'p'-word def g684(self, **words): global twp_matrix, pre_rot, twp_flag, twp_build_params, saved_work_offset_number, saved_work_offset if self.task == 0: # ignore the preview interpreter yield INTERP_EXECUTE_FINISH return INTERP_OK # ! IMPORTANT ! # We need to use 'yield INTERP_EXECUTE_FINISH' here to stop the read ahead # and avoid it executing the rest of the remap ahead of time ## NOTE: No 'self.execute(..)' command can be used after 'yield INTERP_EXECUTE_FINISH' yield INTERP_EXECUTE_FINISH if not hal.get_value(twp_is_active): # ie there is currently no TWP defined # reset the twp parameters reset_twp_params(self) msg = ("G68.4: No TWP active to increment from. Run G68.2 or G68.3 first.") log.debug(msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR # collect the currently active work offset values (ie g54, g55 or other) n = get_current_work_offset(self)[0] # Must be in one of the dedicated offset systems for TWP if False: #n < 6: # reset the twp parameters reset_twp_params(self) msg = ("G68.4 ERROR: Must be in G59, G59.x to increment TWP.") log.debug(msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR # store the current TWP to twp_matrix_current = np.matrix.copy(twp_matrix) c = self.blocks[self.remap_level] p = c.p_number if c.p_flag else 0 if p == 0: # true euler angles (this is the default mode) twp_flag = [int(p), 1, 'empty'] # one call to define the twp in this mode # parse requested order of rotations (default is '313' ie: ZXZ) q = str(int(c.q_number if c.q_flag else 313)) if q not in ['121','131','212','232','313','323']: # reset the twp parameters reset_twp_params(self) msg = ("G68.4 (P0): No recognised Q-Word found.") log.debug(msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR # parse the requested origin x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 # parse the requested rotation of tool-x around the origin r = c.r_number if c.r_flag else 0 # parse requested euler angles th1 = c.i_number if c.i_flag else 0 th2 = c.j_number if c.j_flag else 0 th3 = c.k_number if c.k_flag else 0 # build the translation vector of the twp_matrix twp_origin = [[x], [y], [z]] # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') log.debug('G68.4 (P0): Twp_origin_rotation \n%s',twp_origin_rotation) # build the rotation matrix for the requested euler rotation twp_euler_rotation = twp_calc_euler_rot_matrix(th1, th2, th3, q) log.debug('G68.4 (P0): Twp_euler_rotation \n%s',twp_euler_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_euler_rotation) # combine rotation and translation and form the 4x4 twp-transformation matrix twp_matrix = np.hstack((twp_rotation, twp_origin)) twp_row_4 = [0,0,0,1] twp_matrix = np.vstack((twp_matrix, twp_row_4)) twp_matrix = np.asmatrix(twp_matrix) # update the build state of the twp call twp_flag[2] = 'done' elif p == 1: # non-true euler angles, eg: 'pitch,roll,yaw' twp_flag = [int(p), 1, 'empty'] # one call to define the twp in this mode # parse requested order of rotations (default is '123' ie: XYZ) q = str(int(c.q_number if c.q_flag else 123)) if q not in ['123','132','213','231','312','321']: # reset the twp parameters reset_twp_params(self) msg = ("G68.4 P1: No recognised Q-Word found.") log.debug(msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR # parse the requested origin x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 # parse the requested rotation of tool-x around the origin r = c.r_number if c.r_flag else 0 # parse the requested euler rotation angles th1 = c.i_number if c.i_flag else 0 th2 = c.j_number if c.j_flag else 0 th3 = c.k_number if c.k_flag else 0 # build the translation vector of the twp_matrix twp_origin = [[x], [y], [z]] # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') log.debug('G68.4 P1: Twp_origin_rotation \n%s',twp_origin_rotation) # build the rotation matrix for the requested euler rotation twp_euler_rotation = twp_calc_euler_rot_matrix(th1, th2, th3, q) log.debug('G68.4 P1: Twp_euler_rotation \n%s',twp_euler_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_euler_rotation) # combine rotation and translation and form the 4x4 twp-transformation matrix twp_matrix = np.hstack((twp_rotation, twp_origin)) twp_row_4 = [0,0,0,1] twp_matrix = np.vstack((twp_matrix, twp_row_4)) twp_matrix = np.asmatrix(twp_matrix) # update the build state of the twp call twp_flag[2] = 'done' elif p == 2: # twp defined py 3 points on the plane # if this is the first call for this mode reset the twp_flag flag if not twp_flag: twp_flag = [int(p), 4 , 'empty', 'empty', 'empty', 'empty'] # four calls needed twp_build_params = {'q0':[], 'q1':[], 'q2':[], 'q3':[]} # Point 1: defines the origin of the twp # Point 2: direction from P1 to P2 defines the positive x direction on the twp (tool-x) # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (tool-z) q = int(c.q_number if c.q_flag else 0) # this mode needs four calls to fill all required parameters if q == 0: # define new origin and rotation x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 # parse the requested rotation of tool-x around the origin r = c.r_number if c.r_flag else 0 twp_build_params['q0'] = [x,y,z,r] twp_flag[2] = 'done' elif q == 1: # define point 1 x1 = c.x_number if c.x_flag else 0 y1 = c.y_number if c.y_flag else 0 z1 = c.z_number if c.z_flag else 0 twp_build_params['q1'] = [x1,y1,z1] twp_flag[3] = 'done' elif q == 2: # define point 2 x2 = c.x_number if c.x_flag else 0 y2 = c.y_number if c.y_flag else 0 z2 = c.z_number if c.z_flag else 0 twp_build_params['q2'] = [x2,y2,z2] twp_flag[4] = 'done' elif q == 3: # define point 3 x3 = c.x_number if c.x_flag else 0 y3 = c.y_number if c.y_flag else 0 z3 = c.z_number if c.z_flag else 0 twp_build_params['q3'] = [x3,y3,z3] twp_flag[5] = 'done' else: # reset the twp parameters reset_twp_params(self) msg = ("G68.4 P2: No recognised Q-Word found.") log.debug(msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR # only start calculations once all the parameters have been passed if twp_flag.count('done') == twp_flag[1]: [x, y, z, r] = twp_build_params['q0'][0:4] # build the translation vector of the twp_matrix twp_origin = [[x], [y], [z]] p1 = twp_build_params['q1'][0:3] p2 = twp_build_params['q2'] p3 = twp_build_params['q3'] log.debug("G68.4 P2: Point 1: %s",p1) log.debug("G68.4 P2: Point 2: %s",p2) log.debug("G68.4 P2: Point 3: %s",p3) # build vectors x:P1->P2 and v2:P1->P3 twp_vect_x = [p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]] log.debug("G68.4 P2: Twp_vect_x: \n%s",twp_vect_x) v2 = [p3[0]-p1[0], p3[1]-p1[1], p3[2]-p1[2]] log.debug("G68.4 P2: (v2) %s", v2) # normalize the two vectors twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) v2 = v2 / np.linalg.norm(v2) # we can use the cross product to calculate the tool-z vector # note: if P3 is on the right side of the vector P1->P2 # then the tool-z will be below the twp (ie tool-z will be downwards) twp_vect_z = np.cross(twp_vect_x , v2) log.debug("G68.4 P2: Twp_vect_z %s",twp_vect_z) # we can use the cross product to calculate the tool-y vector twp_vect_y = np.cross(twp_vect_z, twp_vect_x) log.debug("G68.4 P2: Twp_vect_y %s",twp_vect_y) # build the rotation matrix of the twp_matrix from the calculated tool-vectors # first stack the vectors (lists) and then flip diagonally (transpose) # so the vectors are now vertical twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) twp_vect_rotation = np.transpose(twp_vect_rotation_t) log.debug("G68.4 P2: Built the twp-rotation-matrix: \n%s", twp_vect_rotation) # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation try: twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') except Exception as e: log.debug('G68.4 P2: twp_origin_rotation failed ', e) log.debug('G68.4 P2: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_rotation) # add the origin translation on the right twp_matrix = np.hstack((twp_rotation, twp_origin)) # expand to 4x4 array and make into a matrix twp_row_4 = [0,0,0,1] twp_matrix = np.vstack((twp_matrix, twp_row_4)) twp_matrix = np.asmatrix(twp_matrix) log.debug("G68.4 P2: Built twp-transformation-matrix: \n%s", twp_matrix) elif p == 3: # two vectors (vector 1 defines the tool-x and vector 2 defines the tool-z) q = int(c.q_number if c.q_flag else 0) # if this is the first call for this mode reset the twp_flag flag if not twp_flag: twp_flag = [int(p), 2 , 'empty', 'empty'] # two calls needed twp_build_params = {'q0':[], 'q1':[]} if q == 0: # define new origin and first vector (direction of x in the twp) x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 # parse the requested rotation of tool-x around the origin r = c.r_number if c.r_flag else 0 # first vector (direction of x in the twp) i = c.i_number if c.i_flag else 0 j = c.j_number if c.j_flag else 0 k = c.k_number if c.k_flag else 0 twp_build_params['q0'] = [x,y,z,i,j,k,r] twp_flag[2] = 'done' elif q == 1: # define second vector (the normal vector of the twp i1 = c.i_number if c.i_flag else 0 j1 = c.j_number if c.j_flag else 0 k1 = c.k_number if c.k_flag else 0 twp_build_params['q1'] = [i1,j1,k1] twp_flag[3] = 'done' else: # reset the twp parameters reset_twp_params(self) msg = ("G68.4 P3: No recognised Q-Word found.") log.debug(msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR # only start calculations once all the parameters have been passed if twp_flag.count('done') == twp_flag[1]: twp_origin = (x ,y, z) = twp_build_params['q0'][0:3] r = twp_build_params['q0'][6] (i, j, k) = twp_build_params['q0'][3:6] (i1, j1, k1) = twp_build_params['q1'] log.debug("(x, y, z) %s", (x, y, z)) log.debug("(i, j, k) %s", (i, j, k)) log.debug("(i1, j1, k1) %s", (i1, j1, k1)) # build unit vector defining tool-x direction twp_vect_x = [i-x, j-y, k-z] twp_vect_x = twp_vect_x / np.linalg.norm(twp_vect_x) twp_vect_z = [i1, j1, k1] twp_vect_z = twp_vect_z / np.linalg.norm(twp_vect_z) orth = np.dot(twp_vect_x, twp_vect_z) log.debug("orth check: %s", orth) # the two vectors must be orthogonal if orth != 0: # reset the twp parameters reset_twp_params(self) ## reset the parameter values #twp_flag = [int(p), 2 , 'empty', 'empty'] # two calls needed #twp_build_params = {'q0':[], 'q1':[]} msg = ("G68.4 P3: Vectors are not orthogonal.") log.debug(msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR # we can use the cross product to calculate the tool-y vector twp_vect_y = np.cross(twp_vect_z, twp_vect_x) log.debug("G68.4 P3: twp_vect_y %s",twp_vect_y) # build the rotation matrix of the twp_matrix from the calculated tool-vectors # first stack the vectors (lists) and then flip diagonally (transpose) # so the vectors are now vertical twp_vect_rotation_t = np.vstack((twp_vect_x, twp_vect_y)) twp_vect_rotation_t = np.vstack((twp_vect_rotation_t, twp_vect_z)) twp_vect_rotation = np.transpose(twp_vect_rotation_t) log.debug("G68.4 P3: Built twp-rotation-matrix: \n%s", twp_vect_rotation) # we use xzx-euler rotation to create the rotation matrix for the requested origin rotation twp_origin_rotation = twp_calc_euler_rot_matrix(0, r, 0, '131') log.debug('G68.4 P3: Twp-origin-rotation-matrix \n%s',twp_origin_rotation) # calculate the total twp_rotation using matrix multiplication twp_rotation = np.asmatrix(twp_origin_rotation) * np.asmatrix(twp_vect_rotation) # add the origin translation on the right twp_origin = [[x], [y], [z]] twp_matrix = np.hstack((twp_rotation, twp_origin)) # expand to 4x4 array and make into a matrix twp_row_4 = [0,0,0,1] twp_matrix = np.vstack((twp_matrix, twp_row_4)) twp_matrix = np.asmatrix(twp_matrix) log.debug("G68.4 P3: Built twp-transformation-matrix: \n%s", twp_matrix) else: # reset the twp parameters reset_twp_params(self) msg = ("G68.4: No recognised P-Word found.") log.debug(msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR log.debug("G68.4: twp_flag: %s", twp_flag) log.debug("G68.4: calls required: %s", twp_flag.count('done')) log.debug("G68.4: number of calls made: %s", twp_flag.count('done')) if twp_flag.count('done') == twp_flag[1]: log.info('G68.4: requested rotation %s', radians(r)) log.info("G68.4: twp_matrix_current: \n%s", twp_matrix_current) log.info("G68.4: incremental twp_matrix requested: \n%s",twp_matrix) log.info("G68.4: calculating new twp_matrix...") twp_matrix_new = twp_matrix_current * twp_matrix log.info("G68.4: twp_matrix_new: \n%s",twp_matrix_new) twp_origin = [twp_matrix[0,3],twp_matrix[1,3],twp_matrix[2,3]] log.info("G68.4: twp origin: %s", twp_origin) twp_vect_x = [twp_matrix[0,0],twp_matrix[1,0],twp_matrix[2,0]] log.info("G68.4: twp vector-x: %s", twp_vect_x) twp_vect_z = [twp_matrix[0,2],twp_matrix[1,2],twp_matrix[2,2]] log.info("G68.4: twp vector-z: %s", twp_vect_z) log.info("G68.4: incremented twp_matrix: \n%s", twp_matrix_new) twp_matrix = twp_matrix_new # set twp-state to 'defined' (1) self.execute("M68 E2 Q1") yield INTERP_EXECUTE_FINISH gui_update_twp(self) return INTERP_OK