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
