Add nmpc cgmres (#13)

* Add NMPC

* Update README

* Add cgmres

* Update readme

* Add twowheeled of nmpccgmres

* Fix bug

Author: Shunichi09

PythonLinearNonlinearControl/common/utils.py

@@ -116,3 +116,32 @@ def func_x(self, x_1, x_2, u):
             k3[:, i] = dt * func(state + k2, u)
 
         return state + (k0 + 2. * k1 + 2. * k2 + k3) / 6.
+
+
+def line_search(grad, sol, compute_eval_val,
+                init_alpha=0.001, max_iter=100, update_ratio=1.):
+    """ line search
+    Args:
+        grad (numpy.ndarray): gradient
+        sol (numpy.ndarray): sol
+        compute_eval_val (numpy.ndarray): function to compute evaluation value
+
+    Returns: 
+        alpha (float): result of line search 
+    """
+    assert grad.shape == sol.shape
+    base_val = np.inf
+    alpha = init_alpha
+    original_sol = sol.copy()
+
+    for _ in range(max_iter):
+        updated_sol = original_sol - alpha * grad
+        eval_val = compute_eval_val(updated_sol)
+
+        if eval_val < base_val:
+            alpha += init_alpha * update_ratio
+            base_val = eval_val
+        else:
+            break
+
+    return alpha

PythonLinearNonlinearControl/configs/cartpole.py

@@ -148,7 +148,7 @@ def terminal_state_cost_fn(terminal_x, terminal_g_x):
             * CartPoleConfigModule.TERMINAL_WEIGHT
 
     @staticmethod
-    def gradient_cost_fn_with_state(x, g_x, terminal=False):
+    def gradient_cost_fn_state(x, g_x, terminal=False):
         """ gradient of costs with respect to the state
 
         Args:
@@ -177,7 +177,7 @@ def gradient_cost_fn_with_state(x, g_x, terminal=False):
         return cost_dx * CartPoleConfigModule.TERMINAL_WEIGHT
 
     @staticmethod
-    def gradient_cost_fn_with_input(x, u):
+    def gradient_cost_fn_input(x, u):
         """ gradient of costs with respect to the input
 
         Args:
@@ -189,7 +189,7 @@ def gradient_cost_fn_with_input(x, u):
         return 2. * u * np.diag(CartPoleConfigModule.R)
 
     @staticmethod
-    def hessian_cost_fn_with_state(x, g_x, terminal=False):
+    def hessian_cost_fn_state(x, g_x, terminal=False):
         """ hessian costs with respect to the state
 
         Args:
@@ -227,7 +227,7 @@ def hessian_cost_fn_with_state(x, g_x, terminal=False):
         return hessian[np.newaxis, :, :] * CartPoleConfigModule.TERMINAL_WEIGHT
 
     @staticmethod
-    def hessian_cost_fn_with_input(x, u):
+    def hessian_cost_fn_input(x, u):
         """ hessian costs with respect to the input
 
         Args:
@@ -242,7 +242,7 @@ def hessian_cost_fn_with_input(x, u):
         return np.tile(2.*CartPoleConfigModule.R, (pred_len, 1, 1))
 
     @staticmethod
-    def hessian_cost_fn_with_input_state(x, u):
+    def hessian_cost_fn_input_state(x, u):
         """ hessian costs with respect to the state and input
 
         Args:

PythonLinearNonlinearControl/configs/first_order_lag.py

@@ -115,7 +115,7 @@ def terminal_state_cost_fn(terminal_x, terminal_g_x):
             * np.diag(FirstOrderLagConfigModule.Sf)
 
     @staticmethod
-    def gradient_cost_fn_with_state(x, g_x, terminal=False):
+    def gradient_cost_fn_state(x, g_x, terminal=False):
         """ gradient of costs with respect to the state
 
         Args:
@@ -133,7 +133,7 @@ def gradient_cost_fn_with_state(x, g_x, terminal=False):
                 * np.diag(FirstOrderLagConfigModule.Sf))[np.newaxis, :]
 
     @staticmethod
-    def gradient_cost_fn_with_input(x, u):
+    def gradient_cost_fn_input(x, u):
         """ gradient of costs with respect to the input
 
         Args:
@@ -146,7 +146,7 @@ def gradient_cost_fn_with_input(x, u):
         return 2. * u * np.diag(FirstOrderLagConfigModule.R)
 
     @staticmethod
-    def hessian_cost_fn_with_state(x, g_x, terminal=False):
+    def hessian_cost_fn_state(x, g_x, terminal=False):
         """ hessian costs with respect to the state
 
         Args:
@@ -165,7 +165,7 @@ def hessian_cost_fn_with_state(x, g_x, terminal=False):
         return np.tile(2.*FirstOrderLagConfigModule.Sf, (1, 1, 1))
 
     @staticmethod
-    def hessian_cost_fn_with_input(x, u):
+    def hessian_cost_fn_input(x, u):
         """ hessian costs with respect to the input
 
         Args:
@@ -181,7 +181,7 @@ def hessian_cost_fn_with_input(x, u):
         return np.tile(2.*FirstOrderLagConfigModule.R, (pred_len, 1, 1))
 
     @staticmethod
-    def hessian_cost_fn_with_input_state(x, u):
+    def hessian_cost_fn_input_state(x, u):
         """ hessian costs with respect to the state and input
 
         Args:

PythonLinearNonlinearControl/configs/make_configs.py

@@ -1,7 +1,7 @@
 from .first_order_lag import FirstOrderLagConfigModule
-from .two_wheeled import TwoWheeledConfigModule
+from .two_wheeled import TwoWheeledConfigModule, TwoWheeledExtendConfigModule
 from .cartpole import CartPoleConfigModule
-from .nonlinear_sample_system import NonlinearSampleSystemConfigModule
+from .nonlinear_sample_system import NonlinearSampleSystemConfigModule, NonlinearSampleSystemExtendConfigModule
 
 
 def make_config(args):
@@ -12,8 +12,12 @@ def make_config(args):
     if args.env == "FirstOrderLag":
         return FirstOrderLagConfigModule()
     elif args.env == "TwoWheeledConst" or args.env == "TwoWheeledTrack":
+        if args.controller_type == "NMPCCGMRES":
+            return TwoWheeledExtendConfigModule()
         return TwoWheeledConfigModule()
     elif args.env == "CartPole":
         return CartPoleConfigModule()
     elif args.env == "NonlinearSample":
+        if args.controller_type == "NMPCCGMRES":
+            return NonlinearSampleSystemExtendConfigModule()
         return NonlinearSampleSystemConfigModule()

PythonLinearNonlinearControl/configs/nonlinear_sample_system.py

@@ -62,18 +62,11 @@ def __init__(self):
                 "threshold": 1e-6,
             },
             "NMPC": {
-                "threshold": 1e-5,
-                "max_iters": 1000,
-                "learning_rate": 0.1
-            },
-            "NMPC-CGMRES": {
-                "threshold": 1e-3
-            },
-            "NMPC-Newton": {
-                "threshold": 1e-3,
-                "max_iteration": 500,
-                "learning_rate": 1e-3
-            },
+                "threshold": 0.01,
+                "max_iters": 5000,
+                "learning_rate": 0.01,
+                "optimizer_mode": "conjugate"
+            }
         }
 
     @staticmethod
@@ -133,7 +126,7 @@ def terminal_state_cost_fn(terminal_x, terminal_g_x):
         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):
+    def gradient_cost_fn_state(x, g_x, terminal=False):
         """ gradient of costs with respect to the state
 
         Args:
@@ -157,7 +150,7 @@ def gradient_cost_fn_with_state(x, g_x, terminal=False):
         return cost_dx
 
     @staticmethod
-    def gradient_cost_fn_with_input(x, u):
+    def gradient_cost_fn_input(x, u):
         """ gradient of costs with respect to the input
 
         Args:
@@ -169,7 +162,7 @@ def gradient_cost_fn_with_input(x, u):
         return 2. * u * np.diag(NonlinearSampleSystemConfigModule.R)
 
     @staticmethod
-    def hessian_cost_fn_with_state(x, g_x, terminal=False):
+    def hessian_cost_fn_state(x, g_x, terminal=False):
         """ hessian costs with respect to the state
 
         Args:
@@ -197,7 +190,7 @@ def hessian_cost_fn_with_state(x, g_x, terminal=False):
         return hessian[np.newaxis, :, :]
 
     @staticmethod
-    def hessian_cost_fn_with_input(x, u):
+    def hessian_cost_fn_input(x, u):
         """ hessian costs with respect to the input
 
         Args:
@@ -212,7 +205,7 @@ def hessian_cost_fn_with_input(x, u):
         return np.tile(NonlinearSampleSystemConfigModule.R, (pred_len, 1, 1))
 
     @staticmethod
-    def hessian_cost_fn_with_input_state(x, u):
+    def hessian_cost_fn_input_state(x, u):
         """ hessian costs with respect to the state and input
 
         Args:
@@ -294,3 +287,67 @@ def gradient_hamiltonian_state(x, lam, u, g_x):
 
         else:
             raise NotImplementedError
+
+
+class NonlinearSampleSystemExtendConfigModule(NonlinearSampleSystemConfigModule):
+    def __init__(self):
+        super().__init__()
+        self.opt_config = {
+            "NMPCCGMRES": {
+                "threshold": 1e-3,
+                "zeta": 100.,
+                "delta": 0.01,
+                "alpha": 0.5,
+                "tf": 1.,
+                "constraint": True
+            },
+            "NMPCNewton": {
+                "threshold": 1e-3,
+                "max_iteration": 500,
+                "learning_rate": 1e-3
+            }
+        }
+
+    @staticmethod
+    def gradient_hamiltonian_input_with_constraint(x, lam, u, g_x, dummy_u, raw):
+        """
+
+        Args:
+            x (numpy.ndarray): shape(pred_len+1, state_size)
+            lam (numpy.ndarray): shape(pred_len, state_size)
+            u (numpy.ndarray): shape(pred_len, input_size)
+            g_xs (numpy.ndarray): shape(pred_len, state_size)
+            dummy_u (numpy.ndarray): shape(pred_len, input_size)
+            raw (numpy.ndarray): shape(pred_len, input_size), Lagrangian for constraints
+
+        Returns:
+            F (numpy.ndarray), shape(pred_len, 3)
+        """
+        if len(x.shape) == 1:
+            vanilla_F = np.zeros(1)
+            extend_F = np.zeros(1)  # 1 is the same as input size
+            extend_C = np.zeros(1)
+
+            vanilla_F[0] = u[0] + lam[1] + 2. * raw[0] * u[0]
+            extend_F[0] = -0.01 + 2. * raw[0] * dummy_u[0]
+            extend_C[0] = u[0]**2 + dummy_u[0]**2 - \
+                NonlinearSampleSystemConfigModule.INPUT_LOWER_BOUND**2
+
+            F = np.concatenate([vanilla_F, extend_F, extend_C])
+
+        elif len(x.shape) == 2:
+            pred_len, _ = u.shape
+            vanilla_F = np.zeros((pred_len, 1))
+            extend_F = np.zeros((pred_len, 1))  # 1 is the same as input size
+            extend_C = np.zeros((pred_len, 1))
+
+            for i in range(pred_len):
+                vanilla_F[i, 0] = \
+                    u[i, 0] + lam[i, 1] + 2. * raw[i, 0] * u[i, 0]
+                extend_F[i, 0] = -0.01 + 2. * raw[i, 0] * dummy_u[i, 0]
+                extend_C[i, 0] = u[i, 0]**2 + dummy_u[i, 0]**2 - \
+                    NonlinearSampleSystemConfigModule.INPUT_LOWER_BOUND**2
+
+            F = np.concatenate([vanilla_F, extend_F, extend_C], axis=1)
+
+        return F