Add model of nonlinear sample system

Environments.md

@@ -6,6 +6,8 @@
 | Two wheeled System (Constant Goal) | x | ✓ | 3 | 2 | 
 | Two wheeled System (Moving Goal) (Coming soon) | x | ✓ | 3 | 2 | 
 | Cartpole (Swing up) | x | ✓ | 4 | 1 | 
+| Nonlinear Sample System Env | x | ✓ | 2 | 1 | 
+
 
 ## [FistOrderLagEnv](PythonLinearNonlinearControl/envs/first_order_lag.py)
 
@@ -54,3 +56,13 @@ mc = 1, mp = 0.2, l = 0.5, g = 9.81
 ### Cost.
 
 <img src="assets/cartpole_score.png" width="300">
+
+## [Nonlinear Sample System Env](PythonLinearNonlinearControl/envs/nonlinear_sample_system.py)
+
+## System equation.
+
+<img src="assets/nonlinear_sample_system.png" width="400">
+
+### Cost.
+
+<img src="assets/nonlinear_sample_system_score.png" width="400">

PythonLinearNonlinearControl/common/utils.py

@@ -46,15 +46,16 @@ def fit_angle_in_range(angles, min_angle=-np.pi, max_angle=np.pi):
     return output.reshape(output_shape)
 
 
-def update_state_with_Runge_Kutta(state, u, functions, dt=0.01):
+def update_state_with_Runge_Kutta(state, u, functions, dt=0.01, batch=True):
     """ update state in Runge Kutta methods
     Args:
         state (array-like): state of system
         u (array-like): input of system
         functions (list): update function of each state,
-            each function will be called like func(*state, *u)
+            each function will be called like func(state, u)
             We expect that this function returns differential of each state 
         dt (float): float in seconds
+        batch (bool): state and u is given by batch or not
 
     Returns:
         next_state (np.array): next state of system
@@ -68,6 +69,7 @@ def update_state_with_Runge_Kutta(state, u, functions, dt=0.01):
 
         Note that the function return x_dot.
     """
+    if not batch:
         state_size = len(state)
         assert state_size == len(functions), \
             "Invalid functions length, You need to give the state size functions"
@@ -77,27 +79,40 @@ def update_state_with_Runge_Kutta(state, u, functions, dt=0.01):
         k2 = np.zeros(state_size)
         k3 = np.zeros(state_size)
 
-    inputs = np.concatenate([state, u])
+        for i, func in enumerate(functions):
+            k0[i] = dt * func(state, u)
 
         for i, func in enumerate(functions):
-        k0[i] = dt * func(*inputs)
-
-    add_state = state + k0 / 2.
-    inputs = np.concatenate([add_state, u])
+            k1[i] = dt * func(state + k0 / 2., u)
 
         for i, func in enumerate(functions):
-        k1[i] = dt * func(*inputs)
-
-    add_state = state + k1 / 2.
-    inputs = np.concatenate([add_state, u])
+            k2[i] = dt * func(state + k1 / 2., u)
 
         for i, func in enumerate(functions):
-        k2[i] = dt * func(*inputs)
-
-    add_state = state + k2
-    inputs = np.concatenate([add_state, u])
-
-    for i, func in enumerate(functions):
-        k3[i] = dt * func(*inputs)
+            k3[i] = dt * func(state + k2, u)
+
+        return (k0 + 2. * k1 + 2. * k2 + k3) / 6.
+
+    else:
+        batch_size, state_size = state.shape
+        assert state_size == len(functions), \
+            "Invalid functions length, You need to give the state size functions"
+
+        k0 = np.zeros(batch_size, state_size)
+        k1 = np.zeros(batch_size, state_size)
+        k2 = np.zeros(batch_size, state_size)
+        k3 = np.zeros(batch_size, state_size)
+
+        for i, func in enumerate(functions):
+            k0[:, i] = dt * func(state, u)
+
+        for i, func in enumerate(functions):
+            k1[:, i] = dt * func(state + k0 / 2., u)
+
+        for i, func in enumerate(functions):
+            k2[:, i] = dt * func(state + k1 / 2., u)
+
+        for i, func in enumerate(functions):
+            k3[:, i] = dt * func(state + k2, u)
 
         return (k0 + 2. * k1 + 2. * k2 + k3) / 6.

PythonLinearNonlinearControl/envs/nonlinear_sample_system.py

@@ -59,9 +59,9 @@ class NonlinearSampleEnv(Env):
                     self.config["input_lower_bound"],
                     self.config["input_upper_bound"])
 
-        funtions = [self._func_x_1, self._func_x_2]
+        functions = [self._func_x_1, self._func_x_2]
 
-        next_x = update_state_with_Runge_Kutta(self._curr_x, u,
+        next_x = update_state_with_Runge_Kutta(self.curr_x, u,
                                                functions, self.config["dt"])
 
         # cost
@@ -82,16 +82,16 @@ class NonlinearSampleEnv(Env):
             self.step_count > self.config["max_step"], \
             {"goal_state": self.g_x}
 
-    def _func_x_1(self, x_1, x_2, u):
+    def _func_x_1(self, x, u):
         """
         """
-        x_dot = x_2
+        x_dot = x[1]
         return x_dot
 
-    def _func_x_2(self, x_1, x_2, u):
+    def _func_x_2(self, x, u):
         """
         """
-        x_dot = (1. - x_1**2 - x_2**2) * x_2 - x_1 + u
+        x_dot = (1. - x[0]**2 - x[1]**2) * x[1] - x[0] + u
         return x_dot
 
     def plot_func(self, to_plot, i=None, history_x=None, history_g_x=None):