def jacobian(self, theta):
[x_1, x_2, x_3, x_4] = self.dh2dq(theta)
q = np.zeros(8)
q[0] = -(1 / 2) * np.sin(theta[0] / 2) * np.cos(pi / 4)
q[1] = -(1 / 2) * np.sin(theta[0] / 2) * np.sin(pi / 4)
q[2] = (1 / 2) * np.cos(theta[0] / 2) * np.sin(pi / 4)
q[3] = (1 / 2) * np.cos(theta[0] / 2) * np.cos(pi / 4)
dx_1_dtheta_1 = DQ(q)
q = np.zeros(8)
q[0] = -(1 / 2) * np.sin((theta[1] + pi / 2) / 2)
q[3] = (1 / 2) * np.cos((theta[1] + pi / 2) / 2)
q[5] = (self.L_1 / 2) * q[0]
q[6] = (self.L_2 / 2) * q[3]
dx_2_dtheta_2 = DQ(q)
q = np.zeros(8)
q[0] = (1 / 2) * np.sin((theta[2] - theta[1] - pi / 2) / 2)
q[3] = -(1 / 2) * np.cos((theta[2] - theta[1] + pi / 2) / 2)
q[5] = (self.L_1 / 2) * q[0]
q[6] = (self.L_2 / 2) * q[3]
dx_3_dtheta_2 = DQ(q)
q = np.zeros(8)
q[0] = -(1 / 2) * np.sin((theta[2] - theta[1] - pi / 2) / 2)
q[3] = (1 / 2) * np.cos((theta[2] - theta[1] + pi / 2) / 2)
q[5] = (self.L_1 / 2) * q[0]
q[6] = (self.L_2 / 2) * q[3]
dx_3_dtheta_3 = DQ(q)
q = np.zeros(8)
q[0] = -(1 / 2) * np.sin(theta[2] / 2) * np.cos(pi / 4)
q[1] = (1 / 2) * np.sin(theta[2] / 2) * np.sin(pi / 4)
q[2] = (1 / 2) * np.cos(theta[2] / 2) * np.sin(pi / 4)
q[3] = -(1 / 2) * np.cos(theta[2] / 2) * np.cos(pi / 4)
dx_4_dtheta_3 = DQ(q)
J = np.array([vec8(dx_1_dtheta_1*x_2*x_3*x_4), \
vec8(x_1*dx_2_dtheta_2*x_3*x_4 + x_1*x_2*dx_3_dtheta_2*x_4), \
vec8(x_1*x_2*dx_3_dtheta_3*x_4 + x_1*x_2*x_3*dx_4_dtheta_3)])
J = np.transpose(J)
return J
def jacobp(analytical_jacobian, x):
dq_x = DQ(x)
dq_x_conj_P = dq_x.P()
dq_x_conj_P = dq_x_conj_P.conj()
(rows, cols) = analytical_jacobian.shape
aux_J1 = np.zeros((4, cols))
aux_J2 = np.zeros((4, cols))
for i in range(0, 4):
for j in range(0, cols):
aux_J1[i, j] = analytical_jacobian[i, j]
aux_J2[i, j] = analytical_jacobian[(i + 4), j]
aux = np.dot(hamiplus4(dq_x.D()), DQ_kinematics.C4)
Jp = 2 * (np.dot(haminus4(dq_x_conj_P), aux_J2)) + 2 * (np.dot(
aux, aux_J1))
return Jp
def __init__(self, dh_matrix, convention):
if str(convention) != "standard" and str(convention) != "modified":
raise RuntimeError(
"Bad DQ_kinematics(dh_matrix, convention) call: convention must be 'standard' or 'modified' "
)
(rows, cols) = dh_matrix.shape
if rows != 4 and rows != 5:
raise RuntimeError(
"Bad DQ_kinematics(dh_matrix, convention) call: dh_matrix should be 5xn or 4xn"
)
self.dh_matrix = dh_matrix
self.curr_base = DQ(1)
self.curr_effector = DQ(1)
self.dh_matrix_convention = convention
def jacobian(self, theta):
q_effector = self.raw_fkm(theta)
z = DQ(0)
q = DQ(1)
J = np.zeros((8, (self.links() - self.n_dummy())))
ith = -1
for i in range(0, self.links()):
# Use the standard DH convention
if self.convention() == "standard":
z = self.get_z(q.q)
# Use the modified DH convention
else:
alpha = (self.alpha())[i]
a = (self.a())[i]
dummy = (self.dummy())[i]
w = DQ([
0, 0, -sin(alpha),
cos(alpha), 0, 0, -a * cos(alpha), -a * sin(alpha)
])
z = 0.5 * q * w * q.conj()
if dummy == 0:
q = q * self.dh2dq(theta[ith + 1], i + 1)
aux_j = z * q_effector
for k in range(0, 8):
J[k, ith + 1] = aux_j.q[k]
ith = ith + 1
else:
#Dummy joints don't contribute to the Jacobian
q = q * self.dh2dq(0.0, (i + 1))
# Takes the base's displacement into account
aux_J = haminus8(self.curr_effector)
aux_J = np.dot(hamiplus8(self.curr_base), aux_J)
J = np.dot(aux_J, J)
return J
def dh2dq(self, theta):
q = np.zeros(8)
q[0] = np.cos(theta[0] / 2) * np.cos(pi / 4)
q[1] = np.cos(theta[0] / 2) * np.sin(pi / 4)
q[2] = np.sin(theta[0] / 2) * np.sin(pi / 4)
q[3] = np.sin(theta[0] / 2) * np.cos(pi / 4)
x_1 = DQ(q)
q = np.zeros(8)
q[0] = np.cos((theta[1] + pi / 2) / 2)
q[3] = np.sin((theta[1] + pi / 2) / 2)
q[5] = (self.L_1 / 2) * q[0]
q[6] = (self.L_2 / 2) * q[3]
x_2 = DQ(q)
q = np.zeros(8)
q[0] = np.cos((theta[2] - theta[1] - pi / 2) / 2)
q[3] = np.sin((theta[2] - theta[1] - pi / 2) / 2)
q[5] = (self.L_1 / 2) * q[0]
q[6] = (self.L_2 / 2) * q[3]
x_3 = DQ(q)
q = np.zeros(8)
q[0] = np.cos(theta[2] / 2) * np.cos(pi / 4)
q[1] = -np.cos(theta[2] / 2) * np.sin(pi / 4)
q[2] = np.sin(theta[2] / 2) * np.sin(pi / 4)
q[3] = -np.sin(theta[2] / 2) * np.cos(pi / 4)
x_4 = DQ(q)
return [x_1, x_2, x_3, x_4]
def callback(msg):
posx = msg.x
posy = msg.y
posz = msg.z
position = np.array(posx, posy, posz)
kinematics = MeArmKinematics()
#mearm = MeArm(baseServoPin = 4, rightServoPin = 17, leftServoPin = 27, handServoPin = 22)
theta = np.array([0, 0, 0])
theta1 = np.arctan2(position[1], position[0])
r = DQ([np.cos(theta1 / 2), 0, 0, np.sin(theta1 / 2), 0, 0, 0, 0])
p = DQ([0, position[0], position[1], position[2], 0, 0, 0, 0])
xd = r + (1 / 2) * DQ.E_ * p * r
error = 1
epsilon = 0.1
dt = 0.1
while np.linalg.norm(error) > epsilon:
x = kinematics.fkm(theta)
J = kinematics.jacobian(theta)
error = vec8(xd - x)
theta = theta + np.linalg.pinv(J) @ error * dt
#mearm.setTheta(theta * 180/np.pi)
time.sleep(0.0001)
print(theta * 180 / np.pi)
mearm.setHand(95)
input()
mearm.closeConn()
def raw_fkm(self, theta):
(size, ) = theta.shape
if size != (self.links() - self.n_dummy()):
raise RuntimeError(
"Bad raw_fkm(theta_vec) call: Incorrect number of joint variables"
)
q = DQ(1)
j = 0
dummy = self.dummy()
for i in range(0, self.links()):
if dummy[i] == 1:
q = q * self.dh2dq(0.0, i + 1)
j = j + 1
else:
q = q * self.dh2dq(theta[i - j], i + 1)
return q
def effector(self):
return DQ(self.effector)
def base(self):
return DQ(self.curr_base)
def set_effector(self, effector):
self.curr_effector = effector
return DQ(self.curr_effector)
def set_base(self, base):
self.curr_base = base
return DQ(self.curr_base)
inferSchema="true",
header="true")
df_file = main_functions.name_convert(df_file, data_dict)
try:
df_out = main_functions.customUnion(df_file, df_out)
except:
df_out = df_file
# This step is used when merging data to existing data in the system
#df_out = main_dunctions.customUnion(new_merge, merge_all)
# Only keep consenting participants. Think about how to do this when not dealing with participant info
#df_out = df_out.filter(func.col('21') == 'Consenting')
# Update gender terms
df_out = DQ.fix_gender(data_dict, df_out)
# Collect Ids of each type
type_dict = {}
for d_type in data_dict['Type'].unique().tolist():
list_ = data_dict.loc[data_dict['Type'] == d_type]['FieldID'].astype(
str).tolist()
type_dict[d_type] = list_
# Collect Ids for each value type
value_type_dict = {}
for v_type in data_dict['ValueType'].unique().tolist():
list_ = data_dict.loc[data_dict['ValueType'] == v_type]['FieldID'].astype(
str).tolist()
value_type_dict[v_type] = list_
print("Input the desired coordinates")
quit()
else:
position = np.array(
[float(sys.argv[1]),
float(sys.argv[2]),
float(sys.argv[3])])
kinematics = MeArmKinematics()
#mearm = MeArm(baseServoPin = 4, rightServoPin = 17, leftServoPin = 27, handServoPin = 22)
theta = np.array([0, 0, 0])
theta1 = np.arctan2(position[1], position[0])
r = DQ([np.cos(theta1 / 2), 0, 0, np.sin(theta1 / 2), 0, 0, 0, 0])
p = DQ([0, position[0], position[1], position[2], 0, 0, 0, 0])
xd = r + (1 / 2) * DQ.E_ * p * r
error = 1
epsilon = 0.1
dt = 0.1
while np.linalg.norm(error) > epsilon:
x = kinematics.fkm(theta)
J = kinematics.jacobian(theta)
error = vec8(xd - x)
theta = theta + np.dot(np.linalg.pinv(J), error) * dt