pybounds 0.0.4__py3-none-any.whl → 0.0.6__py3-none-any.whl

pybounds/drone_simulator.py

@@ -0,0 +1,422 @@
+
+import warnings
+import numpy as np
+import matplotlib.pyplot as plt
+import do_mpc
+from .util import FixedKeysDict, SetDict
+
+
+class Simulator(object):
+    def __init__(self, f, h, dt=0.01, n=None, m=None,
+                 state_names=None, input_names=None, measurement_names=None,
+                 params_simulator=None, mpc_horizon=10):
+
+        """ Simulator.
+
+        :param callable f: dynamics function f(X, U, t)
+        :param callable h: measurement function h(X, U, t)
+        :param float dt: sampling time in seconds
+        :param int n: number of states, optional but cannot be set if state_names is set
+        :param int m: number of inputs, optional but cannot be set if input_names is set
+        :param list state_names: names of states, optional but cannot be set if n is set
+        :param list input_names: names of inputs, optional but cannot be set if m is set
+        :param list measurement_names: names of measurements, optional
+        :param dict params_simulator: simulation parameters, optional
+        """
+
+        self.f = f
+        self.h = h
+        self.dt = dt
+
+        # Set state names
+        if state_names is None:  # default state names
+            if n is None:
+                raise ValueError('must set state_names or n')
+            else:
+                self.n = int(n)
+
+            self.state_names = ['x_' + str(n) for n in range(self.n)]
+        else:  # state names given
+            if n is not None:
+                raise ValueError('cannot set state_names and n')
+
+            self.state_names = list(state_names)
+            self.n = len(self.state_names)
+            # if len(self.state_names) != self.n:
+            #     raise ValueError('state_names must have length equal to x0')
+
+        # Set input names
+        if input_names is None:  # default input names
+            if m is None:
+                raise ValueError('must set input_names or m')
+            else:
+                self.m = int(m)
+
+            self.input_names = ['u_' + str(m) for m in range(self.m)]
+        else:  # input names given
+            if m is not None:
+                raise ValueError('cannot set in and n')
+
+            self.input_names = list(input_names)
+            self.m = len(self.input_names)
+            # if len(self.input_names) != self.m:
+            #     raise ValueError('input_names must have length equal to u0')
+
+        # Run measurement function to get measurement size
+        x0 = np.ones(self.n)
+        u0 = np.ones(self.m)
+        y = self.h(x0, u0)
+        self.p = len(y)  # number of measurements
+
+        # Set measurement names
+        if measurement_names is None:  # default measurement names
+            self.measurement_names = ['y_' + str(p) for p in range(self.p)]
+        else:
+            self.measurement_names = measurement_names
+            if len(self.measurement_names) != self.p:
+                raise ValueError('measurement_names must have length equal to y')
+
+        # Initialize time vector
+        self.w = 11  # initialize for w time-steps, but this can change later
+        self.time = np.arange(0, self.w * self.dt, step=self.dt)  # time vector
+
+        # Define initial states & initialize state time-series
+        self.x0 = {}
+        self.x = {}
+        for n, state_name in enumerate(self.state_names):
+            self.x0[state_name] = x0[n]
+            self.x[state_name] = x0[n] * np.ones(self.w)
+
+        self.x0 = FixedKeysDict(self.x0)
+        self.x = FixedKeysDict(self.x)
+
+        # Initialize input time-series
+        self.u = {}
+        for m, input_name in enumerate(self.input_names):
+            self.u[input_name] = u0[m] * np.ones(self.w)
+
+        self.u = FixedKeysDict(self.u)
+
+        # Initialize measurement time-series
+        self.y = {}
+        for p, measurement_name in enumerate(self.measurement_names):
+            self.y[measurement_name] = 0.0 * np.ones(self.w)
+
+        self.y = FixedKeysDict(self.y)
+
+        # Initialize state set-points
+        self.setpoint = {}
+        for n, state_name in enumerate(self.state_names):
+            self.setpoint[state_name] = 0.0 * np.ones(self.w)
+
+        self.setpoint = FixedKeysDict(self.setpoint)
+
+        # Define continuous-time MPC model
+        self.model = do_mpc.model.Model('continuous')
+
+        # Define state variables
+        X = []
+        for n, state_name in enumerate(self.state_names):
+            x = self.model.set_variable(var_type='_x', var_name=state_name, shape=(1, 1))
+            X.append(x)
+
+        # Define input variables
+        U = []
+        for m, input_name in enumerate(self.input_names):
+            u = self.model.set_variable(var_type='_u', var_name=input_name, shape=(1, 1))
+            U.append(u)
+
+        # Define dynamics
+        Xdot = self.f(X, U)
+        for n, state_name in enumerate(self.state_names):
+            self.model.set_rhs(state_name, Xdot[n])
+
+        # Add time-varying set-point variables for later use with MPC
+        for n, state_name in enumerate(self.state_names):
+            x = self.model.set_variable(var_type='_tvp', var_name=state_name + str('_set'), shape=(1, 1))
+
+        # Build model
+        self.model.setup()
+
+        # Define simulator & simulator parameters
+        self.simulator = do_mpc.simulator.Simulator(self.model)
+
+        # Set simulation parameters
+        if params_simulator is None:
+            self.params_simulator = {
+                'integration_tool': 'idas',  # cvodes, idas
+                'abstol': 1e-8,
+                'reltol': 1e-8,
+                't_step': self.dt
+            }
+        else:
+            self.params_simulator = params_simulator
+
+        self.simulator.set_param(**self.params_simulator)
+
+        # Setup MPC
+        self.mpc = do_mpc.controller.MPC(self.model)
+        self.mpc_horizon = mpc_horizon
+        setup_mpc = {
+            'n_horizon': self.mpc_horizon,
+            'n_robust': 0,
+            'open_loop': 0,
+            't_step': self.dt,
+            'state_discretization': 'collocation',
+            'collocation_type': 'radau',
+            'collocation_deg': 2,
+            'collocation_ni': 1,
+            'store_full_solution': False,
+
+            # Use MA27 linear solver in ipopt for faster calculations:
+            'nlpsol_opts': {'ipopt.linear_solver': 'mumps',  # mumps, MA27
+                            'ipopt.print_level': 0,
+                            'ipopt.sb': 'yes',
+                            'print_time': 0,
+                            }
+        }
+
+        self.mpc.set_param(**setup_mpc)
+
+        # Get template's for MPC time-varying parameters
+        self.mpc_tvp_template = self.mpc.get_tvp_template()
+        self.simulator_tvp_template = self.simulator.get_tvp_template()
+
+        # Set time-varying set-point functions
+        self.mpc.set_tvp_fun(self.mpc_tvp_function)
+        self.simulator.set_tvp_fun(self.simulator_tvp_function)
+
+        # Setup simulator
+        self.simulator.setup()
+
+    def simulator_tvp_function(self, t):
+        """ Set the set-point function for MPC simulator.
+        :param t: current time
+        """
+
+        mpc_horizon = self.mpc._settings.n_horizon
+
+        # Set current step index
+        k_step = int(np.round(t / self.dt))
+        if k_step >= mpc_horizon:  # point is beyond end of input data
+            k_step = mpc_horizon - 1  # set point beyond input data to last point
+
+        # Update current set-point
+        for n, state_name in enumerate(self.state_names):
+            self.simulator_tvp_template[state_name + '_set'] = self.setpoint[state_name][k_step]
+
+        return self.simulator_tvp_template
+
+    def mpc_tvp_function(self, t):
+        """ Set the set-point function for MPC optimizer.
+        """
+
+        mpc_horizon = self.mpc._settings.n_horizon
+
+        # Set current step index
+        k_step = int(np.round(t / self.dt))
+
+        # Update set-point time horizon
+        for k in range(mpc_horizon + 1):
+            k_set = k_step + k
+            if k_set >= self.w:  # horizon is beyond end of input data
+                k_set = self.w - 1  # set part of horizon beyond input data to last point
+
+            # Update each set-point over time horizon
+            for n, state_name in enumerate(self.state_names):
+                self.mpc_tvp_template['_tvp', k, state_name + '_set'] = self.setpoint[state_name][k_set]
+
+        return self.mpc_tvp_template
+
+    def set_initial_state(self, x0):
+        """ Update the initial state.
+        """
+
+        if x0 is not None:  # initial state given
+            if isinstance(x0, dict):  # in dict format
+                SetDict().set_dict_with_overwrite(self.x0, x0)  # update only the states in the dict given
+            elif isinstance(x0, list) or isinstance(x0, tuple) or (
+            x0, np.ndarray):  # list, tuple,  or numpy array format
+                x0 = np.array(x0).squeeze()
+                for n, key in enumerate(self.x0.keys()):  # each state
+                    self.x0[key] = x0[n]
+            else:
+                raise Exception('x0 must be either a dict, tuple, list, or numpy array')
+
+    def update_dict(self, data=None, name=None):
+        """ Update.
+        """
+
+        update = getattr(self, name)
+
+        if data is not None:  # data given
+            if isinstance(data, dict):  # in dict format
+                SetDict().set_dict_with_overwrite(update, data)  # update only the inputs in the dict given
+            elif isinstance(data, list) or isinstance(data, tuple):  # list or tuple format, each input vector in each element
+                for n, k in enumerate(update.keys()):  # each state
+                    update[k] = data[n]
+            elif isinstance(data, np.ndarray):  # numpy array format given as matrix where columns are the different inputs
+                if len(data.shape) <= 1:  # given as 1d array, so convert to column vector
+                    data = np.atleast_2d(data).T
+
+                for n, key in enumerate(update.keys()):  # each input
+                    update[key] = data[:, n]
+
+            else:
+                raise Exception(name + ' must be either a dict, tuple, list, or numpy array')
+
+        # Make sure inputs are the same size
+        points = np.array([update[key].shape[0] for key in update.keys()])
+        points_check = points == points[0]
+        if not np.all(points_check):
+            raise Exception(name + ' not the same size')
+
+    def simulate(self, x0=None, u=None, mpc=False, return_full_output=False):
+        """
+        Simulate the system.
+
+        :params x0: initial state dict or array
+        :params u: input dict or array
+        :params return_full_output: boolean to run (time, x, u, y) instead of y
+        """
+
+        if (mpc is True) and (u is not None):
+            raise Exception('u must be None if running MPC')
+
+        if (mpc is False) and (u is None):
+            warnings.warn('not running MPC or setting u directly')
+
+        # Update the initial state
+        if x0 is None:
+            if mpc:  # set the initial state to start at set-point if running MPC
+                x0 = {}
+                for state_name in self.state_names:
+                    x0[state_name] = self.setpoint[state_name][0]
+
+                self.set_initial_state(x0=x0)
+        else:
+            self.set_initial_state(x0=x0)
+
+        # Update the inputs
+        self.update_dict(u, name='u')
+
+        # Concatenate the inputs, where rows are individual inputs and columns are time-steps
+        if mpc:
+            self.w = np.vstack(list(self.setpoint.values())).shape[1]
+            u_sim = np.zeros((self.w, self.m))  # preallocate input array
+        else:
+            self.w = np.vstack(list(self.u.values())).shape[1]
+            u_sim = np.vstack(list(self.u.values())).T
+
+        # Update time vector
+        T = (self.w - 1) * self.dt
+        self.time = np.linspace(0, T, num=self.w)
+
+        # Set array to store simulated states, where rows are individual states and columns are time-steps
+        x_step = np.array(list(self.x0.values()))  # initialize state
+        x = np.nan * np.zeros((self.w, self.n))
+        x[0, :] = x_step.copy()
+
+        # Initialize the simulator
+        self.simulator.t0 = self.time[0]
+        self.simulator.x0 = x_step.copy()
+        self.simulator.set_initial_guess()
+
+        # Initialize MPC
+        if mpc:
+            self.mpc.setup()
+            self.mpc.t0 = self.time[0]
+            self.mpc.x0 = x_step.copy()
+            self.mpc.u0 = np.zeros((self.m, 1))
+            self.mpc.set_initial_guess()
+
+        # Run simulation
+        for k in range(1, self.w):
+            # Set input
+            if mpc:  # run MPC step
+                u_step = self.mpc.make_step(x_step)
+            else:  # use inputs directly
+                u_step = u_sim[k - 1:k, :].T
+
+            # Store inputs
+            u_sim[k - 1, :] = u_step.squeeze()
+
+            # Simulate one time step given current inputs
+            x_step = self.simulator.make_step(u_step)
+
+            # Store new states
+            x[k, :] = x_step.squeeze()
+
+        # Last input has no effect, so keep it the same as previous time-step
+        if mpc:
+            u_sim[-1, :] = u_sim[-2, :]
+
+        # Update the inputs
+        self.update_dict(u_sim, name='u')
+
+        # Update state trajectory
+        self.update_dict(x, name='x')
+
+        # Calculate measurements
+        x_list = list(self.x.values())
+        u_list = list(self.u.values())
+        y = self.h(x_list, u_list)
+
+        # Set measurements
+        self.update_dict(y, name='y')
+
+        # Return the measurements in array format
+        y_array = np.vstack(list(self.y.values())).T
+
+        if return_full_output:
+            return self.time.copy(), self.x.copy(), self.u.copy(), self.u.copy()
+        else:
+            return y_array
+
+    def get_time_states_inputs_measurements(self):
+        return self.time.copy(), self.x.copy(), self.u.copy(), self.u.copy()
+
+    def plot(self, name='x', dpi=150, plot_kwargs=None):
+        """ Plot states, inputs.
+        """
+
+        if plot_kwargs is None:
+            plot_kwargs = {
+                'color': 'black',
+                'linewidth': 2.0,
+                'linestyle': '-',
+                'marker': '.',
+                'markersize': 0
+            }
+
+            if name == 'x':
+                plot_kwargs['color'] = 'firebrick'
+            elif name == 'u':
+                plot_kwargs['color'] = 'royalblue'
+            elif name == 'y':
+                plot_kwargs['color'] = 'seagreen'
+            elif name == 'setpoint':
+                plot_kwargs['color'] = 'gray'
+
+        plot_dict = getattr(self, name)
+        plot_data = np.array(list(plot_dict.values()))
+        n = plot_data.shape[0]
+
+        fig, ax = plt.subplots(n, 1, figsize=(4, n * 1.5), dpi=dpi, sharex=True)
+        ax = np.atleast_1d(ax)
+
+        for n, key in enumerate(plot_dict.keys()):
+            ax[n].plot(self.time, plot_dict[key], label='set-point', **plot_kwargs)
+            ax[n].set_ylabel(key, fontsize=7)
+
+            # Also plot the states if plotting setpoint
+            if name == 'setpoint':
+                ax[n].plot(self.time, self.x[key], label=key, color='firebrick', linestyle='-', linewidth=0.5)
+                ax[n].legend(fontsize=6)
+
+        ax[-1].set_xlabel('time', fontsize=7)
+        ax[0].set_title(name, fontsize=8, fontweight='bold')
+
+
+        for a in ax.flat:
+            a.tick_params(axis='both', labelsize=6)

pybounds/observability.py

@@ -1,9 +1,10 @@
-
 import numpy as np
 import pandas as pd
-from multiprocessing import Pool
+# from multiprocessing import Pool
+# from pathos.multiprocessing import ProcessingPool as Pool
+from concurrent.futures import ThreadPoolExecutor
 import warnings
-import matplotlib as mpl
+# import matplotlib as mpl
 import matplotlib.pyplot as plt
 from mpl_toolkits.axes_grid1.inset_locator import inset_axes
 import sympy as sp
@@ -11,10 +12,10 @@ from .util import LatexStates
 
 
 class EmpiricalObservabilityMatrix:
-    def __init__(self, simulator, x0, time, u, eps=1e-5, parallel=False):
+    def __init__(self, simulator, x0, u, eps=1e-5, parallel=False):
         """ Construct an empirical observability matrix O.
 
-        :param callable Simulator: Simulator object : y = simulator(x0, u, **kwargs)
+        :param callable simulator: simulator object  that has a method y = simulator.simulate(x0, u, **kwargs)
             y is (w x p) array. w is the number of time-steps and p is the number of measurements
         :param dict/list/np.array x0: initial state for Simulator
         :param dict/np.array u: inputs array
@@ -24,7 +25,6 @@ class EmpiricalObservabilityMatrix:
 
         # Store inputs
         self.simulator = simulator
-        self.time = time.copy()
         self.eps = eps
         self.parallel = parallel
 
@@ -47,6 +47,9 @@ class EmpiricalObservabilityMatrix:
         # Number of outputs
         self.p = self.y_nominal.shape[1]
 
+        # Number of time-steps
+        self.w = self.y_nominal.shape[0]  # of points in time window
+
         # Check for state/measurement names
         if hasattr(self.simulator, 'state_names'):
             self.state_names = self.simulator.state_names
@@ -59,7 +62,6 @@ class EmpiricalObservabilityMatrix:
             self.measurement_names = ['y_' + str(p) for p in range(self.p)]
 
         # Perturbation amounts
-        self.w = len(self.time)  # of points in time window
         self.delta_x = eps * np.eye(self.n)  # perturbation amount for each state
         self.delta_y = np.zeros((self.p, self.n, self.w))  # preallocate delta_y
         self.y_plus = np.zeros((self.w, self.n, self.p))
@@ -92,8 +94,11 @@ class EmpiricalObservabilityMatrix:
         # Run simulations for perturbed initial conditions
         state_index = np.arange(0, self.n).tolist()
         if self.parallel:  # multiprocessing
-            with Pool(4) as pool:
-                results = pool.map(self.simulate, state_index)
+            # with Pool(4) as pool:
+            #     results = pool.map(self.simulate, state_index)
+
+            with ThreadPoolExecutor() as executor:
+                results = list(executor.map(self.simulate, state_index))
 
             for n, r in enumerate(results):
                 delta_y, y_plus, y_minus = r
@@ -147,10 +152,10 @@ class SlidingEmpiricalObservabilityMatrix:
     def __init__(self, simulator, t_sim, x_sim, u_sim, w=None, eps=1e-5,
                  parallel_sliding=False, parallel_perturbation=False):
         """ Construct empirical observability matrix O in sliding windows along a trajectory.
-        
+
         :param callable simulator: Simulator object : y = simulator(x0, u, **kwargs)
             y is (w x p) array. w is the number of time-steps and p is the number of measurements
-        :param np.array t_sim: time vector size N
+        :param np.array t_sim: time values along state trajectory array (N, 1)
         :param np.array x_sim: state trajectory array (N, n), can also be dict
         :param np.array u_sim: input array (N, m), can also be dict
         :param np.array w: window size for O calculations, will automatically set how many windows to compute
@@ -201,7 +206,7 @@ class SlidingEmpiricalObservabilityMatrix:
             raise ValueError('window size must be smaller than trajectory length')
 
         # All the indices to calculate O
-        self.O_index = np.arange(0, self.N - self.w + 1,  step=1)  # indices to compute O
+        self.O_index = np.arange(0, self.N - self.w + 1, step=1)  # indices to compute O
         self.O_time = self.t_sim[self.O_index]  # times to compute O
         self.n_point = len(self.O_index)  # # of times to calculate O
 
@@ -229,8 +234,12 @@ class SlidingEmpiricalObservabilityMatrix:
         # Construct O's
         n_point_range = np.arange(0, self.n_point).astype(int)
         if self.parallel_sliding:  # multiprocessing
-            with Pool(4) as pool:
-                results = pool.map(self.construct, n_point_range)
+            # with Pool(4) as pool:
+            #     results = pool.map(self.construct, n_point_range)
+
+            with ThreadPoolExecutor(max_workers=12) as executor:
+                results = list(executor.map(self.construct, n_point_range))
+
                 for r in results:
                     self.O_sliding.append(r[0])
                     self.O_df_sliding.append(r[1])
@@ -262,7 +271,8 @@ class SlidingEmpiricalObservabilityMatrix:
         u_win = self.u_sim[win, :]  # inputs in window
 
         # Calculate O for window
-        EOM = EmpiricalObservabilityMatrix(self.simulator, x0, t_win0, u_win, eps=self.eps, parallel=self.parallel_perturbation)
+        EOM = EmpiricalObservabilityMatrix(self.simulator, x0, u_win, eps=self.eps,
+                                           parallel=self.parallel_perturbation)
         self.EOM = EOM
 
         # Store data

pybounds/simulator.py

@@ -1,5 +1,7 @@
 
+import warnings
 import numpy as np
+import matplotlib.pyplot as plt
 import do_mpc
 from .util import FixedKeysDict, SetDict
 
@@ -7,7 +9,7 @@ from .util import FixedKeysDict, SetDict
 class Simulator(object):
     def __init__(self, f, h, dt=0.01, n=None, m=None,
                  state_names=None, input_names=None, measurement_names=None,
-                 params_simulator=None):
+                 params_simulator=None, mpc_horizon=10):
 
         """ Simulator.
 
@@ -63,7 +65,7 @@ class Simulator(object):
         # Run measurement function to get measurement size
         x0 = np.ones(self.n)
         u0 = np.ones(self.m)
-        y = self.h(x0, u0, 0)
+        y = self.h(x0, u0)
         self.p = len(y)  # number of measurements
 
         # Set measurement names
@@ -75,32 +77,40 @@ class Simulator(object):
                 raise ValueError('measurement_names must have length equal to y')
 
         # Initialize time vector
-        w = 10  # initialize for w time-steps, but this can change later
-        self.time = np.arange(0, w * self.dt + self.dt / 2, step=self.dt)  # time vector
+        self.w = 11  # initialize for w time-steps, but this can change later
+        self.time = np.arange(0, self.w * self.dt, step=self.dt)  # time vector
 
         # Define initial states & initialize state time-series
         self.x0 = {}
         self.x = {}
         for n, state_name in enumerate(self.state_names):
             self.x0[state_name] = x0[n]
-            self.x[state_name] = x0[n] * np.ones(w)
+            self.x[state_name] = x0[n] * np.ones(self.w)
 
         self.x0 = FixedKeysDict(self.x0)
+        self.x = FixedKeysDict(self.x)
 
         # Initialize input time-series
         self.u = {}
         for m, input_name in enumerate(self.input_names):
-            self.u[input_name] = u0[m] * np.ones(w)
+            self.u[input_name] = u0[m] * np.ones(self.w)
 
         self.u = FixedKeysDict(self.u)
 
         # Initialize measurement time-series
         self.y = {}
         for p, measurement_name in enumerate(self.measurement_names):
-            self.y[measurement_name] = 0.0 * np.ones(w)
+            self.y[measurement_name] = 0.0 * np.ones(self.w)
 
         self.y = FixedKeysDict(self.y)
 
+        # Initialize state set-points
+        self.setpoint = {}
+        for n, state_name in enumerate(self.state_names):
+            self.setpoint[state_name] = 0.0 * np.ones(self.w)
+
+        self.setpoint = FixedKeysDict(self.setpoint)
+
         # Define continuous-time MPC model
         self.model = do_mpc.model.Model('continuous')
 
@@ -117,10 +127,14 @@ class Simulator(object):
             U.append(u)
 
         # Define dynamics
-        Xdot = self.f(X, U, 0)
+        Xdot = self.f(X, U)
         for n, state_name in enumerate(self.state_names):
             self.model.set_rhs(state_name, Xdot[n])
 
+        # Add time-varying set-point variables for later use with MPC
+        for n, state_name in enumerate(self.state_names):
+            x = self.model.set_variable(var_type='_tvp', var_name=state_name + str('_set'), shape=(1, 1))
+
         # Build model
         self.model.setup()
 
@@ -139,8 +153,81 @@ class Simulator(object):
             self.params_simulator = params_simulator
 
         self.simulator.set_param(**self.params_simulator)
+
+        # Setup MPC
+        self.mpc = do_mpc.controller.MPC(self.model)
+        self.mpc_horizon = mpc_horizon
+        setup_mpc = {
+            'n_horizon': self.mpc_horizon,
+            'n_robust': 0,
+            'open_loop': 0,
+            't_step': self.dt,
+            'state_discretization': 'collocation',
+            'collocation_type': 'radau',
+            'collocation_deg': 2,
+            'collocation_ni': 1,
+            'store_full_solution': False,
+
+            # Use MA27 linear solver in ipopt for faster calculations:
+            'nlpsol_opts': {'ipopt.linear_solver': 'mumps',  # mumps, MA27
+                            'ipopt.print_level': 0,
+                            'ipopt.sb': 'yes',
+                            'print_time': 0,
+                            }
+        }
+
+        self.mpc.set_param(**setup_mpc)
+
+        # Get template's for MPC time-varying parameters
+        self.mpc_tvp_template = self.mpc.get_tvp_template()
+        self.simulator_tvp_template = self.simulator.get_tvp_template()
+
+        # Set time-varying set-point functions
+        self.mpc.set_tvp_fun(self.mpc_tvp_function)
+        self.simulator.set_tvp_fun(self.simulator_tvp_function)
+
+        # Setup simulator
         self.simulator.setup()
 
+    def simulator_tvp_function(self, t):
+        """ Set the set-point function for MPC simulator.
+        :param t: current time
+        """
+
+        mpc_horizon = self.mpc._settings.n_horizon
+
+        # Set current step index
+        k_step = int(np.round(t / self.dt))
+        if k_step >= mpc_horizon:  # point is beyond end of input data
+            k_step = mpc_horizon - 1  # set point beyond input data to last point
+
+        # Update current set-point
+        for n, state_name in enumerate(self.state_names):
+            self.simulator_tvp_template[state_name + '_set'] = self.setpoint[state_name][k_step]
+
+        return self.simulator_tvp_template
+
+    def mpc_tvp_function(self, t):
+        """ Set the set-point function for MPC optimizer.
+        """
+
+        mpc_horizon = self.mpc._settings.n_horizon
+
+        # Set current step index
+        k_step = int(np.round(t / self.dt))
+
+        # Update set-point time horizon
+        for k in range(mpc_horizon + 1):
+            k_set = k_step + k
+            if k_set >= self.w:  # horizon is beyond end of input data
+                k_set = self.w - 1  # set part of horizon beyond input data to last point
+
+            # Update each set-point over time horizon
+            for n, state_name in enumerate(self.state_names):
+                self.mpc_tvp_template['_tvp', k, state_name + '_set'] = self.setpoint[state_name][k_set]
+
+        return self.mpc_tvp_template
+
     def set_initial_state(self, x0):
         """ Update the initial state.
         """
@@ -156,33 +243,35 @@ class Simulator(object):
             else:
                 raise Exception('x0 must be either a dict, tuple, list, or numpy array')
 
-    def set_inputs(self, u):
-        """ Update the inputs.
+    def update_dict(self, data=None, name=None):
+        """ Update.
         """
 
-        if u is not None:  # inputs given
-            if isinstance(u, dict):  # in dict format
-                SetDict().set_dict_with_overwrite(self.u, u)  # update only the inputs in the dict given
-            elif isinstance(u, list) or isinstance(u, tuple):  # list or tuple format, each input vector in each element
-                for n, k in enumerate(self.u.keys()):  # each input
-                    self.u[k] = u[n]
-            elif isinstance(u, np.ndarray):  # numpy array format given as matrix where columns are the different inputs
-                if len(u.shape) <= 1:  # given as 1d array, so convert to column vector
-                    u = np.atleast_2d(u).T
+        update = getattr(self, name)
+
+        if data is not None:  # data given
+            if isinstance(data, dict):  # in dict format
+                SetDict().set_dict_with_overwrite(update, data)  # update only the inputs in the dict given
+            elif isinstance(data, list) or isinstance(data, tuple):  # list or tuple format, each input vector in each element
+                for n, k in enumerate(update.keys()):  # each state
+                    update[k] = data[n]
+            elif isinstance(data, np.ndarray):  # numpy array format given as matrix where columns are the different inputs
+                if len(data.shape) <= 1:  # given as 1d array, so convert to column vector
+                    data = np.atleast_2d(data).T
 
-                for m, key in enumerate(self.u.keys()):  # each input
-                    self.u[key] = u[:, m]
+                for n, key in enumerate(update.keys()):  # each input
+                    update[key] = data[:, n]
 
             else:
-                raise Exception('u must be either a dict, tuple, list, or numpy array')
+                raise Exception(name + ' must be either a dict, tuple, list, or numpy array')
 
         # Make sure inputs are the same size
-        points = np.array([self.u[key].shape[0] for key in self.u.keys()])
+        points = np.array([update[key].shape[0] for key in update.keys()])
         points_check = points == points[0]
         if not np.all(points_check):
-            raise Exception('inputs are not the same size')
+            raise Exception(name + ' not the same size')
 
-    def simulate(self, x0=None, u=None, return_full_output=False):
+    def simulate(self, x0=None, u=None, mpc=False, return_full_output=False):
         """
         Simulate the system.
 
@@ -191,34 +280,63 @@ class Simulator(object):
         :params return_full_output: boolean to run (time, x, u, y) instead of y
         """
 
+        if (mpc is True) and (u is not None):
+            raise Exception('u must be None if running MPC')
+
+        if (mpc is False) and (u is None):
+            warnings.warn('not running MPC or setting u directly')
+
         # Update the initial state
-        self.set_initial_state(x0=x0.copy())
+        if x0 is None:
+            if mpc:  # set the initial state to start at set-point if running MPC
+                x0 = {}
+                for state_name in self.state_names:
+                    x0[state_name] = self.setpoint[state_name][0]
+
+                self.set_initial_state(x0=x0)
+        else:
+            self.set_initial_state(x0=x0)
 
         # Update the inputs
-        self.set_inputs(u=u)
+        self.update_dict(u, name='u')
 
         # Concatenate the inputs, where rows are individual inputs and columns are time-steps
-        u_sim = np.vstack(list(self.u.values())).T
-        n_point = u_sim.shape[0]
+        if mpc:
+            self.w = np.vstack(list(self.setpoint.values())).shape[1]
+            u_sim = np.zeros((self.w, self.m))  # preallocate input array
+        else:
+            self.w = np.vstack(list(self.u.values())).shape[1]
+            u_sim = np.vstack(list(self.u.values())).T
 
         # Update time vector
-        T = (n_point - 1) * self.dt
-        self.time = np.linspace(0, T, num=n_point)
+        T = (self.w - 1) * self.dt
+        self.time = np.linspace(0, T, num=self.w)
 
         # Set array to store simulated states, where rows are individual states and columns are time-steps
         x_step = np.array(list(self.x0.values()))  # initialize state
-        x_sim = np.nan * np.zeros((n_point, self.n))
-        x_sim[0, :] = x_step.copy()
+        x = np.nan * np.zeros((self.w, self.n))
+        x[0, :] = x_step.copy()
 
         # Initialize the simulator
         self.simulator.t0 = self.time[0]
         self.simulator.x0 = x_step.copy()
         self.simulator.set_initial_guess()
 
+        # Initialize MPC
+        if mpc:
+            self.mpc.setup()
+            self.mpc.t0 = self.time[0]
+            self.mpc.x0 = x_step.copy()
+            self.mpc.u0 = np.zeros((self.m, 1))
+            self.mpc.set_initial_guess()
+
         # Run simulation
-        for k in range(1, n_point):
+        for k in range(1, self.w):
             # Set input
-            u_step = u_sim[k - 1:k, :].T
+            if mpc:  # run MPC step
+                u_step = self.mpc.make_step(x_step)
+            else:  # use inputs directly
+                u_step = u_sim[k - 1:k, :].T
 
             # Store inputs
             u_sim[k - 1, :] = u_step.squeeze()
@@ -227,34 +345,87 @@ class Simulator(object):
             x_step = self.simulator.make_step(u_step)
 
             # Store new states
-            x_sim[k, :] = x_step.squeeze()
+            x[k, :] = x_step.squeeze()
+
+        # Last input has no effect, so keep it the same as previous time-step
+        if mpc:
+            u_sim[-1, :] = u_sim[-2, :]
 
         # Update the inputs
-        self.set_inputs(u=u_sim)
+        self.update_dict(u_sim, name='u')
 
         # Update state trajectory
-        for n, key in enumerate(self.x.keys()):
-            self.x[key] = x_sim[:, n]
+        self.update_dict(x, name='x')
 
         # Calculate measurements
         x_list = list(self.x.values())
         u_list = list(self.u.values())
-        y = self.h(x_list, u_list, 0)
+        y = self.h(x_list, u_list)
 
-        # Set outputs
-        self.y = {}
-        for p, measurement_name in enumerate(self.measurement_names):
-            self.y[measurement_name] = y[p]
+        # Set measurements
+        self.update_dict(y, name='y')
 
-        # Return the outputs in array format
+        # Return the measurements in array format
         y_array = np.vstack(list(self.y.values())).T
 
         if return_full_output:
-            return self.time.copy(), self.x.copy(), self.u.copy(), self.u.copy()
+            return self.time.copy(), self.x.copy(), self.u.copy(), self.y.copy()
         else:
             return y_array
 
-    def get_time_states_input_measurements(self):
+    def get_time_states_inputs_measurements(self):
         return self.time.copy(), self.x.copy(), self.u.copy(), self.u.copy()
 
+    def plot(self, name='x', dpi=150, plot_kwargs=None):
+        """ Plot states, inputs.
+        """
+
+        if plot_kwargs is None:
+            plot_kwargs = {
+                'color': 'black',
+                'linewidth': 2.0,
+                'linestyle': '-',
+                'marker': '.',
+                'markersize': 0
+            }
 
+            if name == 'x':
+                plot_kwargs['color'] = 'firebrick'
+            elif name == 'u':
+                plot_kwargs['color'] = 'royalblue'
+            elif name == 'y':
+                plot_kwargs['color'] = 'seagreen'
+            elif name == 'setpoint':
+                plot_kwargs['color'] = 'gray'
+
+        plot_dict = getattr(self, name)
+        plot_data = np.array(list(plot_dict.values()))
+        n = plot_data.shape[0]
+
+        fig, ax = plt.subplots(n, 1, figsize=(4, n * 1.5), dpi=dpi, sharex=True)
+        ax = np.atleast_1d(ax)
+
+        for n, key in enumerate(plot_dict.keys()):
+            ax[n].plot(self.time, plot_dict[key], label=name, **plot_kwargs)
+            ax[n].set_ylabel(key, fontsize=7)
+
+            # Also plot the states if plotting setpoint
+            if name == 'setpoint':
+                ax[n].plot(self.time, self.x[key], label=key, color='firebrick', linestyle='-', linewidth=0.5)
+                ax[n].legend(fontsize=6)
+
+                y = self.x[key]
+                y_min = np.min(y)
+                y_max = np.max(y)
+                delta = y_max - y_min
+                if np.abs(delta) < 0.01:
+                    margin = 0.1
+                    ax[n].set_ylim(y_min - margin, y_max + margin)
+                else:
+                    margin = 0.0
+
+        ax[-1].set_xlabel('time', fontsize=7)
+        ax[0].set_title(name, fontsize=8, fontweight='bold')
+
+        for a in ax.flat:
+            a.tick_params(axis='both', labelsize=6)

pybounds/util.py

@@ -1,9 +1,9 @@
-
 import numpy as np
 import matplotlib.pyplot as plt
 import matplotlib.collections as mcoll
 import matplotlib.patheffects as path_effects
 
+
 class FixedKeysDict(dict):
     def __init__(self, *args, **kwargs):
         super(FixedKeysDict, self).__init__(*args, **kwargs)
@@ -74,6 +74,7 @@ class LatexStates:
                      'v_perp': r'$v_{\perp}$',
                      'phi': r'$\phi$',
                      'phidot': r'$\dot{\phi}$',
+                     'phi_dot': r'$\dot{\phi}$',
                      'phiddot': r'$\ddot{\phi}$',
                      'w': r'$w$',
                      'zeta': r'$\zeta$',
@@ -95,9 +96,46 @@ class LatexStates:
                      'v_para_dot': r'$\dot{v_{\parallel}}$',
                      'v_perp_dot': r'$\dot{v_{\perp}}$',
                      'v_para_dot_ratio': r'$\frac{\Delta v_{\parallel}}{v_{\parallel}}$',
-                     'sigma': r'$\sigma$',
-                     'rho': r'$\rho$',
-                     'beta': r'$\beta$'
+                     'x':  r'$x$',
+                     'y':  r'$y$',
+                     'v_x': r'$v_{x}$',
+                     'v_y': r'$v_{y}$',
+                     'v_z': r'$v_{z}$',
+                     'w_x': r'$w_{x}$',
+                     'w_y': r'$w_{y}$',
+                     'w_z': r'$w_{z}$',
+                     'a_x': r'$a_{x}$',
+                     'a_y': r'$a_{y}$',
+                     'vx': r'$v_x$',
+                     'vy': r'$v_y$',
+                     'vz': r'$v_z$',
+                     'wx': r'$w_x$',
+                     'wy': r'$w_y$',
+                     'wz': r'$w_z$',
+                     'ax': r'$ax$',
+                     'ay': r'$ay$',
+                     'beta': r'$\beta',
+                     'thetadot': r'$\dot{\theta}$',
+                     'theta_dot': r'$\dot{\theta}$',
+                     'psidot': r'$\dot{\psi}$',
+                     'psi_dot': r'$\dot{\psi}$',
+                     'theta': r'$\theta$',
+                     'Yaw': r'$\psi$',
+                     'R': r'$\phi$',
+                     'P': r'$\theta$',
+                     'dYaw': r'$\dot{\psi}$',
+                     'dP': r'$\dot{\theta}$',
+                     'dR': r'$\dot{\phi}$',
+                     'acc_x': r'$\dot{v}x$',
+                     'acc_y': r'$\dot{v}y$',
+                     'acc_z': r'$\dot{v}z$',
+                     'Psi': r'$\Psi$',
+                     'Ix': r'$I_x$',
+                     'Iy': r'$I_y$',
+                     'Iz': r'$I_z$',
+                     'Jr': r'$J_r$',
+                     'Dl': r'$D_l$',
+                     'Dr': r'$D_r$',
                      }
 
         if dict is not None:
@@ -159,4 +197,4 @@ def colorline(x, y, z, ax=None, cmap=plt.get_cmap('copper'), norm=None, linewidt
 
     ax.add_collection(lc)
 
-    return lc
+    return lc

{pybounds-0.0.4.dist-info → pybounds-0.0.6.dist-info}/METADATA

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: pybounds
-Version: 0.0.4
+Version: 0.0.6
 Summary: Bounding Observability for Uncertain Nonlinear Dynamics Systems (BOUNDS)
 Home-page: https://pypi.org/project/pybounds/
 Author: Ben Cellini, Burak Boyacioglu, Floris van Breugel
@@ -16,16 +16,9 @@ License-File: LICENSE
 
 Python implementation of BOUNDS: Bounding Observability for Uncertain Nonlinear Dynamic Systems.
 
-<p align="center">
-  <a href="https://pynumdiff.readthedocs.io/en/master/" target="_blank" >
-
-[//]: # (    <img alt="Python for Numerical Differentiation of noisy time series data" src="docs/source/_static/logo_PyNumDiff.png" width="300" height="200" />)
-  </a>
-</p>
-
 <p align="center">
     <a href="https://pypi.org/project/pybounds/">
-        <img src="https://badge.fury.io/py/pynumdiff.svg" alt="PyPI version" height="18"></a>
+        <img src="https://badge.fury.io/py/pybounds.svg" alt="PyPI version" height="18"></a>
 </p>
 
 ## Introduction
@@ -42,14 +35,17 @@ pip install pybounds
 ```
 
 ## Notebook examples
-There is currently one simple example notebook. More to come.
+For a simple system
 *  Monocular camera with optic fow measurements: [mono_camera_example.ipynb](examples%2Fmono_camera_example.ipynb)
 
+For a more complex system
+*  Fly-wind: [fly_wind_example.ipynb](examples%2Ffly_wind_example.ipynb)
+
 ## Citation
 
 If you use the code or methods from this package, please cite the following paper:
 
-Benjamin Cellini, Burak Boyacıoğlu, Stanley David Stupski, and Floris van Breugel. Discovering and exploiting active sensing motifs for estimation with empirical observability. (2024) bioRxiv.
+Benjamin Cellini, Burak Boyacioglu, Stanley David Stupski, and Floris van Breugel. Discovering and exploiting active sensing motifs for estimation with empirical observability. (2024) bioRxiv.
 
 ## Related packages
 This repository is the evolution of the EISO repo (https://github.com/BenCellini/EISO), and is intended as a companion to the repository directly associated with the paper above.

pybounds-0.0.6.dist-info/RECORD

@@ -0,0 +1,10 @@
+pybounds/__init__.py,sha256=so9LuRNw2V8MGsDs-RPstGGgJtBFp2YGolQbS0rVfhw,348
+pybounds/drone_simulator.py,sha256=-CrQVpNfiBDFECd6H7FU5has4sYGW1gyS2RhOgXUqZg,15858
+pybounds/observability.py,sha256=4tdK6AK678zoorbkQ2psvzMRLY32CIj2QwVb-0w-GXk,27541
+pybounds/simulator.py,sha256=ReaCRHA-DjiE1EbmCStw2Top9EyJeg41S_lO-iqnjv4,16194
+pybounds/util.py,sha256=Gs0UgqgLXTJI9FZww90iJhqU02iJ31bXBURjGiq3YzM,7401
+pybounds-0.0.6.dist-info/LICENSE,sha256=kqeyRXtRGgBVZdXYeIX4zR9l2KZ2rqIBVEiPMTjxjcI,1093
+pybounds-0.0.6.dist-info/METADATA,sha256=MAp33xdjWLOfB17N32E-GdKRZnaCHtIwBc65XmFQXb4,2155
+pybounds-0.0.6.dist-info/WHEEL,sha256=GJ7t_kWBFywbagK5eo9IoUwLW6oyOeTKmQ-9iHFVNxQ,92
+pybounds-0.0.6.dist-info/top_level.txt,sha256=V-ofnWE3m_UkXTXJwNRD07n14m5R6sc6l4NadaCCP_A,9
+pybounds-0.0.6.dist-info/RECORD,,

pybounds-0.0.4.dist-info/RECORD

@@ -1,9 +0,0 @@
-pybounds/__init__.py,sha256=so9LuRNw2V8MGsDs-RPstGGgJtBFp2YGolQbS0rVfhw,348
-pybounds/observability.py,sha256=OqJ7fZmflvgZ9dQprIkZ3aahmvwLKVa-5RRHGpnv_gI,27058
-pybounds/simulator.py,sha256=GiQHDGkRtiUsMiiNB8tLBXbG2d5UQJwa7WvXXalFtXg,9748
-pybounds/util.py,sha256=xxmXmpLR3yK923X6wAPKp_5w814cO3m9OqF1XIt9S8c,5818
-pybounds-0.0.4.dist-info/LICENSE,sha256=kqeyRXtRGgBVZdXYeIX4zR9l2KZ2rqIBVEiPMTjxjcI,1093
-pybounds-0.0.4.dist-info/METADATA,sha256=u32cRGxW05dWQFSAAKk_oQ-WYpXki6CBMSBvU-j3jT0,2376
-pybounds-0.0.4.dist-info/WHEEL,sha256=GJ7t_kWBFywbagK5eo9IoUwLW6oyOeTKmQ-9iHFVNxQ,92
-pybounds-0.0.4.dist-info/top_level.txt,sha256=V-ofnWE3m_UkXTXJwNRD07n14m5R6sc6l4NadaCCP_A,9
-pybounds-0.0.4.dist-info/RECORD,,