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)
