Add nonlinear sample system

Author: Shunichi09

PythonLinearNonlinearControl/common/utils.py

@@ -91,17 +91,17 @@ def func_x(self, x_1, x_2, u):
         for i, func in enumerate(functions):
             k3[i] = dt * func(state + k2, u)
 
-        return (k0 + 2. * k1 + 2. * k2 + k3) / 6.
+        return state + (k0 + 2. * k1 + 2. * k2 + k3) / 6.
 
     else:
         batch_size, state_size = state.shape
         assert state_size == len(functions), \
             "Invalid functions length, You need to give the state size functions"
 
-        k0 = np.zeros(batch_size, state_size)
-        k1 = np.zeros(batch_size, state_size)
-        k2 = np.zeros(batch_size, state_size)
-        k3 = np.zeros(batch_size, state_size)
+        k0 = np.zeros((batch_size, state_size))
+        k1 = np.zeros((batch_size, state_size))
+        k2 = np.zeros((batch_size, state_size))
+        k3 = np.zeros((batch_size, state_size))
 
         for i, func in enumerate(functions):
             k0[:, i] = dt * func(state, u)
@@ -115,4 +115,4 @@ def func_x(self, x_1, x_2, u):
         for i, func in enumerate(functions):
             k3[:, i] = dt * func(state + k2, u)
 
-        return (k0 + 2. * k1 + 2. * k2 + k3) / 6.
+        return state + (k0 + 2. * k1 + 2. * k2 + k3) / 6.

PythonLinearNonlinearControl/configs/make_configs.py

@@ -1,6 +1,7 @@
 from .first_order_lag import FirstOrderLagConfigModule
 from .two_wheeled import TwoWheeledConfigModule
 from .cartpole import CartPoleConfigModule
+from .nonlinear_sample_system import NonlinearSampleSystemConfigModule
 
 
 def make_config(args):
@@ -14,3 +15,5 @@ def make_config(args):
         return TwoWheeledConfigModule()
     elif args.env == "CartPole":
         return CartPoleConfigModule()
+    elif args.env == "NonlinearSample":
+        return NonlinearSampleSystemConfigModule()

PythonLinearNonlinearControl/configs/nonlinear_sample_system.py

@@ -0,0 +1,219 @@
+import numpy as np
+
+
+class NonlinearSampleSystemConfigModule():
+    # parameters
+    ENV_NAME = "NonlinearSampleSystem-v0"
+    PLANNER_TYPE = "Const"
+    TYPE = "Nonlinear"
+    TASK_HORIZON = 2500
+    PRED_LEN = 10
+    STATE_SIZE = 2
+    INPUT_SIZE = 1
+    DT = 0.01
+    R = np.diag([0.01])
+    Q = None
+    Sf = None
+    # bounds
+    INPUT_LOWER_BOUND = np.array([-0.5])
+    INPUT_UPPER_BOUND = np.array([0.5])
+
+    def __init__(self):
+        """
+        """
+        # opt configs
+        self.opt_config = {
+            "Random": {
+                "popsize": 5000
+            },
+            "CEM": {
+                "popsize": 500,
+                "num_elites": 50,
+                "max_iters": 15,
+                "alpha": 0.3,
+                "init_var": 9.,
+                "threshold": 0.001
+            },
+            "MPPI": {
+                "beta": 0.6,
+                "popsize": 5000,
+                "kappa": 0.9,
+                "noise_sigma": 0.5,
+            },
+            "MPPIWilliams": {
+                "popsize": 5000,
+                "lambda": 1.,
+                "noise_sigma": 0.9,
+            },
+            "iLQR": {
+                "max_iter": 500,
+                "init_mu": 1.,
+                "mu_min": 1e-6,
+                "mu_max": 1e10,
+                "init_delta": 2.,
+                "threshold": 1e-6,
+            },
+            "DDP": {
+                "max_iter": 500,
+                "init_mu": 1.,
+                "mu_min": 1e-6,
+                "mu_max": 1e10,
+                "init_delta": 2.,
+                "threshold": 1e-6,
+            },
+            "NMPC-CGMRES": {
+            },
+            "NMPC-Newton": {
+            },
+        }
+
+    @staticmethod
+    def input_cost_fn(u):
+        """ input cost functions
+
+        Args:
+            u (numpy.ndarray): input, shape(pred_len, input_size)
+                or shape(pop_size, pred_len, input_size)
+        Returns:
+            cost (numpy.ndarray): cost of input, shape(pred_len, input_size) or
+                shape(pop_size, pred_len, input_size)
+        """
+        return (u**2) * np.diag(NonlinearSampleSystemConfigModule.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(pred_len, state_size)
+                or shape(pop_size, pred_len, state_size)
+        Returns:
+            cost (numpy.ndarray): cost of state, shape(pred_len, 1) or
+                shape(pop_size, pred_len, 1)
+        """
+
+        if len(x.shape) > 2:
+            return (0.5 * (x[:, :, 0]**2) +
+                    0.5 * (x[:, :, 1]**2))[:, :, np.newaxis]
+
+        elif len(x.shape) > 1:
+            return (0.5 * (x[:, 0]**2) + 0.5 * (x[:, 1]**2))[:, np.newaxis]
+
+        return 0.5 * (x[0]**2) + 0.5 * (x[1]**2)
+
+    @ 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, shape(pred_len, ) or
+                shape(pop_size, pred_len)
+        """
+
+        if len(terminal_x.shape) > 1:
+            return (0.5 * (terminal_x[:, 0]**2) +
+                    0.5 * (terminal_x[:, 1]**2))[:, np.newaxis]
+
+        return 0.5 * (terminal_x[0]**2) + 0.5 * (terminal_x[1]**2)
+
+    @ staticmethod
+    def gradient_cost_fn_with_state(x, g_x, terminal=False):
+        """ gradient of costs with respect to the state
+
+        Args:
+            x (numpy.ndarray): state, shape(pred_len, state_size)
+            g_x (numpy.ndarray): goal state, shape(pred_len, state_size)
+
+        Returns:
+            l_x (numpy.ndarray): gradient of cost, shape(pred_len, state_size)
+                or shape(1, state_size)
+        """
+        if not terminal:
+            cost_dx0 = x[:, 0]
+            cost_dx1 = x[:, 1]
+            cost_dx = np.stack((cost_dx0, cost_dx1), axis=1)
+            return cost_dx
+
+        cost_dx0 = x[0]
+        cost_dx1 = x[1]
+        cost_dx = np.array([[cost_dx0, cost_dx1]])
+
+        return cost_dx
+
+    @ staticmethod
+    def gradient_cost_fn_with_input(x, u):
+        """ gradient of costs with respect to the input
+
+        Args:
+            x (numpy.ndarray): state, shape(pred_len, state_size)
+            u (numpy.ndarray): goal state, shape(pred_len, input_size)
+        Returns:
+            l_u (numpy.ndarray): gradient of cost, shape(pred_len, input_size)
+        """
+        return 2. * u * np.diag(NonlinearSampleSystemConfigModule.R)
+
+    @ staticmethod
+    def hessian_cost_fn_with_state(x, g_x, terminal=False):
+        """ hessian costs with respect to the state
+
+        Args:
+            x (numpy.ndarray): state, shape(pred_len, state_size)
+            g_x (numpy.ndarray): goal state, shape(pred_len, state_size)
+        Returns:
+            l_xx (numpy.ndarray): gradient of cost,
+                shape(pred_len, state_size, state_size) or
+                shape(1, state_size, state_size) or
+        """
+        if not terminal:
+            (pred_len, state_size) = x.shape
+            hessian = np.eye(state_size)
+            hessian = np.tile(hessian, (pred_len, 1, 1))
+            hessian[:, 0, 0] = 1.
+            hessian[:, 1, 1] = 1.
+
+            return hessian
+
+        state_size = len(x)
+        hessian = np.eye(state_size)
+        hessian[0, 0] = 1.
+        hessian[1, 1] = 1.
+
+        return hessian[np.newaxis, :, :]
+
+    @ staticmethod
+    def hessian_cost_fn_with_input(x, u):
+        """ hessian costs with respect to the input
+
+        Args:
+            x (numpy.ndarray): state, shape(pred_len, state_size)
+            u (numpy.ndarray): goal state, shape(pred_len, input_size)
+        Returns:
+            l_uu (numpy.ndarray): gradient of cost,
+                shape(pred_len, input_size, input_size)
+        """
+        (pred_len, _) = u.shape
+
+        return np.tile(NonlinearSampleSystemConfigModule.R, (pred_len, 1, 1))
+
+    @ staticmethod
+    def hessian_cost_fn_with_input_state(x, u):
+        """ hessian costs with respect to the state and input
+
+        Args:
+            x (numpy.ndarray): state, shape(pred_len, state_size)
+            u (numpy.ndarray): goal state, shape(pred_len, input_size)
+        Returns:
+            l_ux (numpy.ndarray): gradient of cost ,
+                shape(pred_len, input_size, state_size)
+        """
+        (_, state_size) = x.shape
+        (pred_len, input_size) = u.shape
+
+        return np.zeros((pred_len, input_size, state_size))

PythonLinearNonlinearControl/configs/nonlinear_system_sample.py

@@ -2,6 +2,7 @@
 from .two_wheeled import TwoWheeledConstEnv
 from .two_wheeled import TwoWheeledTrackEnv
 from .cartpole import CartPoleEnv
+from .nonlinear_sample_system import NonlinearSampleSystemEnv
 
 
 def make_env(args):
@@ -14,5 +15,7 @@ def make_env(args):
         return TwoWheeledTrackEnv()
     elif args.env == "CartPole":
         return CartPoleEnv()
+    elif args.env == "NonlinearSample":
+        return NonlinearSampleSystemEnv()
 
     raise NotImplementedError("There is not {} Env".format(args.env))

PythonLinearNonlinearControl/envs/make_envs.py

@@ -5,7 +5,7 @@
 from ..common.utils import update_state_with_Runge_Kutta
 
 
-class NonlinearSampleEnv(Env):
+class NonlinearSampleSystemEnv(Env):
     """ Nonlinear Sample Env
     """
 
@@ -15,12 +15,12 @@ def __init__(self):
         self.config = {"state_size": 2,
                        "input_size": 1,
                        "dt": 0.01,
-                       "max_step": 250,
+                       "max_step": 2000,
                        "input_lower_bound": [-0.5],
                        "input_upper_bound": [0.5],
                        }
 
-        super(NonlinearSampleEnv, self).__init__(self.config)
+        super(NonlinearSampleSystemEnv, self).__init__(self.config)
 
     def reset(self, init_x=np.array([2., 0.])):
         """ reset state
@@ -62,7 +62,8 @@ def step(self, u):
         functions = [self._func_x_1, self._func_x_2]
 
         next_x = update_state_with_Runge_Kutta(self.curr_x, u,
-                                               functions, self.config["dt"])
+                                               functions, self.config["dt"],
+                                               batch=False)
 
         # cost
         cost = 0
@@ -83,18 +84,14 @@ def step(self, u):
             {"goal_state": self.g_x}
 
     def _func_x_1(self, x, u):
-        """
-        """
         x_dot = x[1]
         return x_dot
 
     def _func_x_2(self, x, u):
-        """
-        """
-        x_dot = (1. - x[0]**2 - x[1]**2) * x[1] - x[0] + u
+        x_dot = (1. - x[0]**2 - x[1]**2) * x[1] - x[0] + u[0]
         return x_dot
 
     def plot_func(self, to_plot, i=None, history_x=None, history_g_x=None):
         """
         """
-        raise ValueError("NonlinearSampleEnv does not have animation")
+        raise ValueError("NonlinearSampleSystemEnv does not have animation")

PythonLinearNonlinearControl/envs/nonlinear_sample_system.py

@@ -1,6 +1,7 @@
 from .first_order_lag import FirstOrderLagModel
 from .two_wheeled import TwoWheeledModel
 from .cartpole import CartPoleModel
+from .nonlinear_sample_system import NonlinearSampleSystemModel
 
 
 def make_model(args, config):
@@ -11,5 +12,7 @@ def make_model(args, config):
         return TwoWheeledModel(config)
     elif args.env == "CartPole":
         return CartPoleModel(config)
+    elif args.env == "NonlinearSample":
+        return NonlinearSampleSystemModel(config)
 
     raise NotImplementedError("There is not {} Model".format(args.env))