answer_vector.append([x,y,z])
print("answer_vector done")
#Linearization
equilibrium_points = []
for element in answer_vector:
equilibrium_points.append(concatenate((element, zeros(len(speeds))), axis=1))
print("equilibrium_points done")
equilibrium_dict = []
for element in equilibrium_points:
equilibrium_dict.append(dict(zip(coordinates + speeds, element)))
print("equilibrium_dict done")
#Jacobian fo forcing vector w.r.t. states and inputs
tor_dict = dict(zip([l_ankle_torque], [0]))
F_A = forcing_vector.jacobian(coordinates + speeds)
F_B = forcing_vector.subs(tor_dict).jacobian(specified)
print("jacobian done")
#Substitute in values for the variables in the forcing vector
F_A = F_A.subs(parameter_dict)
F_B = F_B.subs(parameter_dict)
print("subs done")
forcing_a = []
forcing_b = []
M = []
#Create the vectors storing jacobians about every equilibrium point
for element in equilibrium_dict:
forcing_a.append(F_A.subs(element))
'specified': array([0.0,0.0,0.0])} # Simulate # ======== frames_per_sec = 60 final_time = 10.0 t = linspace(0.0, final_time, final_time * frames_per_sec) #Create dictionaries for the values for equilibrium point 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 = F_A.subs(equilibrium_dict) F_A = F_A.subs(parameter_dict) F_B = 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 = M.subs(parameter_dict) M = array(M.tolist(), dtype = float)
