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 _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 _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 _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 __initialize_simulation(self, x0, time_points, udata, integrator_options_user):
self.__x0 = inputchecks.check_states_data(x0, self.__system.nx, 0)
time_points = inputchecks.check_time_points_input(time_points)
number_of_integration_steps = time_points.size - 1
time_steps = time_points[1:] - time_points[:-1]
udata = inputchecks.check_controls_data(udata, self.__system.nu, number_of_integration_steps)
self.__simulation_input = ci.vertcat([np.atleast_2d(time_steps), udata])
integrator_options = integrator_options_user.copy()
integrator_options.update({"t0": 0, "tf": 1}) # , "number_of_finite_elements": 1})
# integrator = ci.Integrator("integrator", "rk", \
integrator = ci.Integrator("integrator", "cvodes", self.__dae_scaled, integrator_options)
self.__simulation = integrator.mapaccum("simulation", number_of_integration_steps)
def __initialize_simulation(self, x0, time_points, udata, \
integrator_options_user):
self.__x0 = inputchecks.check_states_data(x0, self.__system.nx, 0)
time_points = inputchecks.check_time_points_input(time_points)
number_of_integration_steps = time_points.size - 1
time_steps = time_points[1:] - time_points[:-1]
udata = inputchecks.check_controls_data(udata, self.__system.nu, \
number_of_integration_steps)
self.__simulation_input = ci.vertcat([np.atleast_2d(time_steps), udata])
integrator_options = integrator_options_user.copy()
integrator_options.update({"t0": 0, "tf": 1, "expand": True}) # , "number_of_finite_elements": 1})
# integrator = ci.Integrator("integrator", "rk", \
integrator = ci.Integrator("integrator", "cvodes", \
self.__dae_scaled, integrator_options)
self.__simulation = integrator.mapaccum("simulation", \
number_of_integration_steps)
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,
])