'dot_q1, dot_q2, dot_q3, dot_q4,' +
'ddot_q1, ddot_q2, ddot_q3, ddot_q4\n')
fm.write('Time[s], theta1, theta2, theta3, theta4,' +
'thetad1, thetad2, thetad3, thetad4,' +
'dot_theta1, dot_theta2, dot_theta3, dot_theta4,' +
'ddot_theta1, ddot_theta2, ddot_theta3, ddot_theta4\n')
fp.write('Time[s], X, Y, Xd, Yd, lambdax, lambday\n')
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)
# 慣性行列の定義
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]
fl = open('2dof_simulation_link_data.csv', 'w')
fm = open('2dof_simulation_motor_data.csv', 'w')
fl.write('Time[s], q1, q2, qd1, qd2, dot_q1, dot_q2, ddot_q1, ddot_q2\n')
fm.write(
'Time[s], theta1, theta2, thetad1, thetad2, dot_theta1, dot_theta2, ddot_theta1, ddot_theta2\n'
)
for i in range(ST):
time = i * sampling_time # 時間の設定
# オイラー法の定義
q1, q2, dot_q1, dot_q2 = sl.EulerMethod(q1, q2, dot_q1, dot_q2,
ddot_q1, ddot_q2,
sampling_time)
(theta1, theta2, dot_theta1,
dot_theta2) = sl.EulerMethod(theta1, theta2, dot_theta1, dot_theta2,
ddot_theta1, ddot_theta2, sampling_time)
# 慣性行列の定義
M1, M2, M3, M4 = sl.inertia_term_2dof(m1, m2, l1, l2, lg1, lg2, I1, I2,
q2)
# 逆行列の掃き出し
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)
ST = int(sl.simulation_time(count_time, sampling_time))
f = open('2dof_simulation.csv', 'w')
Parameters = [
'Time[s]', 'q1', 'q2', 'qd1', 'qd2', 'dot_q1', 'dot_q2', 'ddot_q1',
'ddot_q2'
]
f.write('Time[s], q1, q2, qd1, qd2, dot_q1, dot_q2, ddot_q1, ddot_q2\n')
row_data = []
for i in range(ST):
time = i * sampling_time
q1, q2, dot_q1, dot_q2 = sl.EulerMethod(q1, q2, dot_q1, dot_q2,
ddot_q1, ddot_q2,
sampling_time)
M1, M2, M3, M4 = sl.inertia_term_2dof(m1, m2, l1, l2, lg1, lg2, I1, I2,
q2)
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)
tau1 = sl.PIDcontrol(kp1, kv1, ki1, qd1, q1, dot_qd1, dot_q1, sum_q1)
tau2 = sl.PIDcontrol(kp2, kv2, ki2, qd2, q2, dot_qd2, dot_q2, sum_q2)
ddot_q1 = sl.calculate_angular_acceleration(Minv1, Minv2, tau1, tau2,
h1, h2, G1, G2)
tauf_data = []
# Time Parametas
simulate_time = 3.0 # シミュレート時間
sampling_time = 0.001 # サンプリングタイム
time_log = []
ST = int(sl.simulation_time(simulate_time, sampling_time))
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