label_name3 = ['X position', 'Y position', 'Xd position', 'Yd position']
label_name4 = ['λx', 'λy']
label_name5 = ["k1 = {}, k2 = {}".format(k1[0], k1[1])]
label_name = [
label_name1, label_name2, label_name3, label_name4, label_name5
]
xlabel_name = ['Time[s]', 'X[m]', 'θ-q']
ylabel_name = ['Angle[deg]', 'Position[m]', 'Force [N]', 'Y[m]', 'K']
plt.figure(figsize=(11, 12))
plt.subplot(321)
pr.print_graph(title_name[0],
time_log,
q_data,
label_name[0],
xlabel_name[0],
ylabel_name[0],
num_plot_data=4)
plt.subplot(322)
pr.print_graph(title_name[1],
time_log,
theta_data,
label_name[1],
xlabel_name[0],
ylabel_name[0],
num_plot_data=4)
plt.subplot(323)
pr.print_graph(title_name[2],
row = "{}, {}, {}, {}, {}, {}, {}, {}, {}\n".format(
time, degrees(q1), degrees(q2), degrees(qd1), degrees(qd2), dot_q1,
dot_q2, ddot_q1, ddot_q2)
time_log = sl.save_log(time, time_log)
deg1 = sl.save_log(degrees(q1), deg1)
deg2 = sl.save_log(degrees(q2), deg2)
dot_deg1 = sl.save_log(dot_q1, dot_deg1)
dot_deg2 = sl.save_log(dot_q2, dot_deg2)
ddot_deg1 = sl.save_log(ddot_q1, ddot_deg1)
ddot_deg2 = sl.save_log(ddot_q2, ddot_deg2)
f.write(row)
x_data = [time_log]
y_data = [deg1, deg2]
label_name = ['deg1', 'deg2']
plt.figure(figsize=(7, 3))
pr.print_graph("Link Angle",
time_log,
y_data,
label_name,
"Time[s]",
"Angle[deg]",
num_plot_data=2)
plt.show()
f.close()
label_name9 = ['qd1', 'qd2', 'qd3', 'qd4', 'qd5']
label_name = [
label_name1, label_name2, label_name3, label_name4, label_name5,
label_name6, label_name7, label_name8, label_name9
]
xlabel_name = ['Time[s]', 'X[m]', 'θ-q[rad]']
ylabel_name = ['Angle[deg]', 'Position[m]', 'Force [N]', 'Y[m]', 'K[Nm]']
plt.figure(figsize=(11, 12))
plt.subplot(521)
pr.print_graph(title_name[0],
time_log,
q_data,
label_name[0],
xlabel_name[0],
ylabel_name[0],
num_plot_data=4)
plt.subplot(522)
pr.print_graph(title_name[1],
time_log,
theta_data,
label_name[1],
xlabel_name[0],
ylabel_name[0],
num_plot_data=4)
plt.subplot(523)
pr.print_graph(title_name[2],
fm.write(m_data)
# Print result
x_data = [time_log]
link_data = [link1, link2, desired_angle1, desired_angle2]
label_name1 = ['Link1', 'Link2', 'Desired angle1', 'Desired angle2']
motor_data = [motor1, motor2, desired_angle1, desired_angle2]
label_name2 = ['Motor1', 'Motor2', 'Desired angle1', 'Desired angle2']
plt.figure(figsize=(5, 5))
plt.subplot(2, 1, 1)
pr.print_graph("Link Angle",
time_log,
link_data,
label_name1,
"Time[s]",
"Angle[deg]",
num_plot_data=4)
plt.subplot(2, 1, 2)
pr.print_graph("Motor Angle",
time_log,
motor_data,
label_name2,
"Time[s]",
"Angle[deg]",
num_plot_data=4)
plt.show()
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)
l_data = [L_data]
title_name = ['Force-Deviation']
label_name = ['q']
xlabel_name = ['Time[s]', 'X[m]', 'θ-q[rad]', 'External force[N]']
ylabel_name = [
'Angle[deg]', 'Position[m]', 'Y[m]', 'K(θ-q)[Nm]', 'Deviation[m]'
]
plt.figure(figsize=(6, 3))
pr.print_graph(title_name[0],
tauf_data,
l_data,
label_name[0],
xlabel_name[3],
ylabel_name[4],
num_plot_data=1)
print(r)
print(L)
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]',
num_plot_data=2)
plt.savefig('2_2sim0303.eps')
plt.show()