def generate_robix_command(thetas):
return "MOVE 1 TO {}, 2 TO {}, 3 TO {}, 4 TO {}, 5 TO {}".format(
convert_degrees_to_robix('theta_1', thetas[0]),
convert_degrees_to_robix('theta_2', thetas[1]),
convert_degrees_to_robix('theta_3', thetas[2]),
convert_degrees_to_robix('theta_4', thetas[3]),
convert_degrees_to_robix('theta_5', thetas[4]),
)
def generate_robix_command(thetas):
return "MOVE 1 TO {}, 2 TO {}, 3 TO {}, 4 TO {}, 5 TO {}, 6 TO {};".format(
convert_degrees_to_robix('theta_1', thetas[0]),
convert_degrees_to_robix('theta_2', thetas[1]),
convert_degrees_to_robix('theta_3', thetas[2]),
convert_degrees_to_robix('theta_4', thetas[3]),
convert_degrees_to_robix('theta_5', thetas[4]),
convert_degrees_to_robix('theta_6', thetas[5]),
)
import numpy as np from kinematics.forward import convert_robix_to_degrees, convert_degrees_to_robix angle = 45 # 45 - 90*np.random.rand() print angle robix = convert_degrees_to_robix(angle, name='theta_1') print robix print convert_robix_to_degrees(robix, name='theta_1')
def inverse_kinematics(q_matrix):
q11 = q_matrix[0, 0]
q12 = q_matrix[0, 1]
q13 = q_matrix[0, 2]
q14 = q_matrix[0, 3]
q21 = q_matrix[1, 0]
q22 = q_matrix[1, 1]
q23 = q_matrix[1, 2]
q24 = q_matrix[1, 3]
q31 = q_matrix[2, 0]
q32 = q_matrix[2, 1]
q33 = q_matrix[2, 2]
q34 = q_matrix[2, 3]
# Defining Di and Li
# Di vaLues
d1 = robix['theta_1']['d']
d2 = robix['theta_2']['d']
d3 = robix['theta_3']['d']
d4 = robix['theta_4']['d']
d5 = robix['theta_5']['d']
l1 = robix['theta_1']['l']
l2 = robix['theta_2']['l']
l3 = robix['theta_3']['l']
l4 = robix['theta_4']['l']
l5 = robix['theta_5']['l']
"""
# finding thetas
"""
# Theta 3
T3 = asind((q34 - d5 * q33 - d1) / l3)
# Theta 4
T4 = acosd(-1 * q33) - T3 - 90
# Theta 5
T5 = atan2d(-1 * q32, q31) - 10
m = (q11) / ((q23 / q13) * sind(T5 + 10) - q33 * cosd(T5 + 10))
n = (q12) / (cosd(T5 + 10) + (q13 / q23) * q33 * sind(T5 + 10))
T1 = atan2d((q24 - 10 * q23 - l3 * n * cosd(T3) - l2 * n),
(q14 - 10 * q13 - l3 * m * cosd(T3) - l2 * m))
T2 = atan2d(q23, q13) - T1
t = [
np.round(-1 * np.real(T1), 1),
np.round(-1 * np.real(T2), 1),
np.round(-1 * np.real(T3), 1),
np.round(np.real(T4), 1),
np.round(-1 * np.real(T5), 1)
]
for item in t:
_item = _item = np.round(item, 1)
if not (_item >= robix['theta_{}'.format(t.index(item) + 1)]['min'] and
_item <= robix['theta_{}'.format(t.index(item) + 1)]['max']):
error = ValueError(
"calculated theta_{} = {} out of range [{}, {}]: not a valid robot config"
.format(
t.index(item) + 1, item,
robix['theta_{}'.format(t.index(item) + 1)]['min'],
robix['theta_{}'.format(t.index(item) + 1)]['max']))
raise (error)
t_robix = t
for i in range(len(t)):
t_robix[i] = convert_degrees_to_robix('theta_{}'.format(i + 1), t[i])
return t_robix
actual_thetas = []
predicted_thetas = []
theta_x_acc = []
errors = 0
for thetas in 60 - 120 * np.random.rand(NUM_TRIALS, 5):
# t = np.matmul(forward, np.array([[0], [0], [0], [1]]))
# print t
# actual_thetas.append(t)
print("input angles: {}".format(np.round(thetas, 2)))
thetas_robix = []
for i in range(len(thetas)):
thetas_robix.append(
convert_degrees_to_robix('theta_{}'.format(i + 1), thetas[i]))
print("input robix: {}".format(thetas_robix))
try:
forward = forward_kinematics(thetas)
predicted = inverse_kinematics(forward)
actual_thetas.append(thetas_robix)
predicted_thetas.append(predicted)
print("predicted inverse robix: {}".format(predicted))
except ValueError as e:
print(e)
errors += 1
print("errors: {}".format(errors))
predicted_thetas = np.array(predicted_thetas)
print(np.absolute(np.array(actual_thetas) - predicted_thetas).max(axis=0))
def inverse_kinematics(q_matrix):
q11 = q_matrix[0, 0]
q12 = q_matrix[0, 1]
q13 = q_matrix[0, 2]
q14 = q_matrix[0, 3]
q21 = q_matrix[1, 0]
q22 = q_matrix[1, 1]
q23 = q_matrix[1, 2]
q24 = q_matrix[1, 3]
q31 = q_matrix[2, 0]
q32 = q_matrix[2, 1]
q33 = q_matrix[2, 2]
q34 = q_matrix[2, 3]
# Defining Di and Li
# Di vaLues
d1 = robix['theta_1']['d']
d2 = robix['theta_2']['d']
d3 = robix['theta_3']['d']
d4 = robix['theta_4']['d']
d5 = robix['theta_5']['d']
l1 = robix['theta_1']['l']
l2 = robix['theta_2']['l']
l3 = robix['theta_3']['l']
l4 = robix['theta_4']['l']
l5 = robix['theta_5']['l']
"""
# finding thetas
"""
# Theta 3
T3 = asind((q34-d5*q33-d1)/l3)
# Theta 4
T4 = acosd(-1*q33)-T3-90
# Theta 5
T5 = atan2d(-1*q32, q31)-10
m = (q11)/((q23/q13)*sind(T5+10)-q33*cosd(T5+10))
n = (q12)/(cosd(T5+10)+(q13/q23)*q33*sind(T5+10))
T1 = atan2d((q24-10*q23-l3*n*cosd(T3)-l2*n),
(q14-10*q13-l3*m*cosd(T3)-l2*m))
T2 = atan2d(q23, q13)-T1
t = [np.round(-1*np.real(T1), 1),
np.round(-1*np.real(T2), 1),
np.round(-1*np.real(T3), 1),
np.round(np.real(T4), 1),
np.round(-1*np.real(T5), 1)]
for item in t:
_item = _item = np.round(item, 1)
if not (_item >= robix['theta_{}'.format(t.index(item)+1)]['min'] and _item <= robix['theta_{}'.format(t.index(item)+1)]['max']):
error = ValueError("calculated theta_{} = {} out of range [{}, {}]: not a valid robot config"
.format(t.index(item)+1, item, robix['theta_{}'.format(t.index(item)+1)]['min'],
robix['theta_{}'.format(t.index(item)+1)]['max']))
raise(error)
t_robix = t
for i in range(len(t)):
t_robix[i] = convert_degrees_to_robix('theta_{}'.format(i+1), t[i])
return t_robix
NUM_TRIALS = 100
actual_thetas = []
predicted_thetas = []
theta_x_acc = []
errors = 0
for thetas in 60 - 120*np.random.rand(NUM_TRIALS, 5):
# t = np.matmul(forward, np.array([[0], [0], [0], [1]]))
# print t
# actual_thetas.append(t)
print("input angles: {}".format(np.round(thetas, 2)))
thetas_robix = []
for i in range(len(thetas)):
thetas_robix.append(convert_degrees_to_robix('theta_{}'.format(i+1), thetas[i]))
print("input robix: {}".format(thetas_robix))
try:
forward = forward_kinematics(thetas)
predicted = inverse_kinematics(forward)
actual_thetas.append(thetas_robix)
predicted_thetas.append(predicted)
print("predicted inverse robix: {}".format(predicted))
except ValueError as e:
print(e)
errors += 1
print("errors: {}".format(errors))
predicted_thetas = np.array(predicted_thetas)
print(np.absolute(np.array(actual_thetas) - predicted_thetas).max(axis=0))