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)