Minv1, Minv2, Minv3, Minv4 = sl.invm_2dof(M1, M2, M3, M4)
# コリオリ項の定義
h1, h2 = sl.coriolis_item_2dof(m2, l1, lg2, q2, dot_q1, dot_q2)
# 重力項の定義
# G1, G2 = sl.gravity_item_2dof(m1, m2, l1, lg1, lg2, q1, q2, g)
G1, G2 = 0.0, 0.0
# 入力
tau1 = sl.PIDcontrol(kp1, kv1, ki1, thetad1, theta1, dot_thetad1,
dot_theta1, sum_theta1)
tau2 = sl.PIDcontrol(kp2, kv2, ki2, thetad2, theta2, dot_thetad2,
dot_theta2, sum_theta2)
# 偏差と非線形弾性特性値の計算
e1 = sl.difference_part(theta1, q1)
e2 = sl.difference_part(theta2, q2)
e = [e1, e2]
K1 = sl.non_linear_item(k1[0], k2[0], e[0])
K2 = sl.non_linear_item(k1[1], k2[1], e[1])
K1 = K1 * e[0]
K2 = K2 * e[1]
K = [K1, K2]
# 各加速度の計算
ddot_q1 = sl.calculate_angular_acceleration(Minv1, Minv2, K[0], K[1],
h1, h2, G1, G2, D, dot_q1)
ddot_q2 = sl.calculate_angular_acceleration(Minv3, Minv4, K[0], K[1],
h1, h2, G1, G2, D, dot_q2)
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)
f, A = sl.input_forces(ll,
q,
dot_q,
H,
D,
K,
Jt,
P,
Q,
for j in range(200):
tauf = 0.1 * j
for i in tqdm(range(ST)):
time = i * sampling_time # 時間の設定
q, dot_q, ddot_q = sl.EulerMethod(q, dot_q, ddot_q, sampling_time)
# theta, dot_theta, ddot_theta = sl.EulerMethod(theta, dot_theta,
# ddot_theta,
# sampling_time)
# tau = kp * (thetad2 - theta[0]) - kv * dot_theta[0]
# 偏差と非線形弾性特性値の計算
e = sl.difference_part(theta, q, Gear_ratio)
K = sl.non_linear_item(k, e)
# 各角加速度計算
ddot_q[0] = (K[0] + tauf - D * dot_q[0]) / A
# ddot_theta[0] = (Gear_ratio * tau - K[0] - pow(Gear_ratio, 2) * B * dot_theta[0]) / pow(Gear_ratio, 2) / Mm
# Save time log
time_log = pr.save_part_log(time, time_log)
q_data = pr.save_part_log(degrees(q[0]), q_data)
qdata = [q_data]
dot_q_data = pr.save_part_log(degrees(dot_q[0]), dot_q_data)
ddot_q_data = pr.save_part_log(degrees(ddot_q[0]), ddot_q_data)