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 #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()