def _apply_controls_to_measurements(self, udata, qdata): udata = inputchecks.check_controls_data(udata, \ self._discretization.system.nu, \ self._discretization.number_of_controls) qdata = inputchecks.check_constant_controls_data(qdata, \ self._discretization.system.nq) optimization_variables_for_measurements = ci.veccat([ \ self._discretization.optimization_variables["U"], self._discretization.optimization_variables["Q"], self._discretization.optimization_variables["X"], self._discretization.optimization_variables["EPS_U"], self._discretization.optimization_variables["P"], ]) optimization_variables_controls_applied = ci.veccat([ \ udata, qdata, self._discretization.optimization_variables["X"], self._discretization.optimization_variables["EPS_U"], self._discretization.optimization_variables["P"], ]) measurements_fcn = ci.mx_function( \ "measurements_fcn", \ [optimization_variables_for_measurements], \ [self._discretization.measurements]) self._measurements_controls_applied = \ measurements_fcn(optimization_variables_controls_applied)
def _apply_controls_to_measurements(self, udata, qdata): udata = inputchecks.check_controls_data(udata, \ self._discretization.system.nu, \ self._discretization.number_of_controls) qdata = inputchecks.check_constant_controls_data(qdata, \ self._discretization.system.nq) optimization_variables_for_measurements = ci.veccat([ \ self._discretization.optimization_variables["U"], self._discretization.optimization_variables["Q"], self._discretization.optimization_variables["X"], self._discretization.optimization_variables["EPS_U"], self._discretization.optimization_variables["P"], ]) optimization_variables_controls_applied = ci.veccat([ \ udata, qdata, self._discretization.optimization_variables["X"], self._discretization.optimization_variables["EPS_U"], self._discretization.optimization_variables["P"], ]) measurements_fcn = ci.mx_function( \ "measurements_fcn", \ [optimization_variables_for_measurements], \ [self._discretization.measurements]) [self._measurements_controls_applied] = \ measurements_fcn([optimization_variables_controls_applied])
def _set_optimization_variables_lower_bounds(self, umin, qmin, xmin, x0): umin_user_provided = umin umin = inputchecks.check_controls_data(umin, \ self._discretization.system.nu, 1) if umin_user_provided is None: umin = -np.inf * np.ones(umin.shape) Umin = ci.repmat(umin, 1, \ self._discretization.optimization_variables["U"].shape[1]) qmin_user_provided = qmin qmin = inputchecks.check_constant_controls_data(qmin, \ self._discretization.system.nq) if qmin_user_provided is None: qmin = -np.inf * np.ones(qmin.shape) Qmin = qmin xmin_user_provided = xmin xmin = inputchecks.check_states_data(xmin, \ self._discretization.system.nx, 0) if xmin_user_provided is None: xmin = -np.inf * np.ones(xmin.shape) Xmin = ci.repmat(xmin, 1, \ self._discretization.optimization_variables["X"].shape[1]) Xmin[:,0] = x0 self._optimization_variables_lower_bounds = ci.veccat([ \ Umin, Qmin, Xmin, ])
def __generate_simulation_ode(self, pdata, qdata): p = inputchecks.check_parameter_data(pdata, self.__system.np) q = inputchecks.check_constant_controls_data(qdata, self.__system.nq) ode_fcn = ci.mx_function("ode_fcn", \ [self.__system.u, self.__system.q, self.__system.x, \ self.__system.eps_u, self.__system.p], \ [self.__system.f]) # Needs to be changes for allowance of explicit time dependecy! self.__ode_parameters_applied = ode_fcn.call([ \ self.__system.u, q, self.__system.x, \ np.zeros(self.__system.neps_u), p])[0]
def _set_optimization_variables_upper_bounds(self, umax, qmax, xmax, x0): umax_user_provided = umax umax = inputchecks.check_controls_data(umax, \ self._discretization.system.nu, 1) if umax_user_provided is None: umax = np.inf * np.ones(umax.shape) Umax = ci.repmat(umax, 1, \ self._discretization.optimization_variables["U"].shape[1]) qmax_user_provided = qmax qmax = inputchecks.check_constant_controls_data(qmax, \ self._discretization.system.nq) if qmax_user_provided is None: qmax = -np.inf * np.ones(qmax.shape) Qmax = qmax xmax_user_provided = xmax xmax = inputchecks.check_states_data(xmax, \ self._discretization.system.nx, 0) if xmax_user_provided is None: xmax = np.inf * np.ones(xmax.shape) Xmax = ci.repmat(xmax, 1, \ self._discretization.optimization_variables["X"].shape[1]) Xmax[:,0] = x0 self._optimization_variables_upper_bounds = ci.veccat([ \ Umax, Xmax, ])
def __generate_simulation_ode(self, pdata, qdata): p = inputchecks.check_parameter_data(pdata, self.__system.np) q = inputchecks.check_constant_controls_data(qdata, self.__system.nq) ode_fcn = ci.mx_function( "ode_fcn", [self.__system.u, self.__system.q, self.__system.x, self.__system.eps_u, self.__system.p], [self.__system.f], ) # Needs to be changes for allowance of explicit time dependency! self.__ode_parameters_applied = ode_fcn( [self.__system.u, q, self.__system.x, np.zeros(self.__system.neps_u), p] )[0]
def _set_optimization_variables_initials(self, qinit, x0, uinit): self.simulation = Simulation(self._discretization.system, \ self._pdata, qinit) self.simulation.run_system_simulation(x0, \ self._discretization.time_points, uinit, print_status = False) xinit = self.simulation.simulation_results repretitions_xinit = \ self._discretization.optimization_variables["X"][:,:-1].shape[1] / \ self._discretization.number_of_intervals Xinit = ci.repmat(xinit[:, :-1], repretitions_xinit, 1) Xinit = ci.horzcat([ \ Xinit.reshape((self._discretization.system.nx, \ Xinit.size() / self._discretization.system.nx)), xinit[:, -1], ]) uinit = inputchecks.check_controls_data(uinit, \ self._discretization.system.nu, \ self._discretization.number_of_intervals) Uinit = uinit qinit = inputchecks.check_constant_controls_data(qinit, \ self._discretization.system.nq) Qinit = qinit self._optimization_variables_initials = ci.veccat([ \ Uinit, Qinit, Xinit, ])
def _set_optimization_variables_initials(self, qinit, x0, uinit): self.simulation = Simulation(self._discretization.system, \ self._pdata, qinit) self.simulation.run_system_simulation(x0, \ self._discretization.time_points, uinit, print_status = False) xinit = self.simulation.simulation_results repretitions_xinit = \ self._discretization.optimization_variables["X"][:,:-1].shape[1] / \ self._discretization.number_of_intervals Xinit = ci.repmat(xinit[:, :-1], repretitions_xinit, 1) Xinit = ci.horzcat([ \ Xinit.reshape((self._discretization.system.nx, \ Xinit.numel() / self._discretization.system.nx)), xinit[:, -1], ]) uinit = inputchecks.check_controls_data(uinit, \ self._discretization.system.nu, \ self._discretization.number_of_intervals) Uinit = uinit qinit = inputchecks.check_constant_controls_data(qinit, \ self._discretization.system.nq) Qinit = qinit self._optimization_variables_initials = ci.veccat([ \ Uinit, Qinit, Xinit, ])