def solve_orientation_prismatic(robo, symo, X_joints): """ Function that solves the orientation equation of the three prismatic joints case. (to find the three angles) Parameters: =========== 1) X_joint: The three revolute joints for the prismatic case """ [i,j,k] = X_joints # X joints vector [S,S1,S2,S3,x] = [0,0,0,0,0] # Book convention indexes [N,N1,N2,N3,y] = [1,1,1,1,1] # Book convention indexes [A,A1,A2,A3,z] = [2,2,2,2,2] # Book convention indexes robo.theta[i] = robo.theta[i] + robo.gamma[robo.ant[i]] robo.theta[j] = robo.theta[j] + robo.gamma[robo.ant[j]] robo.theta[k] = robo.theta[k] + robo.gamma[robo.ant[k]] T6k = dgm(robo, symo, 6, k, fast_form=True,trig_subs=True) Ti0 = dgm(robo, symo, robo.ant[i], 0, fast_form=True, trig_subs=True) Tji = dgm(robo, symo, j-1, i, fast_form=True, trig_subs=True) Tjk = dgm(robo, symo, j, k-1, fast_form=True, trig_subs=True) S3N3A3 = _rot_trans(axis=x, th=-robo.alpha[i], p=0)*Ti0*T_GENERAL*T6k # S3N3A3 = rot(x,-alphai)*rot(z,-gami)*aiTo*SNA S2N2A2 = _rot_trans(axis=x, th=-robo.alpha[j], p=0)*Tji # S2N2A2 = iTa(j)*rot(x,-alphaj) S1N1A1 = Tjk*_rot_trans(axis=x, th=-robo.alpha[k], p=0) # S1N1A1 = jTa(k)*rot(x,alphak) S3N3A3 = Matrix([ S3N3A3[:3, :3] ]) S2N2A2 = Matrix([ S2N2A2[:3, :3] ]) S1N1A1 = Matrix([ S1N1A1[:3, :3] ]) SNA = Matrix([ T_GENERAL[:3, :3] ]) SNA = symo.replace(trigsimp(SNA), 'SNA') # solve thetai # eq 1.100 (page 49) eq_type = 3 offset = robo.gamma[robo.ant[i]] robo.r[i] = robo.r[i] + robo.b[robo.ant[i]] el1 = S2N2A2[z,S2]*S3N3A3[x,A3] + S2N2A2[z,N2]*S3N3A3[y,A3] el2 = S2N2A2[z,S2]*S3N3A3[y,A3] - S2N2A2[z,N2]*S3N3A3[x,A3] el3 = S2N2A2[z,A2]*S3N3A3[z,A3] - S1N1A1[z,A1] coef = [el1,el2,el3] _equation_solve(symo, coef, eq_type, robo.theta[i], offset) # solve thetaj # eq 1.102 eq_type = 4 offset = robo.gamma[robo.ant[j]] robo.r[j] = robo.r[j] + robo.b[robo.ant[j]] coef = [S1N1A1[x,A1] , -S1N1A1[y,A1] , -SNA[x,A] , S1N1A1[y,A1] , S1N1A1[x,A1] , -SNA[y,A] ] _equation_solve(symo, coef, eq_type, robo.theta[j], offset) # solve thetak # eq 1.103 eq_type = 4 offset = robo.gamma[robo.ant[k]] robo.r[k] = robo.r[k] + robo.b[robo.ant[k]] coef = [S1N1A1[z,S1] , S1N1A1[z,N1] , -SNA[z,S] , S1N1A1[z,N1] , -S1N1A1[z,S1] , -SNA[z,N] ] _equation_solve(symo, coef, eq_type, robo.theta[k], offset) return
def solve_position_prismatic(robo, symo, pieper_joints): """ Function that solves the position equation of the three prismatic joints case. (to find the three angles) Parameters: =========== 1) Pieper_joints: The three prismatic joints for the prismatic case """ eq_type = 0 # Type 0: Linear system [i, j, k] = pieper_joints # Pieper joints vector [x, y, z] = [0, 1, 2] # Book convention indexes robo.theta[i] = robo.theta[i] + robo.gamma[robo.ant[i]] robo.theta[j] = robo.theta[j] + robo.gamma[robo.ant[j]] robo.theta[k] = robo.theta[k] + robo.gamma[robo.ant[k]] T6k = dgm(robo, symo, 6, k, fast_form=True, trig_subs=True) Ti0 = dgm(robo, symo, robo.ant[i], 0, fast_form=True, trig_subs=True) Tji = dgm(robo, symo, j - 1, i, fast_form=True, trig_subs=True) Tjk = dgm(robo, symo, j, k - 1, fast_form=True, trig_subs=True) S3N3A3P3 = _rot_trans(axis=z, th=-robo.theta[i], p=0) * _rot_trans( axis=x, th=-robo.alpha[i], p=-robo.d[i] ) * Ti0 * T_GENERAL * T6k # S3N3A3 = rot(z,-thetai)*Trans(x,-di)*rot(x,-alphai)*a(i)To*SNAP*6Tk S2N2A2P2 = _rot_trans(axis=z, th=-robo.theta[j], p=0) * _rot_trans( axis=x, th=-robo.alpha[j], p=-robo.d[j] ) * Tji # S2N2A2 = rot(z,-thetaj)*Trans(x,-dj)*rot(x,-alphaj)*a(j)Ti S1N1A1P1 = Tjk * _rot_trans( axis=x, th=robo.alpha[k], p=robo.d[k]) * _rot_trans( axis=z, th=robo.theta[k], p=0) # S1N1A1 = jTa(k)*rot(x,alphak)*Trans(x,dk)*rot(z,thetak) S2N2A2 = array(S2N2A2P2[:3, :3]) P3 = array(S3N3A3P3[:3, 3]) P2 = array(S2N2A2P2[:3, 3]) P1 = array(S1N1A1P1[:3, 3]) t2 = S2N2A2[:3, 2] P4 = P1 - dot(S2N2A2, P3) - P2 t1 = S1N1A1P1[:3, 2] coef = Matrix([[t1[0], t1[1], t1[2]], [t2[0], t2[1], t2[2]], [P4[0][0], P4[1][0], P4[2][0]]]) rijk = [robo.r[i], robo.r[j], robo.r[k]] offset = [robo.b[robo.ant[k]], robo.b[robo.ant[j]], robo.b[robo.ant[i]]] _equation_solve(symo, coef, eq_type, rijk, offset) return
def solve_position_prismatic(robo, symo, pieper_joints): """ Function that solves the position equation of the three prismatic joints case. (to find the three angles) Parameters: =========== 1) Pieper_joints: The three prismatic joints for the prismatic case """ eq_type = 0 # Type 0: Linear system [i,j,k] = pieper_joints # Pieper joints vector [x,y,z] = [0,1,2] # Book convention indexes robo.theta[i] = robo.theta[i] + robo.gamma[robo.ant[i]] robo.theta[j] = robo.theta[j] + robo.gamma[robo.ant[j]] robo.theta[k] = robo.theta[k] + robo.gamma[robo.ant[k]] T6k = dgm(robo, symo, 6, k, fast_form=True, trig_subs=True) Ti0 = dgm(robo, symo, robo.ant[i], 0, fast_form=True, trig_subs=True) Tji = dgm(robo, symo, j-1, i, fast_form=True, trig_subs=True) Tjk = dgm(robo, symo, j, k-1, fast_form=True, trig_subs=True) S3N3A3P3 = _rot_trans(axis=z, th=-robo.theta[i], p=0)*_rot_trans(axis=x, th=-robo.alpha[i], p=-robo.d[i])*Ti0*T_GENERAL*T6k # S3N3A3 = rot(z,-thetai)*Trans(x,-di)*rot(x,-alphai)*a(i)To*SNAP*6Tk S2N2A2P2 = _rot_trans(axis=z, th=-robo.theta[j], p=0)*_rot_trans(axis=x, th=-robo.alpha[j], p=-robo.d[j])*Tji # S2N2A2 = rot(z,-thetaj)*Trans(x,-dj)*rot(x,-alphaj)*a(j)Ti S1N1A1P1 = Tjk*_rot_trans(axis=x, th=robo.alpha[k], p=robo.d[k])*_rot_trans(axis=z, th=robo.theta[k], p=0) # S1N1A1 = jTa(k)*rot(x,alphak)*Trans(x,dk)*rot(z,thetak) S2N2A2 = array( S2N2A2P2[:3, :3] ) P3 = array( S3N3A3P3[:3, 3] ) P2 = array( S2N2A2P2[:3, 3] ) P1 = array( S1N1A1P1[:3, 3] ) t2 = S2N2A2[:3, 2] P4 = P1 - dot(S2N2A2, P3) - P2 t1 = S1N1A1P1[:3,2] coef = Matrix([[t1[0],t1[1],t1[2]],[t2[0],t2[1],t2[2]],[P4[0][0],P4[1][0],P4[2][0]]]) rijk = [robo.r[i], robo.r[j], robo.r[k]] offset = [robo.b[robo.ant[k]], robo.b[robo.ant[j]], robo.b[robo.ant[i]]] _equation_solve(symo, coef, eq_type, rijk, offset) return
def solve_orientation_prismatic(robo, symo, X_joints): """ Function that solves the orientation equation of the three prismatic joints case. (to find the three angles) Parameters: =========== 1) X_joint: The three revolute joints for the prismatic case """ [i, j, k] = X_joints # X joints vector [S, S1, S2, S3, x] = [0, 0, 0, 0, 0] # Book convention indexes [N, N1, N2, N3, y] = [1, 1, 1, 1, 1] # Book convention indexes [A, A1, A2, A3, z] = [2, 2, 2, 2, 2] # Book convention indexes robo.theta[i] = robo.theta[i] + robo.gamma[robo.ant[i]] robo.theta[j] = robo.theta[j] + robo.gamma[robo.ant[j]] robo.theta[k] = robo.theta[k] + robo.gamma[robo.ant[k]] T6k = dgm(robo, symo, 6, k, fast_form=True, trig_subs=True) Ti0 = dgm(robo, symo, robo.ant[i], 0, fast_form=True, trig_subs=True) Tji = dgm(robo, symo, j - 1, i, fast_form=True, trig_subs=True) Tjk = dgm(robo, symo, j, k - 1, fast_form=True, trig_subs=True) S3N3A3 = _rot_trans( axis=x, th=-robo.alpha[i], p=0 ) * Ti0 * T_GENERAL * T6k # S3N3A3 = rot(x,-alphai)*rot(z,-gami)*aiTo*SNA S2N2A2 = _rot_trans(axis=x, th=-robo.alpha[j], p=0) * Tji # S2N2A2 = iTa(j)*rot(x,-alphaj) S1N1A1 = Tjk * _rot_trans(axis=x, th=-robo.alpha[k], p=0) # S1N1A1 = jTa(k)*rot(x,alphak) S3N3A3 = Matrix([S3N3A3[:3, :3]]) S2N2A2 = Matrix([S2N2A2[:3, :3]]) S1N1A1 = Matrix([S1N1A1[:3, :3]]) SNA = Matrix([T_GENERAL[:3, :3]]) SNA = symo.replace(trigsimp(SNA), 'SNA') # solve thetai # eq 1.100 (page 49) eq_type = 3 offset = robo.gamma[robo.ant[i]] robo.r[i] = robo.r[i] + robo.b[robo.ant[i]] el1 = S2N2A2[z, S2] * S3N3A3[x, A3] + S2N2A2[z, N2] * S3N3A3[y, A3] el2 = S2N2A2[z, S2] * S3N3A3[y, A3] - S2N2A2[z, N2] * S3N3A3[x, A3] el3 = S2N2A2[z, A2] * S3N3A3[z, A3] - S1N1A1[z, A1] coef = [el1, el2, el3] _equation_solve(symo, coef, eq_type, robo.theta[i], offset) # solve thetaj # eq 1.102 eq_type = 4 offset = robo.gamma[robo.ant[j]] robo.r[j] = robo.r[j] + robo.b[robo.ant[j]] coef = [ S1N1A1[x, A1], -S1N1A1[y, A1], -SNA[x, A], S1N1A1[y, A1], S1N1A1[x, A1], -SNA[y, A] ] _equation_solve(symo, coef, eq_type, robo.theta[j], offset) # solve thetak # eq 1.103 eq_type = 4 offset = robo.gamma[robo.ant[k]] robo.r[k] = robo.r[k] + robo.b[robo.ant[k]] coef = [ S1N1A1[z, S1], S1N1A1[z, N1], -SNA[z, S], S1N1A1[z, N1], -S1N1A1[z, S1], -SNA[z, N] ] _equation_solve(symo, coef, eq_type, robo.theta[k], offset) return
def solve_position(robo, symo, com_key, X_joints, fc, fs, fr, f0, g): """ Function that solves the position equation for the four spherical cases. Parameters: =========== 1) com_key: X joints combination 2) X_joints: Type of the X joints 3) fc, fs, fr, f0: Coefficients of equation (1.23.b) 4) g: g = [gx; gy; gz] -> Vector containing constant values """ [i, j, k] = X_joints [x, y, z] = [0, 1, 2] # Book convention indexes offset = zeros(1, len(X_joints)) if robo.sigma[k] == 0: robo.r[k] = robo.r[k] + robo.b[robo.ant[k]] offset[2] = robo.gamma[robo.ant[k]] elif robo.sigma[k] == 1: robo.theta[k] = robo.theta[k] + robo.gamma[robo.ant[k]] offset[2] = robo.b[robo.ant[k]] if robo.sigma[j] == 0: # If the joint j is revolute robo.r[j] = robo.r[j] + robo.b[robo.ant[j]] offset[1] = robo.gamma[robo.ant[j]] T = _rot_trans(axis=z, th=0, p=robo.r[j]) elif robo.sigma[j] == 1: # If the joint j is prismatic robo.theta[j] = robo.theta[j] + robo.gamma[robo.ant[j]] offset[1] = robo.b[robo.ant[j]] T = _rot_trans(axis=z, th=robo.theta[j], p=0) tc = T * fc tc = symo.replace(trigsimp(tc), 'tc', k) ts = T * fs ts = symo.replace(trigsimp(ts), 'ts', k) tr = T * fr tr = symo.replace(trigsimp(tr), 'tr', k) t0 = T * f0 t0 = symo.replace(trigsimp(t0), 't0', k) if robo.sigma[i] == 0: # If joint i is revolute robo.r[i] = robo.r[i] + robo.b[robo.ant[i]] offset[0] = robo.gamma[robo.ant[i]] T = _rot_trans(axis=z, th=0, p=-robo.r[i]) else: # If the joint i is prismatic robo.theta[i] = robo.theta[i] + robo.gamma[robo.ant[i]] offset[0] = robo.b[robo.ant[i]] T = _rot_trans(axis=z, th=-robo.theta[i], p=0) G = T * g G = symo.replace(trigsimp(G), 'G') # Conditions to get the solution for its possible X joints combination if (com_key == 0) or (com_key == 1): # X joints: RRX with X:either R or P joint if sin(robo.alpha[j]) == 0: # sin(alphaj) equal 0 sin_alphaj_eq_0(robo, symo, X_joints, tc, ts, tr, t0, G, offset) elif robo.d[j] == 0: # dj equal 0 dj_eq_0(robo, symo, X_joints, tc, ts, tr, t0, G, offset) elif (sin(robo.alpha[j]) != 0) and (robo.d[j] != 0): # sin(alphaj) and dj not equal to 0 dj_and_sin_alpha_dif_0(robo, symo, X_joints, tc, ts, tr, t0, G, offset) elif (com_key == 2) or (com_key == 3): # X joints: RPX with X:either R or P joint if cos(robo.alpha[j]) == 0: # cos(alphaj) equal 0 cos_alpha_equal_0(robo, symo, X_joints, tc, ts, tr, t0, G, offset) elif cos(robo.alpha[j]) != 0: # cos(alphaj) not equal to 0 cos_alpha_dif_0(robo, symo, X_joints, tc, ts, tr, t0, G, offset) elif (com_key == 4) or (com_key == 5): # X joints: PRX with X:either R or P joint if cos(robo.alpha[j]) == 0: # cos(alphaj) equal 0 cos_alpha_equal_zero(robo, symo, X_joints, tc, ts, tr, t0, G, offset) elif cos(robo.alpha[j]) != 0: # cos(alphaj) not equal to 0 cos_alpha_dif_zero(robo, symo, X_joints, tc, ts, tr, t0, G, offset) else: # X joints: PPX with X:either R or P joint ij_prismatic(robo, symo, X_joints, tc, ts, tr, t0, G, offset) return
def solve_position(robo, symo, com_key, X_joints, fc, fs, fr, f0, g): """ Function that solves the position equation for the four spherical cases. Parameters: =========== 1) com_key: X joints combination 2) X_joints: Type of the X joints 3) fc, fs, fr, f0: Coefficients of equation (1.23.b) 4) g: g = [gx; gy; gz] -> Vector containing constant values """ [i,j,k] = X_joints [x,y,z] = [0,1,2] # Book convention indexes offset = zeros(1,len(X_joints)) if robo.sigma[k] == 0: robo.r[k] = robo.r[k] + robo.b[robo.ant[k]] offset[2] = robo.gamma[robo.ant[k]] elif robo.sigma[k] == 1: robo.theta[k] = robo.theta[k] + robo.gamma[robo.ant[k]] offset[2] = robo.b[robo.ant[k]] if robo.sigma[j] == 0: # If the joint j is revolute robo.r[j] = robo.r[j] + robo.b[robo.ant[j]] offset[1] = robo.gamma[robo.ant[j]] T = _rot_trans(axis=z, th=0, p=robo.r[j]) elif robo.sigma[j] == 1: # If the joint j is prismatic robo.theta[j] = robo.theta[j] + robo.gamma[robo.ant[j]] offset[1] = robo.b[robo.ant[j]] T = _rot_trans(axis=z, th=robo.theta[j], p=0) tc = T*fc tc = symo.replace(trigsimp(tc), 'tc', k) ts = T*fs ts = symo.replace(trigsimp(ts), 'ts', k) tr = T*fr tr = symo.replace(trigsimp(tr), 'tr', k) t0 = T*f0 t0 = symo.replace(trigsimp(t0), 't0', k) if robo.sigma[i] == 0: # If joint i is revolute robo.r[i] = robo.r[i] + robo.b[robo.ant[i]] offset[0] = robo.gamma[robo.ant[i]] T = _rot_trans(axis=z, th=0, p=-robo.r[i]) else: # If the joint i is prismatic robo.theta[i] = robo.theta[i] + robo.gamma[robo.ant[i]] offset[0] = robo.b[robo.ant[i]] T = _rot_trans(axis=z, th=-robo.theta[i], p=0) G = T*g G = symo.replace(trigsimp(G), 'G') # Conditions to get the solution for its possible X joints combination if (com_key == 0) or (com_key == 1): # X joints: RRX with X:either R or P joint if sin(robo.alpha[j]) == 0: # sin(alphaj) equal 0 sin_alphaj_eq_0(robo, symo, X_joints, tc, ts, tr, t0, G, offset) elif robo.d[j] == 0: # dj equal 0 dj_eq_0(robo, symo, X_joints, tc, ts, tr, t0, G, offset) elif (sin(robo.alpha[j]) != 0) and (robo.d[j] != 0): # sin(alphaj) and dj not equal to 0 dj_and_sin_alpha_dif_0(robo, symo, X_joints, tc, ts, tr, t0, G, offset) elif (com_key == 2) or (com_key == 3): # X joints: RPX with X:either R or P joint if cos(robo.alpha[j]) == 0: # cos(alphaj) equal 0 cos_alpha_equal_0(robo, symo, X_joints, tc, ts, tr, t0, G, offset) elif cos(robo.alpha[j]) != 0: # cos(alphaj) not equal to 0 cos_alpha_dif_0(robo, symo, X_joints, tc, ts, tr, t0, G, offset) elif (com_key == 4) or (com_key == 5): # X joints: PRX with X:either R or P joint if cos(robo.alpha[j]) == 0: # cos(alphaj) equal 0 cos_alpha_equal_zero(robo, symo, X_joints, tc, ts, tr, t0, G, offset) elif cos(robo.alpha[j]) != 0: # cos(alphaj) not equal to 0 cos_alpha_dif_zero(robo, symo, X_joints, tc, ts, tr, t0, G, offset) else: # X joints: PPX with X:either R or P joint ij_prismatic(robo, symo, X_joints, tc, ts, tr, t0, G, offset) return