import sys import traceback import numpy #import pprint from math import sin,cos,atan2,sqrt,pi,degrees from interpreter import * from emccanon import MESSAGE, SET_MOTION_OUTPUT_BIT, CLEAR_MOTION_OUTPUT_BIT,SET_AUX_OUTPUT_BIT,CLEAR_AUX_OUTPUT_BIT from util import lineno, call_pydevd throw_exceptions = 1 # raises InterpreterException if execute() or read() fail #Remap for Incremental moves in tool coordinates "G91.2 x y z a b c def g912(self, **words): #genserkins.c uses Rot[X,Y,Z] fixed angles rotation also called XYZ-Tait-Bryan angles #First rotation: "c"-rotation is equal to rotation around z = Z-world-axis #Second rotation: "b"-rotation is equal to rotation around the y'-axis # "a"-rotation is always equal to rotation around x'' = X-tool-axis #requested increments c = self.blocks[self.remap_level] dx = c.x_number if c.x_flag else 0 dy = c.y_number if c.y_flag else 0 dz = c.z_number if c.z_flag else 0 da = c.a_number if c.a_flag else 0 db = c.b_number if c.b_flag else 0 dc = c.c_number if c.c_flag else 0 #current position and orientation old_x = self.current_x old_y = self.current_y old_z = self.current_z old_a = self.AA_current old_b = self.BB_current old_c = self.CC_current #calculate terms for rotation matrices sa = sin(old_a*(pi/180)) sb = sin(old_b*(pi/180)) sc = sin(old_c*(pi/180)) ca = cos(old_a*(pi/180)) cb = cos(old_b*(pi/180)) cc = cos(old_c*(pi/180)) sda = sin(da*(pi/180)) sdb = sin(db*(pi/180)) sdc = sin(dc*(pi/180)) cda = cos(da*(pi/180)) cdb = cos(db*(pi/180)) cdc = cos(dc*(pi/180)) #---Calculation of the new position---: #This is used to calculate the new POSITION in world coordinates according to the #requested incremental moves along the tool coordinate axis (G91.2 x y z ...) #Transformation matrix for the kinematics used in genserkins A = [[ cb*cc , sa*sb*cc - ca*sc , ca*sb*cc + sa*sc ], [ cb*sc , sa*sb*sc + ca*cc , ca*sb*sc - sa*cc ], [ -sb , sa*cb , ca*cb ]] #requested move in vector form Pd = [[ dx ], [ dy ], [ dz ]] #A*Pd - matrix multiplication (the order is important! A*Pd != Pd*A) #'import numpy' is required for this P = [[sum(a*b for a,b in zip(A_row,Pd_col)) for Pd_col in zip(*Pd)] for A_row in A] #position in world coordinates is the sum of the old position and the rotated increments x_new = old_x + P[0][0] y_new = old_y + P[1][0] z_new = old_z + P[2][0] #pp = pprint.PrettyPrinter(indent=4) #pp.pprint(A) #---Calculation of the new orientation--- #This is used to calculate the new ORIENTATION in world coordinates according to the #requested incremental moves along the tool coordinate axis (G91.2 ... a b c) #matrix for rotation by angle 'c' around z-axis Rz = [[ cdc, -sdc, 0 ], [ sdc, cdc, 0 ], [ 0, 0 , 1 ]] #matrix for rotation by angle 'b' around y-axis Ry = [[ cdb , 0, sdb ], [ 0 , 1, 0 ], [-sdb , 0, cdb ]] #matrix for rotation by angle 'a' around y-axis Rx = [[ 1 , 0 , 0 ], [ 0 , cda, -sda ], [ 0 , sda, cda ]] ''' #--Incremental rotation in WORLD-coordinates-- #this rotates the Tool-center-point around the X-,Y-,Z-axis of the WORLD-coordinate-system #Matrix multiplications Rx*Ry*Rz*A (starting from right to left) #multiply Rz * A A = [[sum(a*b for a,b in zip(Rz_row,A_col)) for A_col in zip(*A)] for Rz_row in Rz] #multiply Ry * A A = [[sum(a*b for a,b in zip(Ry_row,A_col)) for A_col in zip(*A)] for Ry_row in Ry] #multiply Rx * A A = [[sum(a*b for a,b in zip(Rx_row,A_col)) for A_col in zip(*A)] for Rx_row in Rx] ''' #--Incremental rotation in TOOL-coordinates-- #this rotates the Tool-center-point around the X-,Y-,Z-axis of the TOOL-coordinate-system #Matrix multiplications A*Rx*Ry*Rz (starting from right to left) #multiply Ry * Rz C = [[sum(a*b for a,b in zip(Ry_row,Rz_col)) for Rz_col in zip(*Rz)] for Ry_row in Ry] #multiply Rx * C B = [[sum(a*b for a,b in zip(Rx_row,C_col)) for C_col in zip(*C)] for Rx_row in Rx] #multiply A * B A = [[sum(a*b for a,b in zip(A_row,B_col)) for B_col in zip(*B)] for A_row in A] #now the required numerical results of all the elements of the transformation matrix have #been calculated and we can solve the equations for the required angles a,b,c #calculate new Euler angles a,b,c c_new_r = atan2((A[1][0]),A[0][0]) c_new = degrees(c_new_r) b_new_r = atan2(-A[2][0],sqrt(A[2][1]*A[2][1]+A[2][2]*A[2][2])) b_new = degrees(b_new_r) a_new_r = atan2((A[2][1]),A[2][2]) a_new = degrees(a_new_r) #print(x_new,y_new,z_new,a_new,b_new,c_new) #send new coordinates as G0 move to LinuxCNC print("G0 X%f Y%f Z%f A%f B%f C%f" % (x_new,y_new,z_new,a_new,b_new,c_new)) self.execute("G0 X%f Y%f Z%f A%f B%f C%f" % (x_new,y_new,z_new,a_new,b_new,c_new),lineno()) return INTERP_OK #Remap for Incremental xyz-moves in TOOL coordinates , and incremental abc-moves in WORLD - coordinates #"G91.2 x y z a b c def g913(self, **words): #genserkins.c uses Rot[X,Y,Z] fixed angles rotation also called XYZ-Tait-Bryan angles #First rotation: "c"-rotation is equal to rotation around z = Z-world-axis #Second rotation: "b"-rotation is equal to rotation around the y'-axis # "a"-rotation is always equal to rotation around x'' = X-tool-axis #requested increments c = self.blocks[self.remap_level] dx = c.x_number if c.x_flag else 0 dy = c.y_number if c.y_flag else 0 dz = c.z_number if c.z_flag else 0 da = c.a_number if c.a_flag else 0 db = c.b_number if c.b_flag else 0 dc = c.c_number if c.c_flag else 0 #current position and orientation old_x = self.current_x old_y = self.current_y old_z = self.current_z old_a = self.AA_current old_b = self.BB_current old_c = self.CC_current #calculate terms for rotation matrices sa = sin(old_a*(pi/180)) sb = sin(old_b*(pi/180)) sc = sin(old_c*(pi/180)) ca = cos(old_a*(pi/180)) cb = cos(old_b*(pi/180)) cc = cos(old_c*(pi/180)) sda = sin(da*(pi/180)) sdb = sin(db*(pi/180)) sdc = sin(dc*(pi/180)) cda = cos(da*(pi/180)) cdb = cos(db*(pi/180)) cdc = cos(dc*(pi/180)) #---Calculation of the new position---: #This is used to calculate the new POSITION in world coordinates according to the #requested incremental moves along the tool coordinate axis (G91.2 x y z ...) #Transformation matrix for the kinematics used in genserkins A = [[ cb*cc , sa*sb*cc - ca*sc , ca*sb*cc + sa*sc ], [ cb*sc , sa*sb*sc + ca*cc , ca*sb*sc - sa*cc ], [ -sb , sa*cb , ca*cb ]] #requested move in vector form Pd = [[ dx ], [ dy ], [ dz ]] #A*Pd - matrix multiplication (the order is important! A*Pd != Pd*A) #'import numpy' is required for this P = [[sum(a*b for a,b in zip(A_row,Pd_col)) for Pd_col in zip(*Pd)] for A_row in A] #position in world coordinates is the sum of the old position and the rotated increments x_new = old_x + P[0][0] y_new = old_y + P[1][0] z_new = old_z + P[2][0] #pp = pprint.PrettyPrinter(indent=4) #pp.pprint(A) #---Calculation of the new orientation--- #This is used to calculate the new ORIENTATION in world coordinates according to the #requested incremental moves along the tool coordinate axis (G91.2 ... a b c) #matrix for rotation by angle 'c' around z-axis Rz = [[ cdc, -sdc, 0 ], [ sdc, cdc, 0 ], [ 0, 0 , 1 ]] #matrix for rotation by angle 'b' around y-axis Ry = [[ cdb , 0, sdb ], [ 0 , 1, 0 ], [-sdb , 0, cdb ]] #matrix for rotation by angle 'a' around y-axis Rx = [[ 1 , 0 , 0 ], [ 0 , cda, -sda ], [ 0 , sda, cda ]] #--Incremental rotation in WORLD-coordinates-- #this rotates the Tool-center-point around the X-,Y-,Z-axis of the WORLD-coordinate-system #Matrix multiplications Rx*Ry*Rz*A (starting from right to left) #multiply Rz * A A = [[sum(a*b for a,b in zip(Rz_row,A_col)) for A_col in zip(*A)] for Rz_row in Rz] #multiply Ry * A A = [[sum(a*b for a,b in zip(Ry_row,A_col)) for A_col in zip(*A)] for Ry_row in Ry] #multiply Rx * A A = [[sum(a*b for a,b in zip(Rx_row,A_col)) for A_col in zip(*A)] for Rx_row in Rx] #now the required numerical results of all the elements of the transformation matrix have #been calculated and we can solve the equations for the required angles a,b,c #calculate new Euler angles a,b,c c_new_r = atan2((A[1][0]),A[0][0]) c_new = degrees(c_new_r) b_new_r = atan2(-A[2][0],sqrt(A[2][1]*A[2][1]+A[2][2]*A[2][2])) b_new = degrees(b_new_r) a_new_r = atan2((A[2][1]),A[2][2]) a_new = degrees(a_new_r) #print(x_new,y_new,z_new,a_new,b_new,c_new) #send new coordinates as G0 move to LinuxCNC self.execute("G0 X%f Y%f Z%f A%f B%f C%f" % (x_new,y_new,z_new,a_new,b_new,c_new),lineno()) return INTERP_OK