# 拘束項
lam = sl.binding_force(invPhi, f, A)
# モータ角加速度の計算
ddot_theta = sl.motor_angular_acceleration(Mm, Tau, B, dot_theta, K)
# エクセル用log_data保存
link_data = pr.save_angle_excel_log(time, q, qd, dot_q, ddot_q)
motor_data = pr.save_angle_excel_log(time, theta, thetad, dot_theta,
ddot_theta)
position_data = pr.save_position_log(time, position[0], position[1],
xd, yd, lam[0], lam[1])
# Save time log
time_log = pr.save_part_log(time, time_log)
# Save link_data(radian)
qd1_data = pr.save_part_log(degrees(qd[0]), qd1_data)
qd2_data = pr.save_part_log(degrees(qd[1]), qd2_data)
qd3_data = pr.save_part_log(degrees(qd[2]), qd3_data)
qd4_data = pr.save_part_log(degrees(qd[3]), qd4_data)
qd_data = [qd1_data, qd2_data, qd3_data, qd4_data]
q1_data = pr.save_part_log(degrees(q[0]), q1_data)
q2_data = pr.save_part_log(degrees(q[1]), q2_data)
q3_data = pr.save_part_log(degrees(q[2]), q3_data)
q4_data = pr.save_part_log(degrees(q[3]), q4_data)
q_data = [q1_data, q2_data, q3_data, q4_data]
dot_q1_data = pr.save_part_log(dot_q[0], dot_q1_data)
# 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)
x = ll[0] * cos(q[0])
y = ll[0] * sin(q[0])
r = sqrt(pow(ll[0] - x, 2) + pow(0 - y, 2))
L = 2 * ll[0] * 3.14 * degrees(q[0]) / 360
tauf_data.append(tauf)
L_data.append(L)
# モータ角加速度の計算
ddot_theta = sl.motor_angular_acceleration(Mm, Tau, B, dot_theta,
K_error, N)
C = sl.make_circle(eps1, time, xd1, yd1)
# エクセル用log_data保存
# link_data = pr.save_angle_excel_log(time, q, qd, dot_q, ddot_q)
# motor_data = pr.save_angle_excel_log(time, theta, thetad,
# dot_theta, ddot_theta)
position_data = pr.save_position_log(time, position1[0], position1[1],
xd1, yd1, lam[0], lam[1])
# Save Circle data
circlex_data = pr.save_part_log(C[0], circlex_data)
circley_data = pr.save_part_log(C[1], circley_data)
# Save time log
time_log = pr.save_part_log(time, time_log)
# Save K log
K_data1 = pr.save_part_log(K_error[0], K_data1)
K_data2 = pr.save_part_log(K_error[1], K_data2)
K_data3 = pr.save_part_log(0, K_data3)
K_data4 = pr.save_part_log(K_error[3], K_data4)
non_liner_data = [K_data1, K_data2, K_data3, K_data4]
# Save link_data(radian)
qd1_data = pr.save_part_log(degrees(qd[0]), qd1_data)
qd2_data = pr.save_part_log(degrees(qd[1]), qd2_data)
Jt1 = sl.transpose_matrix(J1)
Jt2 = sl.transpose_matrix(J2)
Jt2I = sl.inverse_matrix(Jt2)
f2 = Jt2I.dot(tau2)
n = sl.unit_vector(-x20, -y20)
f2_norm = sqrt(pow(f2[0], 2) + pow(f2[1], 2))
nfx = n[0] * f2_norm
nfy = n[1] * f2_norm
nf = [nfx, nfy]
tau1 = Jt1.dot(nf)
tau11_data = pr.save_part_log(tau1[0], tau11_data)
tau12_data = pr.save_part_log(tau1[1], tau12_data)
r_data = pr.save_part_log(r, r_data)
tau1_data = [tau11_data, tau12_data]
label_name = ['tau11', 'tau12']
plt.figure(figsize=(5, 5))
pr.print_graph('tau1-rdata',
r_data,
tau1_data,
label_name,
'r[m]',
'Torque[Nm]',