def gait2dpi_u(self, xs, xsd, xs_meas, vs, u, uo, p, index_open,
index_imp):
half_state = int(self.num_states / 2)
x = xs[:half_state]
xd = xs[half_state:]
xdd = xsd[half_state:]
QQ, dQQ_dx, dQQ_dxd, dQQ_dxdd, GRF, dGRF_dx, dGRF_dxd, sticks = \
autolev_rhs(x, xd, xdd, vs, self.constants_dict)
tor_imp, dtor_dx, dtor_dp = self.impedance_control(
xs, xs_meas, p, index_imp)
tor_nor, dtor_duo = self.normal_torque(uo, index_open)
tor = np.zeros(half_state)
tor[self.index_open_motion] = u
tor[self.index_control_motion] = tor_nor + tor_imp
f = np.zeros(self.num_states)
f[:half_state] = xsd[:half_state] - xs[half_state:]
f[half_state:] = (tor - QQ) / self.scaling
dfdx = np.zeros((self.num_states, self.num_states))
dfdx[:half_state, half_state:] = -np.eye(half_state)
dfdx[half_state:, :half_state] = -np.reshape(
dQQ_dx, (half_state, half_state)) / self.scaling
dfdx[
half_state:,
half_state:] = -np.reshape(dQQ_dxd,
(half_state, half_state)) / self.scaling
dfdx[half_state +
self.index_control_motion, :] += dtor_dx / self.scaling
dfdxd = np.zeros((self.num_states, self.num_states))
dfdxd[:half_state, :half_state] = np.eye(half_state)
dfdxd[
half_state:,
half_state:] = -np.reshape(dQQ_dxdd,
(half_state, half_state)) / self.scaling
dfdu = np.zeros((self.num_states, self.num_open))
dfdu[half_state +
self.index_open_motion, :] = np.eye(self.num_open) / self.scaling
dfduo = np.zeros((self.num_states, self.num_normal * self.num_control))
dfduo[half_state +
self.index_control_motion, :] = dtor_duo / self.scaling
dfdp = np.zeros((self.num_states, self.num_par))
dfdp[half_state +
self.index_control_motion, :] = dtor_dp / self.scaling
return f, dfdx, dfdxd, dfdu, dfduo, dfdp
ax8.bar(xaxis, impedance[14:16], align='center', alpha=0.3)
ax8.plot(xaxis, imped_sim[14:16], 'ro')
fig4.savefig(store_path + 'ForwardSimulation_ImpedanceGains.png')
constants_dict = map_values_to_autolev_symbols(constants_dict)
sticks_pred = np.zeros((num_nodes, 20))
qd = np.zeros(9)
qdd = np.zeros(9)
belts = np.zeros(num_nodes)
GRF = np.zeros((num_nodes, 6))
for o in range(num_nodes):
_, _, _, _, GRF[o, :], _, _, sticks_pred[o, :] = \
autolev_rhs(Result[o, :9], qd, qdd,
vs_meas[o, :], constants_dict)
if o == 0:
belts[o] = 0
else:
belts[o] = belts[o - 1] + np.sum(vs_meas[o, :] -
(0.8 + 0.4 * T)) / 2 * 0.02
time_plot = np.linspace(0, num_nodes * interval, num_nodes)
anni_names = store_path + 'annimation.mp4'
fps_def = 12.5
animate_pendulum(time_plot, sticks_pred, belts, fps_def, filename=anni_names)
fig8 = plt.figure(figsize=(16, 8))
ax11 = fig8.add_subplot(3, 2, 1)
plt.ylabel('left Fx (N)', fontsize=14)
ax12 = fig8.add_subplot(3, 2, 2)
def gait2dpi_u(self, xs, xsd, vs, u_close, u_stanceL, u_stanceR, u_ankle, iniL, iniR):
# separate angle, anglar velocity, and anglar acceleration
x = xs[:9]
xd = xs[9:18]
xdd = xsd[9:18]
# gait 2d dynamics functions' residule, derivatives
QQ, dQQ_dx, dQQ_dxd, dQQ_dxdd, GRF, dGRF_dx, dGRF_dxd, sticks = \
autolev_rhs(x, xd, xdd, vs, self.constants_dict)
if iniL[0] and iniR[0]:
left_torque = u_stanceL
right_torque = u_stanceR
torque = np.zeros(9)
torque[3:5] = left_torque
torque[5] = u_ankle[0]
torque[6:8] = right_torque
torque[8] = u_ankle[1]
dTorque_dConLOpen = np.zeros((9, 2))
dTorque_dConROpen = np.zeros((9, 2))
dTorque_dConAnkle = np.zeros((9, 2))
dTorque_dConLOpen[3, 0] = 1
dTorque_dConLOpen[4, 1] = 1
dTorque_dConAnkle[5, 0] = 1
dTorque_dConROpen[6, 0] = 1
dTorque_dConROpen[7, 1] = 1
dTorque_dConAnkle[8, 1] = 1
f = np.zeros(self.num_states)
f[:9] = xsd[:9] - xs[9:18]
f[9:18] = (torque - QQ)/self.scaling
df_dXs = np.zeros((self.num_states, self.num_states))
df_dXsd = np.zeros((self.num_states, self.num_states))
df_dConLOpen = np.zeros((self.num_states, 2))
df_dConROpen = np.zeros((self.num_states, 2))
df_dConAnkle = np.zeros((self.num_states, 2))
df_dConClose = np.zeros((self.num_states, len(u_close)))
df_dXs[:9,9:18] = -np.eye(9)
df_dXs[9:18,:9] = (-np.reshape(dQQ_dx, (9, 9)))/self.scaling
df_dXs[9:18,9:18] = (-np.reshape(dQQ_dxd, (9, 9)))/self.scaling
df_dXs[10, 9] = 0.0
# force row 10 col 9 element in dfdx be zero, since it
# sometimes is zero, but sometimes is very small number (10^-10).
# This caused jacobian structure unstable. Thereforce its element
# is force to be zero here.
df_dXsd[:9,:9] = np.eye(9)
df_dXsd[9:18,9:18] = -np.reshape(dQQ_dxdd, (9, 9))/self.scaling
df_dConLOpen[9:18, :] = dTorque_dConLOpen/self.scaling
df_dConROpen[9:18, :] = dTorque_dConROpen/self.scaling
df_dConAnkle[9:18, :] = dTorque_dConAnkle/self.scaling
elif iniL[0]:
left_torque = u_stanceL
(right_torque, dRightTorque_dXs, dRightTorque_dCon,
fRight, dfRight_dXs, dfRight_dCon) =\
self.swing_control(xs, u_close, iniR[1:3], iniR[3:5], sta_left=0, sta_right=1)
torque = np.zeros(9)
torque[3:5] = left_torque
torque[5] = u_ankle[0]
torque[6:8] = right_torque
torque[8] = u_ankle[1]
dTorque_dConClose = np.zeros((9, len(u_close)))
dTorque_dConLOpen = np.zeros((9, 2))
dTorque_dConAnkle = np.zeros((9, 2))
dTorque_dConClose[6:8, :] = dRightTorque_dCon
dTorque_dConLOpen[3, 0] = 1
dTorque_dConLOpen[4, 1] = 1
dTorque_dConAnkle[5, 0] = 1
dTorque_dConAnkle[8, 1] = 1
dTorque_dXs = np.zeros((9, self.num_states))
dTorque_dXs[6:8, :] = dRightTorque_dXs
f = np.zeros(self.num_states)
f[:9] = xsd[:9] - xs[9:18]
f[9:18] = (torque - QQ)/self.scaling
f[18:22] = fRight
df_dXs = np.zeros((self.num_states, self.num_states))
df_dXsd = np.zeros((self.num_states, self.num_states))
df_dConLOpen = np.zeros((self.num_states, 2))
df_dConROpen = np.zeros((self.num_states, 2))
df_dConAnkle = np.zeros((self.num_states, 2))
df_dConClose = np.zeros((self.num_states, len(u_close)))
df_dXs[:9,9:18] = -np.eye(9)
df_dXs[9:18,:9] = (dTorque_dXs[:, :9]-np.reshape(dQQ_dx, (9, 9)))/self.scaling
df_dXs[9:18,9:18] = (dTorque_dXs[:, 9:18]-np.reshape(dQQ_dxd, (9, 9)))/self.scaling
df_dXs[9:18, 18:22] = dTorque_dXs[:, 18:22]/self.scaling
df_dXs[18:22,:] = dfRight_dXs
df_dXs[10, 9] = 0.0
# force row 10 col 9 element in dfdx be zero, since it
# sometimes is zero, but sometimes is very small number (10^-10).
# This caused jacobian structure unstable. Thereforce its element
# is force to be zero here.
df_dXsd[:9,:9] = np.eye(9)
df_dXsd[9:18,9:18] = -np.reshape(dQQ_dxdd, (9, 9))/self.scaling
df_dConLOpen[9:18, :] = dTorque_dConLOpen/self.scaling
df_dConAnkle[9:18, :] = dTorque_dConAnkle/self.scaling
df_dConClose[9:18, :] = dTorque_dConClose/self.scaling
df_dConClose[18:22, :] = dfRight_dCon
elif iniR[0]:
(left_torque, dLeftTorque_dXs, dLeftTorque_dCon,
fLeft, dfLeft_dXs, dfLeft_dCon) =\
self.swing_control(xs, u_close, iniL[1:3], iniL[3:5], sta_left=1, sta_right=0)
right_torque = u_stanceR
torque = np.zeros(9)
torque[3:5] = left_torque
torque[5] = u_ankle[0]
torque[6:8] = right_torque
torque[8] = u_ankle[1]
dTorque_dConClose = np.zeros((9, len(u_close)))
dTorque_dConROpen = np.zeros((9, 2))
dTorque_dConAnkle = np.zeros((9, 2))
dTorque_dConClose[3:5, :] = dLeftTorque_dCon
dTorque_dConAnkle[5, 0] = 1
dTorque_dConROpen[6, 0] = 1
dTorque_dConROpen[7, 1] = 1
dTorque_dConAnkle[8, 1] = 1
dTorque_dXs = np.zeros((9, self.num_states))
dTorque_dXs[3:5, :] = dLeftTorque_dXs
f = np.zeros(self.num_states)
f[:9] = xsd[:9] - xs[9:18]
f[9:18] = (torque - QQ)/self.scaling
f[18:22] = fLeft
df_dXs = np.zeros((self.num_states, self.num_states))
df_dXsd = np.zeros((self.num_states, self.num_states))
df_dConLOpen = np.zeros((self.num_states, 2))
df_dConROpen = np.zeros((self.num_states, 2))
df_dConAnkle = np.zeros((self.num_states, 2))
df_dConClose = np.zeros((self.num_states, len(u_close)))
df_dXs[:9,9:18] = -np.eye(9)
df_dXs[9:18,:9] = (dTorque_dXs[:, :9]-np.reshape(dQQ_dx, (9, 9)))/self.scaling
df_dXs[9:18,9:18] = (dTorque_dXs[:, 9:18]-np.reshape(dQQ_dxd, (9, 9)))/self.scaling
df_dXs[9:18, 18:22] = dTorque_dXs[:, 18:22]/self.scaling
df_dXs[18:22,:] = dfLeft_dXs
df_dXs[10, 9] = 0.0
# force row 10 col 9 element in dfdx be zero, since it
# sometimes is zero, but sometimes is very small number (10^-10).
# This caused jacobian structure unstable. Thereforce its element
# is force to be zero here.
df_dXsd[:9,:9] = np.eye(9)
df_dXsd[9:18,9:18] = -np.reshape(dQQ_dxdd, (9, 9))/self.scaling
df_dConROpen[9:18, :] = dTorque_dConROpen/self.scaling
df_dConAnkle[9:18, :] = dTorque_dConAnkle/self.scaling
df_dConClose[9:18, :] = dTorque_dConClose/self.scaling
df_dConClose[18:22, :] = dfLeft_dCon
return f, df_dXs, df_dXsd, df_dConClose, df_dConLOpen, df_dConROpen, df_dConAnkle
StringP += " "
StringP += "\n"
outfile.write(StringP)
show_nodes_plot = 1001
with open(para_name, 'r') as f:
subj_info = yaml.load(f)
constants_dict = map_values_to_autolev_symbols(subj_info)
sticks = np.zeros((show_nodes_plot, 20))
belt_disp = np.zeros(show_nodes_plot)
for qq in range(show_nodes_plot):
x = show_motion_2d[qq, :9]
xd = np.zeros(9)
xdd = np.zeros(9)
vs = show_belt[qq, :]
if qq > 0:
belt_disp[qq] = belt_disp[qq - 1] - (show_belt[
qq, 0] + show_belt[qq - 1, 0]) / 2 * 0.01 - 0.8 * 0.01
_, _, _, _, _, _, _, sticks[qq, :] = \
autolev_rhs(x, xd, xdd, vs, constants_dict)
t = show_motion[:show_nodes_plot, 0] - show_motion[0, 0]
animate_pendulum(t, sticks, belt_disp, filename=video_save_name)