def c_determinant(ABa2):
controllability_det = []
for a,b in ABa2[0]:
controllability_det.append(det_controllable(a, b[:,1].reshape(4,1)))
order = np.argsort(ABa2[1])
xs = np.array(ABa2[1])[order]
ys = np.array(controllability_det)[order]
return [xs, ys]
#from utils import controllable
init_vprinting()
inputA = open('double_block_linearized_A_full.pkl', 'rb')
inputB = open('double_block_linearized_B_full.pkl', 'rb')
inputa2 = open('double_block_angle_two.pkl','rb')
linearA = pickle.load(inputA)
linearB = pickle.load(inputB)
angle_2 = pickle.load(inputa2)
inputA.close()
inputB.close()
inputa2.close()
B = []
for b in linearB:
B.append(b[:,1].reshape(4,1))
controllability_det = []
for a,b in zip(linearA,B):
controllability_det.append(det_controllable(a,b))
fig = plt.figure()
plt.grid(True)
plt.scatter(angle_2, controllability_det)
plt.show()
inputB = open('double_pendulum_linearized_B_murray_params_7v8.pkl', 'rb')
inputa2 = open('equils_a2_plot_murray_params_7v8.pkl', 'rb')
A = pickle.load(inputA)
B = pickle.load(inputB)
a2 = pickle.load(inputa2)
inputA.close()
inputB.close()
inputa2.close()
#state A and input B values for linearized function
controllability_det = []
for a, b in zip(A,B):
controllability_det.append(det_controllable(a, b[:,1].reshape(4,1)))
outputC = open('double_pendulum_controllability_plot_murray_params_7v8.pkl', 'wb')
pickle.dump(controllability_det, outputC)
outputC.close()
order = np.argsort(a2)
xs = np.array(a2)[order]
ys = np.array(controllability_det)[order]
fig = plt.figure(figsize = (13,13))
plt.grid(b = True, which = 'both')
plt.plot(xs, ys)
plt.xlabel('angle_2')
plt.ylabel('determinant')
inputB = open("triple_pen_linearized_B_useful.pkl", "rb")
inputB2 = open("triple_pen_linearized_B2.pkl", "rb")
inputB3 = open("triple_pen_linearized_B3.pkl", "rb")
A = pickle.load(inputA)
B = pickle.load(inputB)
B2 = pickle.load(inputB2)
B3 = pickle.load(inputB3)
inputA.close()
inputB.close()
inputB2.close()
inputB3.close()
controllability_det = []
controllability_det_two = []
controllability_det_three = []
# calculate determinant of controllability matrix
for a, b in zip(A, B):
controllability_det_two.append(det_controllable(a, b[:, 1].reshape(6, 1)))
controllability_det_three.append(det_controllable(a, b[:, 2].reshape(6, 1)))
outputdet2 = open("triple_pen_controllability_angle_two_useful.pkl", "wb")
outputdet3 = open("triple_pen_controllability_angle_three_useful.pkl", "wb")
pickle.dump(controllability_det_two, outputdet2)
pickle.dump(controllability_det_three, outputdet3)
outputdet2.close()
outputdet3.close()
