class NLP:
def __init__(self):
super().__init__()
self.model = Hopper()
self.__setOptimizationParams__(total_duration=0.4, epsilon=0.)
self.opti = ca.Opti()
self.var_dt = False
self.__setVariables__()
self.__setConstraints__()
self.__setCosts__()
def __setOptimizationParams__(self, total_duration, epsilon):
self.T = total_duration
self.epsilon = epsilon
def __setVariables__(self):
if self.var_dt:
self.h = self.opti.variable(1)
self.opti.subject_to(self.opti.bounded(0.01, self.h,
self.model.dt))
# self.opti.subject_to(ca.sum1(self.h) == self.T / self.N)
self.opti.set_initial(self.h, 0.05)
else:
self.h = 0.05
self.N = int(self.T / self.h)
# self.states = []
# self.dstates = []
# self.actions = []
# self.forces, self.gammas = [], []
#
# for i in range(self.N):
# self.states.append(self.opti.variable(self.model.n_generalized, 1))
# self.dstates.append(self.opti.variable(self.model.n_generalized, 1))
#
# self.actions.append(self.opti.variable(self.model.dof, 1))
# self.forces.append(self.opti.variable(self.model.dims + 1, self.model.n_contact))
# self.gammas.append(self.opti.variable(1))
self.states = self.opti.variable(self.model.n_generalized, self.N)
self.dstates = self.opti.variable(self.model.n_generalized, self.N)
self.actions = self.opti.variable(self.model.dof, self.N)
self.forces, self.gammas = self.opti.variable(
3, self.N), self.opti.variable(1, self.N)
# self.start_state = ca.DM([0, (self.model.length[1, 0] * np.cos(np.pi/6) +
# self.model.length[2, 0]) * np.cos(np.pi/6), -np.pi/6, np.pi/6])
#
# self.end_state = ca.DM([2, (self.model.length[1, 0] * np.cos(np.pi/6) +
# self.model.length[2, 0]) * np.cos(np.pi/6), -np.pi/6, np.pi/6])
# self.start_state = ca.DM([0, (self.model.length[1, 0] +
# self.model.length[2, 0]), 0, 0])
#
# self.end_state = ca.DM([2, (self.model.length[1, 0] +
# self.model.length[2, 0]), 0, 0])
self.start_state = ca.DM(
[0, (self.model.length[1, 0] + self.model.length[2, 0]) * 0.7])
self.end_state = ca.DM(
[4, (self.model.length[1, 0] + self.model.length[2, 0]) * 0.7])
def __setConstraints__(self):
# self.opti.subject_to(self.states[:, 0] == self.start_state)
# self.opti.subject_to(self.states[:, -1] == self.end_state)
self.opti.subject_to(self.states[0:2, 0] == self.start_state)
self.opti.subject_to(self.states[0:2, -1] == self.end_state)
self.opti.subject_to(self.dstates[:,
0] == [0] * self.model.n_generalized)
self.opti.subject_to(self.dstates[:, -1] == [0] *
self.model.n_generalized)
for k in range(self.N - 1):
q_1, dq_1 = self.states[:, k], self.dstates[:, k]
q_2, dq_2 = self.states[:, k + 1], self.dstates[:, k + 1]
u_1, u_2 = self.actions[:, k], self.actions[:, k + 1]
lam_1, lam_2 = self.forces[:, k], self.forces[:, k + 1]
k_1 = self.model.kinematics(q=q_1)
k_2 = self.model.kinematics(q=q_2)
self.opti.subject_to(
k_1['p'][1, 1] >= self.model.terrain.heightMap(k_1['p'][0, 1]))
self.opti.subject_to(k_1['p'][1, 0] >= k_1['p'][1, 1])
self.opti.subject_to(k_1['p'][0, 1] <= q_1[0])
# if k == self.N/2:
# self.opti.subject_to(k_1['p'][1, 1] >= self.model.length[1, 0])
if k == 0:
self.opti.subject_to(
k_1['p'][1,
1] == self.model.terrain.heightMap(k_1['p'][0,
1]))
if k == self.N - 2:
self.opti.subject_to(
k_2['p'][1,
1] == self.model.terrain.heightMap(k_2['p'][0,
1]))
# self.opti.subject_to(k_1['p'][0, 1] <= q_1[0])
f_1 = self.model.dynamics(q=q_1, dq=dq_1, lam=lam_1)
f_2 = self.model.dynamics(q=q_2, dq=dq_2, lam=lam_2)
h = self.h
self.opti.subject_to(q_1 - q_2 + h * dq_2 == 0)
self.opti.subject_to(f_2['H'] @ (dq_2 - dq_1) - h *
(f_2['C'] @ dq_2 + f_2['G'] - f_2['B'] @ u_2 -
f_2['B_J_C'].T @ f_2['B_lam']) == 0)
# friction constraints
# self.opti.subject_to(lam_1 >= 0)
# self.opti.subject_to(f_1['phi'] >= 0)
# self.opti.subject_to(f_1['phi'].T @ lam_1 == 0)
lam_1_z, lam_1_xp, lam_1_xm = self.forces[2, k], self.forces[
0, k], self.forces[1, k]
gam_1 = self.gammas[k]
psi_1 = f_1['psi']
self.opti.subject_to(f_1['phi'] >= 0)
self.opti.subject_to(
[lam_1_z >= 0, lam_1_xp >= 0, lam_1_xm >= 0, gam_1 >= 0])
self.opti.subject_to(
self.model.terrain.mu * lam_1_z - lam_1_xp - lam_1_xm >= 0)
self.opti.subject_to(gam_1 + psi_1 >= 0)
self.opti.subject_to(gam_1 - psi_1 >= 0)
# self.opti.subject_to(f_1['phi'].T @ lam_1_z <= self.epsilon)
self.opti.subject_to(f_1['phi'].T @ lam_1_z == 0)
self.opti.subject_to((self.model.terrain.mu * lam_1_z - lam_1_xp -
lam_1_xm).T @ gam_1 == 0)
# self.opti.subject_to((self.model.terrain.mu * lam_1_z - lam_1_xp - lam_1_xm).T @ lam_1_z >= -self.epsilon)
# self.opti.subject_to((gam_1 + psi_1).T @ lam_1_xp <= self.epsilon)
self.opti.subject_to((gam_1 + psi_1).T @ lam_1_xp == 0)
# self.opti.subject_to((gam_1 - psi_1).T @ lam_1_xm <= self.epsilon)
self.opti.subject_to((gam_1 - psi_1).T @ lam_1_xm == 0)
########################
self.opti.subject_to(
self.opti.bounded(
self.start_state[0].full() * ca.MX.ones(1, self.N),
self.states[0, :],
self.end_state[0].full() * ca.MX.ones(1, self.N)))
# self.opti.subject_to(self.opti.bounded((-np.pi/2.5)*ca.MX.ones(1, self.N),
# self.states[2, :],
# ca.MX.zeros(1, self.N)))
# self.opti.subject_to(self.opti.bounded((np.pi/7)*ca.MX.ones(1, self.N),
# self.states[3, :],
# (2*np.pi/3)*ca.MX.ones(1, self.N)))
self.opti.subject_to(
self.opti.bounded(-15 * ca.MX.ones(2, self.N), self.actions,
15 * ca.MX.ones(2, self.N)))
def __setCosts__(self):
Q = ca.diag(ca.DM([0.1, 0.1, 10, 10]))
R = ca.diag(ca.DM([100, 100]))
cost = 0
for k in range(self.N):
q, dq = self.states[:, k], self.dstates[:, k]
u = self.actions[:, k]
# cost += dq.T @ Q @ dq + u.T @ R @ u
cost += (dq[2] * u[0])**2 + (dq[3] * u[1])**2
self.opti.minimize(cost)
def __solve__(self):
p_opts = {"expand": True}
s_opts = {"max_iter": 3000}
self.opti.solver("ipopt", p_opts, s_opts)
try:
self.solution = self.opti.solve_limited()
self.debug = False
except:
self.debug = True
def __interpolate__(self):
self.x_B = []
self.dx_B = []
self.z_B = []
self.dz_B = []
self.q_H = []
self.dq_H = []
self.q_K = []
self.dq_K = []
self.u_K = []
self.u_H = []
if self.debug:
if self.var_dt:
self.dt = self.opti.debug.value(self.h)
else:
self.dt = self.h
for i in range(self.N):
self.x_B.append(self.opti.debug.value(self.states[0, i]))
self.z_B.append(self.opti.debug.value(self.states[1, i]))
self.q_H.append(self.opti.debug.value(self.states[2, i]))
self.q_K.append(self.opti.debug.value(self.states[3, i]))
self.dx_B.append(self.opti.debug.value(self.dstates[0, i]))
self.dz_B.append(self.opti.debug.value(self.dstates[1, i]))
self.dq_H.append(self.opti.debug.value(self.dstates[2, i]))
self.dq_K.append(self.opti.debug.value(self.dstates[3, i]))
self.u_H.append(self.opti.debug.value(self.actions[0, i]))
self.u_K.append(self.opti.debug.value(self.actions[1, i]))
else:
if self.var_dt:
self.dt = self.solution.value(self.h)
else:
self.dt = self.h
for i in range(self.N):
self.x_B.append(self.solution.value(self.states[0, i]))
self.z_B.append(self.solution.value(self.states[1, i]))
self.q_H.append(self.solution.value(self.states[2, i]))
self.q_K.append(self.solution.value(self.states[3, i]))
self.dx_B.append(self.solution.value(self.dstates[0, i]))
self.dz_B.append(self.solution.value(self.dstates[1, i]))
self.dq_H.append(self.solution.value(self.dstates[2, i]))
self.dq_K.append(self.solution.value(self.dstates[3, i]))
self.u_H.append(self.solution.value(self.actions[0, i]))
self.u_K.append(self.solution.value(self.actions[1, i]))
self.k = self.model.dt / 8
self.t = np.linspace(0, self.N * self.dt,
int(self.N * self.dt / self.k))
print(self.dt)
self.t_points = np.linspace(0, self.N * self.dt, self.N)
x_B_spline_function = ca.interpolant('LUT', 'bspline', [self.t_points],
self.x_B)
self.x_B_spline = x_B_spline_function(self.t)
z_B_spline_function = ca.interpolant('LUT', 'bspline', [self.t_points],
self.z_B)
self.z_B_spline = z_B_spline_function(self.t)
q_H_spline_function = ca.interpolant('LUT', 'bspline', [self.t_points],
self.q_H)
self.q_H_spline = q_H_spline_function(self.t)
q_K_spline_function = ca.interpolant('LUT', 'bspline', [self.t_points],
self.q_K)
self.q_K_spline = q_K_spline_function(self.t)
dx_B_spline_function = ca.interpolant('LUT', 'bspline',
[self.t_points], self.dx_B)
self.dx_B_spline = dx_B_spline_function(self.t)
dz_B_spline_function = ca.interpolant('LUT', 'bspline',
[self.t_points], self.dz_B)
self.dz_B_spline = dz_B_spline_function(self.t)
dq_H_spline_function = ca.interpolant('LUT', 'bspline',
[self.t_points], self.dq_H)
self.dq_H_spline = dq_H_spline_function(self.t)
dq_K_spline_function = ca.interpolant('LUT', 'bspline',
[self.t_points], self.dq_K)
self.dq_K_spline = dq_K_spline_function(self.t)
self.__plot__()
def __plot__(self):
fig = plt.figure()
fig.tight_layout()
ax1 = fig.add_subplot(311)
ax1.grid()
ax1.plot(self.t_points, self.x_B, 'o', label='xB')
ax1.plot(self.t_points, self.z_B, 'o', label='zB')
# ax1.plot(self.t_points, self.q_H, 'o', label='qH')
# ax1.plot(self.t_points, self.q_K, 'o', label='qK')
ax1.plot(self.t, self.x_B_spline, '-', color='black')
ax1.plot(self.t, self.z_B_spline, '-', color='black')
# ax1.plot(self.t, self.q_H_spline, '-', color='black')
# ax1.plot(self.t, self.q_K_spline, '-', color='black')
ax1.legend()
ax2 = fig.add_subplot(312)
ax2.grid()
ax2.plot(self.t_points, self.dx_B, 'o', label='dxB')
ax2.plot(self.t_points, self.dz_B, 'o', label='dzB')
# ax2.plot(self.t_points, self.dq_H, 'o', label='dqH')
# ax2.plot(self.t_points, self.dq_K, 'o', label='dqK')
ax2.plot(self.t, self.dx_B_spline, '-', color='black')
ax2.plot(self.t, self.dz_B_spline, '-', color='black')
# ax2.plot(self.t, self.dq_H_spline, '-', color='black')
# ax2.plot(self.t, self.dq_K_spline, '-', color='black')
ax2.legend()
ax3 = fig.add_subplot(313)
ax3.grid()
ax3.plot(self.t_points, self.u_H, '-', label='u1')
ax3.plot(self.t_points, self.u_K, '-', label='u2')
ax3.legend()
plt.show()
self.model.visualize(self.x_B_spline.full(), self.z_B_spline.full(),
self.q_H_spline.full(), self.q_K_spline.full(),
self.t, self.k)
