# ヤコビとヤコビ転置
J = sl.jacobi_matrix(ll, q)
Jt = sl.transpose_matrix(J)
# 手先位置導出
X1 = ll[0] * cos(q[0]) + ll[1] * cos(q[0] + q[1])
Y1 = ll[0] * sin(q[0]) + ll[1] * sin(q[0] + q[1])
position1 = [X1, Y1]
X2 = ll[2] * cos(q[2]) + ll[3] * cos(q[2] + q[3]) + r0[0]
Y2 = ll[2] * sin(q[2]) + ll[3] * sin(q[2] + q[3]) + r0[1]
position2 = [X2, Y2]
# 偏差積分値の計算
sum_q[0] = sl.sum_position_difference(sum_q[0], qd[0], q[0],
sampling_time)
sum_q[1] = sl.sum_position_difference(sum_q[1], qd[1], q[1],
sampling_time)
sum_q[2] = sl.sum_position_difference(sum_q[2], qd[2], q[2],
sampling_time)
sum_q[3] = sl.sum_position_difference(sum_q[3], qd[3], q[3],
sampling_time)
sum_q = [sum_q[0], sum_q[1], sum_q[2], sum_q[3]]
Tau, actf, thetad = sl.PIDcontrol_eforce_base_3dof(
gain1, gain2, theta, dot_theta, Xd, position1, Jt, k, qd, sum_q, N,
force_gain, eps, Fconstant)
# 偏差と非線形弾性特性値の計算
e = sl.difference_part(theta, q, N)
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,
sum_X)
# 偏差と非線形弾性特性値の計算
e = sl.difference_part(theta, q, N)
K = sl.non_linear_item(k, e)
# 拘束力とダイナミクス右辺の計算
dot_P, P, dot_Q, Q = sl.restraint_part(ll, q, dot_q)
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])
Y2 = ll[3] * sin(q[3]) + ll[4] * sin(q[3] + q[4])
position2 = [X2, Y2]
R = sqrt(pow(X, 2) + pow(Y, 2))
phi = radians(atan2(Y, X))
phi1 = radians(q[0] + q[1] + q[2])
phi2 = radians(q[3] + q[4])
# 偏差積分値の計算
sum_r = sl.sum_position_difference(sum_r, Rd, R, sampling_time)
sum_phi = sl.sum_position_difference(sum_phi, phid, phi,
sampling_time)
sum_polar = [sum_r, sum_phi]
sum_q[0] = sl.sum_position_difference(sum_q[0], qd[0], q[0],
sampling_time)
sum_q[1] = sl.sum_position_difference(sum_q[1], qd[1], q[1],
sampling_time)
sum_q[2] = sl.sum_position_difference(sum_q[2], qd[2], q[2],
sampling_time)
sum_q[3] = sl.sum_position_difference(sum_q[3], qd[3], q[3],
sampling_time)
sum_q[4] = sl.sum_position_difference(sum_q[4], qd[4], q[4],
sampling_time)
sum_q = [sum_q[0], sum_q[1], sum_q[2], sum_q[3], sum_q[4]]