answer_vector.append([angle_1, angle_2])
print("Answer Vector Done")
#Linearization
equilibrium_points = []
for element in answer_vector:
equilibrium_points.append(concatenate((zeros(len(speeds)), element), axis=1))
print("Equilibrium Ponts done")
equilibrium_dict = []
for element in equilibrium_points:
equilibrium_dict.append(dict(zip(speeds + coordinates, element)))
print("Equilibrium dict done")
#Jacobian of forcing vector w.r.t. states and inputs
F_A = forcing_vector.jacobian(coordinates + speeds)
F_B = forcing_vector.subs(tor_dict).jacobian(specified)
print("Jacobian done")
#substitute in values for the evariables in the forcing vector
F_A = F_A.subs(parameter_dict)
F_B = F_B.subs(parameter_dict)
mass_matrix = mass_matrix.subs(parameter_dict)
print("Subs done")
forcing_a = []
forcing_b = []
M = []
for element in equilibrium_dict:
forcing_a.append(F_A.subs(element))
args = {'constants': numerical_constants,
'specified': numerical_specified}
frames_per_sec = 60
final_time = 5.0
t = linspace(0.0, final_time, final_time*frames_per_sec)
right_hand_side(x0, 0.0, args)
#Create dictionaries for the values for the equilibrium point of (0,0) i.e. pointing straight up
equilibrium_point = zeros(len(coordinates + speeds))
equilibrium_dict = dict(zip(coordinates + speeds, equilibrium_point))
#Jacobian of the forcing vector w.r.t. states and inputs
F_A = forcing_vector.jacobian(coordinates + speeds)
F_B = forcing_vector.jacobian(specified)
#Substitute in the values for the variables in the forcing vector
F_A = simplify(F_A.subs(equilibrium_dict))
F_A = F_A.subs(parameter_dict)
F_B = simplify(F_B.subs(equilibrium_dict).subs(parameter_dict))
#Convert into a floating point numpy array
F_A = array(F_A.tolist(), dtype=float)
F_B = array(F_B.tolist(), dtype=float)
M = mass_matrix.subs(equilibrium_dict)
M = simplify(M)
M = M.subs(parameter_dict)
M = array(M.tolist(), dtype = float)
