ADD: added iLQR and DDP

Author: Shunichi09

.gitignore

@@ -109,7 +109,6 @@ celerybeat.pid
 # Environments
 .env
 .venv
-env/
 venv/
 ENV/
 env.bak/

PythonLinearNonlinearControl/configs/first_order_lag.py

@@ -43,22 +43,19 @@ def __init__(self):
                 "kappa": 0.9,
                 "noise_sigma": 0.5,
             },
-           "iLQR":{
-           },
-           "cgmres-NMPC":{
-           },
-           "newton-NMPC":{
-           },
+           "MPC":{
+           }
         }   
 
     @staticmethod
     def input_cost_fn(u):
         """ input cost functions
         Args:
-            u (numpy.ndarray): input, shape(input_size, )
-                or shape(pop_size, input_size)
+            u (numpy.ndarray): input, shape(pred_len, input_size)
+                or shape(pop_size, pred_len, input_size)
         Returns:
-            cost (numpy.ndarray): cost of input, none or shape(pop_size, )
+            cost (numpy.ndarray): cost of input, shape(pred_len, input_size) or
+                shape(pop_size, pred_len, input_size)
         """
         return (u**2) * np.diag(FirstOrderLagConfigModule.R)
 
@@ -67,11 +64,12 @@ 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)
+                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, none or shape(pop_size, )
+            cost (numpy.ndarray): cost of state, shape(pred_len, state_size) or
+                shape(pop_size, pred_len, state_size)
         """
         return ((x - g_x)**2) * np.diag(FirstOrderLagConfigModule.Q)
 
@@ -84,7 +82,8 @@ def terminal_state_cost_fn(terminal_x, terminal_g_x):
             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, )
+            cost (numpy.ndarray): cost of state, shape(pred_len, ) or
+                shape(pop_size, pred_len)
         """
         return ((terminal_x - terminal_g_x)**2) \
                 * np.diag(FirstOrderLagConfigModule.Sf)

PythonLinearNonlinearControl/configs/two_wheeled.py

@@ -5,13 +5,13 @@ class TwoWheeledConfigModule():
     ENV_NAME = "TwoWheeled-v0"
     TYPE = "Nonlinear"
     TASK_HORIZON = 1000
-    PRED_LEN = 10
+    PRED_LEN = 20
     STATE_SIZE = 3
     INPUT_SIZE = 2
     DT = 0.01
     # cost parameters
-    R = np.eye(INPUT_SIZE)
-    Q = np.eye(STATE_SIZE)
+    R = np.eye(INPUT_SIZE) * 0.1
+    Q = np.eye(STATE_SIZE) * 0.5
     Sf = np.eye(STATE_SIZE)
     # bounds
     INPUT_LOWER_BOUND = np.array([-1.5, 3.14])
@@ -40,34 +40,50 @@ def __init__(self):
                 "noise_sigma": 0.5,
             },
            "iLQR":{
+                "max_iter": 500,
+                "mu": 1.,
+                "mu_min": 1e-6,
+                "mu_max": 1e10,
+                "init_delta": 2.,
+                "threshold": 1e-6,
+           },
+           "DDP":{
+                "max_iter": 500,
+                "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(input_size, )
-                or shape(pop_size, input_size)
+            u (numpy.ndarray): input, shape(pred_len, input_size)
+                or shape(pop_size, pred_len, input_size)
         Returns:
-            cost (numpy.ndarray): cost of input, none or shape(pop_size, )
+            cost (numpy.ndarray): cost of input, shape(pred_len, input_size) or
+                shape(pop_size, pred_len, input_size)
         """
-        return (u**2) * np.diag(TwoWheeledConfigModule.R) * 0.1
+        return (u**2) * np.diag(TwoWheeledConfigModule.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)
+                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, none or shape(pop_size, )
+            cost (numpy.ndarray): cost of state, shape(pred_len, state_size) or
+                shape(pop_size, pred_len, state_size)
         """
         return ((x - g_x)**2) * np.diag(TwoWheeledConfigModule.Q)
 
@@ -80,7 +96,93 @@ def terminal_state_cost_fn(terminal_x, terminal_g_x):
             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, )
+            cost (numpy.ndarray): cost of state, shape(pred_len, ) or
+                shape(pop_size, pred_len)
         """
         return ((terminal_x - terminal_g_x)**2) \
-                * np.diag(TwoWheeledConfigModule.Sf)
+                * np.diag(TwoWheeledConfigModule.Sf)
+    
+    @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:
+            return 2. * (x - g_x) * np.diag(TwoWheeledConfigModule.Q)
+        
+        return (2. * (x - g_x) \
+            * np.diag(TwoWheeledConfigModule.Sf))[np.newaxis, :]
+
+    @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(TwoWheeledConfigModule.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, _) = x.shape
+            return -g_x[:, :, np.newaxis] \
+                * np.tile(2.*TwoWheeledConfigModule.Q, (pred_len, 1, 1))               
+        
+        return -g_x[:, np.newaxis] \
+            * np.tile(2.*TwoWheeledConfigModule.Sf, (1, 1, 1))    
+
+    @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(2.*TwoWheeledConfigModule.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/controllers/controller.py

@@ -24,7 +24,8 @@ def obtain_sol(self, curr_x, g_xs):
         Returns:
             opt_input (numpy.ndarray): optimal input, shape(input_size, )
         """
-        raise NotImplementedError("Implement gradient of hamitonian with respect to the state")
+        raise NotImplementedError("Implement the algorithm to \
+                                   get optimal input")
 
     def calc_cost(self, curr_x, samples, g_xs):
         """ calculate the cost of input samples