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 05fc53261a1..7431b8ad635 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 @@ -914,8 +914,9 @@ def g682(self, **words): # 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) + # calculate the total twp_rotation using matrix multiplication, + # R turns about the plane's own Z + twp_rotation = np.asmatrix(twp_euler_rotation) * np.asmatrix(twp_origin_rotation) # combine rotation and translation and form the 4x4 twp-transformation matrix twp_matrix = np.hstack((twp_rotation, twp_origin)) twp_row_4 = [0,0,0,1] @@ -961,8 +962,9 @@ def g682(self, **words): # 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) + # calculate the total twp_rotation using matrix multiplication, + # R turns about the plane's own Z + twp_rotation = np.asmatrix(twp_euler_rotation) * np.asmatrix(twp_origin_rotation) # combine rotation and translation and form the 4x4 twp-transformation matrix twp_matrix = np.hstack((twp_rotation, twp_origin)) twp_row_4 = [0,0,0,1] @@ -981,39 +983,38 @@ def g682(self, **words): # 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':[]} + twp_flag = [int(p), 3 , 'empty', 'empty', 'empty'] # Q1, Q2 and Q3 needed, Q0 optional + twp_build_params = {'q0':[0, 0, 0], 'q1':[], 'q2':[], 'q3':[]} # Point 1: defines the origin of the twp # Point 2: direction from P1 to P2 defines the positive x direction on the twp (x-vector) # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (z-vector) q = int(c.q_number if c.q_flag else 0) - # this mode needs four calls to fill all required parameters - if q == 0: # define new origin and rotation + # this mode needs three calls, plus an optional Q0 first + if q == 0 and 'done' in twp_flag[2:]: + q = -1 # Q0 after the points, refused below + if q == 0: # shift of the origin from point 1, along the plane's axes x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # 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' + twp_build_params['q0'] = [x,y,z] elif q == 1: # define point 1 x1 = c.x_number if c.x_flag else 0 y1 = c.y_number if c.y_flag else 0 z1 = c.z_number if c.z_flag else 0 twp_build_params['q1'] = [x1,y1,z1] - twp_flag[3] = 'done' + twp_flag[2] = 'done' elif q == 2: # define point 2 x2 = c.x_number if c.x_flag else 0 y2 = c.y_number if c.y_flag else 0 z2 = c.z_number if c.z_flag else 0 twp_build_params['q2'] = [x2,y2,z2] - twp_flag[4] = 'done' + twp_flag[3] = 'done' elif q == 3: # define point 3 x3 = c.x_number if c.x_flag else 0 y3 = c.y_number if c.y_flag else 0 z3 = c.z_number if c.z_flag else 0 twp_build_params['q3'] = [x3,y3,z3] - twp_flag[5] = 'done' + twp_flag[4] = 'done' else: # reset the twp parameters reset_twp_params() @@ -1024,11 +1025,23 @@ def g682(self, **words): yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR + # R may come on any one block of the definition + if c.r_flag: + if 'r_q' in twp_build_params: + msg = ("G68.2 P2: R given twice, on Q%d and on Q%d" % (twp_build_params['r_q'], q)) + reset_twp_params() + log.debug(' ' + msg) + emccanon.CANON_ERROR(msg) + yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed + yield INTERP_EXIT # w/o this the error does not abort a running gcode program + return INTERP_ERROR + twp_build_params['r_q'] = q + twp_build_params['r'] = radians(c.r_number) + # only start calculations once all the parameters have been passed if twp_flag.count('done') == twp_flag[1]: - [x, y, z, r] = twp_build_params['q0'][0:4] - # build the translation vector of the twp_matrix - twp_origin = [[x], [y], [z]] + [x, y, z] = twp_build_params['q0'][0:3] + r = twp_build_params.get('r', 0) p1 = twp_build_params['q1'][0:3] p2 = twp_build_params['q2'] p3 = twp_build_params['q3'] @@ -1064,9 +1077,12 @@ def g682(self, **words): 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) + # calculate the total twp_rotation using matrix multiplication, + # R turns about the plane's own Z + twp_rotation = np.asmatrix(twp_vect_rotation) * np.asmatrix(twp_origin_rotation) # add the origin translation on the right + # the origin is point 1, moved by Q0's x y z along the plane's own axes + twp_origin = np.add(np.asmatrix([[p1[0]], [p1[1]], [p1[2]]]), twp_rotation * np.asmatrix([[x], [y], [z]])) twp_matrix = np.hstack((twp_rotation, twp_origin)) # expand to 4x4 array and make into a matrix twp_row_4 = [0,0,0,1] @@ -1077,16 +1093,14 @@ def g682(self, **words): 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':[]} + twp_build_params = {'q1':[], 'q2':[]} log.debug(' twp_build_params: %s', twp_build_params) - if q == 0: # define new origin of the twp + if q == 1: # define new origin and first vector (direction of x in the twp) x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 @@ -1096,13 +1110,13 @@ def g682(self, **words): 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_build_params['q1'] = [x,y,z,i,j,k,r] twp_flag[2] = 'done' - elif q == 1: # define second vector (the normal vector of the twp + elif q == 2: # define second vector (the normal vector of the twp) i1 = c.i_number if c.i_flag else 0 j1 = c.j_number if c.j_flag else 0 k1 = c.k_number if c.k_flag else 0 - twp_build_params['q1'] = [i1,j1,k1] + twp_build_params['q2'] = [i1,j1,k1] twp_flag[3] = 'done' else: # reset the twp parameters @@ -1116,30 +1130,37 @@ def g682(self, **words): # 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'] + twp_origin = (x ,y, z) = twp_build_params['q1'][0:3] + r = twp_build_params['q1'][6] + (i, j, k) = twp_build_params['q1'][3:6] + (i1, j1, k1) = twp_build_params['q2'] log.debug("(x, y, z): %s", (x, y, z)) log.debug("(i, j, k): %s", (i, j, k)) log.debug("(i1, j1, k1): %s", (i1, j1, k1)) - # 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) + # X is kept as given and Z is projected onto the plane normal + # to it, as Fanuc does; vectors 5 degrees or more off square, + # or a zero vector, are refused + twp_vect_x = np.array([i, j, k], dtype=float) + twp_vect_z = np.array([i1, j1, k1], dtype=float) + len_x = np.linalg.norm(twp_vect_x) + len_z = np.linalg.norm(twp_vect_z) + orth = 1.0 + if len_x > 1e-12 and len_z > 1e-12: + twp_vect_x = twp_vect_x / len_x + twp_vect_z = twp_vect_z / len_z + orth = np.dot(twp_vect_x, twp_vect_z) log.debug(" orth check: %s", orth) - # the two vectors must be orthogonal - if orth > 0.001: + if abs(orth) >= sin(radians(5)): reset_twp_params() - msg = ("G68.2 P3: Vectors are not orthogonal.") + msg = ("G68.2 P3: The vectors are zero or 5 degrees or more off square.") log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR + twp_vect_z = twp_vect_z - orth * twp_vect_x + twp_vect_z = twp_vect_z / np.linalg.norm(twp_vect_z) # we can use the cross product to calculate the y vector twp_vect_y = np.cross(twp_vect_z, twp_vect_x) log.debug(" G68.2 P3: twp_vect_y %s",twp_vect_y) @@ -1156,8 +1177,9 @@ def g682(self, **words): 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) + # calculate the total twp_rotation using matrix multiplication, + # R turns about the plane's own Z + twp_rotation = np.asmatrix(twp_vect_rotation) * np.asmatrix(twp_origin_rotation) # add the origin translation on the right twp_origin = [[x], [y], [z]] twp_matrix = np.hstack((twp_rotation, twp_origin)) @@ -1215,10 +1237,10 @@ def g684(self, **words): ## 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 + if not hal.get_value(twp_is_defined): # ie there is currently no TWP defined # reset the twp parameters reset_twp_params() - msg = ("G68.4: No TWP active to increment from. Run G68.2 or G68.3 first.") + msg = ("G68.4: No TWP defined to increment from. Run G68.2 or G68.3 first.") log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed @@ -1280,8 +1302,9 @@ def g684(self, **words): # 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) + # calculate the total twp_rotation using matrix multiplication, + # R turns about the plane's own Z + twp_rotation = np.asmatrix(twp_euler_rotation) * np.asmatrix(twp_origin_rotation) # combine rotation and translation and form the 4x4 twp-transformation matrix twp_matrix = np.hstack((twp_rotation, twp_origin)) twp_row_4 = [0,0,0,1] @@ -1327,8 +1350,9 @@ def g684(self, **words): # 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) + # calculate the total twp_rotation using matrix multiplication, + # R turns about the plane's own Z + twp_rotation = np.asmatrix(twp_euler_rotation) * np.asmatrix(twp_origin_rotation) # combine rotation and translation and form the 4x4 twp-transformation matrix twp_matrix = np.hstack((twp_rotation, twp_origin)) twp_row_4 = [0,0,0,1] @@ -1347,39 +1371,38 @@ def g684(self, **words): # 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':[]} + twp_flag = [int(p), 3 , 'empty', 'empty', 'empty'] # Q1, Q2 and Q3 needed, Q0 optional + twp_build_params = {'q0':[0, 0, 0], 'q1':[], 'q2':[], 'q3':[]} # Point 1: defines the origin of the twp # Point 2: direction from P1 to P2 defines the positive x direction on the twp (x-vector) # Point 3: defines the positive y side and with P1 and P2 defines the xy work plane (z-vector) q = int(c.q_number if c.q_flag else 0) - # this mode needs four calls to fill all required parameters - if q == 0: # define new origin and rotation + # this mode needs three calls, plus an optional Q0 first + if q == 0 and 'done' in twp_flag[2:]: + q = -1 # Q0 after the points, refused below + if q == 0: # shift of the origin from point 1, along the plane's axes x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 - # 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' + twp_build_params['q0'] = [x,y,z] elif q == 1: # define point 1 x1 = c.x_number if c.x_flag else 0 y1 = c.y_number if c.y_flag else 0 z1 = c.z_number if c.z_flag else 0 twp_build_params['q1'] = [x1,y1,z1] - twp_flag[3] = 'done' + twp_flag[2] = 'done' elif q == 2: # define point 2 x2 = c.x_number if c.x_flag else 0 y2 = c.y_number if c.y_flag else 0 z2 = c.z_number if c.z_flag else 0 twp_build_params['q2'] = [x2,y2,z2] - twp_flag[4] = 'done' + twp_flag[3] = 'done' elif q == 3: # define point 3 x3 = c.x_number if c.x_flag else 0 y3 = c.y_number if c.y_flag else 0 z3 = c.z_number if c.z_flag else 0 twp_build_params['q3'] = [x3,y3,z3] - twp_flag[5] = 'done' + twp_flag[4] = 'done' else: # reset the twp parameters reset_twp_params() @@ -1390,11 +1413,23 @@ def g684(self, **words): yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR + # R may come on any one block of the definition + if c.r_flag: + if 'r_q' in twp_build_params: + msg = ("G68.4 P2: R given twice, on Q%d and on Q%d" % (twp_build_params['r_q'], q)) + reset_twp_params() + log.debug(' ' + msg) + emccanon.CANON_ERROR(msg) + yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed + yield INTERP_EXIT # w/o this the error does not abort a running gcode program + return INTERP_ERROR + twp_build_params['r_q'] = q + twp_build_params['r'] = radians(c.r_number) + # only start calculations once all the parameters have been passed if twp_flag.count('done') == twp_flag[1]: - [x, y, z, r] = twp_build_params['q0'][0:4] - # build the translation vector of the twp_matrix - twp_origin = [[x], [y], [z]] + [x, y, z] = twp_build_params['q0'][0:3] + r = twp_build_params.get('r', 0) p1 = twp_build_params['q1'][0:3] p2 = twp_build_params['q2'] p3 = twp_build_params['q3'] @@ -1430,9 +1465,12 @@ def g684(self, **words): 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) + # calculate the total twp_rotation using matrix multiplication, + # R turns about the plane's own Z + twp_rotation = np.asmatrix(twp_vect_rotation) * np.asmatrix(twp_origin_rotation) # add the origin translation on the right + # the origin is point 1, moved by Q0's x y z along the plane's own axes + twp_origin = np.add(np.asmatrix([[p1[0]], [p1[1]], [p1[2]]]), twp_rotation * np.asmatrix([[x], [y], [z]])) twp_matrix = np.hstack((twp_rotation, twp_origin)) # expand to 4x4 array and make into a matrix twp_row_4 = [0,0,0,1] @@ -1443,14 +1481,12 @@ def g684(self, **words): 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) + twp_build_params = {'q1':[], 'q2':[]} + if q == 1: # define new origin and first vector (direction of x in the twp) x = c.x_number if c.x_flag else 0 y = c.y_number if c.y_flag else 0 z = c.z_number if c.z_flag else 0 @@ -1460,13 +1496,13 @@ def g684(self, **words): 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_build_params['q1'] = [x,y,z,i,j,k,r] twp_flag[2] = 'done' - elif q == 1: # define second vector (the normal vector of the twp + elif q == 2: # define second vector (the normal vector of the twp) i1 = c.i_number if c.i_flag else 0 j1 = c.j_number if c.j_flag else 0 k1 = c.k_number if c.k_flag else 0 - twp_build_params['q1'] = [i1,j1,k1] + twp_build_params['q2'] = [i1,j1,k1] twp_flag[3] = 'done' else: # reset the twp parameters @@ -1480,34 +1516,37 @@ def g684(self, **words): # 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'] + twp_origin = (x ,y, z) = twp_build_params['q1'][0:3] + r = twp_build_params['q1'][6] + (i, j, k) = twp_build_params['q1'][3:6] + (i1, j1, k1) = twp_build_params['q2'] log.debug("(x, y, z) %s", (x, y, z)) log.debug("(i, j, k) %s", (i, j, k)) log.debug("(i1, j1, k1) %s", (i1, j1, k1)) - # 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) + # X is kept as given and Z is projected onto the plane normal + # to it, as Fanuc does; vectors 5 degrees or more off square, + # or a zero vector, are refused + twp_vect_x = np.array([i, j, k], dtype=float) + twp_vect_z = np.array([i1, j1, k1], dtype=float) + len_x = np.linalg.norm(twp_vect_x) + len_z = np.linalg.norm(twp_vect_z) + orth = 1.0 + if len_x > 1e-12 and len_z > 1e-12: + twp_vect_x = twp_vect_x / len_x + twp_vect_z = twp_vect_z / len_z + orth = np.dot(twp_vect_x, twp_vect_z) log.debug(" orth check: %s", orth) - # the two vectors must be orthogonal - if orth != 0: - # reset the twp parameters + if abs(orth) >= sin(radians(5)): 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.") + msg = ("G68.4 P3: The vectors are zero or 5 degrees or more off square.") log.debug(' ' + msg) emccanon.CANON_ERROR(msg) yield INTERP_EXECUTE_FINISH # w/o this the error message is not displayed yield INTERP_EXIT # w/o this the error does not abort a running gcode program return INTERP_ERROR + twp_vect_z = twp_vect_z - orth * twp_vect_x + twp_vect_z = twp_vect_z / np.linalg.norm(twp_vect_z) # we can use the cross product to calculate the y vector twp_vect_y = np.cross(twp_vect_z, twp_vect_x) log.debug(" G68.4 P3: twp_vect_y %s",twp_vect_y) @@ -1524,8 +1563,9 @@ def g684(self, **words): 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) + # calculate the total twp_rotation using matrix multiplication, + # R turns about the plane's own Z + twp_rotation = np.asmatrix(twp_vect_rotation) * np.asmatrix(twp_origin_rotation) # add the origin translation on the right twp_origin = [[x], [y], [z]] twp_matrix = np.hstack((twp_rotation, twp_origin)) 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..aaae4385c9d 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,150 @@ 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: a shift of 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; R on one block) +G68.2 P3 Q1 X- Y- Z- I- J- K- (two vectors: the origin and +X, then) +G68.2 P3 Q2 I- J- K- (+Z, the normal) +G68.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) +---- + +.'G68.2 X50 Y30 Z40 J30': a plane tilted 30 degrees about X, on G54, on the machine coordinates +image::images/g68-2-plane.svg["G68.2 tilted work plane",align="center"] + +'X', 'Y' and 'Z' are the plane's origin ('P2' gives it by points, below) 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 origin is the first point. The 'Q0' block, + which may be left out, moves the origin by its 'X', 'Y', 'Z' along the + plane's own axes, after 'R': 'Q0 X10' puts the origin 10 along the plane's + X from the first point, the way 'X10' in a program under the plane would. + 'R' may be given on any one of the blocks. +* '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 is kept as given. The normal need not be exactly at right + angles to it, within 5 degrees; the part of the normal along X is dropped. + +'R' turns the plane about its own Z after everything else, in degrees. + +The forms follow the Fanuc tilted working plane commands, with a few +extensions: 'R' on 'P0', 'P1' and 'P3' (Fanuc gives it with 'P2' only), +'Q' on 'P0' (Fanuc's Euler angles are always Z, X, Z), 'P1' orders that +repeat an axis, such as 'Q121', besides the six that name each axis once, +and all three angles zero, which is the plane of the coordinate system +underneath rather than an error, as on a Fanuc with parameter ATW set. + +The blocks of a 'P2' or 'P3' definition have to follow one another; any +other block in between is an error. A definition left unfinished in MDI is +dropped when a program is opened. A definition with a plane already active +replaces it, with the words in the coordinate system underneath, not in the +old plane. + +'G68.4' takes any 'G68.2' form and composes it onto the active plane: the +words are in the plane, and the result is a new plane relative to the old +one. It needs a plane to build on. + +The G-code preview draws every plane a program defines, so a program can +be checked for where its planes sit before it runs: a rectangle lying in +the plane, over the moves made under it with a margin around them, the +plane's own X, Y and Z at the centre of the rectangle in the colours of +the machine axes, and a cross at the plane's origin, which need not lie +in the rectangle: a program drilling a sphere puts every plane's origin +at the centre and works out on the surface. The rectangle is drawn at the plane's Z where the program comes +down to it, and otherwise at the end of the Z range the program worked +in that is nearest to it, so a drilling cycle that stays above the +origin shows its holes where they are cut; a plane nothing moved under +is drawn as a square sized from the program. A plane restated with the +same words is one plane, not one per restatement. The plane in effect +on the machine, the one the last executed 'G68.2' set, is drawn over +the others in its own colour, so as the program runs the plane it is +in stands out from the ones it has been in and will be in; a plane set +from MDI shows the same way, as a square with nothing under it. 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. + +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 two are 5 degrees or more off square. +* A word the form does not read is given: 'I', 'J' or 'K' with 'P2', 'X', + 'Y', 'Z' or 'R' on the 'P3' 'Q2' block, or 'A', 'B', 'C', 'U', 'V' or 'W' + with any form. +* 'R' is given on more than one block of a 'P2' definition. +* '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/images/g68-2-plane.svg b/docs/src/gcode/images/g68-2-plane.svg new file mode 100644 index 00000000000..13ed8c6df1d --- /dev/null +++ b/docs/src/gcode/images/g68-2-plane.svg @@ -0,0 +1,48 @@ + + + + + + + + + + +X + + +Y + + +Z + +G53 + + + +X + + +Y + + +Z + +G54 + + + + +J30 + + +X + + +Y + + +Z +G68.2 + + diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc index 90f9a06b4ec..44f9f9dcc54 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/preview_helpers.py b/lib/python/preview_helpers.py index 8193b0309ac..d4ac90be166 100644 --- a/lib/python/preview_helpers.py +++ b/lib/python/preview_helpers.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +from math import atan2, degrees, hypot def create_unitcode_and_initcode(s, inifile): a_axis_wrapped = inifile.getbool("AXIS_A", "WRAPPED_ROTARY", fallback=False) @@ -21,7 +22,36 @@ def create_unitcode_and_initcode(s, inifile): position = "%s%.8f " % (axis, pos) initcode += position active_gcodes = s.gcodes - for i in (3,6,14,7,5,4,9,12,10,16,8,11,13,15): + # 12 is the tilted work plane, restated with its words in unitcode + for i in (3,6,14,7,5,4,9,10,16,8,11,13,15): if active_gcodes[i] > -1: initcode = initcode + 'G' + str(active_gcodes[i]/10) + ' ' + plane = workplane_code(s) + if plane: + unitcode = unitcode + ' ' + plane return unitcode, initcode + +def workplane_code(s): + """The tilted work plane in effect on the machine as one G68.2 block, + or None. The bare code in s.gcodes cannot restate it: G68.2 alone is + a plane at the origin and G68.4 alone has nothing to build on. The + words are in machine units, so the block goes where the machine's G20 + or G21 is in force; a later unit change converts the plane. The block + selects the machine's coordinate system first, since the plane sits on + it and a later block may reselect it but not change it.""" + if not getattr(s, "g68_active", 0): + return None + r = s.g68_rotation + o = s.g68_offset + # G68.2 P1 turns about X by I, then Y by J, then Z by K: Rz Ry Rx + c = hypot(r[0], r[3]) + j = atan2(-r[6], c) + if c > 1e-12: + i = atan2(r[7], r[8]) + k = atan2(r[3], r[0]) + else: + i = 0.0 + k = atan2(-r[1], r[4]) + system = next((g for g in s.gcodes[1:] if 540 <= g <= 593), 540) + return "G%s G68.2 P1 X%.12f Y%.12f Z%.12f I%.12f J%.12f K%.12f" % ( + system // 10 if system % 10 == 0 else system / 10, o[0], o[1], o[2], degrees(i), degrees(j), degrees(k)) diff --git a/lib/python/rs274/glcanon.py b/lib/python/rs274/glcanon.py index 8d01545cb3c..8dfdd2c4ed5 100644 --- a/lib/python/rs274/glcanon.py +++ b/lib/python/rs274/glcanon.py @@ -182,6 +182,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. @@ -256,6 +259,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 ------------------------------------------------ @@ -543,6 +548,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 @@ -1023,6 +1032,12 @@ def get_workpiece_opacity(self): blending no one can explain.""" return min(1.0, max(0.0, float(getattr(self, 'workpiece_opacity', 0.0)))) + 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 @@ -1163,6 +1178,7 @@ def frame_context(self) -> glcanon_scene.FrameContext: show_small_origin=self.show_small_origin, show_workpiece=self.get_show_workpiece(), workpiece_opacity=self.get_workpiece_opacity(), + show_workplane=self.get_show_workplane(), program_alpha=self.get_program_alpha(), grid_size=self.get_grid_size(), highlight_line=self.get_highlight_line(), @@ -1247,6 +1263,15 @@ def posstrs(self): positions[X] = _x * math.cos(t) - _y * math.sin(t) positions[Y] = _x * math.sin(t) + _y * math.cos(t) positions = [(i-j) for i, j in zip(positions, s.g92_offset)] + if s.g68_active: + # the tilted work plane sits inside G92 + r = s.g68_rotation + _x = positions[X] - s.g68_offset[X] + _y = positions[Y] - s.g68_offset[Y] + _z = positions[Z] - s.g68_offset[Z] + positions[X] = r[0]*_x + r[3]*_y + r[6]*_z + positions[Y] = r[1]*_x + r[4]*_y + r[7]*_z + positions[Z] = r[2]*_x + r[5]*_y + r[8]*_z else: positions = list(positions) diff --git a/lib/python/rs274/glcanon_scene.py b/lib/python/rs274/glcanon_scene.py index 72c5160ce59..8d3e999a55d 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 @@ -109,6 +110,26 @@ WORKPIECE_AMBIENT = (0.65, 0.65, 0.65) WORKPIECE_DIFFUSE = (0.35, 0.35, 0.35) +#: 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) @@ -222,7 +243,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', 'workpiece_opacity', + 'show_workpiece', 'workpiece_opacity', '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 @@ -304,6 +325,7 @@ class FrameContext: #: every host) is the wireframe alone. A rendering mode, not a visibility #: flag - ``show_workpiece`` still decides whether stock is drawn at all. workpiece_opacity: float + show_workplane: bool program_alpha: bool #: Ground-grid spacing in internal units; ``0`` means "no grid", and is the #: grid part's visibility gate. @@ -2640,6 +2662,224 @@ def _triangles(*corners: tuple[Float64Points, return out +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): """Stock declared by ``(WORKPIECE,...)`` comments. @@ -2734,6 +2974,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__([ @@ -2750,6 +2991,8 @@ def __init__(self) -> None: (self.backplot, lambda ctx: ctx.show_live_plot), (self.workpiece, lambda ctx: ctx.show_workpiece and ctx.canon is not None), + (self.workplane, lambda ctx: ctx.show_workplane + and ctx.canon is not None), (self.tool, lambda ctx: ctx.show_tool), (self.overlay, lambda ctx: ctx.enable_dro), ]) diff --git a/lib/python/rs274/interpret.py b/lib/python/rs274/interpret.py index 8815f7f8696..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 3809d6bd90c..eb113e65399 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: @@ -1418,6 +1423,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); @@ -1701,6 +1709,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 c2f4d1b6923..6a742f5fd9b 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 d31f9a5096a..46a7eafcb75 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -897,6 +897,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() @@ -1539,6 +1559,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 2be2c1b6f72..708ec2a9f52 100644 --- a/src/emc/nml_intf/emcops.cc +++ b/src/emc/nml_intf/emcops.cc @@ -152,6 +152,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 95f1f71f4b9..49226eb60e3 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 ffcd652b5a6..c79d3e90661 100644 --- a/src/emc/rs274ngc/gcode_renderer.cc +++ b/src/emc/rs274ngc/gcode_renderer.cc @@ -58,6 +58,42 @@ void GCodeRenderer::set_spindle_speed(double rpm) { / (2 * M_PI) * rpm; } +// The plane's origin and axes on the machine, through the transform the way +// a move endpoint on this line goes, so the drawn plane and the moves in it +// agree. Recorded here rather than at hand over because the offsets in +// force at the definition are what place it, and they move on. +void GCodeRenderer::set_g68_frame(const WorkFrame &frame) { + 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, @@ -289,6 +325,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) @@ -456,28 +504,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(), @@ -486,6 +549,35 @@ static py::tuple renderer_transform(py::handle self, py::handle points) { return py::make_tuple(point_rows(machine), point_rows(display)); } +// 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); +} + // `gcode.RendererCanon` and its time-estimating subclass. These hold a // reference of their own: the module attribute is deletable, and a type freed // under us would leave every later PyObject_TypeCheck reading freed memory. @@ -575,6 +667,13 @@ void renderer_canon_register(py::module_ &m) { "TimeEstimateCanon", py::tuple(), tns); m.attr("TimeEstimateCanon") = timed; time_estimate_canon_type = (PyTypeObject *)timed.release().ptr(); + 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(); } @@ -784,7 +883,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_; @@ -805,14 +906,6 @@ void GCodeRenderer::untransform(const Point9 &in, Point9 &out) const { out -= g92_; } -// 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 @@ -923,6 +1016,7 @@ void GCodeRenderer::fill(int line_number, const Point9 &p1, const Point9 &p2, 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]; @@ -1211,7 +1305,7 @@ void GCodeRenderer::render_arc(int line_number, double first_end, double second_ if(suppress_ > 0) return; double radius = 0.0; 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_, &radius); @@ -1266,6 +1360,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, @@ -1292,6 +1387,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); @@ -1331,10 +1429,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 8451b31a8f8..e62171777fe 100644 --- a/src/emc/rs274ngc/gcode_renderer.hh +++ b/src/emc/rs274ngc/gcode_renderer.hh @@ -134,6 +134,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() {}; ~PreviewData(); @@ -186,6 +220,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(); @@ -210,6 +245,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, @@ -373,6 +409,7 @@ public: g92_ = offsets; } void set_xy_rotation(double degrees) override; + 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; } @@ -467,6 +504,11 @@ private: // One move into the geometry: extents, length, then its vertices. void fill(int line_number, const Point9 &p1, const Point9 &p2, 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); @@ -485,7 +527,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 @@ -508,6 +550,8 @@ 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 + long workplane_ = -1; // its record, or none in effect Point9 lo_ = {}; // chain point Point9 tool_ = {}; // xo..wo diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index b8e9012d28c..09f8675dac0 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -361,6 +361,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); @@ -540,6 +547,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) { @@ -1197,10 +1213,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 fd371446afc..dbc9c520f6e 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 // --------------------------------------------------------------------------- @@ -144,6 +176,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 6eb4e1c24f8..14dd80bf570 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 35e9d50027e..037cbb00bd5 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[AXIS_A] && (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(AXIS_A, s->AA_current + s->tool_offset.a + s->AA_origin_offset + s->AA_axis_offset); double b = PROGRAM_TO_USER_AX(AXIS_B, s->BB_current + s->tool_offset.b + s->BB_origin_offset + s->BB_axis_offset); double c = PROGRAM_TO_USER_AX(AXIS_C, 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); @@ -4733,6 +4747,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))); @@ -4972,6 +4988,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; @@ -5454,6 +5473,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); @@ -6720,16 +6741,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 ed341d48049..758e0fd62dc 100644 --- a/src/emc/rs274ngc/interp_find.cc +++ b/src/emc/rs274ngc/interp_find.cc @@ -190,31 +190,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[AXIS_A]) { @@ -526,12 +509,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[AXIS_A]) { CHP(unwrap_rotary(AA_2, AA_1, diff --git a/src/emc/rs274ngc/interp_internal.cc b/src/emc/rs274ngc/interp_internal.cc index 4ec4f7aafb5..bcccebe27c2 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 8066f0eac72..d98f2e5a155 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -274,6 +274,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, @@ -378,6 +381,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, @@ -392,7 +396,7 @@ enum phases { // Modal groups // also indices into g_modes -// unused: 9,11 +// unused: 11 enum ModalGroups { GM_MODAL_0 = 0, @@ -404,7 +408,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, @@ -780,6 +784,17 @@ 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 + int g68_seq_r_q; // the P2 block that gave R, or -1 + double g68_seq_word[4][7]; // per Q: x y z i j k r 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 6287d06dc93..b00c7206c2d 100644 --- a/src/emc/rs274ngc/interp_namedparams.cc +++ b/src/emc/rs274ngc/interp_namedparams.cc @@ -763,26 +763,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_remap.cc b/src/emc/rs274ngc/interp_remap.cc index 286ee98363f..9f197242fcb 100644 --- a/src/emc/rs274ngc/interp_remap.cc +++ b/src/emc/rs274ngc/interp_remap.cc @@ -75,8 +75,13 @@ bool Interp::is_user_defined_m_code(block_pointer block, setup_pointer settings, return (is_m_code_remappable(m_code) && settings->m_remapped[m_code]); } +// the tilted work plane codes are built in, but configs that remap them, +// like the nutating TWP sims, keep their own bool Interp::is_g_code_remappable(int g_code) -{ return g_code > 0 && g_code < 1000 && gees[g_code] == -1; } +{ + return g_code > 0 && g_code < 1000 && + (gees[g_code] == -1 || g_code == G_68_2 || g_code == G_68_4 || g_code == G_69); +} bool Interp::is_user_defined_g_code(int g_code) { return is_g_code_remappable(g_code) && _setup.g_remapped[g_code]; } diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index 7ac607b8edc..c340ff98a14 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -104,6 +104,15 @@ 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_r_q(-1), + 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..bd9453330e3 --- /dev/null +++ b/src/emc/rs274ngc/interp_workplane.cc @@ -0,0 +1,462 @@ +/******************************************************************** +* 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")); + CHKS((block->a_flag || block->b_flag || block->c_flag || + block->u_flag || block->v_flag || block->w_flag), + _("G68.2 takes no A, B, C, U, V or W: the plane is placed by X, Y and Z")); + + 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; a shift of the origin in the plane's own axes, P2's + // Q0 X Y Z, is applied once the rotation is known + double shift[3] = {0.0, 0.0, 0.0}; + { + 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; + s->g68_seq_r_q = -1; + } 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); + } + // a three-point definition that began at Q1 has left Q0 out + if (s->g68_seq_have & (1 << 1)) { first = 1; } + 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); + } + // every block of a sequence takes only the words its form reads + if (p == 2) { + CHKS((block->i_flag || block->j_flag || block->k_flag), + _("G68.2 P2 takes no I, J or K: its points are X, Y and Z")); + // R may come on any block of the definition, but only once + if (block->r_flag) { + CHKS((s->g68_seq_r_q >= 0), _("G68.2 P2 takes R once; Q%d already gave it"), s->g68_seq_r_q); + s->g68_seq_r_q = q; + } + } else if (q == 2) { + CHKS((block->x_flag || block->y_flag || block->z_flag), + _("G68.2 P3 Q2 takes no X, Y or Z: the origin is on the Q1 block")); + CHKS((block->r_flag), _("G68.2 P3 takes R on the Q1 block only")); + } + 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; the origin is the first point, moved by Q0's X Y Z + // along the plane's own axes; R comes from whichever block gave it + 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); + for (int i = 0; i < 3; i++) { + origin[i] = p1[i]; + shift[i] = (s->g68_seq_have & 1) ? s->g68_seq_word[0][i] : 0.0; + } + r = (s->g68_seq_r_q >= 0) ? s->g68_seq_word[s->g68_seq_r_q][6] : 0.0; + } else { + // two vectors: the origin and +X on the first block, +Z on the + // second; X is kept and Z is projected onto the plane normal to + // it, as Fanuc does, and vectors 5 degrees or more off square + // are refused + 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++) { x[i] = q1[3 + i]; z[i] = q2[3 + i]; } + len = vec_norm(x); + CHKS((len < 1e-12), _("G68.2 P3: the X direction is a zero vector")); + for (int i = 0; i < 3; i++) { x[i] /= len; } + 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; } + along = x[0]*z[0] + x[1]*z[1] + x[2]*z[2]; + CHKS((fabs(along) >= sin(5 * M_PI / 180)), + _("G68.2 P3: the X and Z directions are 5 degrees or more off square")); + for (int i = 0; i < 3; i++) { z[i] -= along * x[i]; } + len = vec_norm(z); + for (int i = 0; i < 3; i++) { z[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); + mat_apply(rotation, &shift[0], &shift[1], &shift[2]); + for (int i = 0; i < 3; i++) { origin[i] += shift[i]; } + *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 fd6151a4b70..63dc9b73b0b 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 6054da8a41a..8460d939814 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -367,6 +367,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 f594b4dc204..e0a8e5985e9 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -1221,6 +1221,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 */ @@ -1440,6 +1443,9 @@ int Interp::open(const char *filename) //!< string: the name of the input NC-pro CHKS((_setup.file_pointer == NULL), NCE_UNABLE_TO_OPEN_FILE, filename); Interp::nurbs_reset_global_variables(); // jf + // a three-point or two-vector plane definition left unfinished in MDI + // does not continue into a program + _setup.g68_seq_code = 0; line = _setup.linetext; for (index = -1; index == -1;) { /* skip blank lines */ @@ -2737,6 +2743,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 706a6e41fcd..efffbf169cf 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -191,13 +191,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 @@ -207,6 +240,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); @@ -218,6 +253,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); @@ -242,10 +279,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; @@ -508,6 +549,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, @@ -2427,7 +2488,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; @@ -2661,7 +2724,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. @@ -3442,6 +3505,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 6c1ff465b30..14ae62bc1bd 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; @@ -1995,6 +1996,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; @@ -2586,6 +2596,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 0fcf4da8130..dd18ab24762 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); } @@ -1577,6 +1590,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 9365254805f..77bf513a918 100755 --- a/src/emc/usr_intf/axis/scripts/axis.py +++ b/src/emc/usr_intf/axis/scripts/axis.py @@ -64,6 +64,7 @@ def tkerror(self, arg): steady_seconds) from hershey import Hershey from propertywindow import properties +import preview_helpers import rs274.options import nf import locale @@ -1320,6 +1321,9 @@ def open_file_guts(f, filtered=False, addrecent=True): if not interpname: unitcode = "G%d" % (20 + (s.linear_units == 1)) initcodes.append(unitcode) + plane = preview_helpers.workplane_code(s) + if plane: + initcodes.append(plane) initcodes.append("g90") initcodes.append("t%d m6" % s.tool_in_spindle) for i in range(9): @@ -1341,6 +1345,8 @@ def open_file_guts(f, filtered=False, addrecent=True): # In particular, after issuing a non-modal G like G10, that # will appear at s.gcodes[2] which caused issue #269 if i in (0, 1, 2): continue + # 12 is the tilted work plane, restated above with its words + if i == 12: continue if g == -1: continue if g == 960: # Issue #1232 initcodes.append("G96 S%.0f" % s.settings[2]) @@ -1660,6 +1666,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 c7fd631da99..867f6dfa9a1 100644 --- a/src/emc/usr_intf/halui.cc +++ b/src/emc/usr_intf/halui.cc @@ -2033,28 +2033,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/canon.py b/tests/gcode-renderer/canon.py index 41c6dc88e2f..8e20be946fe 100644 --- a/tests/gcode-renderer/canon.py +++ b/tests/gcode-renderer/canon.py @@ -221,7 +221,8 @@ class FakePreview: def __init__(self, planes, lines, kinds, tools=None, moves=None, rapid_length=0.0, cutting_length=0.0, 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) @@ -241,6 +242,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 @@ -270,6 +272,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/gcode-renderer/programs.py b/tests/gcode-renderer/programs.py index 3210c284d53..68caa2f16a0 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/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() diff --git a/tests/interp/g68-frame/expected b/tests/interp/g68-frame/expected new file mode 100644 index 00000000000..5b51be90cb6 --- /dev/null +++ b/tests/interp/g68-frame/expected @@ -0,0 +1,64 @@ + 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, the first one its origin") + 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("Q0 moves the origin along the plane's own axes: to 11 17 32") + 23 N..... SET_G68_FRAME(11.0000, 17.0000, 32.0000, [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, -1.0000, 0.0000, 1.0000, 0.0000], 1) + 24 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 25 N..... COMMENT("and after R: to 8 17 31") + 26 N..... SET_G68_FRAME(8.0000, 17.0000, 31.0000, [0.0000, -1.0000, 0.0000, 0.0000, 0.0000, -1.0000, 1.0000, 0.0000, 0.0000], 1) + 27 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 28 N..... COMMENT("R on a point block turns the plane the same way: to 8 17 31") + 29 N..... SET_G68_FRAME(8.0000, 17.0000, 31.0000, [0.0000, -1.0000, 0.0000, 0.0000, 0.0000, -1.0000, 1.0000, 0.0000, 0.0000], 1) + 30 N..... STRAIGHT_TRAVERSE(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 31 N..... COMMENT("the same plane by two vectors: X is kept and Z, 3 degrees off square, is squared to it") + 32 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) + 33 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 34 N..... COMMENT("R turns the plane about its own Z") + 35 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) + 36 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 37 N..... COMMENT("an arc in the plane") + 38 N..... SET_FEED_RATE(100.0000) + 39 N..... STRAIGHT_FEED(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 40 N..... ARC_FEED(2.0000, 0.0000, 1.0000, 0.0000, -1, 0.0000, 0.0000, 0.0000, 0.0000) + 41 N..... COMMENT("G53 inside the plane goes to absolute coordinates") + 42 N..... STRAIGHT_TRAVERSE(-30.0000, 10.0000, 20.0000, 0.0000, 0.0000, 0.0000) + 43 N..... COMMENT("and #5021 reports them") + 44 N..... MESSAGE(" abs 100.000000 200.000000 300.000000 prog -30.000000 10.000000 20.000000") + 45 N..... COMMENT("a probe result comes back in plane coordinates: nothing to run here") + 46 N..... COMMENT("a tool length change moves the program coordinates along the plane axis that is world Z") + 47 N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 7.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000) + 48 N..... MESSAGE(" prog -37.000000 10.000000 20.000000") + 49 N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000) + 50 N..... COMMENT("G68.4 composes: a further 90 about the plane's X") + 51 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) + 52 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 53 N..... COMMENT("G69 cancels") + 54 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) + 55 N..... STRAIGHT_TRAVERSE(1.0000, 2.0000, 3.0000, 0.0000, 0.0000, 0.0000) + 56 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + 57 N..... SET_XY_ROTATION(0.0000) + 58 N..... SET_FEED_MODE(0, 0) + 59 N..... SET_FEED_RATE(0.0000) + 60 N..... STOP_SPINDLE_TURNING(0) + 61 N..... SET_SPINDLE_MODE(0 0.0000) + 62 N..... PROGRAM_END() + 63 N..... ON_RESET() + 64 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..a78fa60afd7 --- /dev/null +++ b/tests/interp/g68-frame/g68.ngc @@ -0,0 +1,61 @@ +% +(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, the first one its origin) +g68.2 p2 q1 x10 y20 z30 +g68.2 p2 q2 x15 y20 z30 +g68.2 p2 q3 x10 y20 z35 +g0 x1 y2 z3 +(Q0 moves the origin along the plane's own axes: to 11 17 32) +g68.2 p2 q0 x1 y2 z3 +g68.2 p2 q1 x10 y20 z30 +g68.2 p2 q2 x15 y20 z30 +g68.2 p2 q3 x10 y20 z35 +g0 x0 y0 z0 +(and after R: to 8 17 31) +g68.2 p2 q0 x1 y2 z3 r90 +g68.2 p2 q1 x10 y20 z30 +g68.2 p2 q2 x15 y20 z30 +g68.2 p2 q3 x10 y20 z35 +g0 x0 y0 z0 +(R on a point block turns the plane the same way: to 8 17 31) +g68.2 p2 q0 x1 y2 z3 +g68.2 p2 q1 x10 y20 z30 +g68.2 p2 q2 x15 y20 z30 +g68.2 p2 q3 x10 y20 z35 r90 +g0 x0 y0 z0 +(the same plane by two vectors: X is kept and Z, 3 degrees off square, is squared to it) +g68.2 p3 q1 x10 y20 z30 i1 j0 k0 +g68.2 p3 q2 i0.05 j-1 k0 +g0 x1 y2 z3 +(R turns the plane about its own Z) +g68.2 p1 q123 x10 y20 z30 i90 j0 k0 r90 +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]}" diff --git a/tests/interp/g68-words/expected b/tests/interp/g68-words/expected new file mode 100644 index 00000000000..a35a512b500 --- /dev/null +++ b/tests/interp/g68-words/expected @@ -0,0 +1,16 @@ +p2-ijk: G68.2 P2 takes no I, J or K: its points are X, Y and Z +p2-r-q1: ok +p2-r-q3: ok +p2-r-twice: G68.2 P2 takes R once; Q0 already gave it +p2-r-q0: ok +p2-no-q0: ok +p2-q0-late: G68.2 P2 expects Q2 here +p3-q2-xyz: G68.2 P3 Q2 takes no X, Y or Z: the origin is on the Q1 block +p3-q2-r: G68.2 P3 takes R on the Q1 block only +p3-q1-r: ok +p3-4deg: ok +p3-6deg: G68.2 P3: the X and Z directions are 5 degrees or more off square +p3-zero: G68.2 P3: the X direction is a zero vector +p1-a: G68.2 takes no A, B, C, U, V or W: the plane is placed by X, Y and Z +g684-b: G68.2 takes no A, B, C, U, V or W: the plane is placed by X, Y and Z +g684-none: G68.4 needs an active tilted work plane to build on diff --git a/tests/interp/g68-words/test.sh b/tests/interp/g68-words/test.sh new file mode 100755 index 00000000000..c30586f57e9 --- /dev/null +++ b/tests/interp/g68-words/test.sh @@ -0,0 +1,25 @@ +#!/bin/bash +# Each G68.2 form takes only the words it reads; any other is refused +# instead of being ignored. One case per program, the message or ok. +case_() { + printf '%s\nM2\n' "$2" > case.ngc + msg=$(rs274 -g case.ngc 2>&1 | grep -v -e '^ *[0-9]* N\.\.\.\.\.' -e '^executing' | head -1) + echo "$1: ${msg:-ok}" +} +case_ p2-ijk $'G68.2 P2 Q1 X0 Y0 Z0 I1\nG68.2 P2 Q2 X1 Y0 Z0\nG68.2 P2 Q3 X0 Y1 Z0' +case_ p2-r-q1 $'G68.2 P2 Q1 X0 Y0 Z0 R10\nG68.2 P2 Q2 X1 Y0 Z0\nG68.2 P2 Q3 X0 Y1 Z0' +case_ p2-r-q3 $'G68.2 P2 Q1 X0 Y0 Z0\nG68.2 P2 Q2 X1 Y0 Z0\nG68.2 P2 Q3 X0 Y1 Z0 R10' +case_ p2-r-twice $'G68.2 P2 Q0 X0 Y0 Z0 R10\nG68.2 P2 Q1 X0 Y0 Z0\nG68.2 P2 Q2 X1 Y0 Z0 R5\nG68.2 P2 Q3 X0 Y1 Z0' +case_ p2-r-q0 $'G68.2 P2 Q0 X0 Y0 Z0 R10\nG68.2 P2 Q1 X0 Y0 Z0\nG68.2 P2 Q2 X1 Y0 Z0\nG68.2 P2 Q3 X0 Y1 Z0' +case_ p2-no-q0 $'G68.2 P2 Q1 X0 Y0 Z0\nG68.2 P2 Q2 X1 Y0 Z0\nG68.2 P2 Q3 X0 Y1 Z0' +case_ p2-q0-late $'G68.2 P2 Q1 X0 Y0 Z0\nG68.2 P2 Q0 X0 Y0 Z0' +case_ p3-q2-xyz $'G68.2 P3 Q1 X0 Y0 Z0 I1 J0 K0\nG68.2 P3 Q2 X1 I0 J0 K1' +case_ p3-q2-r $'G68.2 P3 Q1 X0 Y0 Z0 I1 J0 K0\nG68.2 P3 Q2 I0 J0 K1 R5' +case_ p3-q1-r $'G68.2 P3 Q1 X0 Y0 Z0 I1 J0 K0 R5\nG68.2 P3 Q2 I0 J0 K1' +case_ p3-4deg $'G68.2 P3 Q1 X0 Y0 Z0 I1 J0 K0\nG68.2 P3 Q2 I0.07 J0 K1' +case_ p3-6deg $'G68.2 P3 Q1 X0 Y0 Z0 I1 J0 K0\nG68.2 P3 Q2 I-0.106 J0 K1' +case_ p3-zero $'G68.2 P3 Q1 X0 Y0 Z0 I0 J0 K0\nG68.2 P3 Q2 I0 J0 K1' +case_ p1-a $'G68.2 P1 X1 J30 A10' +case_ g684-b $'G68.2 X1 J30\nG68.4 B2 I10' +case_ g684-none $'G68.4 I10' +rm -f case.ngc diff --git a/tests/remap/g68-override/expected b/tests/remap/g68-override/expected new file mode 100644 index 00000000000..5431f62e144 --- /dev/null +++ b/tests/remap/g68-override/expected @@ -0,0 +1,17 @@ + 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..... MESSAGE(" remapped G68.2 p=0.000000 x=1.000000 j=30.000000") + N..... MESSAGE(" remapped G68.4 i=10.000000") + N..... MESSAGE(" remapped G69") + N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 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/remap/g68-override/r682.ngc b/tests/remap/g68-override/r682.ngc new file mode 100644 index 00000000000..d4f6794ee0f --- /dev/null +++ b/tests/remap/g68-override/r682.ngc @@ -0,0 +1,4 @@ +o sub +(debug, remapped G68.2 p=#

x=# j=#) +o endsub +m2 diff --git a/tests/remap/g68-override/r684.ngc b/tests/remap/g68-override/r684.ngc new file mode 100644 index 00000000000..b09d030075e --- /dev/null +++ b/tests/remap/g68-override/r684.ngc @@ -0,0 +1,4 @@ +o sub +(debug, remapped G68.4 i=#) +o endsub +m2 diff --git a/tests/remap/g68-override/r69.ngc b/tests/remap/g68-override/r69.ngc new file mode 100644 index 00000000000..92a9db6330c --- /dev/null +++ b/tests/remap/g68-override/r69.ngc @@ -0,0 +1,4 @@ +o sub +(debug, remapped G69) +o endsub +m2 diff --git a/tests/remap/g68-override/test.ini b/tests/remap/g68-override/test.ini new file mode 100644 index 00000000000..c2f5ee03093 --- /dev/null +++ b/tests/remap/g68-override/test.ini @@ -0,0 +1,11 @@ +[EMC] +DEBUG=0 +LOG_LEVEL=0 + +[RS274NGC] +SUBROUTINE_PATH = . + +# G68.2, G68.4 and G69 are built in; a REMAP of them takes over +REMAP=G68.2 modalgroup=1 argspec=pqxyzijkr ngc=r682 +REMAP=G68.4 modalgroup=1 argspec=pqxyzijkr ngc=r684 +REMAP=G69 modalgroup=1 ngc=r69 diff --git a/tests/remap/g68-override/test.ngc b/tests/remap/g68-override/test.ngc new file mode 100644 index 00000000000..32df4a338a3 --- /dev/null +++ b/tests/remap/g68-override/test.ngc @@ -0,0 +1,4 @@ +G68.2 P0 X1 J30 +G68.4 I10 +G69 +M2 diff --git a/tests/remap/g68-override/test.sh b/tests/remap/g68-override/test.sh new file mode 100755 index 00000000000..99269de600c --- /dev/null +++ b/tests/remap/g68-override/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash +export INI_FILE_NAME=test.ini +rs274 -i "$INI_FILE_NAME" -g test.ngc | awk '{$1=""; print}' +exit "${PIPESTATUS[0]}" diff --git a/tests/workplane-preview/compose.ngc b/tests/workplane-preview/compose.ngc new file mode 100644 index 00000000000..d4bd048cbab --- /dev/null +++ b/tests/workplane-preview/compose.ngc @@ -0,0 +1,3 @@ +G68.4 I5 +G1 X1 Y2 Z-0.5 F2000 +M2 diff --git a/tests/workplane-preview/expected b/tests/workplane-preview/expected new file mode 100644 index 00000000000..29ffdcc2685 --- /dev/null +++ b/tests/workplane-preview/expected @@ -0,0 +1,6 @@ +plane: same end +composed: same end +composed then G68.4: same end +gimbal: same end +no plane: preview line 1: G68.4 needs an active tilted work plane to build on; machine: G68.4 needs an active tilted work plane to build on +unfinished MDI sequence: same end diff --git a/tests/workplane-preview/move.ngc b/tests/workplane-preview/move.ngc new file mode 100644 index 00000000000..42a8f621d33 --- /dev/null +++ b/tests/workplane-preview/move.ngc @@ -0,0 +1,2 @@ +G1 X1 Y2 Z-0.5 F2000 +M2 diff --git a/tests/workplane-preview/test-ui.py b/tests/workplane-preview/test-ui.py new file mode 100755 index 00000000000..e52f1759059 --- /dev/null +++ b/tests/workplane-preview/test-ui.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python3 +# A tilted work plane set in MDI reaches the preview: the preview parse +# starts from the same plane as the machine and ends where the machine does. +import math, sys, time +import gcode, linuxcnc, linuxcnc_util, preview_helpers +from rs274.interpret import Translated + +class Canon(Translated): + def __init__(self, s): + self.s = s + self.parameter_file = "test.var" + self.end = None + def __getattr__(self, name): + if name.startswith("_"): + raise AttributeError(name) + return lambda *a, **k: None + def set_g5x_offset(self, i, x, y, z, *rest): + self.g5x_offset_x, self.g5x_offset_y, self.g5x_offset_z = x, y, z + def set_g92_offset(self, x, y, z, *rest): + self.g92_offset_x, self.g92_offset_y, self.g92_offset_z = x, y, z + def set_xy_rotation(self, t): + self.rotation_xy = t + self.rotation_cos = math.cos(math.radians(t)) + self.rotation_sin = math.sin(math.radians(t)) + def set_g68_frame(self, x, y, z, *rest): + self.g68_offset = (x, y, z) + self.g68_rotation = rest[:9] + self.g68_active = rest[9] + def straight_feed(self, *a): + self.end = self.rotate_and_translate(*a)[:3] + def get_tool(self, pocket): + return (-1,) + (0,) * 13 + def get_external_length_units(self): + return self.s.linear_units + def get_external_angular_units(self): + return 1.0 + def get_axis_mask(self): + return self.s.axis_mask + def get_block_delete(self): + return 0 + +result = open("result", "w") +def say(*a): + print(*a, file=result, flush=True) + +c = linuxcnc.command() +e = linuxcnc.error_channel() +s = linuxcnc.stat() +l = linuxcnc_util.LinuxCNC(command=c, status=s, error=e) +inifile = linuxcnc.ini("test.ini") +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.mode(linuxcnc.MODE_MANUAL) +c.home(-1) +l.wait_for_home(joints=[1, 1, 1, 1, 0, 0, 0, 0, 0]) + +def errors(): + msgs = [] + while True: + m = e.poll() + if not m: + return msgs + msgs.append(m[1]) + +def mdi(*cmds): + c.mode(linuxcnc.MODE_MDI) + c.wait_complete() + for cmd in cmds: + c.mdi(cmd) + c.wait_complete() + l.wait_for_interp_state(linuxcnc.INTERP_IDLE) + +def case(name, prog, *setup): + mdi("G69", *setup) + errors() + s.poll() + canon = Canon(s) + res, seq = gcode.parse(prog, canon, *preview_helpers.create_unitcode_and_initcode(s, inifile)) + c.mode(linuxcnc.MODE_AUTO) + c.wait_complete() + c.program_open(prog) + c.wait_complete() + c.auto(linuxcnc.AUTO_RUN, 0) + c.wait_complete() + time.sleep(0.3) + l.wait_for_interp_state(linuxcnc.INTERP_IDLE, timeout=30) + s.poll() + machine = errors() + if res > gcode.MIN_ERROR: + say("%s: preview line %d: %s; machine: %s" % (name, seq, gcode.strerror(res).strip(), "; ".join(machine))) + return + d = max(abs(canon.end[i] - s.position[i]) for i in range(3)) + say("%s: %s%s" % (name, "same end" if d < 1e-9 else "ends %g apart" % d, + "; machine: " + "; ".join(machine) if machine else "")) + +case("plane", "move.ngc", "G21", "G68.2 X10 Y20 Z5 I10 J20 K30") +case("composed", "move.ngc", "G21", "G68.2 X10 Y20 Z5 I10 J20 K30", "G68.4 X2 J15") +case("composed then G68.4", "compose.ngc", "G21", "G68.2 X10 Y20 Z5 I10 J20 K30", "G68.4 X2 J15") +case("gimbal", "move.ngc", "G20", "G68.2 P1 X0.5 Y0.2 Z0.1 I20 J90 K30") +case("no plane", "compose.ngc", "G20") +case("unfinished MDI sequence", "move.ngc", "G20", "G68.2 P2 Q1 X0 Y0 Z0") +c.state(linuxcnc.STATE_OFF) diff --git a/tests/workplane-preview/test.ini b/tests/workplane-preview/test.ini new file mode 100644 index 00000000000..51a235f0cff --- /dev/null +++ b/tests/workplane-preview/test.ini @@ -0,0 +1,105 @@ +[EMC] +VERSION = 1.1 +MACHINE = workplane_preview + DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[FILTER] + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G20 G40 G49 G54 G64P0.005 G80 G90 G94 G97 M5 M9 +PARAMETER_FILE = test.var + +[PYTHON] + +[EMCMOT] +EMCMOT = motmod +COMM_TIMEOUT = 1.0 +BASE_PERIOD = 600000 +SERVO_PERIOD = 1000000 + +[EMCIO] +TOOL_TABLE = tool.tbl +TOOL_CHANGE_POSITION = 0 0 2 + +[HAL] +HALUI = halui +HALFILE = LIB:core_sim.hal +HALFILE = LIB:simulated_home.hal +HALFILE = LIB:lathe.hal + +[TRAJ] +COORDINATES = X Y Z A +LINEAR_UNITS = inch +ANGULAR_UNITS = degree +DEFAULT_LINEAR_VELOCITY = 50.0 +DEFAULT_LINEAR_ACCEL = 500.0 +DEFAULT_ANGULAR_VELOCITY = 45.0 +MAX_LINEAR_VELOCITY = 200.0 +MAX_LINEAR_ACCEL = 250.0 +MAX_ANGULAR_VELOCITY = 90.0 + +[KINS] +KINEMATICS = trivkins coordinates=XYZA +JOINTS = 4 + +[AXIS_X] +MIN_LIMIT = -250.0 +MAX_LIMIT = 250.0 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1250 + +[AXIS_Y] +MIN_LIMIT = -250.0 +MAX_LIMIT = 250.0 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1250 + +[AXIS_Z] +MIN_LIMIT = -50.0 +MAX_LIMIT = 100.0 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1250 + +[AXIS_A] +MIN_LIMIT = -360.0 +MAX_LIMIT = 360.0 +MAX_VELOCITY = 90.0 +MAX_ACCELERATION = 1200.0 + + +[JOINT_0] +TYPE = LINEAR +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1250 +MIN_LIMIT = -250.0 +MAX_LIMIT = 250.0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1250 +MIN_LIMIT = -250.0 +MAX_LIMIT = 250.0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +MAX_VELOCITY = 100 +MAX_ACCELERATION = 1250 +MIN_LIMIT = -50.0 +MAX_LIMIT = 100.0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +MAX_VELOCITY = 90.0 +MAX_ACCELERATION = 1200.0 +HOME_SEQUENCE = 0 diff --git a/tests/workplane-preview/test.sh b/tests/workplane-preview/test.sh new file mode 100755 index 00000000000..0317460472d --- /dev/null +++ b/tests/workplane-preview/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash -e +rm -f result test.var test.var.bak +linuxcnc -r test.ini +rm -f test.var test.var.bak