Add: two wheeled model and environment

Author: Shunichi09

.travis.yml

@@ -0,0 +1,14 @@
+language: python
+
+python:
+  - 3.7
+
+install:
+  - pip install --upgrade pip setuptools wheel
+  - pip install coveralls
+
+script:
+  - coverage run --source=PythonLinearNonlinearControl setup.py test
+
+after_success:
+  - coveralls

PythonLinearNonlinearControl/configs/first_order_lag.py

@@ -23,8 +23,6 @@ class FirstOrderLagConfigModule():
 
     def __init__(self):
         """ 
-        Args:
-            save_dit (str): save directory
         """
         # opt configs
         self.opt_config = {

PythonLinearNonlinearControl/configs/make_configs.py

@@ -1,9 +1,12 @@
 from .first_order_lag import FirstOrderLagConfigModule
+from .two_wheeled import TwoWheeledConfigModule
 
 def make_config(args):
     """
     Returns:
         config (ConfigModule class): configuration for the each env
     """
     if args.env == "FirstOrderLag":
-        return FirstOrderLagConfigModule()
+        return FirstOrderLagConfigModule()
+    elif args.env == "TwoWheeledConst" or args.env == "TwoWheeled":
+        return TwoWheeledConfigModule()

PythonLinearNonlinearControl/configs/two_wheeled.py

@@ -0,0 +1,86 @@
+import numpy as np
+
+class TwoWheeledConfigModule():
+    # parameters
+    ENV_NAME = "TwoWheeled-v0"
+    TYPE = "Nonlinear"
+    TASK_HORIZON = 1000
+    PRED_LEN = 10
+    STATE_SIZE = 3
+    INPUT_SIZE = 2
+    DT = 0.01
+    # cost parameters
+    R = np.eye(INPUT_SIZE)
+    Q = np.eye(STATE_SIZE)
+    Sf = np.eye(STATE_SIZE)
+    # bounds
+    INPUT_LOWER_BOUND = np.array([-1.5, 3.14])
+    INPUT_UPPER_BOUND = np.array([1.5, 3.14])
+
+    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":1.,
+                "threshold":0.001
+            },
+            "MPPI":{
+                "beta" : 0.6,
+                "popsize": 5000,
+                "kappa": 0.9,
+                "noise_sigma": 0.5,
+            },
+           "iLQR":{
+           },
+           "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)
+        Returns:
+            cost (numpy.ndarray): cost of input, none or shape(pop_size, )
+        """
+        return (u**2) * np.diag(TwoWheeledConfigModule.R) * 0.1
+    
+    @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)
+        Returns:
+            cost (numpy.ndarray): cost of state, none or shape(pop_size, )
+        """
+        return ((x - g_x)**2) * np.diag(TwoWheeledConfigModule.Q)
+
+    @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, none or shape(pop_size, )
+        """
+        return ((terminal_x - terminal_g_x)**2) \
+                * np.diag(TwoWheeledConfigModule.Sf)

PythonLinearNonlinearControl/envs/first_order_lag.py

@@ -70,13 +70,13 @@ def reset(self, init_x=None):
             self.curr_x = init_x
 
         # goal
-        self.goal_state = np.array([0., 0, -2., 3.])
+        self.g_x = np.array([0., 0, -2., 3.])
 
         # clear memory
         self.history_x = []
         self.history_g_x = []
 
-        return self.curr_x, {"goal_state": self.goal_state}
+        return self.curr_x, {"goal_state": self.g_x}
 
     def step(self, u):
         """
@@ -99,15 +99,17 @@ def step(self, u):
         # cost
         cost = 0
         cost = np.sum(u**2)
-        cost += np.sum((self.curr_x-g_x)**2)
+        cost += np.sum((self.curr_x - self.g_x)**2)
 
         # save history
         self.history_x.append(next_x.flatten())
-        self.history_g_x.append(self.goal_state.flatten())
+        self.history_g_x.append(self.g_x.flatten())
 
         # update
         self.curr_x = next_x.flatten()
         # update costs
         self.step_count += 1
 
-        return next_x.flatten(), cost, self.step_count > self.config["max_step"], {"goal_state" : self.goal_state}
+        return next_x.flatten(), cost, \
+               self.step_count > self.config["max_step"], \
+               {"goal_state" : self.g_x}

PythonLinearNonlinearControl/envs/make_envs.py

@@ -1,8 +1,11 @@
 from .first_order_lag import FirstOrderLagEnv
+from .two_wheeled import TwoWheeledConstEnv
 
 def make_env(args):
 
     if args.env == "FirstOrderLag":
         return FirstOrderLagEnv()
+    elif args.env == "TwoWheeledConst":
+        return TwoWheeledConstEnv()
 
-    raise NotImplementedError("There is not {} Env".format(name))
+    raise NotImplementedError("There is not {} Env".format(args.env))

PythonLinearNonlinearControl/envs/two_wheeled.py

@@ -1,96 +1,49 @@
 import numpy as np
-import scipy
-from scipy import integrate
+
 from .env import Env
 
-class TwoWheeledConstEnv(Env):
-    """ Two wheeled robot with constant goal Env
+def step_two_wheeled_env(curr_x, u, dt, method="Oylar"):
+    """ step two wheeled enviroment
+    
+    Args:
+        curr_x (numpy.ndarray): current state, shape(state_size, )
+        u (numpy.ndarray): input, shape(input_size, )
+        dt (float): sampling time
+    Returns:
+        next_x (numpy.ndarray): next state, shape(state_size. )
+    
+    Notes:
+        TODO: deal with another method, like Runge Kutta
     """
-    def __init__(self):
-        """
-        """
-        self.config = {"state_size" : 3,\
-                       "input_size" : 2,\
-                       "dt" : 0.01,\
-                       "max_step" : 500,\
-                       "input_lower_bound": [-1.5, -3.14],\
-                       "input_upper_bound": [1.5, 3.14],
-                       }
-
-        super(TwoWheeledEnv, self).__init__(self.config)
+    B = np.array([[np.cos(curr_x[-1]), 0.],
+                  [np.sin(curr_x[-1]), 0.],
+                  [0., 1.]])
 
-    def reset(self, init_x=None):
-        """ reset state
-        Returns:
-            init_x (numpy.ndarray): initial state, shape(state_size, )  
-            info (dict): information
-        """
-        self.step_count = 0
-        
-        self.curr_x = np.zeros(self.config["state_size"])
-
-        if init_x is not None:
-            self.curr_x = init_x
-
-        # goal
-        self.goal_state = np.array([0., 0, -2., 3.])
-        
-        # clear memory
-        self.history_x = []
-        self.history_g_x = []
-
-        return self.curr_x, {"goal_state": self.goal_state}
-
-    def step(self, u):
-        """
-        Args:
-            u (numpy.ndarray) : input, shape(input_size, )
-        Returns:
-            next_x (numpy.ndarray): next state, shape(state_size, ) 
-            cost (float): costs
-            done (bool): end the simulation or not
-            info (dict): information 
-        """
-        # clip action
-        u = np.clip(u,
-                    self.config["input_lower_bound"],
-                    self.config["input_lower_bound"])
+    x_dot = np.matmul(B, u[:, np.newaxis])
 
-        # step
-        next_x = np.matmul(self.A, self.curr_x[:, np.newaxis]) \
-                 + np.matmul(self.B, u[:, np.newaxis])
-
-        # TODO: implement costs
+    next_x = x_dot.flatten() * dt + curr_x
 
-        # save history
-        self.history_x.append(next_x.flatten())
-        self.history_g_x.append(self.goal_state.flatten())
-        
-        # update
-        self.curr_x = next_x.flatten()
-        # update costs
-        self.step_count += 1
+    return next_x
 
-        return next_x.flatten(), 0., self.step_count > self.config["max_step"], {"goal_state" : self.goal_state}
-    
-class TwoWheeledEnv(Env):
-    """ Two wheeled robot Env
+class TwoWheeledConstEnv(Env):
+    """ Two wheeled robot with constant goal Env
     """
     def __init__(self):
         """
         """
         self.config = {"state_size" : 3,\
                        "input_size" : 2,\
                        "dt" : 0.01,\
-                       "max_step" : 500,\
+                       "max_step" : 1000,\
                        "input_lower_bound": [-1.5, -3.14],\
                        "input_upper_bound": [1.5, 3.14],
                        }
 
-        super(TwoWheeledEnv, self).__init__(self.config)
+        super(TwoWheeledConstEnv, self).__init__(self.config)
 
     def reset(self, init_x=None):
         """ reset state
+
         Returns:
             init_x (numpy.ndarray): initial state, shape(state_size, )  
             info (dict): information
@@ -103,16 +56,17 @@ def reset(self, init_x=None):
             self.curr_x = init_x
 
         # goal
-        self.goal_state = np.array([0., 0, -2., 3.])
+        self.g_x = np.array([5., 5., 0.])
 
         # clear memory
         self.history_x = []
         self.history_g_x = []
 
-        return self.curr_x, {"goal_state": self.goal_state}
+        return self.curr_x, {"goal_state": self.g_x}
 
     def step(self, u):
-        """
+        """ step environments
+
         Args:
             u (numpy.ndarray) : input, shape(input_size, )
         Returns:
@@ -124,22 +78,25 @@ def step(self, u):
         # clip action
         u = np.clip(u,
                     self.config["input_lower_bound"],
-                    self.config["input_lower_bound"])
+                    self.config["input_upper_bound"])
 
         # step
-        next_x = np.matmul(self.A, self.curr_x[:, np.newaxis]) \
-                 + np.matmul(self.B, u[:, np.newaxis])
+        next_x = step_two_wheeled_env(self.curr_x, u, self.config["dt"])
 
-        # TODO: implement costs
+        # TODO: costs
+        costs = 0.
+        costs += 0.1 * np.sum(u**2)
+        costs += np.sum((self.curr_x - self.g_x)**2)
 
         # save history
         self.history_x.append(next_x.flatten())
-        self.history_g_x.append(self.goal_state.flatten())
+        self.history_g_x.append(self.g_x.flatten())
 
         # update
         self.curr_x = next_x.flatten()
         # update costs
         self.step_count += 1
 
-        return next_x.flatten(), 0., self.step_count > self.config["max_step"], {"goal_state" : self.goal_state}
-    
+        return next_x.flatten(), costs, \
+               self.step_count > self.config["max_step"], \
+               {"goal_state" : self.g_x}

PythonLinearNonlinearControl/models/make_models.py

@@ -1,8 +1,11 @@
 from .first_order_lag import FirstOrderLagModel
+from .two_wheeled import TwoWheeledModel
 
 def make_model(args, config):
 
     if args.env == "FirstOrderLag":
         return FirstOrderLagModel(config)
+    elif args.env == "TwoWheeledConst" or args.env == "TwoWheeled":
+        return TwoWheeledModel(config)
 
     raise NotImplementedError("There is not {} Model".format(args.env))