Skip to content

Commit

Permalink
New attributes lb_opt_x and ub_opt_x are now considering scaling vari…
Browse files Browse the repository at this point in the history
…ables
  • Loading branch information
Felix-Mac committed Aug 17, 2022
1 parent a68607c commit 250605d
Show file tree
Hide file tree
Showing 3 changed files with 60 additions and 49 deletions.
28 changes: 14 additions & 14 deletions do_mpc/controller.py
Original file line number Diff line number Diff line change
Expand Up @@ -1081,32 +1081,32 @@ def _update_bounds(self):
"""
if self.cons_check_colloc_points: # Constraints for all collocation points.
# Dont bound the initial state
self.lb_opt_x['_x', 1:self.n_horizon] = self._x_lb.cat/self._x_scaling
self.ub_opt_x['_x', 1:self.n_horizon] = self._x_ub.cat/self._x_scaling
self.lb_opt_x['_x', 1:self.n_horizon] = self._x_lb.cat
self.ub_opt_x['_x', 1:self.n_horizon] = self._x_ub.cat

# Bounds for the algebraic variables:
self.lb_opt_x['_z'] = self._z_lb.cat/self._z_scaling
self.ub_opt_x['_z'] = self._z_ub.cat/self._z_scaling
self.lb_opt_x['_z'] = self._z_lb.cat
self.ub_opt_x['_z'] = self._z_ub.cat

# Terminal bounds
self.lb_opt_x['_x', self.n_horizon, :, -1] = self._x_terminal_lb.cat/self._x_scaling
self.ub_opt_x['_x', self.n_horizon, :, -1] = self._x_terminal_ub.cat/self._x_scaling
self.lb_opt_x['_x', self.n_horizon, :, -1] = self._x_terminal_lb.cat
self.ub_opt_x['_x', self.n_horizon, :, -1] = self._x_terminal_ub.cat
else: # Constraints only at the beginning of the finite Element
# Dont bound the initial state
self.lb_opt_x['_x', 1:self.n_horizon, :, -1] = self._x_lb.cat/self._x_scaling
self.ub_opt_x['_x', 1:self.n_horizon, :, -1] = self._x_ub.cat/self._x_scaling
self.lb_opt_x['_x', 1:self.n_horizon, :, -1] = self._x_lb.cat
self.ub_opt_x['_x', 1:self.n_horizon, :, -1] = self._x_ub.cat

# Bounds for the algebraic variables:
self.lb_opt_x['_z', :, :, 0] = self._z_lb.cat/self._z_scaling
self.ub_opt_x['_z', :, : ,0] = self._z_ub.cat/self._z_scaling
self.lb_opt_x['_z', :, :, 0] = self._z_lb.cat
self.ub_opt_x['_z', :, : ,0] = self._z_ub.cat

# Terminal bounds
self.lb_opt_x['_x', self.n_horizon, :, -1] = self._x_terminal_lb.cat/self._x_scaling
self.ub_opt_x['_x', self.n_horizon, :, -1] = self._x_terminal_ub.cat/self._x_scaling
self.lb_opt_x['_x', self.n_horizon, :, -1] = self._x_terminal_lb.cat
self.ub_opt_x['_x', self.n_horizon, :, -1] = self._x_terminal_ub.cat

# Bounds for the inputs along the horizon
self.lb_opt_x['_u'] = self._u_lb.cat/self._u_scaling
self.ub_opt_x['_u'] = self._u_ub.cat/self._u_scaling
self.lb_opt_x['_u'] = self._u_lb.cat
self.ub_opt_x['_u'] = self._u_ub.cat

# Bounds for the slack variables:
self.lb_opt_x['_eps'] = self._eps_lb.cat
Expand Down
33 changes: 19 additions & 14 deletions do_mpc/estimator.py
Original file line number Diff line number Diff line change
Expand Up @@ -1149,35 +1149,40 @@ def make_step(self, y0):

def _update_bounds(self):
"""Private method to update the bounds of the optimization variables based on the current values defined with :py:attr:`scaling`.
.. note::
Bounds are automatically scaled as they invoke the :py:attr:lb_opt_x` and :py:attr:`ub_opt_x` methods. Scaling is done automatically in these methods.
"""

if self.cons_check_colloc_points: # Constraints for all collocation points.
# Bounds for the states on all discretize values along the horizon
self.lb_opt_x['_x'] = self._x_lb.cat/self._x_scaling
self.ub_opt_x['_x'] = self._x_ub.cat/self._x_scaling
self.lb_opt_x['_x'] = self._x_lb.cat
self.ub_opt_x['_x'] = self._x_ub.cat

# Bounds for the algebraic states along the horizon
self.lb_opt_x['_z'] = self._z_lb.cat/self._z_scaling
self.ub_opt_x['_z'] = self._z_ub.cat/self._z_scaling
self.lb_opt_x['_z'] = self._z_lb.cat
self.ub_opt_x['_z'] = self._z_ub.cat
else: # Constraints only at the beginning of the finite Element
# Bounds for the states on all discretize values along the horizon
self.lb_opt_x['_x', 1:self.n_horizon, -1] = self._x_lb.cat/self._x_scaling
self.ub_opt_x['_x', 1:self.n_horizon, -1] = self._x_ub.cat/self._x_scaling
self.lb_opt_x['_x', 1:self.n_horizon, -1] = self._x_lb.cat
self.ub_opt_x['_x', 1:self.n_horizon, -1] = self._x_ub.cat

# Bounds for the algebraic states along the horizon
self.lb_opt_x['_z', :, 0] = self._z_lb.cat/self._z_scaling
self.ub_opt_x['_z', :, 0] = self._z_ub.cat/self._z_scaling
self.lb_opt_x['_z', :, 0] = self._z_lb.cat
self.ub_opt_x['_z', :, 0] = self._z_ub.cat

# Bounds for the inputs along the horizon
self.lb_opt_x['_u'] = self._u_lb.cat/self._u_scaling
self.ub_opt_x['_u'] = self._u_ub.cat/self._u_scaling
self.lb_opt_x['_u'] = self._u_lb.cat
self.ub_opt_x['_u'] = self._u_ub.cat

# Bounds for the slack variables along the horizon:
self.lb_opt_x['_eps'] = self._eps_lb.cat
self.ub_opt_x['_eps'] = self._eps_ub.cat

# Bounds for the inputs along the horizon
self.lb_opt_x['_p_est'] = self._p_est_lb.cat/self._p_est_scaling
self.ub_opt_x['_p_est'] = self._p_est_ub.cat/self._p_est_scaling
self.lb_opt_x['_p_est'] = self._p_est_lb.cat
self.ub_opt_x['_p_est'] = self._p_est_ub.cat

def _prepare_nlp(self):
"""Internal method. See detailed documentation in optimizer.prepare_nlp
Expand Down Expand Up @@ -1248,8 +1253,8 @@ def _prepare_nlp(self):

self.n_opt_aux = opt_aux.shape[0]

self.lb_opt_x = opt_x(-np.inf)
self.ub_opt_x = opt_x(np.inf)
self._lb_opt_x = opt_x(-np.inf)
self._ub_opt_x = opt_x(np.inf)

# Initialize objective function and constraints
obj = DM(0)
Expand Down
48 changes: 27 additions & 21 deletions do_mpc/optimizer.py
Original file line number Diff line number Diff line change
Expand Up @@ -229,55 +229,61 @@ def nlp_cons_ub(self, val):
assert self.flags['prepare_nlp'], 'Cannot query attribute prior to calling prepare_nlp or setup'
self._nlp_cons_ub = val

@property
def lb_opt_x(self):
@IndexedProperty
def lb_opt_x(self, ind):
"""Query and modify the lower bounds of all optimization variables :py:attr:`opt_x`.
This is a more advanced method of setting bounds on optimization variables of the MPC/MHE problem.
Users with less experience are advised to use :py:attr:`bounds` instead.
The attribute returns a nested structure that can be indexed using powerindexing. Please refer to :py:attr:`opt_x` for more details.
.. warning::
.. note::
This is a VERY low level feature and should be used with extreme caution.
It is easy to break the code.
The attribute automatically considers the scaling variables when setting the bounds. See :py:attr:`scaling` for more details.
.. note::
Modifications must be done after calling :py:meth:`prepare_nlp` or :py:meth:`setup` respectively.
"""
return self._lb_opt_x
return self._lb_opt_x[ind]

@lb_opt_x.setter
def lb_opt_x(self, val):
# Cannot overwrite this property
raise Exception('Cannot overwrite lb_opt_x')
def lb_opt_x(self, ind, val):
self._lb_opt_x[ind] = val
# Get canonical index
cind = self._lb_opt_x.f[ind]
# Modify the newly set values by considering the scaling variables. This requires the canonical index.
self._lb_opt_x.master[cind] = self._lb_opt_x.master[cind]/self.opt_x_scaling.master[cind]


@property
def ub_opt_x(self):
"""Query and modify the uppper bounds of all optimization variables :py:attr:`opt_x`.

@IndexedProperty
def ub_opt_x(self, ind):
"""Query and modify the lower bounds of all optimization variables :py:attr:`opt_x`.
This is a more advanced method of setting bounds on optimization variables of the MPC/MHE problem.
Users with less experience are advised to use :py:attr:`bounds` instead.
The attribute returns a nested structure that can be indexed using powerindexing. Please refer to :py:attr:`opt_x` for more details.
.. warning::
.. note::
This is a VERY low level feature and should be used with extreme caution.
It is easy to break the code.
The attribute automatically considers the scaling variables when setting the bounds. See :py:attr:`scaling` for more details.
.. note::
Modifications must be done after calling :py:meth:`prepare_nlp` or :py:meth:`setup` respectively.
"""
return self._ub_opt_x
return self._ub_opt_x[ind]

@ub_opt_x.setter
def ub_opt_x(self, val):
# Cannot overwrite this property
raise Exception('Cannot overwrite ub_opt_x')
def ub_opt_x(self, ind, val):
self._ub_opt_x[ind] = val
# Get canonical index
cind = self._ub_opt_x.f[ind]
# Modify the newly set values by considering the scaling variables. This requires the canonical index.
self._ub_opt_x.master[cind] = self._ub_opt_x.master[cind]/self.opt_x_scaling.master[cind]


@IndexedProperty
Expand Down Expand Up @@ -712,8 +718,8 @@ def solve(self):

solver_call_kwargs = {
'x0': self.opt_x_num,
'lbx': self.lb_opt_x,
'ubx': self.ub_opt_x,
'lbx': self._lb_opt_x,
'ubx': self._ub_opt_x,
'lbg': self.nlp_cons_lb,
'ubg': self.nlp_cons_ub,
'p': self.opt_p_num,
Expand Down

0 comments on commit 250605d

Please sign in to comment.