def __init__(self, Time, X, U, Y, desiredOutput, **params):
self.L = params.get("Link Length", 0.3)
is_number(self.L, "Link Length", default=0.3)
self.Lcm = params.get("Link Center of Mass", 0.085)
is_number(self.Lcm, "Link Center of Mass", default=0.085)
self.rj = params.get("Joint Moment Arm", 0.05)
is_number(self.rj, "Joint Moment Arm", default=0.05)
self.rm = params.get("Motor Moment Arm", 0.02)
is_number(self.rm, "Motor Moment Arm", default=0.02)
self.X = X
self.U = U
self.Y = Y
self.Time = Time
self.x1d = desiredOutput[0, :]
self.Sd = desiredOutput[1, :]
self.fig = plt.figure(figsize=(12, 10))
self.ax0 = self.fig.add_subplot(221) # animation
self.ax1 = self.fig.add_subplot(222) # input (torques)
self.ax2 = self.fig.add_subplot(223) # pendulum angle
self.ax4 = self.fig.add_subplot(224) # stiffness
plt.suptitle("Nonlinear Tendon-Driven Pendulum Example",
fontsize=28,
y=1.05)
self.pendulumGround = plt.Rectangle((-0.25 * self.L, -self.rj),
0.5 * self.L,
self.rj,
Color='0.20')
self.ax0.add_patch(self.pendulumGround)
# Tendon
self.jointWrapAround, = self.ax0.plot(
self.rj * np.sin(np.linspace(0, 2 * np.pi, 1001)),
self.rj * np.cos(np.linspace(0, 2 * np.pi, 1001)),
c='k',
lw=1)
self.motor1WrapAround, = self.ax0.plot(
-self.L + self.rm * np.sin(np.linspace(0, 2 * np.pi, 1001)),
-self.L + self.rm * np.cos(np.linspace(0, 2 * np.pi, 1001)),
c='k',
lw=1)
self.motor2WrapAround, = self.ax0.plot(
self.L + self.rm * np.sin(np.linspace(0, 2 * np.pi, 1001)),
-self.L + self.rm * np.cos(np.linspace(0, 2 * np.pi, 1001)),
c='k',
lw=1)
self.phi1 = 225 * np.pi / 180 - acos(
(self.rj - self.rm) / (np.sqrt(2) * self.L))
self.phi2 = acos(
(self.rj - self.rm) / (np.sqrt(2) * self.L)) - 45 * np.pi / 180
self.springArray = (self.rm * np.abs(
signal.sawtooth(5 * 2 * np.pi * np.linspace(0, 1, 1001) -
np.pi / 2)) - 0.5 * self.rm)
self.spring_y = np.concatenate(
[np.zeros((1001, )), self.springArray,
np.zeros((1001, ))])
self.tendonDeformation1 = self.rm * self.X[2, :] - self.rj * self.X[
0, :]
self.tendonDeformation2 = self.rm * self.X[4, :] + self.rj * self.X[
0, :]
self.maxTendonDeformation = max(
[self.tendonDeformation1.max(),
self.tendonDeformation2.max()])
self.spring1_x = np.concatenate([
np.linspace(
0,
self.L * np.sqrt(2) / 6 *
(2 - self.tendonDeformation1 / self.maxTendonDeformation),
1001),
np.linspace(
self.L * np.sqrt(2) / 6 *
(2 - self.tendonDeformation1 / self.maxTendonDeformation),
self.L * np.sqrt(2) / 6 *
(4 + self.tendonDeformation1 / self.maxTendonDeformation),
1001),
np.linspace(
self.L * np.sqrt(2) / 6 *
(4 + self.tendonDeformation1 / self.maxTendonDeformation),
self.L * np.sqrt(2), 1001)
])
self.spring2_x = np.concatenate([
np.linspace(
0,
self.L * np.sqrt(2) / 6 *
(2 - self.tendonDeformation2 / self.maxTendonDeformation),
1001),
np.linspace(
self.L * np.sqrt(2) / 6 *
(2 - self.tendonDeformation2 / self.maxTendonDeformation),
self.L * np.sqrt(2) / 6 *
(4 + self.tendonDeformation2 / self.maxTendonDeformation),
1001),
np.linspace(
self.L * np.sqrt(2) / 6 *
(4 + self.tendonDeformation2 / self.maxTendonDeformation),
self.L * np.sqrt(2), 1001)
])
self.spring1Arrays = (
np.array([[-self.L + self.rm * np.cos(self.phi1)],
[-self.L + self.rm * np.sin(self.phi1)]]) +
np.array([[
np.cos(self.phi1 - 90 * np.pi / 180),
-np.sin(self.phi1 - 90 * np.pi / 180)
],
[
np.sin(self.phi1 - 90 * np.pi / 180),
np.cos(self.phi1 - 90 * np.pi / 180)
]]) @ np.array([self.spring1_x[:, 0], self.spring_y]))
self.spring2Arrays = (
np.array([[self.L + self.rm * np.cos(self.phi2)],
[-self.L + self.rm * np.sin(self.phi2)]]) +
np.array([[
np.cos(self.phi2 + 90 * np.pi / 180),
-np.sin(self.phi2 + 90 * np.pi / 180)
],
[
np.sin(self.phi2 + 90 * np.pi / 180),
np.cos(self.phi2 + 90 * np.pi / 180)
]]) @ np.array([self.spring2_x[:, 0], self.spring_y]))
self.spring1, = self.ax0.plot(self.spring1Arrays[0, :],
self.spring1Arrays[1, :],
c='k',
lw=1)
self.spring2, = self.ax0.plot(self.spring2Arrays[0, :],
self.spring2Arrays[1, :],
c='k',
lw=1)
# Pendulum
self.pendulum, = self.ax0.plot([0, self.L * np.sin(X[0, 0])],
[0, -self.L * np.cos(X[0, 0])],
Color='0.50',
lw=10,
solid_capstyle='round',
path_effects=[
pe.Stroke(linewidth=12,
foreground='k'),
pe.Normal()
])
# Pendulum Joint
self.pendulumJoint = plt.Circle((0, 0), self.rj, Color='#4682b4')
self.ax0.add_patch(self.pendulumJoint)
self.pendulumJointRivet, = self.ax0.plot([0], [0],
c='k',
marker='o',
lw=2)
# Motors
self.motor1Joint = plt.Circle((-self.L, -self.L),
self.rm,
Color='#4682b4')
self.ax0.add_patch(self.motor1Joint)
self.motor1Rivet, = self.ax0.plot([-self.L], [-self.L],
c='k',
marker='.',
lw=1)
self.motor2Joint = plt.Circle((self.L, -self.L),
self.rm,
Color='#4682b4')
self.ax0.add_patch(self.motor2Joint)
self.motor2Rivet, = self.ax0.plot([self.L], [-self.L],
c='k',
marker='.',
lw=1)
self.max_tau = self.U.max()
if self.max_tau == 0: self.max_tau = 1
self.k = 0.075 * self.L
self.inputIndicator1, = self.ax0.plot(
(-self.L + 2.5 * self.rm * np.cos(
np.linspace(
135 * np.pi / 180, 135 * np.pi / 180 +
np.pi * self.U[0, 0] / self.max_tau, 20))),
(-self.L + 2.5 * self.rm * np.sin(
np.linspace(
135 * np.pi / 180, 135 * np.pi / 180 +
np.pi * self.U[0, 0] / self.max_tau, 20))),
Color='r',
lw=2,
solid_capstyle='round')
self.inputIndicator1Arrow, = self.ax0.plot(
(-self.L + 2.5 * self.rm *
np.cos(135 * np.pi / 180 + np.pi * self.U[0, 0] / self.max_tau) +
self.k * (self.U[0, 0] / self.max_tau)**(1 / 2) * np.array([
np.cos(45 * np.pi / 180 + np.pi * self.U[0, 0] /
self.max_tau - 1.33 * 30 * np.pi / 180), 0,
np.cos(45 * np.pi / 180 +
np.pi * self.U[0, 0] / self.max_tau + 30 * np.pi / 180)
])),
(-self.L + 2.5 * self.rm *
np.sin(135 * np.pi / 180 + np.pi * self.U[0, 0] / self.max_tau) +
self.k * (self.U[0, 0] / self.max_tau)**(1 / 2) * np.array([
np.sin(45 * np.pi / 180 + np.pi * self.U[0, 0] /
self.max_tau - 1.33 * 30 * np.pi / 180), 0,
np.sin(45 * np.pi / 180 +
np.pi * self.U[0, 0] / self.max_tau + 30 * np.pi / 180)
])),
Color='r',
lw=2,
solid_capstyle='round')
self.inputIndicator2, = self.ax0.plot((self.L + 2.5 * self.rm * np.cos(
np.linspace(45 * np.pi / 180 - np.pi * self.U[1, 0] / self.max_tau,
45 * np.pi / 180, 20)
)), (-self.L + 2.5 * self.rm * np.sin(
np.linspace(45 * np.pi / 180 - np.pi * self.U[1, 0] / self.max_tau,
45 * np.pi / 180, 20))),
Color='g',
lw=2,
solid_capstyle='round')
self.inputIndicator2Arrow, = self.ax0.plot(
(self.L + 2.5 * self.rm *
np.cos(45 * np.pi / 180 - np.pi * self.U[1, 0] / self.max_tau) +
self.k * (self.U[1, 0] / self.max_tau)**(1 / 2) * np.array([
np.cos(135 * np.pi / 180 - np.pi * self.U[1, 0] /
self.max_tau - 30 * np.pi / 180), 0,
np.cos(135 * np.pi / 180 - np.pi * self.U[1, 0] /
self.max_tau + 1.33 * 30 * np.pi / 180)
])),
(-self.L + 2.5 * self.rm *
np.sin(45 * np.pi / 180 - np.pi * self.U[1, 0] / self.max_tau) +
self.k * (self.U[1, 0] / self.max_tau)**(1 / 2) * np.array([
np.sin(135 * np.pi / 180 - np.pi * self.U[1, 0] /
self.max_tau - 30 * np.pi / 180), 0,
np.sin(135 * np.pi / 180 - np.pi * self.U[1, 0] /
self.max_tau + 1.33 * 30 * np.pi / 180)
])),
Color='g',
lw=2,
solid_capstyle='round')
self.ax0.get_xaxis().set_ticks([])
self.ax0.get_yaxis().set_ticks([])
self.ax0.set_frame_on(True)
self.ax0.set_xlim(
[-1.1 * self.L - 2.5 * self.rm, 1.1 * self.L + 2.5 * self.rm])
self.ax0.set_ylim([-1.1 * self.L - 2.5 * self.rm, 1.1 * self.L])
self.ax0.set_aspect('equal')
self.timeStamp = self.ax0.text(0,
-self.L,
"Time: " + str(self.Time[0]) + " s",
color='0.50',
fontsize=16,
horizontalalignment='center')
#Input
self.input1, = self.ax1.plot([0], [self.U[0, 0]], color='r')
self.input2, = self.ax1.plot([0], [self.U[1, 0]], color='g')
self.ax1.set_xlim(0, self.Time[-1])
self.ax1.set_xticks(list(np.linspace(0, self.Time[-1], 5)))
self.ax1.set_xticklabels(
[str(0), '', '', '', str(self.Time[-1]) + "s"])
if max(abs(self.U[0, :] - self.U[0, 0])) < 1e-7 and max(
abs(self.U[1, :] - self.U[1, 0])) < 1e-7:
self.ax1.set_ylim([min(self.U[:, 0]) - 5, max(self.U[:, 0]) + 5])
else:
self.RangeU = self.U.max() - self.U.min()
self.ax1.set_ylim([
self.U.min() - 0.1 * self.RangeU,
self.U.max() + 0.1 * self.RangeU
])
self.ax1.spines['right'].set_visible(False)
self.ax1.spines['top'].set_visible(False)
self.ax1.set_title("Motor Torques (Nm)",
fontsize=16,
fontweight=4,
color='k',
y=1.00)
#pendulum angle
self.Y1d = self.Y[0, :] * 180 / np.pi
self.angle, = self.ax2.plot([0], [self.Y1d[0]], color='C0')
self.desiredAngle, = self.ax2.plot(Time,
self.x1d * 180 / np.pi,
c='k',
linestyle='--',
lw=1)
self.ax2.set_xlim(0, self.Time[-1])
self.ax2.set_xticks(list(np.linspace(0, self.Time[-1], 5)))
self.ax2.set_xticklabels(
[str(0), '', '', '', str(self.Time[-1]) + "s"])
if max(abs(self.Y1d - self.Y1d[0])) < 1e-7:
self.ax2.set_ylim([self.Y1d[0] - 2, self.Y1d[0] + 2])
else:
self.RangeY1d = max(self.Y1d) - min(self.Y1d)
self.ax2.set_ylim([
min(self.Y1d) - 0.1 * self.RangeY1d,
max(self.Y1d) + 0.1 * self.RangeY1d
])
# y1_min = np.floor((self.Y[0,:].min()*180/np.pi)/22.5)*22.5
# y1_min = min([y1_min,np.floor((self.x1d.min()*180/np.pi)/22.5)*22.5])
# y1_max = np.ceil((self.Y[0,:].max()*180/np.pi)/22.5)*22.5
# y1_max = max([y1_max,np.ceil((self.x1d.max()*180/np.pi)/22.5)*22.5])
y1_min = 0
y1_max = 360
yticks = np.arange(y1_min, y1_max + 22.5, 22.5)
yticklabels = []
for el in yticks:
if el % 45 == 0:
yticklabels.append(str(int(el)) + r"$^\circ$")
else:
yticklabels.append("")
self.ax2.set_yticks(yticks)
self.ax2.set_yticklabels(yticklabels)
self.ax2.spines['right'].set_visible(False)
self.ax2.spines['top'].set_visible(False)
self.ax2.set_title("Angle (deg)",
fontsize=16,
fontweight=4,
color='C0',
y=1.00)
# pendulum stiffness
self.stiffness, = self.ax4.plot([0], [self.Y[1, 0]], color='C1')
self.desiredStiffness, = self.ax4.plot(Time,
self.Sd,
c='k',
linestyle='--',
lw=1)
self.ax4.set_xlim(0, self.Time[-1])
self.ax4.set_xticks(list(np.linspace(0, self.Time[-1], 5)))
self.ax4.set_xticklabels(
[str(0), '', '', '', str(self.Time[-1]) + "s"])
if max(abs(self.Y[1, :] - self.Y[1, 0])) < 1e-7:
self.ax4.set_ylim([self.Y[1, 0] - 2, self.Y[1, 0] + 2])
else:
self.RangeY2 = max(self.Y[1, :]) - min(self.Y[1, :])
self.ax4.set_ylim([
min(self.Y[1, :]) - 0.1 * self.RangeY2,
max(self.Y[1, :]) + 0.1 * self.RangeY2
])
self.ax4.spines['right'].set_visible(False)
self.ax4.spines['top'].set_visible(False)
self.ax4.set_title("Stiffness (Nm/rad)",
fontsize=16,
fontweight=4,
color='C1',
y=1.00)
def run_babbling_trial(self,
x1o,
plot=False,
saveFigures=False,
saveAsPDF=False,
returnData=False,
saveData=False,
saveParams=False):
is_number(x1o,
"Initial Joint Angle",
notes="Should be between 0 and 2 pi.")
if saveFigures == True:
assert plot == True, "No figures will be generated. Please select plot=True."
poorBabbling = True
count = 0
while poorBabbling:
## Generate babbling input
if self.babblingType == 'continuous':
self.generate_continuous_babbling_input()
else: #self.babblingType=='step'
self.generate_step_babbling_input()
## running the babbling data through the plant
X_o = self.plant.return_X_o(x1o, self.babblingSignals[0, :])
X, U, _ = self.plant.forward_simulation(
X_o, U=self.babblingSignals.T, addTitle="Motor Babbling"
) # need the transpose for the shapes to align (DAH)
tendonDeformation1 = np.array(
[-self.plant.rj, 0, self.plant.rm, 0, 0, 0]) @ X
tendonDeformation2 = np.array(
[self.plant.rj, 0, 0, 0, self.plant.rm, 0]) @ X
minDeformation = np.array([tendonDeformation1,
tendonDeformation2]).min()
if minDeformation < -0.02:
print(
"Poor Babbling! Large Negative Tendon Deformations. Try Again"
)
count += 1
assert count < 100, "count exceeded (negative tendon deformation)."
elif (abs(X[0, :].min() - self.plant.jointAngleBounds['LB']) <= 5 *
(np.pi / 180) and
abs(X[0, :].max() - self.plant.jointAngleBounds['UB']) <= 5 *
(np.pi / 180)):
poorBabbling = False
else:
print("Poor Babbling! Trying again...")
## plot (and save) figures
output = {}
if plot == True:
# self.plant.plot_states(X,Return=True)
self.plant.plot_states_and_inputs(X, U, returnFig=True)
self.plant.plot_output_power_spectrum_and_distribution(
X, returnFigs=True)
self.plant.plot_tendon_tension_deformation_curves(X)
self.plot_signals_power_spectrum_and_amplitude_distribution()
if saveFigures == True:
trialPath = save_figures("babbling_trials/",
babblingParams["Babbling Type"],
self.totalParams,
returnPath=True,
saveAsPDF=saveAsPDF,
saveAsMD=True)
if saveParams == True:
save_params_as_MAT(self.totalParams, path=trialPath)
if saveData == True:
self.save_data(X, filePath=trialPath)
if returnData == True:
output["time"] = self.plant.time
output["X"] = X
output["U"] = U
output["path"] = trialPath
return (output)
else:
output["path"] = trialPath
return (output)
else:
if saveParams == True:
save_params_as_MAT(self.totalParams)
if saveData == True:
self.save_data(X)
if returnData == True:
output["time"] = self.plant.time
output["X"] = X
output["U"] = U
return (output)
else:
if saveParams == True:
save_params_as_MAT(self.totalParams)
if saveData == True:
babblingTrial.save_data(X)
if returnData == True:
output["time"] = self.plant.time
output["X"] = X
output["U"] = U
return (output)
def run_babbling_trial(self,
x1o,
plot=False,
saveFigures=False,
saveAsPDF=False,
returnData=False,
saveData=False,
saveParams=False):
is_number(x1o,
"Initial Joint Angle",
notes="Should be between 0 and 2 pi.")
if saveFigures == True:
assert plot == True, "No figures will be generated. Please select plot=True."
## Generate babbling input
if self.babblingType == 'continuous':
self.generate_continuous_babbling_input()
else: #self.babblingType=='step'
self.generate_step_babbling_input()
## running the babbling data through the plant
X_o = self.plant.return_X_o(x1o, self.babblingSignals[0, :])
X, U, _ = self.plant.forward_simulation(
X_o, U=self.babblingSignals.T, addTitle="Motor Babbling"
) # need the transpose for the shapes to align (DAH)
## plot (and save) figures
output = {}
if plot == True:
self.plant.plot_states(X)
self.plant.plot_joint_angle_power_spectrum_and_distribution(X)
self.plant.plot_tendon_tension_deformation_curves(X)
self.plot_signals_power_spectrum_and_amplitude_distribution()
if saveFigures == True:
trialPath = save_figures("babbling_trials/",
"propAmp",
self.totalParams,
returnPath=True,
saveAsPDF=saveAsPDF)
if saveParams == True:
save_params_as_MAT(self.totalParams, path=trialPath)
if saveData == True:
self.save_data(X, path=trialPath)
if returnData == True:
output["time"] = self.plant.time
output["X"] = X
output["U"] = U
output["path"] = trialPath
return (output)
else:
output["path"] = trialPath
return (output)
else:
if saveParams == True:
save_params_as_MAT(self.totalParams)
if saveData == True:
self.save_data(X)
if returnData == True:
output["time"] = self.plant.time
output["X"] = X
output["U"] = U
return (output)
else:
if saveParams == True:
save_params_as_MAT(self.totalParams)
if saveData == True:
self.save_data(X)
if returnData == True:
output["time"] = self.plant.time
output["X"] = X
output["U"] = U
return (output)
def __init__(self, plant, babblingParams):
self.totalParams = plant.params
self.totalParams.update(babblingParams)
self.plant = plant
self.seed = babblingParams.get("Seed", None)
if self.seed is not None:
is_number(self.seed, "Seed", default="None")
np.random.seed(self.seed)
self.passProbability = babblingParams.get("Pass Probability", 0.001)
is_number(self.passProbability, "Pass Probability", default=0.001)
self.inputBounds = babblingParams.get("Input Bounds", [0, 100])
assert (type(self.inputBounds)==list and len(self.inputBounds)==2), \
"Input Bounds must be a list of length 2. Default is [0,100]."
is_number(self.inputBounds[0], "Input Minimum")
is_number(self.inputBounds[1], "Input Maximum")
assert self.inputBounds[1] > self.inputBounds[
0], "Input Bounds must be in ascending order. Default is [0,100]."
self.inputRange = self.inputBounds[1] - self.inputBounds[0]
self.inputMinimum = self.inputBounds[0]
self.inputMaximum = self.inputBounds[1]
self.lowCutoffFrequency = babblingParams.get("Low Cutoff Frequency", 1)
is_number(self.lowCutoffFrequency, "Low Cutoff Frequency", default=1)
self.highCutoffFrequency = babblingParams.get("High Cutoff Frequency",
10)
is_number(self.highCutoffFrequency,
"High Cutoff Frequency",
default=10)
assert self.lowCutoffFrequency < self.highCutoffFrequency, "The low cutoff frequency for the white noise must be below the high cutoff frequency"
self.filterOrder = babblingParams.get("Buttersworth Filter Order", 5)
is_number(self.filterOrder, "Buttersworth Filter Order", default=5)
assert type(
babblingParams.get("Force Cocontraction", False)
) == bool, "'Force Cocontraction' should be either True or False (default)."
if babblingParams.get("Force Cocontraction", False) == True:
self.cocontraction = True
self.cocontractionSTD = babblingParams.get(
"Cocontraction Standard Deviation", 1)
is_number(self.cocontractionSTD,
"Cocontraction Standard Deviation",
default=1)
else:
self.cocontraction = False
self.babblingType = babblingParams.get("Babbling Type", 'continuous')
assert self.babblingType in [
'continuous', 'step'
], "babblingType must be either 'continuous' (default) or 'step'."
self.filterLength = int(1 / self.highCutoffFrequency / 2 /
self.plant.dt)
def __init__(self, **params):
self.Ij = params.get("Joint Inertia", 1.15e-2) # kg⋅m²
is_number(self.Ij, "Joint Inertia", default=1.15e-2)
self.bj = params.get("Joint Damping", 0.001) # N⋅s⋅m⁻¹
is_number(self.bj, "Joint Damping", default=0.001)
self.mj = params.get("Joint Mass", 0.541) # kg
is_number(self.mj, "Joint Mass", default=0.541)
self.rj = params.get("Joint Moment Arm", 0.05) # m
is_number(self.rj, "Joint Moment Arm", default=0.05)
self.Lcm = params.get("Link Center of Mass", 0.085) # m
is_number(self.Lcm, "Link Center of Mass", default=0.085)
self.L = params.get("Link Length", 0.3) # m
is_number(self.L, "Link Length", default=0.3)
self.Jm = params.get("Motor Inertia", 6.6e-5) # kg⋅m²
is_number(self.Jm, "Motor Inertia", default=6.6e-5)
self.bm = params.get("Motor Damping", 0.00462) # N⋅s⋅m⁻¹
is_number(self.bm, "Motor Damping", default=0.00462)
self.rm = params.get("Motor Moment Arm", 0.01) # m
is_number(self.rm, "Motor Moment Arm", default=0.01)
self.k_spr = params.get("Spring Stiffness Coefficient", 1) # N
is_number(self.k_spr, "", default=1)
self.b_spr = params.get("Spring Shape Coefficient", 100) # unit-less
is_number(self.b_spr, "", default=1)
self.simulationDuration = params.get("Simulation Duration", 1000)
is_number(self.simulationDuration, "Simulation Duration")
self.dt = params.get("dt", 0.01)
is_number(self.dt, "dt")
self.k0 = params.get("Position Gains", {
0: 3162.3,
1: 1101.9,
2: 192.0,
3: 19.6
})
self.ks = params.get("Stiffness Gains", {0: 316.2, 1: 25.1})
self.Lf4h0_list = []
self.Lf2hs_list = []
self.df2dx1_list = []
self.df2dx2_list = []
self.df2dx3_list = []
self.df2dx5_list = []
self.vs_list = []