def make_step_optimizer(self):
arg = self.optimizer.arg
U_offset = self.optimizer.nlp_dict_out['U_offset']
# Extract the optimal control input to be applied
nu = len(self.optimizer.u_mpc)
step_index = int(self.simulator.t0_sim /
self.simulator.t_step_simulator)
# open window during the night is forbiddden
for i in range(U_offset.shape[0]):
# window uppper bound is at postion 2 -> +2
arg['ubx'][U_offset[i][0] +
2] = self.optimizer.tv_p_values[step_index][-3, i]
result = self.optimizer.solver(x0=arg['x0'],
lbx=arg['lbx'],
ubx=arg['ubx'],
lbg=arg['lbg'],
ubg=arg['ubg'],
p=arg['p'])
# Store the full solution
self.optimizer.opt_result_step = data_do_mpc.opt_result(result)
# nu = len(self.optimizer.u_mpc)
# U_offset = self.optimizer.nlp_dict_out['U_offset']
v_opt = self.optimizer.opt_result_step.optimal_solution
self.optimizer.u_mpc = NP.resize(
NP.array(v_opt[U_offset[0][0]:U_offset[0][0] + nu]), (nu))
def make_step_optimizer(self):
arg = self.optimizer.arg
result = self.optimizer.solver(x0=arg['x0'], lbx=arg['lbx'], ubx=arg['ubx'], lbg=arg['lbg'], ubg=arg['ubg'], p = arg['p'])
# Store the full solution
self.optimizer.opt_result_step = data_do_mpc.opt_result(result)
# Extract the optimal control input to be applied
nu = len(self.optimizer.u_mpc)
U_offset = self.optimizer.nlp_dict_out['U_offset']
v_opt = self.optimizer.opt_result_step.optimal_solution
self.optimizer.u_mpc = NP.resize(NP.array(v_opt[U_offset[0][0]:U_offset[0][0]+nu]),(nu))
def make_step_optimizer(self):
arg = self.optimizer.arg
result = self.optimizer.solver(x0=arg['x0'], lbx=arg['lbx'], ubx=arg['ubx'], lbg=arg['lbg'], ubg=arg['ubg'], p = arg['p'])
# Store the full solution
self.optimizer.opt_result_step = data_do_mpc.opt_result(result)
# Extract the optimal control input to be applied
nu = len(self.optimizer.u_mpc)
U_offset = self.optimizer.nlp_dict_out['U_offset']
v_opt = self.optimizer.opt_result_step.optimal_solution
self.optimizer.u_mpc = NP.resize(NP.array(v_opt[U_offset[0][0]:U_offset[0][0]+nu]),(nu))
def make_step_optimizer(self):
arg = self.optimizer.arg
#print('#####################Intial guess' ,arg['x0'], '\n', self.optimizer.nlp_dict_out['X_offset'])
result = self.optimizer.solver(x0=arg['x0'],
lbx=arg['lbx'],
ubx=arg['ubx'],
lbg=arg['lbg'],
ubg=arg['ubg'],
p=arg['p']) ## change intial guess
time_now = datetime.now()
# Store the full solution
self.optimizer.opt_result_step = data_do_mpc.opt_result(result)
# Extract the optimal control input to be applied
nu = len(self.optimizer.u_mpc)
U_offset = self.optimizer.nlp_dict_out['U_offset']
v_opt = self.optimizer.opt_result_step.optimal_solution
self.optimizer.u_mpc = NP.resize(
NP.array(v_opt[U_offset[0][0]:U_offset[0][0] + nu]), (nu))
# Publish N_horizon Topic
nk = self.optimizer.n_horizon
t0 = 0.0
tf = self.optimizer.t_step * nk
tgrid = NP.linspace(t0, t0 + tf, nk + 1)
date_list = NP.array([time_now + timedelta(seconds=x) for x in tgrid])
self.horizon_1.horizon_msg.time_horizon.hr = NP.array(
[x.time().hour for x in date_list])
self.horizon_1.horizon_msg.time_horizon.min = NP.array(
[x.time().minute for x in date_list])
self.horizon_1.horizon_msg.time_horizon.sec = NP.array(
[x.time().second for x in date_list])
self.horizon_1.horizon_msg.time_horizon.msec = NP.array(
[x.time().microsecond for x in date_list])
self.horizon_1.horizon_msg.n_horizon = nk
X_offset = self.optimizer.nlp_dict_out['X_offset']
nx = self.model.x.size(1)
pp_horz = NP.array([0, 0, 0])
for v in range(X_offset.size):
pp_horz = NP.vstack([
pp_horz,
NP.resize(NP.array(v_opt[X_offset[v][0]:X_offset[v][0] + nx]),
(nx))
])
pp_horz = pp_horz[1:]
self.horizon_1.horizon_msg.x = pp_horz[:, 0]
self.horizon_1.horizon_msg.y = pp_horz[:, 1]
self.horizon_1.horizon_msg.theta = pp_horz[:, 1]
self.horizon_1.horizon_pub()
