# 重力項の定義
g1, g2, g3, g4, g5 = 0.0, 0.0, 0.0, 0.0, 0.0
G = [g1, g2, g3, g4, g5]
# 二回微分値の導出
E = sl.define_E(ll, q)
# 逆行列の掃き出し
Phi = sl.phi_matrix_4dof(mm1, mm2, E)
invPhi = sl.inverse_matrix(Phi)
# ヤコビとヤコビ転置
J1 = sl.jacobi_serial3dof(link1, arm1_q)
J2 = sl.jacobi_serial2dof(link2, arm2_q)
J = sl.jacobi(J1, J2)
Jt = sl.transpose_matrix(J)
# 極座標系ヤコビ行列
J1_polar = sl.jacobi_polar_coordinates_3dof(link1, arm1_q)
J2_polar = sl.jacobi_polar_coordinates_2dof(link2, arm2_q)
J_polar = sl.jacobi(J1_polar, J2_polar)
Jt_polar = sl.transpose_matrix(J_polar)
# 手先位置導出
X = ll[0] * cos(q[0]) + ll[1] * cos(q[0] + q[1]) + ll[2] * cos(
q[0] + q[1] + q[2])
Y = ll[0] * sin(q[0]) + ll[1] * sin(q[0] + q[1]) + ll[2] * sin(
q[0] + q[1] + q[2])
position = [X, Y]
X2 = ll[3] * cos(q[3]) + ll[4] * cos(q[3] + q[4])
# 重力項の定義
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_X = [sum_x, sum_y]
# モータ入力
Tau = sl.PID_potiton_control_3dof(gain, Xd, position, Jt, dot_theta,
arm2_q = [q[3], q[4]]
arm1_dotq = [dot_q[0], dot_q[1], dot_q[2]]
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)