# 重力項の定義 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)