def transform_to_next(A, alpha, D, theta, convention='standard'):
"""
Calculats transform from frame J = I-1 to I
in order AI = Rzj( theta )Tzj( D )Txi( A )Rxi( alpha )
and parameters are given in order A, alpha, D, theta,
according to the standard convention:
matrix = mat([[ct, -st*ca, st*sa, A*ct],
[st, ct*ca, -ct*sa, A*st],
[ 0, sa, ca, D ],
[ 0, 0, 0, 1 ]]);
If convention is changed to modified then the transformation
is remapped according to the equivalent modified Denivit-hartenberg,
and performs the mapping from I to I+1.
"""
Rz_J = homogenous_rotation_z(theta)
Tz_J = homogenous_translation_z(D)
Tx_I = homogenous_translation_x(A)
Rx_I = homogenous_rotation_x(alpha)
matrix = matmul(Rz_J, Tz_J, Tx_I, Rx_I)
if convention.lower() == 'standard':
return matrix
elif convention.lower() == 'modified':
modified = mat([
matrix[0, 0],
-matrix[1, 0],
matrix[2, 0],
A,
-matrix[0, 1],
matrix[1, 1],
-matrix[2, 1],
-matrix[2, 1] * D,
matrix[0, 2],
-matrix[1, 2],
matrix[2, 2],
matrix[2, 2] * D,
0,
0,
0,
1,
]).reshape((4, 4))
return modified
else:
raise ArithmeticError(
"As of writing this function only two conventions are allowed: 'standard' or 'modified'."
)
def backward_facing_elbow_down(dh_table, T44):
'''
Calculates backward-facing elbow-down.
Note: This is in reality an elbow-down solution,
the name merely implies it is "flipped" up-side-down
due to the base have turned 180 degrees.
'''
#Geometrical paramaters
wcp = calc_wcp(T44, 0.065)
#First angle - j1, used to adjust a point-position
j1 = calc_j1(wcp, flipped=True)
p0 = mat([70e-3, 0, 352e-3])
p0 = homogenous_rotation_z(j1)[0:3,0:3].dot(p0)
x0 = norm(wcp[0:2] - p0[0:2])
h1 = p0[2]
h2 = wcp[2]
s = h2 - h1
x1 = norm(p0 - wcp)
beta = 380e-3
alpha = 360e-3
th3 = ang_sats2(x1, alpha, beta)
j3 = -90 + th3
th21 = atan2(s, x0)
th22 = atan2(beta*sin2(th3), alpha + beta*cos2(th3))
j2 = -90 + th21 - th22
#packing the solutions in a dynamic way
## j4, j5, j6,\
## j41,j51,j61, \
## j42,j52,j62 = inverse_kinematics_spherical_wrist(dh_table, j1, j2, j3, T44)
##
## return (j1, j2, j3, j4, j5, j6),\
## (j1, j2, j3, j41, j51, j61), \
## (j1, j2, j3, j42, j52, j62)
result = pack_elbow_and_wrists(dh_table, j1, j2, j3, T44)
return result
def elbow_down(dh_table, T44):
'''
Calculates forward-facing elbow-down.
'''
#Geometrical paramaters
wcp = calc_wcp(T44, 0.065)
#First angle - j1, used to adjust a point-position
j1 = calc_j1(wcp, flipped=False)
p0 = mat([70e-3, 0, 352e-3])
p0 = homogenous_rotation_z(j1)[0:3,0:3].dot(p0)
x0 = norm(wcp[0:2] - p0[0:2])
h1 = p0[2]
h2 = wcp[2]
s = h2 - h1
x1 = norm(p0 - wcp)
beta = 380e-3
alpha = 360e-3
th3 = ang_sats2(x1, alpha, beta)
j3 = -th3 - 90
th21 = atan2(s, x0)
th22 = atan2(beta*sin2(th3), alpha + beta*cos2(th3))
j2 = 90 - (th21 - th22)
if norm(wcp[:2])-norm(p0[:2]) < 0:
j2 = -90 + (th21+th22)
## j3 = -90-th3
## j4, j5, j6,\
## j41,j51,j61, \
## j42,j52,j62 = inverse_kinematics_spherical_wrist(dh_table, j1, j2, j3, T44)
##
## return (j1, j2, j3, j4, j5, j6),\
## (j1, j2, j3, j41, j51, j61), \
## (j1, j2, j3, j42, j52, j62)
result = pack_elbow_and_wrists(dh_table, j1, j2, j3, T44)
return result