def run_sim_rand_TT(N, **kwargs): """ Runs one simulation for MINIMUM ACTIVATION TRANSITION control. ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ **kwargs ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 1) Bounds - must be a (2,2) list with each row in ascending order. Default is given by Tension_Bounds. 2) InitialAngularAcceleration - must be a float or an int. Default is 0 (starting from rest). 3) thresh - must be an int. Default is 25. """ thresh = kwargs.get("thresh", 25) assert type( thresh ) == int, "thresh should be an int as it is the number of attempts the program should run before stopping." AnotherIteration = True AttemptNumber = 1 while AnotherIteration == True: X = np.zeros((2, N)) X_o, InitialTensions = find_initial_values_TT(**kwargs) X[:, 0] = X_o U = np.zeros((2, N)) U[:, 0] = InitialTensions.T AddNoise = False if AddNoise == True: np.random.seed(seed=None) NoiseArray = np.random.normal(loc=0.0, scale=0.2, size=(2, N)) else: NoiseArray = np.zeros((2, N)) try: cprint("Attempt #" + str(int(AttemptNumber)) + ":\n", 'green') statusbar = dsb(0, N - 1, title=run_sim_rand_TT.__name__) for i in range(N - 1): U[:, i + 1] = return_U_random_tensions(i, Time, X[:, i], U[:, i], Noise=NoiseArray[:, i]) X[:,i+1] = X[:,i] + dt*np.array([ dX1_dt(X[:,i]),\ dX2_dt(X[:,i],U=U[:,i+1])]) statusbar.update(i) AnotherIteration = False return (X, U) except: print('\n') print(" "*(get_terminal_width()\ - len("...Attempt #" + str(int(AttemptNumber)) + " Failed. "))\ + colored("...Attempt #" + str(int(AttemptNumber)) + " Failed. \n",'red')) AttemptNumber += 1 if AttemptNumber > thresh: AnotherIteration = False return (np.zeros((2, N)), np.zeros((2, N)))
def forward_simulation_FL(self, Time, X_o, X1d, Sd): assert len(X_o) == 6, "X_o must have 6 elements, not " + str( len(X_o)) + "." dt = Time[1] - Time[0] U = np.zeros((2, len(Time) - 1), dtype=np.float64) X = np.zeros((6, len(Time)), dtype=np.float64) X_measured = np.zeros((6, len(Time)), dtype=np.float64) Y = np.zeros((2, len(Time)), dtype=np.float64) X[:, 0] = X_o Y[:, 0] = self.h(X[:, 0]) self.update_state_variables(X_o) statusbar = dsb(0, len(Time) - 1, title="Feedback Linearization") self.desiredOutput = np.array([X1d[0, :], Sd[0, :]]) for i in range(len(Time) - 1): if i > 0: X_measured[0, i] = X[0, i] X_measured[1, i] = (X[0, i] - X[0, i - 1]) / self.dt X_measured[2, i] = X[2, i] X_measured[3, i] = (X[2, i] - X[2, i - 1]) / self.dt X_measured[4, i] = X[4, i] X_measured[5, i] = (X[4, i] - X[4, i - 1]) / self.dt else: X_measured[:, i] = X[:, i] U[:, i] = (self.return_input(X_measured[:, i], X1d[:, i], Sd[:, i])).flatten() X[:, i + 1] = ( X[:, i] + self.dt * (self.f(X[:, i]) + self.g(X[:, i]) @ U[:, np.newaxis, i]).T) Y[:, i + 1] = self.h(X[:, i + 1]) self.update_state_variables(X[:, i + 1]) statusbar.update(i) return (X, U, Y, X_measured)
def forward_simulation(self, Time, X_o, U=None): """ Building our own f_array to reduce the number of calls for f_funcs by making it a static call for each iteration in the FBL instance. """ assert len(X_o) == 6, "X_o must have 6 elements, not " + str( len(X_o)) + "." dt = Time[1] - Time[0] if U is None: U = np.zeros((2, len(Time) - 1)) else: assert np.shape(U) == ( 2, len(Time) - 1 ), "U must be either None (default) of have shape (2,len(Time)-1), not " + str( np.shape(U)) + "." X = np.zeros((6, len(Time))) Y = np.zeros((2, len(Time))) X[:, 0] = X_o Y[:, 0] = self.h(X[:, 0]) statusbar = dsb(0, len(Time) - 1, title="Forward Simulation (Custom)") for i in range(len(Time) - 1): f_array = np.zeros((6, 1)) f_array[0, 0] = self.f1_func(X[:, i]) f_array[1, 0] = self.f2_func(X[:, i]) f_array[2, 0] = self.f3_func(X[:, i]) f_array[3, 0] = self.f4_func(X[:, i]) f_array[4, 0] = self.f5_func(X[:, i]) f_array[5, 0] = self.f6_func(X[:, i]) X[:, i + 1] = (X[:, i] + dt * (f_array + self.g(X[:, i]) @ U[:, np.newaxis, i]).T) Y[:, i + 1] = self.h(X[:, i + 1]) # self.update_state_variables(X[:,i+1]) statusbar.update(i) return (X, U, Y)
def plot_N_sim_MAV(t, TotalX, TotalU, **kwargs): Return = kwargs.get("Return", False) assert type(Return) == bool, "Return should either be True or False" fig1 = plt.figure(figsize=(9, 7)) fig1_title = "Underdetermined Forced-Pendulum Example" plt.title(fig1_title, fontsize=16, color='gray') statusbar = dsb(0, np.shape(TotalX)[0], title=(plot_N_sim_MAV.__name__ + " (" + fig1_title + ")")) for j in range(np.shape(TotalX)[0]): plt.plot(t, (TotalX[j, 0, :]) * 180 / np.pi, '0.70', lw=2) statusbar.update(j) plt.plot(np.linspace(0,t[-1],1001),\ (r(np.linspace(0,t[-1],1001)))*180/np.pi,\ 'r') plt.xlabel("Time (s)") plt.ylabel("Desired Measure (Deg)") fig2 = plt.figure(figsize=(9, 7)) fig2_title = "Error vs. Time" plt.title(fig2_title) statusbar.reset(title=(plot_N_sim_MAV.__name__ + " (" + fig2_title + ")")) for j in range(np.shape(TotalX)[0]): plt.plot(t, (r(t) - TotalX[j, 0, :]) * 180 / np.pi, color='0.70') statusbar.update(j) plt.xlabel("Time (s)") plt.ylabel("Error (Deg)") statusbar.reset( title=(plot_N_sim_MAV.__name__ + " (Plotting States, Inputs, and Muscle Length Comparisons)")) for j in range(np.shape(TotalX)[0]): if j == 0: fig3 = plot_states(t, TotalX[j], Return=True, InputString="Muscle Activations") fig4 = plot_inputs(t, TotalU[j], Return=True, InputString="Muscle Activations") fig5 = plot_l_m_comparison(t, TotalX[j], MuscleLengths=TotalX[j, 4:6, :], Return=True, InputString="Muscle Activation") else: fig3 = plot_states(t,TotalX[j],Return=True,InputString = "Muscle Activations",\ Figure=fig3) fig4 = plot_inputs(t,TotalU[j],Return=True,InputString = "Muscle Activations", \ Figure = fig4) fig5 = plot_l_m_comparison(t,TotalX[j],MuscleLengths = TotalX[j,4:6,:],Return=True,\ InputString = "Muscle Activation", Figure = fig5) statusbar.update(j) if Return == True: return ([fig1, fig2, fig3, fig4, fig5]) else: plt.show()
def return_required_tension(Omega, T_i): Theta_i = np.pi / 6 Theta_f = 2 * np.pi / 3 T_end = (Theta_f - Theta_i) / Omega # N = int(T_end*1000+1) N = 1001 Time = np.linspace(0, T_end, N) X = np.zeros((2, len(Time))) X[0, :] = Theta_i + Omega * Time X[1, :] = Omega * np.ones(np.shape(Time)) Tension = np.zeros(np.shape(Time)) statusbar = dsb(0, len(Time), title="Finding Tensions") Tension = (F_MAX1 * cT * kT) * np.log( np.exp((1 / (lTo1 * kT)) * cumtrapz([v_MTU1(X[:, i]) for i in range(len(Time))], Time)) * (np.exp(T_i / (F_MAX1 * cT * kT)) - 1) + 1) return (Tension, Time)
def dx2_dt(x1, x2, u1, u2): return (-K / M * x1 - B / M * x2 - fT1_func(x1, u1) / M + fT2_func(x1, u1) / M) Xd = np.zeros((2, len(t))) Xd[0, :] = -0.1 * sin(2 * np.pi * t) Xd[1, :] = -0.1 * 2 * np.pi * cos(2 * np.pi * t) X = np.zeros((2, len(t))) X[0, :] = 0 X[1, :] = 0 U = np.zeros((2, len(t))) statusbar = dsb(0, len(t) - 1, title="What are you doing?...") P = np.array([[1, 0], [0, 1]]) q = np.array([[0], [0]]) G = -np.eye(2) A = np.array([k1, -k2]) for i in range(len(t) - 1): h = -np.array([us1, us2]) + np.array([1, -1]) * X[0, i] u = cp.Variable(2) b = np.array(-(K + k1 + k2) * X[0, i] - B * X[1, i] + k1 * us1 - k2 * us2 + M * k_0 * X[0, i] + M * k_1 * X[1, i]) prob = cp.Problem(cp.Minimize((1 / 2) * cp.quad_form(u, P) + q.T @ u), [G @ u <= h, A @ u == b]) prob.solve() u1, u2 = u.value # u1p,u2p = (1/np.sqrt(k1**2+k2**2))*np.array([[k1],[-k2]])*( # -(K+k1+k2)*X[0,i]
def animate_feasible_torque_space(Movement): def return_generator_vectors(dim): """ This function takes the place of the Matlab function ncube.m. Takes in a dimension (dim) and gives all of the vertices needed for a Minkowski sum. """ output = np.matrix(list(itertools.product([0, 1], repeat=dim))).T return(output) test_r11 = np.array([r11(a) for a in Movement.A1[0,:]]) test_r21 = np.array([r21(a) for a in Movement.A1[0,:]]) test_r31 = np.array([r31(a) for a in Movement.A1[0,:]]) test_r12 = np.array([r12(a) for a in Movement.A2[0,:]]) test_r22 = np.array([r22(a) for a in Movement.A2[0,:]]) test_r32 = np.array([r32(a) for a in Movement.A2[0,:]]) R1 = np.concatenate([test_r11[:,np.newaxis],test_r12[:,np.newaxis]],axis=1) R2 = np.concatenate([test_r21[:,np.newaxis],test_r22[:,np.newaxis]],axis=1) R3 = np.concatenate([test_r31[:,np.newaxis],test_r32[:,np.newaxis]],axis=1) T1 = R1*F_MAX1 T2 = R2*F_MAX2 T3 = R3*F_MAX3 GeneratorVectors = return_generator_vectors(3) Hulls = [] max_values = [] statusbar = dsb(0,len(Time),title="Feasible Torque Space") for i in range(len(Time)): T_transpose = np.concatenate( [ T1[i,np.newaxis,:].T, T2[i,np.newaxis,:].T, T3[i,np.newaxis,:].T ], axis=1 ) Vertices = T_transpose*GeneratorVectors max_values.append(abs(Vertices).max(axis=1)) Hulls.append(ConvexHull(Vertices.T)) statusbar.update(i) fig = plt.figure(figsize=(8,10)) ax = plt.gca() max_value = np.concatenate(max_values,axis=1).max(axis=1).max() yaxis, = plt.plot([0,0],[-max_value,max_value],'k') yaxisarrow = plt.scatter([0],[max_value],c='k',marker='^') xaxis, = plt.plot([-max_value,max_value],[0,0],'k') xaxisarrow = plt.scatter([max_value],[0],c='k',marker='>') ax.set_xlim([-1.1*max_value,1.1*max_value]) ax.set_ylim([-1.1*max_value,1.1*max_value]) ax.set_aspect('equal') points = plt.scatter(Hulls[0].points.T[0,:],Hulls[0].points.T[1,:],c='r',marker='o') return_hull_vertices = lambda i: np.concatenate([Hulls[i].vertices,np.array([Hulls[i].vertices[0]])]) initial_hull_vertices = return_hull_vertices(0) outline, = plt.plot(Hulls[0].points[initial_hull_vertices,0], Hulls[0].points[initial_hull_vertices,1], 'r', lw=2) def animate(i): points.set_offsets(Hulls[i].points) hull_vertices = return_hull_vertices(i) outline.set_xdata(Hulls[i].points[initial_hull_vertices,0]) outline.set_ydata(Hulls[i].points[initial_hull_vertices,1]) return points, outline, def init(): points = plt.scatter(Hulls[0].points.T[0,:],Hulls[0].points.T[1,:],c='r',marker='o') initial_hull_vertices = return_hull_vertices(0) outline, = plt.plot(Hulls[0].points[initial_hull_vertices,0], Hulls[0].points[initial_hull_vertices,1], 'r', lw=2) points.set_visible(False) outline.set_visible(False) return points,outline, ani = animation.FuncAnimation( fig, animate, frames=10, interval=50, init_func=init, blit=False ) plt.show()
(np.exp(T_i / (F_MAX1 * cT * kT)) - 1) + 1) return (Tension, Time) def return_required_activation(T): u = (T * np.cos(α1) - F_MAX1 * np.cos(α1)**2 * F_PE1_1([0, 0, 0, 0, lo1, 0, 0, 0])) / (F_MAX1 * np.cos(α1)**2) return (u) U1 = [] T1 = [] Time1 = [] Omega1 = np.arange(0.01, 0.51, 0.01) T_i1 = 100 * np.ones(np.shape(Omega1)) statusbar = dsb(0, len(Omega1), title="Fixed T_i, Sweeping Omega") for i in range(len(Omega1)): T_temp, Time_temp = return_required_tension(Omega1[i], T_i1[i]) T1.append(T_temp) Time1.append(Time_temp) U1.append(return_required_activation(T_temp)) statusbar.update(i) plt.figure() ax1 = plt.gca() ax1.set_title(r"Fixed $T_{i}$, Sweeping $\omega$" + "\n Tension vs. Time") ax1.set_xlabel("Time (s)") ax1.set_ylabel("Tension (T)") for i in range(len(Omega1)): ax1.plot(Time1[i][:-1], T1[i])
# for i in range(len(lTo_array)): # params['lTo']=lTo_array[i] # Recovered_Tension = return_tension_from_muscle_length( # MuscleLength, # MusculotendonLength, # Pennation=Pennation, # **params # ) # Error[i] = ((Kurokawa_Tension-Recovered_Tension.T)**2).mean() # statusbar.update(i) # best_lTo = lTo_array[np.where(Error==min(Error))] # params["lTo"]=best_lTo kT_array = np.linspace(0.001, 2, 1000) Error = np.zeros(np.shape(kT_array)) statusbar = dsb(0, len(kT_array), title="Sweeping kT") for i in range(len(kT_array)): params['kT'] = kT_array[i] Recovered_Tension = return_tension_from_muscle_length(MuscleLength, MusculotendonLength, Pennation=Pennation, **params) Error[i] = ((Kurokawa_Tension - Recovered_Tension.T)**2).mean() statusbar.update(i) best_kT = kT_array[np.where(Error == min(Error))] params["kT"] = best_kT # F_MAX_array = np.linspace(100,2500,1000) # Error = np.zeros(np.shape(F_MAX_array)) # statusbar = dsb(0,len(F_MAX_array),title="Sweeping F_MAX") # for i in range(len(F_MAX_array)):
def run_sim_gauss_act(**kwargs): """ Runs one simulation for NEARBY ACTIVATION BY GAUSSIAN DISTRIBUTION control. ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ **kwargs ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 1) Bounds - must be a (2,2) list with each row in ascending order. Default is given by Tension_Bounds. 2) InitialAngularAcceleration - must be a float or an int. Default is 0 (starting from rest). 3) thresh - must be an int. Default is 25. 4) FixedInitialTension - will be passed to find_viable_initial_values and will fix the value of initial tension. Must be a (2,) numpy.ndarray. Run find_initial_tension outside of the loop for a given seed and then feed it through the pipeline. """ thresh = kwargs.get("thresh",25) assert type(thresh)==int, "thresh should be an int as it is the number of attempts the program should run before stopping." AnotherIteration = True AttemptNumber = 1 while AnotherIteration == True: X = np.zeros((8,N)) InitialTension,InitialMuscleLengths,InitialActivations = \ find_viable_initial_values(**kwargs) X[:,0] = [ r(0), dr(0), InitialTension[0][0], InitialTension[1][0], InitialMuscleLengths[0], InitialMuscleLengths[1], 0, 0] U = np.zeros((2,N)) U[:,0] = InitialActivations AddNoise = False if AddNoise == True: np.random.seed(seed=None) NoiseArray = np.random.normal(loc=0.0,scale=0.2,size=(2,N)) else: NoiseArray = np.zeros((2,N)) try: cprint("Attempt #" + str(int(AttemptNumber)) + ":\n", 'green') statusbar = dsb(0,N-1,title=run_sim_gauss_act.__name__) for i in range(N-1): U[:,i+1] = return_U_gaussian_activations_nearby(i,Time,X[:,i],U[:,i],Noise = NoiseArray[:,i]) X[:,i+1] = X[:,i] + dt*np.array([ dX1_dt(X[:,i]),\ dX2_dt(X[:,i]),\ dX3_dt(X[:,i]),\ dX4_dt(X[:,i]),\ dX5_dt(X[:,i]),\ dX6_dt(X[:,i]),\ dX7_dt(X[:,i],U=U[:,i+1]),\ dX8_dt(X[:,i],U=U[:,i+1]) ]) statusbar.update(i) AnotherIteration = False return(X,U) except: print('\n') print(" "*(get_terminal_width()\ - len("...Attempt #" + str(int(AttemptNumber)) + " Failed. "))\ + colored("...Attempt #" + str(int(AttemptNumber)) + " Failed. \n",'red')) AttemptNumber += 1 if AttemptNumber > thresh: AnotherIteration=False return(np.zeros((8,N)),np.zeros((2,N)))
U = np.zeros(int(params["Time Duration"]/params["dt"]),) TotalCost = np.zeros((int(params["Time Duration"]/params["dt"]),)) params["NumberOfIterations"] = NumberOfIterations params["p_target"] = np.matrix([[np.pi/4,0]]).T params["U_o"] = np.zeros((params["Horizon"]-1,)) params["LearningRate"] = 0.2 X_o = np.array([0,0]) X[:,0] = X_o Time = np.arange(0,params["Time Duration"]+params["dt"],params["dt"]) statusbar = dsb(0,int(params["Time Duration"]/params["dt"]), title="1 DOF, 1 DOA Pendulum (MPC)" ) ## --> Initialize DDP from initial state (X_o) with Horizon = Horizon and U_o = np.zeros((Horizon,)) DDP = DDP_1DOF_1DOA_Torq(X_o,**params) TotalTime = 0 test = [] for i in range(int(params["Time Duration"]/params["dt"])): ## --> Run DDP from (current) initial state (X_o) with Horizon = (Horizon-i) StartTime = time.time() DDP.run_ddp() TotalTime += (time.time()-StartTime) ## --> Update initial state (X_o) to second state of previous DDP and append states list. DDP.set_X_o(DDP.X[:,1]) X[:,i+1] = DDP.X[:,1] # if Time[i]>5 and random.random()>0.2*params["dt"]: # DDP.set_X_o(DDP.X[:,1]+[0,0,10*np.pi/180,0])
def plot_l_m_approximation_error_vs_tendon_tension(t, TotalX, Error, **kwargs): Return = kwargs.get("Return", False) assert type(Return) == bool, "Return should either be True or False" InitialTensions = kwargs.get("InitialTensions", [TotalX[0, 2:4, 0]]) assert type( InitialTensions) == list, "InitialTensions must be a list or arrays" assert all( np.array([ str(type(el)) == "<class 'numpy.ndarray'>" for el in InitialTensions ])), "All elements of InitialTensions must be a numpy.ndarray." NumberOfTensionTrials = len(InitialTensions) TendonTension1 = np.linspace(0.01 * F_MAX1, 0.9 * F_MAX1, 1001) TendonTension2 = np.linspace(0.01 * F_MAX2, 0.9 * F_MAX2, 1001) fig1, axes1 = plt.subplots(2, 2, figsize=(10, 8)) plt.suptitle("Error from MTU Approx vs. Tendon Tension\nMuscle 1", fontsize=16) axes1[0][0].set_xlabel("Tendon Tension (N)") axes1[0][0].set_ylabel("Error (m)") axes1[0][0].set_xlim( TotalX[:, 2, :].min() - 0.1 * (TotalX[:, 2, :].max() - TotalX[:, 2, :].min()), TotalX[:, 2, :].max() + 0.1 * (TotalX[:, 2, :].max() - TotalX[:, 2, :].min())) axes1[0][0].set_ylim( Error[0].min() - 0.1 * (Error[0].max() - Error[0].min()), Error[0].max() + 0.1 * (Error[0].max() - Error[0].min())) # axes1[0][0].plot(TendonTension1,Error1,'0.70',lw=2) axes1[0][1].set_xlabel(r"$\longrightarrow$ Time (s) $\longrightarrow$") axes1[0][1].set_ylim(axes1[0][0].get_ylim()) axes1[0][1].set_yticklabels(["" for el in axes1[0][1].get_yticks()]) axes1[1][0].set_ylabel(r"$\longleftarrow$ Time (s) $\longleftarrow$") axes1[1][0].set_xlim(axes1[0][0].get_xlim()) axes1[1][0].set_xticklabels(["" for el in axes1[0][0].get_xticks()]) axes1[1][0].yaxis.tick_right() axes1[1][0].yaxis.set_label_position("right") axes1[1][0].set_yticks(-np.array(list(range(N_seconds + 1)))) axes1[1][0].set_yticklabels([str(-el) for el in axes1[1][0].get_yticks()]) axes1[1][1].text( 0.00, 0.65, (r'error $= \frac{\tau}{k}\cdot\ln\left(\frac{e^{T_{1}(t)/\tau} - 1}{e^{T_{1}(0)/\tau} - 1} \right )$' ), fontsize=20) axes1[1][1].text( 0.075, 0.475, (r' - $(1 - \cos(\alpha_{1}))\left[l_{m,1}(t) - l_{m,1}(0) \right]$' ), fontsize=16) axes1[1][1].text(0.15, 0.325, (r'where, $\tau = F_{MAX,1}\cdot c^T \cdot k^T$'), fontsize=14) axes1[1][1].text(0.15, 0.15, (r'and $k = \frac{F_{MAX,1}\cdot c^T}{l_{T_{o,1}}}$'), fontsize=14) axes1[1][1].axis('off') fig2, axes2 = plt.subplots(2, 2, figsize=(10, 8)) plt.suptitle("Error from MTU Approx vs. Tendon Tension\nMuscle 2", fontsize=16) axes2[0][0].set_ylabel("Error (m)") axes2[0][0].set_xlabel("Tendon Tension (N)") axes2[0][0].set_xlim( TotalX[:, 3, :].min() - 0.1 * (TotalX[:, 3, :].max() - TotalX[:, 3, :].min()), TotalX[:, 3, :].max() + 0.1 * (TotalX[:, 3, :].max() - TotalX[:, 3, :].min())) axes2[0][0].set_ylim( Error[1].min() - 0.1 * (Error[1].max() - Error[1].min()), Error[1].max() + 0.1 * (Error[1].max() - Error[1].min())) # axes2[0][0].plot(TendonTension2,Error2,'0.70',lw=2) axes2[0][1].set_xlabel(r"$\longrightarrow$ Time (s) $\longrightarrow$") axes2[0][1].set_ylim(axes2[0][0].get_ylim()) axes2[0][1].set_yticklabels(["" for el in axes2[0][1].get_yticks()]) axes2[1][0].set_ylabel(r"$\longleftarrow$ Time (s) $\longleftarrow$") axes2[1][0].set_xlim(axes2[0][0].get_xlim()) axes2[1][0].set_xticklabels(["" for el in axes2[0][0].get_xticks()]) axes2[1][0].yaxis.tick_right() axes2[1][0].yaxis.set_label_position("right") axes2[1][0].set_yticks(-np.array(list(range(N_seconds + 1)))) axes2[1][0].set_yticklabels([str(-el) for el in axes1[1][0].get_yticks()]) axes2[1][1].text( 0.00, 0.65, (r'error $= \frac{\tau}{k}\cdot\ln\left(\frac{e^{T_{2}(t)/\tau} - 1}{e^{T_{2}(0)/\tau} - 1} \right )$' ), fontsize=20) axes2[1][1].text( 0.075, 0.475, (r' - $(1 - \cos(\alpha_{2}))\left[l_{m,2}(t) - l_{m,2}(0) \right]$' ), fontsize=16) axes2[1][1].text(0.15, 0.325, (r'where, $\tau = F_{MAX,2}\cdot c^T \cdot k^T$'), fontsize=14) axes2[1][1].text(0.15, 0.15, (r'and $k = \frac{F_{MAX,2}\cdot c^T}{l_{T_{o,2}}}$'), fontsize=14) axes2[1][1].axis('off') for i in range(NumberOfTensionTrials): error_function_1 = return_error_func_no_pennation( InitialTensions[i][0], F_MAX1, lTo1) error_function_2 = return_error_func_no_pennation( InitialTensions[i][1], F_MAX2, lTo2) Error1 = error_function_1(TendonTension1) Error2 = error_function_2(TendonTension2) axes1[0][0].plot(TendonTension1, Error1, str(1 - InitialTensions[i][0] / F_MAX1), lw=2) axes2[0][0].plot(TendonTension2, Error2, str(1 - InitialTensions[i][1] / F_MAX2), lw=2) statusbar = dsb( 0, np.shape(TotalX)[0], title=plot_l_m_approximation_error_vs_tendon_tension.__name__) for i in range(np.shape(TotalX)[0]): axes1[0][0].plot(TotalX[i, 2, :], Error[0][i]) axes1[0][1].plot(Time, Error[0][i]) axes1[1][0].plot(TotalX[i, 2, :], -Time) axes2[0][0].plot(TotalX[i, 3, :], Error[1][i]) axes2[0][1].plot(Time, Error[1][i]) axes2[1][0].plot(TotalX[i, 3, :], -Time) statusbar.update(i) if Return == True: return ([fig1, fig2]) else: plt.show()
def plot_N_sim_IB_sinus_act(t, TotalX, TotalU, **kwargs): Return = kwargs.get("Return", False) assert type(Return) == bool, "Return should either be True or False" ReturnError = kwargs.get("ReturnError", False) assert type( ReturnError) == bool, "ReturnError should be either True or False." IgnorePennation = kwargs.get("IgnorePennation", False) assert type(IgnorePennation ) == bool, "IgnorePennation should be either True or False." fig1 = plt.figure(figsize=(9, 7)) fig1_title = "Underdetermined Forced-Pendulum Example" plt.title(fig1_title, fontsize=16, color='gray') statusbar = dsb(0, np.shape(TotalX)[0], title=(plot_N_sim_IB_sinus_act.__name__ + " (" + fig1_title + ")")) for j in range(np.shape(TotalX)[0]): plt.plot(t, (TotalX[j, 0, :]) * 180 / np.pi, '0.70', lw=2) statusbar.update(j) plt.plot(np.linspace(0,t[-1],1001),\ (r(np.linspace(0,t[-1],1001)))*180/np.pi,\ 'r') plt.xlabel("Time (s)") plt.ylabel("Desired Measure (Deg)") fig2 = plt.figure(figsize=(9, 7)) fig2_title = "Error vs. Time" plt.title(fig2_title) statusbar.reset(title=(plot_N_sim_IB_sinus_act.__name__ + " (" + fig2_title + ")")) for j in range(np.shape(TotalX)[0]): plt.plot(t, (r(t) - TotalX[j, 0, :]) * 180 / np.pi, color='0.70') statusbar.update(j) plt.xlabel("Time (s)") plt.ylabel("Error (Deg)") statusbar.reset( title=(plot_N_sim_IB_sinus_act.__name__ + " (Plotting States, Inputs, and Muscle Length Comparisons)")) for j in range(np.shape(TotalX)[0]): if j == 0: fig3 = plot_states(t, TotalX[j], Return=True, InputString="Muscle Activations") if Time[-1] > 1: fig3b = plot_states(t[int(1 / dt):int((2 + dt) / dt)], TotalX[j][:, int(1 / dt):int((2 + dt) / dt)], Return=True, InputString="Muscle Activations") fig4 = plot_inputs(t, TotalU[j], Return=True, InputString="Muscle Activations") fig5, Error = plot_l_m_comparison(t, TotalX[j], MuscleLengths=TotalX[j, 4:6, :], Return=True, InputString="Muscle Activation", ReturnError=True, IgnorePennation=IgnorePennation) fig6 = plot_norm_l_m_comparison(t, TotalX[j], MuscleLengths=TotalX[j, 4:6, :], Return=True, InputString="Muscle Activation", ReturnError=False, IgnorePennation=IgnorePennation) fig7, vError = plot_norm_v_m_comparison( t, TotalX[j], Return=True, InputString="Muscle Activation", ReturnError=True, IgnorePennation=IgnorePennation) Error1 = Error[0][np.newaxis, :] Error2 = Error[1][np.newaxis, :] else: fig3 = plot_states(t,TotalX[j],Return=True,InputString = "Muscle Activations",\ Figure=fig3) if Time[-1] > 1: fig3b = plot_states(t[int(1 / dt):int((2 + dt) / dt)], TotalX[j][:, int(1 / dt):int((2 + dt) / dt)], Return=True, InputString="Muscle Activations", Figure=fig3b) fig4 = plot_inputs(t,TotalU[j],Return=True,InputString = "Muscle Activations", \ Figure = fig4) fig5, Error = plot_l_m_comparison(t, TotalX[j], MuscleLengths=TotalX[j, 4:6, :], Return=True, InputString="Muscle Activation", ReturnError=True, IgnorePennation=IgnorePennation, Figure=fig5) fig6 = plot_norm_l_m_comparison(t, TotalX[j], MuscleLengths=TotalX[j, 4:6, :], Return=True, InputString="Muscle Activation", ReturnError=False, IgnorePennation=IgnorePennation, Figure=fig6) fig7, vError = plot_norm_v_m_comparison( t, TotalX[j], Return=True, InputString="Muscle Activation", ReturnError=True, IgnorePennation=IgnorePennation, Figure=fig7) Error1 = np.concatenate([Error1, Error[0][np.newaxis, :]], axis=0) Error2 = np.concatenate([Error2, Error[1][np.newaxis, :]], axis=0) statusbar.update(j) if Return == True: if ReturnError == True: if Time[-1] > 1: return ([fig1, fig2, fig3, fig3b, fig4, fig5, fig6, fig7], [-Error1, -Error2]) else: return ([fig1, fig2, fig3, fig4, fig5, fig6, fig7], [-Error1, -Error2]) else: if Time[-1] > 1: return ([fig1, fig2, fig3, fig3b, fig4, fig5, fig6, fig7]) else: return ([fig1, fig2, fig3, fig4, fig5, fig6, fig7]) else: if ReturnError == True: plt.show() return ([-Error1, -Error2]) else: plt.show()
def run_sim_IB_sinus_act(**kwargs): """ Runs one simulation for INTEGRATOR BACKSTEPPING SINUSOIDAL INPUT control. ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ **kwargs ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 1) Bounds - must be a (2,2) list with each row in ascending order. Default is given by Tension_Bounds. 2) InitialAngularAcceleration - must be a float or an int. Default is 0 (starting from rest). 3) thresh - must be an int. Default is 25. 4) FixedInitialTension - will be passed to find_viable_initial_values and will fix the value of initial tension. Must be a (2,) numpy.ndarray. Run find_initial_tension outside of the loop for a given seed and then feed it through the pipeline. 5) Amps - list of length 2 that has the amplitudes of sinusoidal activation trajectories. 6) Freq - scalar value given in Hz. ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ **kwargs (Passed to find_viable_initial_values()) ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 7) InitialAngularAcceleration - must be either a numpy.float64, float, or int. Default is set to 0 to simulate starting from rest. Choice of reference trajectory *should* not matter as it is either 0 or d2r(0) (either by convention or by choice). 8) InitialAngularSnap - must be either a numpy.float64, float, or int. Default is set to 0 to simulate starting from rest. Choice of reference trajectory *should* not matter as it is either 0 or d4r(0) (either by convention or by choice). 9) FixedInitialMuscleLengths - must be a list of length 2 or None (Default). If is None, then program will assign this value randomly. Used for trials where we wish to hold muscle length constant for different tension levels. """ thresh = kwargs.get("thresh", 25) assert type( thresh ) == int, "thresh should be an int as it is the number of attempts the program should run before stopping." Bounds = kwargs.get("Bounds", Activation_Bounds) assert type(Bounds) == list and np.shape(Bounds) == ( 2, 2), "Bounds should be a list of shape (2,2)." Amp = kwargs.get("Amp", 1) PhaseOffset = kwargs.get("PhaseOffset", 0) if Amp != "Scaled": assert type(Amp) in [int, float], "Amp should be an int or a float." Freq = kwargs.get("Freq", 1) assert type(Freq) in [int, float], "Freq should be an int or a float." AnotherIteration = True AttemptNumber = 1 while AnotherIteration == True: X = np.zeros((8, N)) InitialTension,InitialMuscleLengths,InitialActivations = \ find_viable_initial_values(**kwargs) X[:, 0] = [ r(0), dr(0), InitialTension[0][0], InitialTension[1][0], InitialMuscleLengths[0], InitialMuscleLengths[1], 0, 0 ] U = np.zeros((2, N)) if Amp is "Scaled": Amp = 0.25 * InitialActivations[0] assert Amp > 0, "Amp became negative. Run Again." # import ipdb; ipdb.set_trace() newBase = InitialActivations[0] - Amp * (np.cos(PhaseOffset) - 1) U[0, :] = newBase + Amp * ( np.cos(2 * np.pi * Freq * Time + PhaseOffset) - 1) U[1, 0] = InitialActivations[1] # U[0,:] = InitialActivations[0] + Amp*(np.cos(2*np.pi*Freq*Time+PhaseOffset)-1) # U[1,0] = InitialActivations[1] try: cprint("Attempt #" + str(int(AttemptNumber)) + ":\n", 'green') statusbar = dsb(0, N - 1, title=run_sim_IB_sinus_act.__name__) for i in range(N - 1): U[:, i + 1] = return_U_given_sinusoidal_u1( i, Time, X[:, i], U[0, i + 1]) X[:,i+1] = X[:,i] + dt*np.array([ dX1_dt(X[:,i]),\ dX2_dt(X[:,i]),\ dX3_dt(X[:,i]),\ dX4_dt(X[:,i]),\ dX5_dt(X[:,i]),\ dX6_dt(X[:,i]),\ dX7_dt(X[:,i],U=U[:,i+1]),\ dX8_dt(X[:,i],U=U[:,i+1]) ]) statusbar.update(i) AnotherIteration = False return (X, U) except: print('\n') print(" "*(get_terminal_width()\ - len("...Attempt #" + str(int(AttemptNumber)) + " Failed. "))\ + colored("...Attempt #" + str(int(AttemptNumber)) + " Failed. \n",'red')) AttemptNumber += 1 if AttemptNumber > thresh: AnotherIteration = False return (np.zeros((8, N)), np.zeros((2, N)))
kT = np.linspace(0.0001,0.2/cT,1001) lT = np.linspace(1,1.03,1001) l_Ts_over_l_To = 0.95 cmap = matplotlib.cm.get_cmap('viridis') normalize = matplotlib.colors.Normalize(vmin=min(kT), vmax=max(kT)) colors = [cmap(normalize(value)) for value in kT] max_curvature = np.zeros(np.shape(kT)) estimated_curvature = np.zeros(np.shape(kT)) fig1 = plt.figure() ax1 = plt.gca() fig2 = plt.figure() ax2 = plt.gca() fig3 = plt.figure() ax3 = plt.gca() statusbar = dsb(0,len(kT),title="Estimating Curvature") for i in range(len(kT)): y = cT*kT[i]*np.log(np.exp((l_Ts_over_l_To*lT-LrT)/kT[i])+1) y_prime = (l_Ts_over_l_To)*cT/(1 + np.exp((LrT-l_Ts_over_l_To*lT)/kT[i])) y_dbl_prime = (cT/kT[i])*np.exp((LrT-l_Ts_over_l_To*lT)/kT[i])/((1+np.exp((LrT-l_Ts_over_l_To*lT)/kT[i]))**2)*l_Ts_over_l_To**2 curvature = y_dbl_prime/((1+y_prime**2)**(3/2)) max_curvature[i] = curvature[np.where(curvature == max(curvature))[0][0]] estimated_curvature[i] = l_Ts_over_l_To/kT[i]*2*np.sqrt(3)/9 ax1.plot(lT,curvature,c=colors[i]) ax2.plot(lT,y,c=colors[i]) ax3.scatter([estimated_curvature[i]],[max_curvature[i]],c=[colors[i]]) statusbar.update(i) ax1.set_title("Sweeping " + r"$k^T$") ax1.set_xlabel(r"$\hat{l}_T$") ax1.set_ylabel(r"$\kappa$") cax1, _ = matplotlib.colorbar.make_axes(ax1)
#################################################### ############### Sweeping Across cT ############### #################################################### count = 0 InitialTensionsFromSuccessfulTrials = [] dl_MTU1 = np.cumsum([BIC.v_MTU(X[:, j]) for j in range(len(Time))]) * dt dl_MTU2 = np.cumsum([TRI.v_MTU(X[:, j]) for j in range(len(Time))]) * dt cT_array = np.linspace(37.37 - 2 * 14.67, 37.37 + 2 * 14.67, 101) MuscleLength1 = np.zeros((len(cT_array), len(Time))) MuscleLength2 = np.zeros((len(cT_array), len(Time))) statusbar = dsb(0, len(cT_array), title="Sweeping cT") for i in range(len(cT_array)): MuscleLength1[i, :] = ( (1 / np.cos(BIC.pa)) * dl_MTU1 - (BIC.lTo * BIC.kT / np.cos(BIC.pa)) * np.log( (np.exp(X[2, :] / (BIC.F_MAX * cT_array[i] * BIC.kT)) - 1) / (np.exp(X[2, 0] / (BIC.F_MAX * cT_array[i] * BIC.kT)) - 1)) + X[4, 0]) MuscleLength2[i, :] = ( (1 / np.cos(TRI.pa)) * dl_MTU2 - (TRI.lTo * TRI.kT / np.cos(TRI.pa)) * np.log( (np.exp(X[3, :] / (TRI.F_MAX * cT_array[i] * TRI.kT)) - 1) / (np.exp(X[3, 0] / (TRI.F_MAX * cT_array[i] * TRI.kT)) - 1)) + X[5, 0]) statusbar.update(i)
dt = 0.001 Time = np.arange(0,20+dt,dt) X = np.zeros((12,len(Time))) X[:,0] = [ 0,0, 0,0, 0,0, 0,0, 0,0, 0,0 ] F_levels = np.linspace(0,100,21) Averages = np.zeros((1,len(F_levels))) statusbar = dsb(0,len(F_levels),title="Testing Crimped Tendon") for j in range(len(F_levels)): F_level = F_levels[j] tau = 2 F = np.zeros((2,len(Time))) # F[0,:] = F_level*np.ones((1,len(Time))) F[0,:] = F_level*(1-np.exp(-Time/tau))**2 for i in range(len(Time)-1): X[0,i+1] = X[0,i] + dX1(X[:,i],F[:,i])*dt X[1,i+1] = X[1,i] + dX2(X[:,i],F[:,i])*dt X[2,i+1] = X[2,i] + dX3(X[:,i],F[:,i])*dt X[3,i+1] = X[3,i] + dX4(X[:,i],F[:,i])*dt X[4,i+1] = X[4,i] + dX5(X[:,i],F[:,i])*dt X[5,i+1] = X[5,i] + dX6(X[:,i],F[:,i])*dt X[6,i+1] = X[6,i] + dX7(X[:,i],F[:,i])*dt X[7,i+1] = X[7,i] + dX8(X[:,i],F[:,i])*dt
params["NumberOfIterations"] = NumberOfIterations # params["p_target"] = np.matrix([[5,0,0,0]]).T AbsoluteHorizon = params["Horizon"] params["LearningRate"] = 0.2 X_o = np.array([0,np.pi,0,0]) X[:,0] = X_o Time = np.arange(0,params["Time Duration"]+params["dt"],params["dt"]) thresh1 = 1 statusbar = dsb(0,int(params["Time Duration"]/params["dt"]), title="Cart-Pole Inversion (MPC)" ) ## --> Initialize DDP from initial state (X_o) with Horizon = Horizon and U_o = np.zeros((Horizon,)) DDP = Cart_Pole_DDP(X_o,**params) for i in range(int(params["Time Duration"]/params["dt"])): ## --> Run DDP from (current) initial state (X_o) with Horizon = (Horizon-i) DDP.run_ddp() # ## --> Update Horizon to be 1 step closer than previous # DDP.set_Horizon((AbsoluteHorizon-i)-1) ## --> Update initial state (X_o) to second state of previous DDP and append states list. DDP.set_X_o(DDP.X[:,1]) X[:,i+1] = DDP.X[:,1] # if Time[i]>5 and random.random()>0.2*params["dt"]: # DDP.set_X_o(DDP.X[:,1]+[0,0,10*np.pi/180,0])
cbar1 = matplotlib.colorbar.ColorbarBase(cax1, cmap=cmap) cax1.set_ylabel(r"$\Delta\hat{l}_T$", fontsize=14) cbar1.set_ticks([ el / delta_lT_max for el in np.linspace(0, np.round(delta_lT_max / 0.01) * 0.01, int(np.round(delta_lT_max / 0.01)) + 1) ]) cax1.set_yticklabels([ '{:0.2f}'.format(el) for el in np.linspace(0, np.round(delta_lT_max / 0.01) * 0.01, int(np.round(delta_lT_max / 0.01)) + 1) ]) statusbar = dsb(0, len(fT_array), title="Sweeping % MVC") for i in range(len(fT_array)): fig2 = plt.figure(figsize=(15, 6)) ax2 = Axes3D(fig2) FT = fT_array[i] * np.ones(np.shape(CT)) for j in range(len(cT_array) - 1): for k in range(len(kT_array) - 1): kT = np.average([KT[k, j], KT[k + 1, j]]) cT = np.average([CT[k, j], CT[k, j + 1]]) delta_lT = kT * np.log((np.exp(fT_array[i] / (cT * kT)) - 1) / ((np.exp(initial_force / (cT * kT)) - 1))) verts = [ list( zip([ CT[k, j], CT[k, j + 1], CT[k + 1, j + 1], CT[k + 1, j]
def plot_l_m_error_manifold(t, TotalX, Error, **kwargs): Return = kwargs.get("Return", False) assert type(Return) == bool, "Return should either be True or False" InitialTensions = kwargs.get("InitialTensions", [TotalX[0, 2:4, 0]]) assert type( InitialTensions) == list, "InitialTensions must be a list or arrays" assert all( np.array([ str(type(el)) == "<class 'numpy.ndarray'>" for el in InitialTensions ])), "All elements of InitialTensions must be a numpy.ndarray." Assumptions = kwargs.get("Assumptions", ["penn=0", "vT=0"]) assert (type(Assumptions)==list) \ and (len(Assumptions)!=0) \ and ("vT=0" in Assumptions), \ "Assumptions must be either ['penn=0','vT=0'] (default) or ['vT=0']." NumberOfTensionTrials = len(InitialTensions) fig1 = plt.figure(figsize=(12, 9)) axes1_1 = fig1.add_subplot(221, projection='3d') axes1_2 = fig1.add_subplot(222) axes1_3 = fig1.add_subplot(223) axes1_4 = fig1.add_subplot(224) if ("vT=0" in Assumptions) and ("penn=0" in Assumptions): plt.suptitle( "Error from MTU Approx vs. Tendon Tension\n" + r"(Assuming $\Delta l_T \approx 0$ and $\rho \approx 0$)" + "\n Muscle 1", fontsize=16) elif ("vT=0" in Assumptions): plt.suptitle("Error from MTU Approx vs. Tendon Tension\n" + r"(Assuming $\Delta l_T \approx 0$)" + "\n Muscle 1", fontsize=16) fig2 = plt.figure(figsize=(12, 9)) axes2_1 = fig2.add_subplot(221, projection='3d') axes2_2 = fig2.add_subplot(222) axes2_3 = fig2.add_subplot(223) axes2_4 = fig2.add_subplot(224) if ("vT=0" in Assumptions) and ("penn=0" in Assumptions): plt.suptitle( "Error from MTU Approx vs. Tendon Tension\n" + r"(Assuming $\Delta l_T \approx 0$ and $\rho \approx 0$)" + "\n Muscle 2", fontsize=16) elif ("vT=0" in Assumptions): plt.suptitle("Error from MTU Approx vs. Tendon Tension\n" + r"(Assuming $\Delta l_T \approx 0$)" + "\n Muscle 2", fontsize=16) MTLengthChange1 = np.concatenate([(Time[1] - Time[0]) * np.cumsum( np.array([v_MTU1(TotalX[i, :, j]) for j in range(np.shape(TotalX)[2])]))[np.newaxis, :] for i in range(np.shape(TotalX)[0])], axis=1) MTLengthChange2 = np.concatenate([(Time[1] - Time[0]) * np.cumsum( np.array([v_MTU2(TotalX[i, :, j]) for j in range(np.shape(TotalX)[2])]))[np.newaxis, :] for i in range(np.shape(TotalX)[0])], axis=1) statusbar = dsb( 0, np.shape(TotalX)[0], title=plot_l_m_approximation_error_vs_tendon_tension.__name__) for i in range(np.shape(TotalX)[0]): axes1_1.plot(MTLengthChange1[i, :], TotalX[i, 2, :], Error[0][i]) axes1_2.plot(Time, Error[0][i]) axes1_3.plot(TotalX[i, 2, :], -Time) axes2_1.plot(MTLengthChange2[i, :], TotalX[i, 3, :], Error[1][i]) axes2_2.plot(Time, Error[1][i]) axes2_3.plot(TotalX[i, 3, :], -Time) statusbar.update(i) for i in range(TotalX.shape[0]): error_function_1 = return_error_func(TotalX[i, 2, 0], F_MAX1, lTo1, α1, Assumptions=Assumptions) error_function_2 = return_error_func(TotalX[i, 3, 0], F_MAX2, lTo2, α2, Assumptions=Assumptions) error_function_pennation_included_1 = return_error_func( TotalX[i, 2, 0], F_MAX1, lTo1, α1, Assumptions=["vT=0"]) error_function_pennation_included_2 = return_error_func( TotalX[i, 3, 0], F_MAX2, lTo2, α2, Assumptions=["vT=0"]) MinimumTension1 = TotalX[:, 2, :].min() MaximumTension1 = TotalX[:, 2, :].max() Tension1Range = TotalX[:, 2, :].max() - TotalX[:, 2, :].min() TendonTension1 = np.linspace(MinimumTension1 - 0.05 * Tension1Range, MaximumTension1 + 0.05 * Tension1Range, 1001) MinimumMTLength1 = MTLengthChange1.min() MaximumMTLength1 = MTLengthChange1.max() MTLength1Range = (MTLengthChange1.max() - MTLengthChange1.min()) MTLength1 = np.linspace(MinimumMTLength1 - 0.05 * MTLength1Range, MaximumMTLength1 + 0.05 * MTLength1Range, 1001) MTLength1Mesh, TendonTension1Mesh = \ np.meshgrid(MTLength1,TendonTension1) MinimumTension2 = TotalX[:, 3, :].min() MaximumTension2 = TotalX[:, 3, :].max() Tension2Range = TotalX[:, 3, :].max() - TotalX[:, 3, :].min() TendonTension2 = np.linspace(MinimumTension2 - 0.05 * Tension2Range, MaximumTension2 + 0.05 * Tension2Range, 1001) MinimumMTLength2 = MTLengthChange2.min() MaximumMTLength2 = MTLengthChange2.max() MTLength2Range = (MTLengthChange2.max() - MTLengthChange2.min()) MTLength2 = np.linspace(MinimumMTLength2 - 0.05 * MTLength2Range, MaximumMTLength2 + 0.05 * MTLength2Range, 1001) MTLength2Mesh, TendonTension2Mesh = \ np.meshgrid(MTLength2,TendonTension2) if ("vT=0" in Assumptions) and ("penn=0" in Assumptions): Error1 = error_function_1(TendonTension1Mesh, MTLength1Mesh) Error_Pennation_Included_1 = \ error_function_pennation_included_1(TendonTension1) Error2 = error_function_2(TendonTension2Mesh, MTLength2Mesh) Error_Pennation_Included_2 = \ error_function_pennation_included_2(TendonTension2) elif ("vT=0" in Assumptions): Error1 = error_function_1(TendonTension1Mesh) Error_Pennation_Included_1 = \ error_function_pennation_included_1(TendonTension1) Error2 = error_function_2(TendonTension2Mesh) Error_Pennation_Included_2 = \ error_function_pennation_included_2(TendonTension2) axes1_1.plot_surface(MTLength1Mesh, TendonTension1Mesh, Error1, color=str( np.linspace(0.25, 0.75, TotalX.shape[0])[i])) axes1_4.plot(TendonTension1, Error_Pennation_Included_1, '0.70', lw=3) axes1_4.plot(TotalX[i, 2, :], Error[0][i], lw=3) axes2_1.plot_surface(MTLength2Mesh, TendonTension2Mesh, Error2, color=str( np.linspace(0.25, 0.75, TotalX.shape[0])[i])) axes2_4.plot(TendonTension2, Error_Pennation_Included_2, '0.70', lw=3) axes2_4.plot(TotalX[i, 3, :], Error[1][i], lw=3) axes1_1.set_xlabel("MT Length Change (m)") axes1_1.set_ylabel("Tendon Tension (N)") axes1_1.set_zlabel("Error (m)") axes1_1.view_init(elev=20., azim=-30.) axes1_2.set_xlabel(r"$\longrightarrow$ Time (s) $\longrightarrow$") axes1_2.set_ylim(axes1_1.get_zlim()) axes1_2.spines['right'].set_visible(False) axes1_2.spines['top'].set_visible(False) # axes1_2.set_yticklabels(["" for el in axes1_2.get_yticks()]) axes1_3.set_ylabel(r"$\longleftarrow$ Time (s) $\longleftarrow$") axes1_3.set_xlim(axes1_1.get_ylim()) axes1_3.spines['left'].set_visible(False) axes1_3.spines['bottom'].set_visible(False) # axes1_3.set_xticklabels(["" for el in axes1_1.get_xticks()]) axes1_3.yaxis.tick_right() axes1_3.yaxis.set_label_position("right") axes1_3.set_yticks(-np.array(list(range(N_seconds + 1)))) axes1_3.set_yticklabels([str(-el) for el in axes1_3.get_yticks()]) axes1_4.set_frame_on(True) axes1_4.set_xlim([TendonTension1.min(), TendonTension1.max()]) axes1_4.set_aspect( (np.diff(axes1_4.get_xlim()) / np.diff(axes1_4.get_ylim())), adjustable='box') # axes1_4.set_xticks([]) # axes1_4.set_yticks([]) # axes1_4.text(0.00,0.65, # (r'error $= \frac{\tau}{k\cos(\alpha_{1})}\cdot\ln\left(\frac{e^{T_{1}(t)/\tau} - 1}{e^{T_{1}(0)/\tau} - 1} \right )$'),fontsize=20) # axes1_4.text(0.075,0.475, # (r' - $\frac{1 - \cos(\alpha_{1})}{\cos(\alpha_{1})}\Delta l_{MT,1}$'), fontsize=16) # axes1_4.text(0.15,0.325, # (r'where, $\tau = F_{MAX,1}\cdot c^T \cdot k^T$'),fontsize=14) # axes1_4.text(0.15,0.15, # (r'and $k = \frac{F_{MAX,1}\cdot c^T}{l_{T_{o,1}}}$'),fontsize=14) # axes1_4.axis('off') axes2_1.set_xlabel("MT Length Change (m)") axes2_1.set_ylabel("Tendon Tension (N)") axes2_1.set_zlabel("Error (m)") axes2_1.view_init(elev=20., azim=-30.) axes2_2.set_xlabel(r"$\longrightarrow$ Time (s) $\longrightarrow$") axes2_2.set_ylim(axes2_1.get_zlim()) axes2_2.spines['right'].set_visible(False) axes2_2.spines['top'].set_visible(False) # axes2_2.set_yticklabels(["" for el in axes2_2.get_yticks()]) axes2_3.set_ylabel(r"$\longleftarrow$ Time (s) $\longleftarrow$") axes2_3.set_xlim(axes2_1.get_ylim()) axes2_3.spines['left'].set_visible(False) axes2_3.spines['bottom'].set_visible(False) # axes2_3.set_xticklabels(["" for el in axes2_1.get_xticks()]) axes2_3.yaxis.tick_right() axes2_3.yaxis.set_label_position("right") axes2_3.set_yticks(-np.array(list(range(N_seconds + 1)))) axes2_3.set_yticklabels([str(-el) for el in axes1_3.get_yticks()]) axes2_4.set_frame_on(True) axes2_4.set_xlim([TendonTension2.min(), TendonTension2.max()]) axes2_4.set_aspect( (np.diff(axes2_4.get_xlim()) / np.diff(axes2_4.get_ylim())), adjustable='box') # axes2_4.set_xticks([]) # axes2_4.set_yticks([]) # axes2_4.text(0.00,0.65, # (r'error $= \frac{\tau}{k}\cdot\ln\left(\frac{e^{T_{2}(t)/\tau} - 1}{e^{T_{2}(0)/\tau} - 1} \right )$'),fontsize=20) # axes2_4.text(0.075,0.475, # (r' - $\frac{1 - \cos(\alpha_{2})}{\cos(\alpha_{2})}\Delta l_{MT,2}$'), fontsize=16) # axes2_4.text(0.15,0.325, # (r'where, $\tau = F_{MAX,2}\cdot c^T \cdot k^T$'),fontsize=14) # axes2_4.text(0.15,0.15, # (r'and $k = \frac{F_{MAX,2}\cdot c^T}{l_{T_{o,2}}}$'),fontsize=14) # axes2_4.axis('off') if Return == True: return ([fig1, fig2]) else: plt.show()
N_param = 100 # F_MAX_array = np.linspace(1500,4000,N_param) F_MAX_array = np.array([540]) # lTo_array = np.linspace(0.2,0.7,N_param) lTo_array = np.array([0.367]) # cT_array = np.linspace(14,30,N_param) cT_array = np.array([12.515]) kT_array = np.linspace(0.0001, 0.0159, N_param) # kT_array = np.array([0.0047]) Error = np.zeros( (len(F_MAX_array), len(lTo_array), len(cT_array), len(kT_array))) statusbar = dsb(0, len(F_MAX_array) * len(lTo_array) * len(cT_array) * len(kT_array), title="Sweeping Tendon Parameters") cmap = matplotlib.cm.get_cmap('viridis') normalize = matplotlib.colors.Normalize(vmin=min(kT_array), vmax=max(kT_array)) colors = [cmap(normalize(value)) for value in kT_array] for h in range(len(F_MAX_array)): for i in range(len(lTo_array)): for j in range(len(cT_array)): for k in range(len(kT_array)): params['F_MAX'] = F_MAX_array[h] params['lTo'] = lTo_array[i] params['cT'] = cT_array[j] params['kT'] = kT_array[k] Recovered_Tension = return_tension_from_muscle_length( MuscleLength, MusculotendonLength,
MVC = [] Error = [] ################################ ###### FIND RANDOM POINTS ###### ################################ Seed = None np.random.seed(Seed) random.seed(Seed) numTrials = 10000 count = 0 breakcount = 0 statusbar = dsb(0, numTrials, title="Parallel Coordinates") while count < numTrials: kT_rand = random.uniform(0, kT_intersection) if kT_rand <= 0.20 / max_cT: ub_cT = max_cT else: ub_cT = 0.20 / kT_rand if kT_rand == 0: lb_cT = (1 - initial_force) / (1 - l_Ts_over_l_To) else: try: lb_cT = fsolve( lambda cT: ((1 / l_Ts_over_l_To) * (kT_rand * np.log(np.exp(initial_force / (cT * kT_rand)) - 1) +
def run_sim_IB_sinus_act(**kwargs): """ Runs one simulation for INTEGRATOR BACKSTEPPING SINUSOIDAL INPUT control. ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ **kwargs ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 1) Bounds - must be a (2,2) list with each row in ascending order. Default is given by Tension_Bounds. 2) InitialAngularAcceleration - must be a float or an int. Default is 0 (starting from rest). 3) thresh - must be an int. Default is 25. 4) FixedInitialTension - will be passed to find_viable_initial_values and will fix the value of initial tension. Must be a (2,) numpy.ndarray. Run find_initial_tension outside of the loop for a given seed and then feed it through the pipeline. 5) Amps - list of length 2 that has the amplitudes of sinusoidal activation trajectories. 6) Freq - scalar value given in Hz. ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ **kwargs (Passed to initialize_tendon_tension()) ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 7) ReturnMultipleInitialTensions - must be either True or False. Default is False. 8) Seed - must be a float or an int. Default is None (seeded by current time). 9) Return - must be either true or false (default). When false, function will assign the initial tension value to self. 10) TensionBounds - must be a list of size (2,2) or None (in which case the self.TensionBounds will be used) ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ **kwargs (Passed to find_viable_initial_values()) ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 11) FixedInitialMuscleLengths - must be a list of length 2 or None (Default). If is None, then find_viable_initial_values() will assign this value randomly. Used for trials where we wish to hold muscle length constant for different tension levels. 12) Seed - must be a float or an int. Default is None (seeded by current time). """ thresh = kwargs.get("thresh",25) assert type(thresh)==int, "thresh should be an int as it is the number of attempts the program should run before stopping." Bounds = kwargs.get("Bounds",Activation_Bounds) assert type(Bounds)==list and np.shape(Bounds)==(2,2), "Bounds should be a list of shape (2,2)." Amp = kwargs.get("Amp",1) if Amp is not "Scaled": assert type(Amp) in [int,float], "Amp should be an int or a float." Freq = kwargs.get("Freq",1) assert type(Freq) in [int,float], "Freq should be an int or a float." ICs = kwargs.get("ICs",None) TensionBounds = kwargs.get("TensionBounds",None) Seed = kwargs.get("Seed",None) AnotherIteration = True AttemptNumber = 1 while AnotherIteration == True: Plant = Pendulum_1DOF_2DOA(BIC,TRI,Time) # should combine to one function now. Plant.initialize_tendon_tension( Seed=Seed, Bounds=TensionBounds ) if ICs is None: Plant.find_viable_initial_values(**kwargs) Plant.set_X_o() Plant.set_U_o() else: assert ((len(ICs)==2) and (np.shape(ICs[0])==(8,)) and (np.shape(ICs[1])==(2,))), \ "ICs must be a list that contains X_o (of shape (8,)) and U_o (of shape (2,))." Plant.set_X_o(ICs[0]) Plant.set_U_o(ICs[1]) U = np.zeros((2,N)) if Amp is "Scaled": Amp = 0.75*Plant.U_o[0] # Amp1 = 0.1 # Amp2 = 0.50*InitialActivations[0] # assert InitialActivations[0]<0.15, "InitialActivations too high." # Amp2 = 0.6 # assert (Amp1>=0) and (Amp2>=0), "Amp became negative. Run Again." assert Amp>0, "Amp became negative. Run Again." # phase1 = np.ones(np.shape(Time))*(np.array([np.sin(2*np.pi*Time)])>0) # phase2 = np.ones(np.shape(Time))*(np.array([np.sin(2*np.pi*Time)])<=0) # U[0,:] = ( # InitialActivations[0] # + (Amp1*(1-np.cos(4*np.pi*Time))/2)*phase1 # + (Amp2*(1-np.cos(4*np.pi*Time))/2)*phase2 # ) # U[0,:] = U[0,:]*(U[0,:]>=0) # import ipdb; ipdb.set_trace() shift = 3*np.pi/16 U[0,:] = ( Plant.U_o[0] - Amp*np.sin(2*np.pi*Freq*Time+shift) ) # U[0,:] = InitialActivations[0] + Amp*(1-np.cos(4*np.pi*Freq*Time))/2 U[1,0] = Plant.U_o[1] U_o = U[:,0] ######### # Plant = Pendulum_1DOF_2DOA(BIC,TRI,X_o,U_o,Time) Plant.U[0,:] = U[0,:] try: cprint("Attempt #" + str(int(AttemptNumber)) + ":\n", 'green') statusbar = dsb(0,N-1,title=run_sim_IB_sinus_act.__name__) for i in range(N-1): Plant.update_pendulum_variables(i) Plant.forward_integrate(i) statusbar.update(i) AnotherIteration = False return(Plant.X,Plant.U) except: print('\n') print(" "*(get_terminal_width()\ - len("...Attempt #" + str(int(AttemptNumber)) + " Failed. "))\ + colored("...Attempt #" + str(int(AttemptNumber)) + " Failed. \n",'red')) AttemptNumber += 1 if AttemptNumber > thresh: AnotherIteration=False return(np.zeros((8,N)),np.zeros((2,N)))
def run_sim_FF_sinus_act(**kwargs): """ Runs one simulation for FEEDFORWARD SINUSOIDAL INPUT control. ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ **kwargs ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 1) Bounds - must be a (2,2) list with each row in ascending order. Default is given by Tension_Bounds. 2) InitialAngularAcceleration - must be a float or an int. Default is 0 (starting from rest). 3) thresh - must be an int. Default is 25. 4) FixedInitialTension - will be passed to find_viable_initial_values and will fix the value of initial tension. Must be a (2,) numpy.ndarray. Run find_initial_tension outside of the loop for a given seed and then feed it through the pipeline. 5) Amps - list of length 2 that has the amplitudes of sinusoidal activation trajectories. 6) Freq - scalar value given in Hz. 7) PhaseOffset - scalar value in [0,360). ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ **kwargs (Passed to find_viable_initial_values()) ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 8) InitialAngularAcceleration - must be either a numpy.float64, float, or int. Default is set to 0 to simulate starting from rest. Choice of reference trajectory *should* not matter as it is either 0 or d2r(0) (either by convention or by choice). 9) InitialAngularSnap - must be either a numpy.float64, float, or int. Default is set to 0 to simulate starting from rest. Choice of reference trajectory *should* not matter as it is either 0 or d4r(0) (either by convention or by choice). """ thresh = kwargs.get("thresh", 25) assert type( thresh ) == int, "thresh should be an int as it is the number of attempts the program should run before stopping." Bounds = kwargs.get("Bounds", ActivationBounds) assert type(Bounds) == list and np.shape(Bounds) == ( 2, 2), "Bounds should be a list of shape (2,2)." Amps = kwargs.get("Amps", [0.1, 0.1]) if Amps != "Scaled": assert type(Amps) == list and len( Amps ) == 2, "Amps should be a list of length 2 with values that are within activation bounds." Freq = kwargs.get("Freq", 1) assert type(Freq) in [int, float], "Freq should be an int or a float." PhaseOffset = kwargs.get("PhaseOffset", 90) assert (type(PhaseOffset) in [ int, float ]) and (0 <= PhaseOffset < 360), "PhaseOffset should be an int or a float in [0,360)." AnotherIteration = True AttemptNumber = 1 while AnotherIteration == True: X = np.zeros((8, N)) InitialTension,InitialMuscleLengths,InitialActivations = \ find_viable_initial_values(**kwargs) X[:, 0] = [ r(0), dr(0), InitialTension[0][0], InitialTension[1][0], InitialMuscleLengths[0], InitialMuscleLengths[1], 0, 0 ] U = np.zeros((2, N)) if Amps == "Scaled": Amps = 0.25 * InitialActivations U[0, :] = InitialActivations[0] + Amps[0] * ( np.cos(2 * np.pi * Freq * Time) - 1) U[1, :] = InitialActivations[1] + Amps[1] * ( np.cos(2 * np.pi * Freq * Time - PhaseOffset * (np.pi / 180)) - np.cos(-PhaseOffset * (np.pi / 180))) try: cprint("Attempt #" + str(int(AttemptNumber)) + ":\n", 'green') statusbar = dsb(0, N - 1, title=run_sim_FF_sinus_act.__name__) for i in range(N - 1): X[:,i+1] = X[:,i] + dt*np.array([ dX1_dt(X[:,i]),\ dX2_dt(X[:,i]),\ dX3_dt(X[:,i]),\ dX4_dt(X[:,i]),\ dX5_dt(X[:,i]),\ dX6_dt(X[:,i]),\ dX7_dt(X[:,i],U=U[:,i+1]),\ dX8_dt(X[:,i],U=U[:,i+1]) ]) statusbar.update(i) AnotherIteration = False return (X, U) except: print('\n') print(" "*(get_terminal_width()\ - len("...Attempt #" + str(int(AttemptNumber)) + " Failed. "))\ + colored("...Attempt #" + str(int(AttemptNumber)) + " Failed. \n",'red')) AttemptNumber += 1 if AttemptNumber > thresh: AnotherIteration = False return (np.zeros((8, N)), np.zeros((2, N)))
def plot_N_sim_IB_sinus_act(t,TotalX,TotalU,**kwargs): Return = kwargs.get("Return",False) assert type(Return) == bool, "Return should either be True or False" ReturnError = kwargs.get("ReturnError",False) assert type(ReturnError)==bool, "ReturnError should be either True or False." Normalized = kwargs.get("Normalized",False) assert type(Normalized)==bool, "Normalized should be either True or False." ###################################### ###### Actual/Expected Endpoint ###### ###################################### fig1 = plt.figure(figsize = (9,7)) fig1_title = "Underdetermined Forced-Pendulum Example" plt.title(fig1_title,fontsize=16,color='gray') statusbar = dsb(0,np.shape(TotalX)[0],title=(plot_N_sim_IB_sinus_act.__name__ + " (" + fig1_title +")")) for j in range(np.shape(TotalX)[0]): plt.plot(t,(TotalX[j,0,:])*180/np.pi,'0.70',lw=2) statusbar.update(j) plt.plot(np.linspace(0,t[-1],1001),\ (r(np.linspace(0,t[-1],1001)))*180/np.pi,\ 'r') plt.xlabel("Time (s)") plt.ylabel("Desired Measure (Deg)") ###################################### ########### Endpoint Error ########### ###################################### fig2 = plt.figure(figsize = (9,7)) fig2_title = "Error vs. Time" plt.title(fig2_title) statusbar.reset(title=(plot_N_sim_IB_sinus_act.__name__ + " (" + fig2_title +")")) for j in range(np.shape(TotalX)[0]): plt.plot(t, (r(t)-TotalX[j,0,:])*180/np.pi,color='0.70') statusbar.update(j) plt.xlabel("Time (s)") plt.ylabel("Error (Deg)") ###################################### ############# plot_states ############ ############# plot_inputs ############ ######### plot_l_m_comparison ######## ###################################### statusbar.reset( title=( plot_N_sim_IB_sinus_act.__name__ + " (Plotting States, Inputs, and Muscle Length Comparisons)" ) ) for j in range(np.shape(TotalX)[0]): if j == 0: fig3 = plot_states( t,TotalX[j], Return=True, InputString="Muscle Activations", Normalized=Normalized ) fig4 = plot_inputs( t,TotalU[j], Return=True, InputString="Muscle Activations" ) fig5,Error = plot_l_m_comparison( t,TotalX[j], MuscleLengths=TotalX[j,4:6,:], Return=True, InputString="Muscle Activation", Normalized=Normalized, ReturnError=True ) Error1 = Error[0][np.newaxis,:] Error2 = Error[1][np.newaxis,:] else: fig3 = plot_states( t,TotalX[j], Return=True, InputString="Muscle Activations", Normalized=Normalized, Figure=fig3 ) fig4 = plot_inputs( t,TotalU[j], Return=True, InputString="Muscle Activations", Figure=fig4 ) fig5,Error = plot_l_m_comparison( t,TotalX[j], MuscleLengths=TotalX[j,4:6,:], Return=True, InputString="Muscle Activation", Normalized=Normalized, ReturnError=True, Figure=fig5 ) Error1 = np.concatenate([Error1,Error[0][np.newaxis,:]],axis=0) Error2 = np.concatenate([Error2,Error[1][np.newaxis,:]],axis=0) statusbar.update(j) if Return == True: if ReturnError == True: return([fig1,fig2,fig3,fig4,fig5],[-Error1,-Error2]) else: return([fig1,fig2,fig3,fig4,fig5]) else: if ReturnError == True: plt.show() return([-Error1,-Error2]) else: plt.show()
def plot_l_m_error_manifold(t, TotalX, Error, **kwargs): Return = kwargs.get("Return", False) assert type(Return) == bool, "Return should either be True or False" InitialTensions = kwargs.get("InitialTensions", [TotalX[0, 2:4, 0]]) assert type( InitialTensions) == list, "InitialTensions must be a list or arrays" assert all( np.array([ str(type(el)) == "<class 'numpy.ndarray'>" for el in InitialTensions ])), "All elements of InitialTensions must be a numpy.ndarray." NumberOfTensionTrials = len(InitialTensions) fig1 = plt.figure(figsize=(10, 8)) axes1_1 = fig1.add_subplot(221, projection='3d') axes1_2 = fig1.add_subplot(222) axes1_3 = fig1.add_subplot(223) axes1_4 = fig1.add_subplot(224) plt.suptitle("Error from MTU Approx vs. Tendon Tension\nMuscle 1", fontsize=16) fig2 = plt.figure(figsize=(10, 8)) axes2_1 = fig2.add_subplot(221, projection='3d') axes2_2 = fig2.add_subplot(222) axes2_3 = fig2.add_subplot(223) axes2_4 = fig2.add_subplot(224) plt.suptitle("Error from MTU Approx vs. Tendon Tension\nMuscle 2", fontsize=16) statusbar = dsb( 0, np.shape(TotalX)[0], title=plot_l_m_approximation_error_vs_tendon_tension.__name__) for i in range(np.shape(TotalX)[0]): axes1_1.plot(TotalX[i, 4, :], TotalX[i, 2, :], Error[0][i]) axes1_2.plot(Time, Error[0][i]) axes1_3.plot(TotalX[i, 2, :], -Time) axes2_1.plot(TotalX[i, 5, :], TotalX[i, 3, :], Error[1][i]) axes2_2.plot(Time, Error[1][i]) axes2_3.plot(TotalX[i, 3, :], -Time) statusbar.update(i) for i in range(TotalX.shape[0]): error_function_1 = \ return_error_func(TotalX[i,2,0],TotalX[i,4,0],F_MAX1,lTo1,α1) error_function_2 = \ return_error_func(TotalX[i,3,0],TotalX[i,5,0],F_MAX2,lTo2,α2) MinimumTension1 = TotalX[:, 2, :].min() MaximumTension1 = TotalX[:, 2, :].max() Tension1Range = TotalX[:, 2, :].max() - TotalX[:, 2, :].min() TendonTension1 = np.linspace(MinimumTension1 - 0.05 * Tension1Range, MaximumTension1 + 0.05 * Tension1Range, 1001) MinimumMuscleLength1 = TotalX[:, 4, :].min() MaximumMuscleLength1 = TotalX[:, 4, :].max() MuscleLength1Range = TotalX[:, 4, :].max() - TotalX[:, 4, :].min() MuscleLength1 = np.linspace( MinimumMuscleLength1 - 0.05 * MuscleLength1Range, MaximumMuscleLength1 + 0.05 * MuscleLength1Range, 1001) MuscleLength1Mesh, TendonTension1Mesh = \ np.meshgrid(MuscleLength1,TendonTension1) Error1 = \ error_function_1(TendonTension1Mesh,MuscleLength1Mesh) MinimumTension2 = TotalX[:, 3, :].min() MaximumTension2 = TotalX[:, 3, :].max() Tension2Range = TotalX[:, 3, :].max() - TotalX[:, 3, :].min() TendonTension2 = np.linspace(MinimumTension2 - 0.05 * Tension2Range, MaximumTension2 + 0.05 * Tension2Range, 1001) MinimumMuscleLength2 = TotalX[:, 5, :].min() MaximumMuscleLength2 = TotalX[:, 5, :].max() MuscleLength2Range = TotalX[:, 5, :].max() - TotalX[:, 5, :].min() MuscleLength2 = np.linspace( MinimumMuscleLength2 - 0.05 * MuscleLength2Range, MaximumMuscleLength2 + 0.05 * MuscleLength2Range, 1001) MuscleLength2Mesh, TendonTension2Mesh = \ np.meshgrid(MuscleLength2,TendonTension2) Error2 = \ error_function_2(TendonTension2Mesh,MuscleLength2Mesh) axes1_1.plot_surface(MuscleLength1Mesh, TendonTension1Mesh, Error1, color=str( np.linspace(0.25, 0.75, TotalX.shape[0])[i])) axes2_1.plot_surface(MuscleLength2Mesh, TendonTension2Mesh, Error2, color=str( np.linspace(0.25, 0.75, TotalX.shape[0])[i])) axes1_1.set_xlabel("Muscle Length (m)") axes1_1.set_ylabel("Tendon Tension (N)") axes1_1.set_zlabel("Error (m)") axes1_2.set_xlabel(r"$\longrightarrow$ Time (s) $\longrightarrow$") axes1_2.set_ylim(axes1_1.get_zlim()) # axes1_2.set_yticklabels(["" for el in axes1_2.get_yticks()]) axes1_3.set_ylabel(r"$\longleftarrow$ Time (s) $\longleftarrow$") axes1_3.set_xlim(axes1_1.get_ylim()) # axes1_3.set_xticklabels(["" for el in axes1_1.get_xticks()]) axes1_3.yaxis.tick_right() axes1_3.yaxis.set_label_position("right") axes1_3.set_yticks(-np.array(list(range(N_seconds + 1)))) axes1_3.set_yticklabels([str(-el) for el in axes1_3.get_yticks()]) axes1_4.text( 0.00, 0.65, (r'error $= \frac{\tau}{k}\cdot\ln\left(\frac{e^{T_{1}(t)/\tau} - 1}{e^{T_{1}(0)/\tau} - 1} \right )$' ), fontsize=20) axes1_4.text( 0.075, 0.475, (r' - $(1 - \cos(\alpha_{1}))\left[l_{m,1}(t) - l_{m,1}(0) \right]$' ), fontsize=16) axes1_4.text(0.15, 0.325, (r'where, $\tau = F_{MAX,1}\cdot c^T \cdot k^T$'), fontsize=14) axes1_4.text(0.15, 0.15, (r'and $k = \frac{F_{MAX,1}\cdot c^T}{l_{T_{o,1}}}$'), fontsize=14) axes1_4.axis('off') axes2_1.set_xlabel("Muscle Length (m)") axes2_1.set_ylabel("Tendon Tension (N)") axes2_1.set_zlabel("Error (m)") axes2_2.set_xlabel(r"$\longrightarrow$ Time (s) $\longrightarrow$") axes2_2.set_ylim(axes2_1.get_zlim()) # axes2_2.set_yticklabels(["" for el in axes2_2.get_yticks()]) axes2_3.set_ylabel(r"$\longleftarrow$ Time (s) $\longleftarrow$") axes2_3.set_xlim(axes2_1.get_ylim()) # axes2_3.set_xticklabels(["" for el in axes2_1.get_xticks()]) axes2_3.yaxis.tick_right() axes2_3.yaxis.set_label_position("right") axes2_3.set_yticks(-np.array(list(range(N_seconds + 1)))) axes2_3.set_yticklabels([str(-el) for el in axes1_3.get_yticks()]) axes2_4.text( 0.00, 0.65, (r'error $= \frac{\tau}{k}\cdot\ln\left(\frac{e^{T_{2}(t)/\tau} - 1}{e^{T_{2}(0)/\tau} - 1} \right )$' ), fontsize=20) axes2_4.text( 0.075, 0.475, (r' - $(1 - \cos(\alpha_{2}))\left[l_{m,2}(t) - l_{m,2}(0) \right]$' ), fontsize=16) axes2_4.text(0.15, 0.325, (r'where, $\tau = F_{MAX,2}\cdot c^T \cdot k^T$'), fontsize=14) axes2_4.text(0.15, 0.15, (r'and $k = \frac{F_{MAX,2}\cdot c^T}{l_{T_{o,2}}}$'), fontsize=14) axes2_4.axis('off') if Return == True: return ([fig1, fig2]) else: plt.show()
def return_Ia_error(N, Time, TotalX, **kwargs): NormalizeError = kwargs.get("NormalizeError", False) assert type( NormalizeError) == bool, "NormalizeError must be either True or False" ReturnAllData = kwargs.get("ReturnAllData", False) assert type( ReturnAllData) == bool, "ReturnAllData must be either True or False" fig1 = plt.figure() ax1 = plt.gca() ax1.set_title("Muscle 1") ax1.set_xlabel(r"$\gamma_{dynamic}$") if NormalizeError == True: ax1.set_ylabel("Normalized Average Error Value (pps)") else: ax1.set_ylabel("Average Error Value (pps)") fig2 = plt.figure() ax2 = plt.gca() ax2.set_title("Muscle 2") ax2.set_xlabel(r"$\gamma_{dynamic}$") if NormalizeError == True: ax2.set_ylabel("Normalized Average Error Value (pps)") else: ax2.set_ylabel("Average Error Value (pps)") # ApproxTestVarArray1 = np.zeros((np.shape(TotalX)[0],N,len(Time))) # TestVarArray1 = np.zeros((np.shape(TotalX)[0],N,len(Time))) # ApproxTestVarArray2 = np.zeros((np.shape(TotalX)[0],N,len(Time))) # TestVarArray2 = np.zeros((np.shape(TotalX)[0],N,len(Time))) AllPrimaryAfferent1 = np.zeros((np.shape(TotalX)[0], N, len(Time))) AllPrimaryAfferent2 = np.zeros((np.shape(TotalX)[0], N, len(Time))) AllApproxPrimaryAfferent1 = np.zeros((np.shape(TotalX)[0], N, len(Time))) AllApproxPrimaryAfferent2 = np.zeros((np.shape(TotalX)[0], N, len(Time))) for j in range(np.shape(TotalX)[0]): L_approx_1 = (integrate.cumtrapz( np.array(list(map(lambda X: v_MTU1(X), TotalX[j, :, :].T))), Time, initial=0) + np.ones(len(Time)) * TotalX[j, 4, 0]) / lo1 dL_approx_1 = np.array( list(map(lambda X: v_MTU1(X), TotalX[j, :, :].T))) / lo1 GammaArray_1 = np.zeros(N) AverageErrorArray_1 = np.zeros(N) gamma_dynamic_array = np.linspace(10, 250, N) statusbar1 = dsb(0, N, title="Muscle 1, Trial " + str(j + 1) + "/" + str(np.shape(TotalX)[0])) for i in range(N): gamma_dynamic_1 = gamma_dynamic_array[i] GammaArray_1[i] = gamma_dynamic_1 Approx_Aff_potential_bag_1 = return_primary_afferent_from_single_trial( Time, L_approx_1, dL_approx_1, gamma_dynamic=gamma_dynamic_1) Aff_potential_bag_1 = return_primary_afferent_from_single_trial( Time, TotalX[j, 4, :] / lo1, TotalX[j, 6, :] / lo1, gamma_dynamic=gamma_dynamic_1) AllPrimaryAfferent1[j, i, :] = Aff_potential_bag_1 AllApproxPrimaryAfferent1[j, i, :] = Approx_Aff_potential_bag_1 AverageError1 = \ np.average( abs( Aff_potential_bag_1[int(len(Time)*0.1):] - Approx_Aff_potential_bag_1[int(len(Time)*0.1):] ) ) if NormalizeError == True: PeakToPeak = np.array([ max(Aff_potential_bag_1[i:i + 10000]) - min(Aff_potential_bag_1[i:i + 10000]) for i in range(100, len(Time) - 10000, 100) ]).mean() AverageError1 = AverageError1 / PeakToPeak AverageErrorArray_1[i] = AverageError1 statusbar1.update(i) L_approx_2 = (integrate.cumtrapz( np.array(list(map(lambda X: v_MTU2(X), TotalX[j, :, :].T))), Time, initial=0) + np.ones(len(Time)) * TotalX[j, 5, 0]) / lo2 dL_approx_2 = np.array( list(map(lambda X: v_MTU2(X), TotalX[j, :, :].T))) / lo2 GammaArray_2 = np.zeros(N) AverageErrorArray_2 = np.zeros(N) statusbar2 = dsb(0, N, title="Muscle 2, Trial " + str(j + 1) + "/" + str(np.shape(TotalX)[0])) for i in range(N): gamma_dynamic_1 = gamma_dynamic_array[i] # gamma_dynamic_1 = np.round((10+np.random.rand()*(250-10))/5)*5 GammaArray_2[i] = gamma_dynamic_1 Approx_Aff_potential_bag_1 = return_primary_afferent_from_single_trial( Time, L_approx_2, dL_approx_2, gamma_dynamic=gamma_dynamic_1) Aff_potential_bag_1 = return_primary_afferent_from_single_trial( Time, TotalX[j, 5, :] / lo2, TotalX[j, 7, :] / lo2, gamma_dynamic=gamma_dynamic_1) AllPrimaryAfferent2[j, i, :] = Aff_potential_bag_1 AllApproxPrimaryAfferent2[j, i, :] = Approx_Aff_potential_bag_1 AverageError2 = \ np.average( abs( Aff_potential_bag_1[int(len(Time)*0.1):] - Approx_Aff_potential_bag_1[int(len(Time)*0.1):] ) ) if NormalizeError == True: PeakToPeak = np.array([ max(Aff_potential_bag_1[i:i + 10000]) - min(Aff_potential_bag_1[i:i + 10000]) for i in range(100, len(Time) - 10000, 100) ]).mean() AverageError2 = AverageError2 / PeakToPeak AverageErrorArray_2[i] = AverageError2 statusbar2.update(i) # plt.figure() ax1.plot(GammaArray_1, AverageErrorArray_1) # ax1 = plt.gca() # plt.figure() ax2.plot(GammaArray_2, AverageErrorArray_2) # ax2 = plt.gca() plt.show() if ReturnAllData == True: Data = { "Error is Normalized?": NormalizeError, "Muscle 1": { "Gamma Dynamic Gains": GammaArray_1, "Primary Afferent Signals": AllPrimaryAfferent1, "Approximate Primary Afferent Signals": AllApproxPrimaryAfferent1 }, "Muscle 2": { "Gamma Dynamic Gains": GammaArray_2, "Primary Afferent Signals": AllPrimaryAfferent2, "Approximate Primary Afferent Signals": AllApproxPrimaryAfferent2 }, } return (Data)