Remove and Add: Clear all version1.0 files and folders, and Added version2.0 files

Author: Shunichi09

.gitignore

@@ -1,10 +1,134 @@
-*.csv
-*.log
-*.pickle
-*.mp4
+# folders
+.vscode/
+.pytest_cache/
+result/
 
-.cache/
-.eggs/
+# Byte-compiled / optimized / DLL files
 __pycache__/
-.pytest_cache
-cache/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+pip-wheel-metadata/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+.python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don't work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/

LICENSE

@@ -1,6 +1,6 @@
 MIT License
 
-Copyright (c) 2018 Shunichi Sekiguchi
+Copyright (c) 2020 Shunichi Sekiguchi
 
 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal

PythonLinearNonlinearControl/__init__.py

@@ -0,0 +1,2 @@
+import numpy as np
+

PythonLinearNonlinearControl/common/__init__.py

@@ -0,0 +1,92 @@
+import numpy as np
+
+class FirstOrderLagConfigModule():
+    # parameters
+    ENV_NAME = "FirstOrderLag-v0"
+    TYPE = "Linear"
+    TASK_HORIZON = 1000
+    PRED_LEN = 10
+    STATE_SIZE = 4
+    INPUT_SIZE = 2
+    DT = 0.05
+    # cost parameters
+    R = np.eye(INPUT_SIZE)
+    Q = np.eye(STATE_SIZE)
+    Sf = np.eye(STATE_SIZE)
+    # bounds
+    INPUT_LOWER_BOUND = np.array([-0.5, -0.5])
+    INPUT_UPPER_BOUND = np.array([0.5, 0.5])
+    # DT_INPUT_LOWER_BOUND = np.array([-0.5 * DT, -0.5 * DT])
+    # DT_INPUT_UPPER_BOUND = np.array([0.25 * DT, 0.25 * DT])
+    DT_INPUT_LOWER_BOUND = None
+    DT_INPUT_UPPER_BOUND = None
+
+    def __init__(self):
+        """ 
+        Args:
+            save_dit (str): save directory
+        """
+        # opt configs
+        self.opt_config = {
+            "Random": {
+                "popsize": 5000
+            },
+            "CEM": {
+                "popsize": 500,
+                "num_elites": 50,
+                "max_iters": 15,
+                "alpha": 0.3,
+                "init_var":1.,
+                "threshold":0.001
+            },
+            "MPPI":{
+                "beta" : 0.6,
+                "popsize": 5000,
+                "kappa": 0.9,
+                "noise_sigma": 0.5,
+            },
+           "iLQR":{
+           },
+           "cgmres-NMPC":{
+           },
+           "newton-NMPC":{
+           },
+        }   
+
+    @staticmethod
+    def input_cost_fn(u):
+        """ input cost functions
+        Args:
+            u (numpy.ndarray): input, shape(input_size, )
+                or shape(pop_size, input_size)
+        Returns:
+            cost (numpy.ndarray): cost of input, none or shape(pop_size, )
+        """
+        return (u**2) * np.diag(FirstOrderLagConfigModule.R)
+    
+    @staticmethod
+    def state_cost_fn(x, g_x):
+        """ state cost function
+        Args:
+            x (numpy.ndarray): state, shape(pred_len, state_size)
+                or shape(pop_size,  pred_len, state_size)
+            g_x (numpy.ndarray): goal state, shape(state_size, )
+                or shape(pop_size, state_size)
+        Returns:
+            cost (numpy.ndarray): cost of state, none or shape(pop_size, )
+        """
+        return ((x - g_x)**2) * np.diag(FirstOrderLagConfigModule.Q)
+
+    @staticmethod
+    def terminal_state_cost_fn(terminal_x, terminal_g_x):
+        """
+        Args:
+            terminal_x (numpy.ndarray): terminal state,
+                shape(state_size, ) or shape(pop_size, state_size)
+            terminal_g_x (numpy.ndarray): terminal goal state,
+                shape(state_size, ) or shape(pop_size, state_size)
+        Returns:
+            cost (numpy.ndarray): cost of state, none or shape(pop_size, )
+        """
+        return ((terminal_x - terminal_g_x)**2) \
+                * np.diag(FirstOrderLagConfigModule.Sf)

PythonLinearNonlinearControl/common/utils.py

@@ -0,0 +1,9 @@
+from .first_order_lag import FirstOrderLagConfigModule
+
+def make_config(args):
+    """
+    Returns:
+        config (ConfigModule class): configuration for the each env
+    """
+    if args.env == "FirstOrderLag":
+        return FirstOrderLagConfigModule()

PythonLinearNonlinearControl/configs/__init__.py

@@ -0,0 +1,135 @@
+from logging import getLogger
+
+import numpy as np
+import scipy.stats as stats
+
+from .controller import Controller
+from ..envs.cost import calc_cost
+
+logger = getLogger(__name__)
+
+class CEM(Controller):
+    """ Cross Entropy Method for linear and nonlinear method
+
+    Attributes:
+        history_u (list[numpy.ndarray]): time history of optimal input
+    Ref:
+        Chua, K., Calandra, R., McAllister, R., & Levine, S. (2018).
+        Deep reinforcement learning in a handful of trials 
+        using probabilistic dynamics models.
+        In Advances in Neural Information Processing Systems (pp. 4754-4765).
+    """
+    def __init__(self, config, model):
+        super(CEM, self).__init__(config, model)
+
+        # model
+        self.model = model
+
+        # general parameters
+        self.pred_len = config.PRED_LEN
+        self.input_size = config.INPUT_SIZE
+
+        # cem parameters
+        self.alpha = config.opt_config["CEM"]["alpha"]
+        self.pop_size = config.opt_config["CEM"]["popsize"]
+        self.max_iters = config.opt_config["CEM"]["max_iters"]
+        self.num_elites = config.opt_config["CEM"]["num_elites"]
+        self.epsilon = config.opt_config["CEM"]["threshold"]
+        self.init_var = config.opt_config["CEM"]["init_var"]
+        self.opt_dim = self.input_size * self.pred_len
+
+        # get bound 
+        self.input_upper_bounds = np.tile(config.INPUT_UPPER_BOUND,
+                                          self.pred_len)
+        self.input_lower_bounds = np.tile(config.INPUT_LOWER_BOUND,
+                                          self.pred_len)
+
+        # get cost func
+        self.state_cost_fn = config.state_cost_fn
+        self.terminal_state_cost_fn = config.terminal_state_cost_fn
+        self.input_cost_fn = config.input_cost_fn
+
+        # init mean
+        self.init_mean = np.tile((config.INPUT_UPPER_BOUND \
+                                  + config.INPUT_LOWER_BOUND) / 2.,
+                                 self.pred_len)
+        self.prev_sol = self.init_mean.copy()
+        # init variance
+        var = np.ones_like(config.INPUT_UPPER_BOUND) \
+              * config.opt_config["CEM"]["init_var"]
+        self.init_var = np.tile(var, self.pred_len)
+
+        # save
+        self.history_u = []
+    
+    def clear_sol(self):
+        """ clear prev sol
+        """
+        logger.debug("Clear Sol")
+        self.prev_sol = self.init_mean.copy()
+
+    def obtain_sol(self, curr_x, g_xs):
+        """ calculate the optimal inputs
+
+        Args:
+            curr_x (numpy.ndarray): current state, shape(state_size, )
+            g_xs (numpy.ndarrya): goal trajectory, shape(plan_len, state_size)
+        Returns:
+            opt_input (numpy.ndarray): optimal input, shape(input_size, )
+        """
+        # initialize 
+        opt_count = 0
+
+        # get configuration
+        mean = self.prev_sol.flatten().copy()        
+        var = self.init_var.flatten().copy()
+
+        # make distribution            
+        X = stats.truncnorm(-1, 1,
+                            loc=np.zeros_like(mean),\
+                            scale=np.ones_like(mean))
+        
+        while (opt_count < self.max_iters) and np.max(var) > self.epsilon:
+            # constrained
+            lb_dist = mean - self.input_lower_bounds  
+            ub_dist = self.input_upper_bounds - mean
+            constrained_var = np.minimum(np.minimum(np.square(lb_dist),
+                                                    np.square(ub_dist)),
+                                         var)
+
+            # sample
+            samples = X.rvs(size=[self.pop_size, self.opt_dim]) \
+                      * np.sqrt(constrained_var) \
+                      + mean
+
+            # calc cost
+            # samples.shape = (pop_size, opt_dim)
+            costs = self.calc_cost(curr_x,
+                                   samples.reshape(self.pop_size,
+                                                    self.pred_len,
+                                                    self.input_size),
+                                   g_xs)
+
+            # sort cost
+            elites = samples[np.argsort(costs)][:self.num_elites]
+
+            # new mean
+            new_mean = np.mean(elites, axis=0)
+            new_var = np.var(elites, axis=0)
+
+            # soft update
+            mean = self.alpha * mean + (1. - self.alpha) * new_mean
+            var = self.alpha * var + (1. - self.alpha) * new_var
+
+            logger.debug("Var = {}".format(np.max(var)))
+            logger.debug("Costs = {}".format(np.mean(costs)))
+            opt_count += 1
+        
+        sol = mean.copy()
+        self.prev_sol = np.concatenate((mean[self.input_size:],
+                                        np.zeros(self.input_size)))
+
+        return sol.reshape(self.pred_len, self.input_size).copy()[0]
+
+    def __str__(self):
+        return "CEM"