def compute_Phi(self):
nlp = NonlinearProgram()
g = -gravity[2]
bc_integral_expr = 0.
Phi_i = 0. # Phi_0 = 0
Phi_1 = None
lambda_cost = 0.
lambda_f = self.omega_f**2
lambda_guess = lambda_f
lambda_prev = lambda_f
for i in xrange(self.N):
Phi_lb = self.s_sq[i + 1] * self.lambda_min
Phi_ub = self.s_sq[i + 1] * self.lambda_max
if i == self.N - 1:
if self.omega_i_min is not None:
Phi_lb = max(Phi_lb, self.omega_i_min**2)
if self.omega_i_max is not None:
Phi_ub = min(Phi_ub, self.omega_i_max**2)
Phi_next = nlp.new_variable(
'Phi_%d' % (i + 1), # from Phi_1 to Phi_N
dim=1,
init=[self.s_sq[i + 1] * lambda_guess],
lb=[Phi_lb],
ub=[Phi_ub])
if Phi_1 is None:
Phi_1 = Phi_next
bc_integral_expr += self.Delta[i] / (casadi_sqrt(Phi_next) +
casadi_sqrt(Phi_i))
lambda_i = (Phi_next - Phi_i) / self.Delta[i]
lambda_cost += ((lambda_i - lambda_prev))**2
lambda_prev = lambda_i
nlp.add_constraint(Phi_next - Phi_i,
lb=[self.Delta[i] * self.lambda_min],
ub=[self.Delta[i] * self.lambda_max])
Phi_i = Phi_next
Phi_N = Phi_next
bc_cvx_obj = bc_integral_expr - (self.z_bar / g) * casadi_sqrt(Phi_N)
nlp.add_equality_constraint(bc_cvx_obj, self.zd_bar / g)
nlp.add_equality_constraint(Phi_1, self.Delta[0] * lambda_f)
nlp.extend_cost(lambda_cost)
nlp.create_solver()
Phi_1_N = nlp.solve()
Phi = array([0.] + list(Phi_1_N)) # preprend Phi_0 = 0
if __debug__:
self.solve_times.append(nlp.solve_time)
self.optimal_found = nlp.optimal_found
return Phi
def compute_Phi(self):
nlp = NonlinearProgram()
bc_integral_expr = 0.
Phi_i = 0. # Phi_0 = 0
Phi_1 = None
lambda_cost = 0.
lambda_guess = self.omega_i ** 2
lambda_prev = self.omega_f ** 2
for i in range(self.N):
Phi_next = nlp.new_variable(
'Phi_%d' % (i + 1), # from Phi_1 to Phi_N
dim=1,
init=[self.s_sq[i + 1] * lambda_guess],
lb=[self.s_sq[i + 1] * self.lambda_min],
ub=[self.s_sq[i + 1] * self.lambda_max])
if Phi_1 is None:
Phi_1 = Phi_next
bc_integral_expr += self.Delta[i] / (
casadi_sqrt(Phi_next) + casadi_sqrt(Phi_i))
lambda_i = (Phi_next - Phi_i) / self.Delta[i]
lambda_cost += ((lambda_i - lambda_prev)) ** 2
lambda_prev = lambda_i
nlp.add_constraint(
Phi_next - Phi_i,
lb=[self.Delta[i] * self.lambda_min],
ub=[self.Delta[i] * self.lambda_max])
Phi_i = Phi_next
Phi_N = Phi_next
nlp.add_equality_constraint(bc_integral_expr, self.bc_integral)
nlp.add_equality_constraint(Phi_1, self.Delta[0] * self.omega_f ** 2)
nlp.add_equality_constraint(Phi_N, self.omega_i ** 2)
nlp.extend_cost(lambda_cost)
nlp.create_solver()
Phi_1_N = nlp.solve()
Phi = array([0.] + list(Phi_1_N)) # preprend Phi_0 = 0
if __debug__:
assert len(Phi) == self.N + 1
self.solve_times.append(nlp.solve_time)
self.optimal_found = nlp.optimal_found
return Phi