Update: fix ilqr and ddp, models

Author: Shunichi09

PythonLinearNonlinearControl/configs/two_wheeled.py

@@ -10,9 +10,9 @@ class TwoWheeledConfigModule():
     INPUT_SIZE = 2
     DT = 0.01
     # cost parameters
-    R = np.eye(INPUT_SIZE) * 0.1
-    Q = np.eye(STATE_SIZE) * 0.5
-    Sf = np.eye(STATE_SIZE)
+    R = np.diag([0.1, 0.1])
+    Q = np.diag([1., 1., 0.01])
+    Sf = np.diag([5., 5., 1.])
     # bounds
     INPUT_LOWER_BOUND = np.array([-1.5, 3.14])
     INPUT_UPPER_BOUND = np.array([1.5, 3.14])
@@ -41,15 +41,15 @@ def __init__(self):
             },
            "iLQR":{
                 "max_iter": 500,
-                "mu": 1.,
+                "init_mu": 1.,
                 "mu_min": 1e-6,
                 "mu_max": 1e10,
                 "init_delta": 2.,
                 "threshold": 1e-6,
            },
            "DDP":{
                 "max_iter": 500,
-                "mu": 1.,
+                "init_mu": 1.,
                 "mu_min": 1e-6,
                 "mu_max": 1e10,
                 "init_delta": 2.,

PythonLinearNonlinearControl/controllers/ddp.py

@@ -12,9 +12,11 @@ class DDP(Controller):
     """ Differential Dynamic Programming
 
     Ref:
-        Tassa, Y., Erez, T., & Todorov, E. (2012). . In 2012 IEEE/RSJ International Conference on
+        Tassa, Y., Erez, T., & Todorov, E. (2012). 
+        In 2012 IEEE/RSJ International Conference on
         Intelligent Robots and Systems (pp. 4906-4913). and Study Wolf,
-        https://github.com/studywolf/control
+        https://github.com/studywolf/control, and
+        https://github.com/anassinator/ilqr
     """
     def __init__(self, config, model):
         """
@@ -41,7 +43,8 @@ def __init__(self, config, model):
 
         # controller parameters
         self.max_iter = config.opt_config["DDP"]["max_iter"]
-        self.mu = config.opt_config["DDP"]["mu"]
+        self.init_mu = config.opt_config["DDP"]["init_mu"]
+        self.mu = self.init_mu
         self.mu_min = config.opt_config["DDP"]["mu_min"]
         self.mu_max = config.opt_config["DDP"]["mu_max"]
         self.init_delta = config.opt_config["DDP"]["init_delta"]
@@ -81,6 +84,8 @@ def obtain_sol(self, curr_x, g_xs):
         sol = self.prev_sol.copy()
         converged_sol = False
         update_sol = True
+        self.mu = self.init_mu
+        self.delta = self.init_delta
 
         # line search param
         alphas = 1.1**(-np.arange(10)**2)

PythonLinearNonlinearControl/controllers/ilqr.py

@@ -41,7 +41,8 @@ def __init__(self, config, model):
 
         # controller parameters
         self.max_iter = config.opt_config["iLQR"]["max_iter"]
-        self.mu = config.opt_config["iLQR"]["mu"]
+        self.init_mu = config.opt_config["iLQR"]["init_mu"]
+        self.mu = self.init_mu
         self.mu_min = config.opt_config["iLQR"]["mu_min"]
         self.mu_max = config.opt_config["iLQR"]["mu_max"]
         self.init_delta = config.opt_config["iLQR"]["init_delta"]
@@ -81,6 +82,8 @@ def obtain_sol(self, curr_x, g_xs):
         sol = self.prev_sol.copy()
         converged_sol = False
         update_sol = True
+        self.mu = self.init_mu
+        self.delta = self.init_delta
 
         # line search param
         alphas = 1.1**(-np.arange(10)**2)

PythonLinearNonlinearControl/controllers/make_controllers.py

@@ -18,4 +18,4 @@ def make_controller(args, config, model):
     elif args.controller_type == "iLQR":
         return iLQR(config, model)
     elif args.controller_type == "DDP":
-        return iLQR(config, model)
+        return DDP(config, model)

PythonLinearNonlinearControl/envs/two_wheeled.py

@@ -56,7 +56,7 @@ def reset(self, init_x=None):
             self.curr_x = init_x
 
         # goal
-        self.g_x = np.array([5., 5., 0.])
+        self.g_x = np.array([2.5, 2.5, 0.])
 
         # clear memory
         self.history_x = []

PythonLinearNonlinearControl/models/two_wheeled.py

@@ -121,7 +121,7 @@ def calc_f_xx(xs, us, dt):
         f_xx[:, 0, 2, 2] = -np.cos(xs[:, 2]) * us[:, 0]
         f_xx[:, 1, 2, 2] = -np.sin(xs[:, 2]) * us[:, 0]
 
-        return f_xx
+        return f_xx * dt
 
     @staticmethod
     def calc_f_ux(xs, us, dt):
@@ -144,7 +144,7 @@ def calc_f_ux(xs, us, dt):
         f_ux[:, 0, 0, 2] = -np.sin(xs[:, 2])
         f_ux[:, 1, 0, 2] = np.cos(xs[:, 2])
 
-        return f_ux
+        return f_ux * dt
 
     @staticmethod
     def calc_f_uu(xs, us, dt):
@@ -164,4 +164,4 @@ def calc_f_uu(xs, us, dt):
 
         f_uu = np.zeros((pred_len, state_size, input_size, input_size))
 
-        return f_uu
+        return f_uu * dt