Add unconstrained NMPC (#12)

* Add NMPC

* Update README

Author: Shunichi09

PythonLinearNonlinearControl/configs/first_order_lag.py

@@ -52,15 +52,15 @@ def __init__(self):
             "MPC": {
             },
             "iLQR": {
-                "max_iter": 500,
+                "max_iters": 500,
                 "init_mu": 1.,
                 "mu_min": 1e-6,
                 "mu_max": 1e10,
                 "init_delta": 2.,
                 "threshold": 1e-6,
             },
             "DDP": {
-                "max_iter": 500,
+                "max_iters": 500,
                 "init_mu": 1.,
                 "mu_min": 1e-6,
                 "mu_max": 1e10,

PythonLinearNonlinearControl/configs/nonlinear_sample_system.py

@@ -6,12 +6,12 @@ class NonlinearSampleSystemConfigModule():
     ENV_NAME = "NonlinearSampleSystem-v0"
     PLANNER_TYPE = "Const"
     TYPE = "Nonlinear"
-    TASK_HORIZON = 2500
+    TASK_HORIZON = 2000
     PRED_LEN = 10
     STATE_SIZE = 2
     INPUT_SIZE = 1
     DT = 0.01
-    R = np.diag([0.01])
+    R = np.diag([1.])
     Q = None
     Sf = None
     # bounds
@@ -46,24 +46,33 @@ def __init__(self):
                 "noise_sigma": 0.9,
             },
             "iLQR": {
-                "max_iter": 500,
+                "max_iters": 500,
                 "init_mu": 1.,
                 "mu_min": 1e-6,
                 "mu_max": 1e10,
                 "init_delta": 2.,
                 "threshold": 1e-6,
             },
             "DDP": {
-                "max_iter": 500,
+                "max_iters": 500,
                 "init_mu": 1.,
                 "mu_min": 1e-6,
                 "mu_max": 1e10,
                 "init_delta": 2.,
                 "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
             },
         }
 
@@ -103,7 +112,7 @@ def state_cost_fn(x, g_x):
 
         return 0.5 * (x[0]**2) + 0.5 * (x[1]**2)
 
-    @ staticmethod
+    @staticmethod
     def terminal_state_cost_fn(terminal_x, terminal_g_x):
         """
 
@@ -123,7 +132,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
+    @staticmethod
     def gradient_cost_fn_with_state(x, g_x, terminal=False):
         """ gradient of costs with respect to the state
 
@@ -147,7 +156,7 @@ def gradient_cost_fn_with_state(x, g_x, terminal=False):
 
         return cost_dx
 
-    @ staticmethod
+    @staticmethod
     def gradient_cost_fn_with_input(x, u):
         """ gradient of costs with respect to the input
 
@@ -159,7 +168,7 @@ def gradient_cost_fn_with_input(x, u):
         """
         return 2. * u * np.diag(NonlinearSampleSystemConfigModule.R)
 
-    @ staticmethod
+    @staticmethod
     def hessian_cost_fn_with_state(x, g_x, terminal=False):
         """ hessian costs with respect to the state
 
@@ -187,7 +196,7 @@ def hessian_cost_fn_with_state(x, g_x, terminal=False):
 
         return hessian[np.newaxis, :, :]
 
-    @ staticmethod
+    @staticmethod
     def hessian_cost_fn_with_input(x, u):
         """ hessian costs with respect to the input
 
@@ -202,7 +211,7 @@ def hessian_cost_fn_with_input(x, u):
 
         return np.tile(NonlinearSampleSystemConfigModule.R, (pred_len, 1, 1))
 
-    @ staticmethod
+    @staticmethod
     def hessian_cost_fn_with_input_state(x, u):
         """ hessian costs with respect to the state and input
 
@@ -217,3 +226,71 @@ def hessian_cost_fn_with_input_state(x, u):
         (pred_len, input_size) = u.shape
 
         return np.zeros((pred_len, input_size, state_size))
+
+    @staticmethod
+    def gradient_hamiltonian_input(x, lam, u, g_x):
+        """
+
+        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)
+
+        Returns:
+            F (numpy.ndarray), shape(pred_len, input_size)
+        """
+        if len(x.shape) == 1:
+            input_size = u.shape[0]
+            F = np.zeros(input_size)
+            F[0] = u[0] + lam[1]
+
+            return F
+
+        elif len(x.shape) == 2:
+            pred_len, input_size = u.shape
+            F = np.zeros((pred_len, input_size))
+
+            for i in range(pred_len):
+                F[i, 0] = u[i, 0] + lam[i, 1]
+
+            return F
+
+        else:
+            raise NotImplementedError
+
+    @staticmethod
+    def gradient_hamiltonian_state(x, lam, u, g_x):
+        """
+        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)
+
+        Returns:
+            lam_dot (numpy.ndarray), shape(state_size, )
+        """
+        if len(lam.shape) == 1:
+            state_size = lam.shape[0]
+            lam_dot = np.zeros(state_size)
+            lam_dot[0] = x[0] - (2. * x[0] * x[1] + 1.) * lam[1]
+            lam_dot[1] = x[1] + lam[0] + \
+                (-3. * (x[1]**2) - x[0]**2 + 1.) * lam[1]
+
+            return lam_dot
+
+        elif len(lam.shape) == 2:
+            pred_len, state_size = lam.shape
+            lam_dot = np.zeros((pred_len, state_size))
+
+            for i in range(pred_len):
+                lam_dot[i, 0] = x[i, 0] - \
+                    (2. * x[i, 0] * x[i, 1] + 1.) * lam[i, 1]
+                lam_dot[i, 1] = x[i, 1] + lam[i, 0] + \
+                    (-3. * (x[i, 1]**2) - x[i, 0]**2 + 1.) * lam[i, 1]
+
+            return lam_dot
+
+        else:
+            raise NotImplementedError

PythonLinearNonlinearControl/configs/two_wheeled.py

@@ -23,9 +23,15 @@ class TwoWheeledConfigModule():
     Sf = np.diag([5., 5., 1.])
     """
     # for track goal
+    """
     R = np.diag([0.01, 0.01])
     Q = np.diag([2.5, 2.5, 0.01])
     Sf = np.diag([2.5, 2.5, 0.01])
+    """
+    # for track goal to NMPC
+    R = np.diag([0.1, 0.1])
+    Q = np.diag([0.1, 0.1, 0.1])
+    Sf = np.diag([0.25, 0.25, 0.25])
 
     # bounds
     INPUT_LOWER_BOUND = np.array([-1.5, -3.14])
@@ -62,21 +68,26 @@ def __init__(self):
                 "noise_sigma": 1.,
             },
             "iLQR": {
-                "max_iter": 500,
+                "max_iters": 500,
                 "init_mu": 1.,
                 "mu_min": 1e-6,
                 "mu_max": 1e10,
                 "init_delta": 2.,
                 "threshold": 1e-6,
             },
             "DDP": {
-                "max_iter": 500,
+                "max_iters": 500,
                 "init_mu": 1.,
                 "mu_min": 1e-6,
                 "mu_max": 1e10,
                 "init_delta": 2.,
                 "threshold": 1e-6,
             },
+            "NMPC": {
+                "threshold": 1e-3,
+                "max_iters": 1000,
+                "learning_rate": 0.1
+            },
             "NMPC-CGMRES": {
             },
             "NMPC-Newton": {
@@ -232,3 +243,86 @@ def hessian_cost_fn_with_input_state(x, u):
         (pred_len, input_size) = u.shape
 
         return np.zeros((pred_len, input_size, state_size))
+
+    @staticmethod
+    def gradient_hamiltonian_input(x, lam, u, g_x):
+        """
+
+        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)
+
+        Returns:
+            F (numpy.ndarray), shape(pred_len, input_size)
+        """
+        if len(x.shape) == 1:
+            input_size = u.shape[0]
+            F = np.zeros(input_size)
+            F[0] = u[0] * TwoWheeledConfigModule.R[0, 0] + \
+                lam[0] * np.cos(x[2]) + lam[1] * np.sin(x[2])
+            F[1] = u[1] * TwoWheeledConfigModule.R[1, 1] + lam[2]
+
+            return F
+
+        elif len(x.shape) == 2:
+            pred_len, input_size = u.shape
+            F = np.zeros((pred_len, input_size))
+
+            for i in range(pred_len):
+                F[i, 0] = u[i, 0] * TwoWheeledConfigModule.R[0, 0] + \
+                    lam[i, 0] * np.cos(x[i, 2]) + lam[i, 1] * np.sin(x[i, 2])
+                F[i, 1] = u[i, 1] * TwoWheeledConfigModule.R[1, 1] + lam[i, 2]
+
+            return F
+        else:
+            raise NotImplementedError
+
+    @staticmethod
+    def gradient_hamiltonian_state(x, lam, u, g_x):
+        """
+        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)
+
+        Returns:
+            lam_dot (numpy.ndarray), shape(state_size, )
+        """
+        if len(lam.shape) == 1:
+            state_size = lam.shape[0]
+            lam_dot = np.zeros(state_size)
+            lam_dot[0] = \
+                (x[0] - g_x[0]) * TwoWheeledConfigModule.Q[0, 0]
+            lam_dot[1] = \
+                (x[1] - g_x[1]) * TwoWheeledConfigModule.Q[1, 1]
+
+            relative_angle = fit_angle_in_range(x[2] - g_x[2])
+            lam_dot[2] = \
+                relative_angle * TwoWheeledConfigModule.Q[2, 2] \
+                - lam[0] * u[0] * np.sin(x[2]) \
+                + lam[1] * u[0] * np.cos(x[2])
+
+            return lam_dot
+
+        elif len(lam.shape) == 2:
+            pred_len, state_size = lam.shape
+            lam_dot = np.zeros((pred_len, state_size))
+
+            for i in range(pred_len):
+                lam_dot[i, 0] = \
+                    (x[i, 0] - g_x[i, 0]) * TwoWheeledConfigModule.Q[0, 0]
+                lam_dot[i, 1] = \
+                    (x[i, 1] - g_x[i, 1]) * TwoWheeledConfigModule.Q[1, 1]
+
+                relative_angle = fit_angle_in_range(x[i, 2] - g_x[i, 2])
+                lam_dot[i, 2] = \
+                    relative_angle * TwoWheeledConfigModule.Q[2, 2] \
+                    - lam[i, 0] * u[i, 0] * np.sin(x[i, 2]) \
+                    + lam[i, 1] * u[i, 0] * np.cos(x[i, 2])
+
+            return lam_dot
+        else:
+            raise NotImplementedError

PythonLinearNonlinearControl/controllers/ddp.py

@@ -40,7 +40,7 @@ def __init__(self, config, model):
             config.hessian_cost_fn_with_input_state
 
         # controller parameters
-        self.max_iter = config.opt_config["DDP"]["max_iter"]
+        self.max_iters = config.opt_config["DDP"]["max_iters"]
         self.init_mu = config.opt_config["DDP"]["init_mu"]
         self.mu = self.init_mu
         self.mu_min = config.opt_config["DDP"]["mu_min"]
@@ -88,7 +88,7 @@ def obtain_sol(self, curr_x, g_xs):
         # line search param
         alphas = 1.1**(-np.arange(10)**2)
 
-        while opt_count < self.max_iter:
+        while opt_count < self.max_iters:
             accepted_sol = False
 
             # forward

PythonLinearNonlinearControl/controllers/ilqr.py

@@ -38,7 +38,7 @@ def __init__(self, config, model):
             config.hessian_cost_fn_with_input_state
 
         # controller parameters
-        self.max_iter = config.opt_config["iLQR"]["max_iter"]
+        self.max_iters = config.opt_config["iLQR"]["max_iters"]
         self.init_mu = config.opt_config["iLQR"]["init_mu"]
         self.mu = self.init_mu
         self.mu_min = config.opt_config["iLQR"]["mu_min"]
@@ -81,7 +81,7 @@ def obtain_sol(self, curr_x, g_xs):
         # line search param
         alphas = 1.1**(-np.arange(10)**2)
 
-        while opt_count < self.max_iter:
+        while opt_count < self.max_iters:
             accepted_sol = False
 
             # forward

PythonLinearNonlinearControl/controllers/make_controllers.py

@@ -5,6 +5,7 @@
 from .mppi_williams import MPPIWilliams
 from .ilqr import iLQR
 from .ddp import DDP
+from .nmpc import NMPC
 
 
 def make_controller(args, config, model):
@@ -23,5 +24,7 @@ def make_controller(args, config, model):
         return iLQR(config, model)
     elif args.controller_type == "DDP":
         return DDP(config, model)
+    elif args.controller_type == "NMPC":
+        return NMPC(config, model)
 
     raise ValueError("No controller: {}".format(args.controller_type))