arm2_dotq = [dot_q[3], dot_q[4]]
arm1_ddotq = [ddot_q[0], ddot_q[1], ddot_q[2]]
arm2_ddotq = [ddot_q[3], ddot_q[4]]
# Desired Parametas
dot_qd = [0.0, 0.0, 0.0, 0.0, 0.0]
# ヤコビ行列
J1 = sl.jacobi_serial3dof(link1, arm1_q)
J2 = sl.jacobi_serial2dof(link2, arm2_q)
Jt1 = sl.transpose_matrix(J1)
Jt2 = sl.transpose_matrix(J2)
InvJ2 = sl.inverse_matrix(Jt2)
J1_seudo = sl.pseudo_inverse_matrix(J1, Jt1)
# 3*3単位行列
eye = np.eye(3)
f2 = Jt2.dot(tau2)
X1 = sl.inverse_matrix(eye - Jt1.dot(J1_seudo))
X2 = ((Jt1.dot(-f2)) - tau1)
k = X1.dot(X2)
N = (eye - Jt1.dot(J1_seudo))
# 慣性行列の定義
mm = sl.moment_matrix_3dof(m, ll, lg, Inertia, q)
# コリオリ項の定義
H = sl.coriolis_item_3dof(m, ll, lg, Inertia, q, dot_q)
# 重力項の定義
g1, g2, g3, g4 = 0.0, 0.0, 0.0, 0.0
G = [g1, g2, g3, g4]
# 二回微分値の導出
E = sl.twice_differential_values(ll, q)
# 逆行列の掃き出し
Phi = sl.phi_matrix(mm, E)
invPhi = sl.inverse_matrix(Phi)
# ヤコビとヤコビ転置
J = sl.jacobi_matrix(ll, q)
Jt = sl.transpose_matrix(J)
# 手先位置導出
X = ll[0] * cos(q[0]) + ll[1] * cos(q[0] + q[1])
Y = ll[0] * sin(q[0]) + ll[1] * sin(q[0] + q[1])
position = [X, Y]
# 偏差積分値の計算
sum_x = sl.sum_position_difference(sum_x, xd, X, sampling_time)
sum_y = sl.sum_position_difference(sum_y, yd, Y, sampling_time)
sum_q = [0.0, 0.0, 0.0, 0.0]
arm1_dotq = [dot_q[0], dot_q[1]]
arm2_dotq = [dot_q[2], dot_q[3]]
arm1_ddotq = [ddot_q[0], ddot_q[1]]
arm2_ddotq = [ddot_q[2], ddot_q[3]]
# ヤコビとヤコビ転置
J1 = sl.jacobi_serial2dof(link1, arm1_q)
J2 = sl.jacobi_serial2dof(link2, arm2_q)
Jt1 = sl.transpose_matrix(J1)
Jt2 = sl.transpose_matrix(J2)
Jt2I = sl.inverse_matrix(Jt2)
f2 = Jt2I.dot(tau2)
n = sl.unit_vector(-x20, -y20)
f2_norm = sqrt(pow(f2[0], 2) + pow(f2[1], 2))
nfx = n[0] * f2_norm
nfy = n[1] * f2_norm
nf = [nfx, nfy]
tau1 = Jt1.dot(nf)
tau11_data = pr.save_part_log(tau1[0], tau11_data)
tau12_data = pr.save_part_log(tau1[1], tau12_data)
r_data = pr.save_part_log(r, r_data)