Merge pull request #6 from Shunichi09/develop

Add: TwoWheeledTrack Env

PythonLinearNonlinearControl/common/utils.py

@@ -1 +1,44 @@
 import numpy as np
+
+def rotate_pos(pos, angle):
+    """ Transformation the coordinate in the angle
+    
+    Args:
+        pos (numpy.ndarray): local state, shape(data_size, 2) 
+        angle (float): rotate angle, in radians
+    Returns:
+        rotated_pos (numpy.ndarray): shape(data_size, 2)
+    """
+    rot_mat = np.array([[np.cos(angle), -np.sin(angle)],
+                        [np.sin(angle), np.cos(angle)]])
+
+    return np.dot(pos, rot_mat.T)
+
+def fit_angle_in_range(angles, min_angle=-np.pi, max_angle=np.pi):
+    """ Check angle range and correct the range
+    
+    Args:
+        angle (numpy.ndarray): in radians
+        min_angle (float): maximum of range in radians, default -pi
+        max_angle (float): minimum of range in radians, default pi
+    Returns: 
+        fitted_angle (numpy.ndarray): range angle in radians
+    """
+    if max_angle < min_angle:
+        raise ValueError("max angle must be greater than min angle")
+    if (max_angle - min_angle) < 2.0 * np.pi:
+        raise ValueError("difference between max_angle \
+                          and min_angle must be greater than 2.0 * pi")
+    
+    output = np.array(angles)
+    output_shape = output.shape
+
+    output = output.flatten()
+    output -= min_angle
+    output %= 2 * np.pi
+    output += 2 * np.pi
+    output %= 2 * np.pi
+    output += min_angle
+
+    output = np.minimum(max_angle, np.maximum(min_angle, output))
+    return output.reshape(output_shape)

PythonLinearNonlinearControl/configs/cartpole.py

@@ -3,6 +3,7 @@ import numpy as np
 class CartPoleConfigModule():
     # parameters
     ENV_NAME = "CartPole-v0"
+    PLANNER_TYPE = "Const"
     TYPE = "Nonlinear"
     TASK_HORIZON = 500
     PRED_LEN = 50
@@ -10,9 +11,9 @@ class CartPoleConfigModule():
     INPUT_SIZE = 1
     DT = 0.02
     # cost parameters
-    R = np.diag([1.])  # 0.01 is worked for MPPI and CEM and MPPIWilliams
+    R = np.diag([0.01])  # 0.01 is worked for MPPI and CEM and MPPIWilliams
                        # 1. is worked for iLQR 
-    Terminal_Weight = 1.
+    TERMINAL_WEIGHT = 1.
     Q = None
     Sf = None
     # bounds
@@ -23,6 +24,7 @@ class CartPoleConfigModule():
     MC = 1.
     L = 0.5
     G = 9.81
+    CART_SIZE = (0.15, 0.1)
 
     def __init__(self):
         """ 
@@ -76,6 +78,7 @@ class CartPoleConfigModule():
     @staticmethod
     def input_cost_fn(u):
         """ input cost functions
+
         Args:
             u (numpy.ndarray): input, shape(pred_len, input_size)
                 or shape(pop_size, pred_len, input_size)
@@ -88,6 +91,7 @@ class CartPoleConfigModule():
     @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)
@@ -118,6 +122,7 @@ class CartPoleConfigModule():
     @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)
@@ -133,13 +138,13 @@ class CartPoleConfigModule():
                    + 12. * ((np.cos(terminal_x[:, 2]) + 1.)**2) \
                    + 0.1 * (terminal_x[:, 1]**2) \
                    + 0.1 * (terminal_x[:, 3]**2))[:, np.newaxis] \
-                    * CartPoleConfigModule.Terminal_Weight
+                    * CartPoleConfigModule.TERMINAL_WEIGHT
             
         return (6. * (terminal_x[0]**2) \
                + 12. * ((np.cos(terminal_x[2]) + 1.)**2) \
                + 0.1 * (terminal_x[1]**2) \
                + 0.1 * (terminal_x[3]**2)) \
-                * CartPoleConfigModule.Terminal_Weight
+                * CartPoleConfigModule.TERMINAL_WEIGHT
     
     @staticmethod
     def gradient_cost_fn_with_state(x, g_x, terminal=False):
@@ -168,7 +173,7 @@ class CartPoleConfigModule():
         cost_dx3 = 0.2 * x[3]
         cost_dx = np.array([[cost_dx0, cost_dx1, cost_dx2, cost_dx3]])
         
-        return cost_dx * CartPoleConfigModule.Terminal_Weight
+        return cost_dx * CartPoleConfigModule.TERMINAL_WEIGHT
 
     @staticmethod
     def gradient_cost_fn_with_input(x, u):
@@ -177,7 +182,6 @@ class CartPoleConfigModule():
         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)
         """
@@ -190,7 +194,6 @@ class CartPoleConfigModule():
         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
@@ -220,7 +223,7 @@ class CartPoleConfigModule():
                           * -np.cos(x[2])
         hessian[3, 3] = 0.2
 
-        return hessian[np.newaxis, :, :] * CartPoleConfigModule.Terminal_Weight
+        return hessian[np.newaxis, :, :] * CartPoleConfigModule.TERMINAL_WEIGHT
 
     @staticmethod
     def hessian_cost_fn_with_input(x, u):
@@ -229,7 +232,6 @@ class CartPoleConfigModule():
         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)
@@ -245,7 +247,6 @@ class CartPoleConfigModule():
         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)

PythonLinearNonlinearControl/configs/make_configs.py

@@ -9,7 +9,7 @@ def make_config(args):
     """
     if args.env == "FirstOrderLag":
         return FirstOrderLagConfigModule()
-    elif args.env == "TwoWheeledConst" or args.env == "TwoWheeled":
+    elif args.env == "TwoWheeledConst" or args.env == "TwoWheeledTrack":
         return TwoWheeledConfigModule()
     elif args.env == "CartPole":
         return CartPoleConfigModule()

PythonLinearNonlinearControl/configs/two_wheeled.py

@@ -1,21 +1,37 @@
 import numpy as np
+from matplotlib.axes import Axes
+
+from ..plotters.plot_objs import square_with_angle, square
+from ..common.utils import fit_angle_in_range
 
 class TwoWheeledConfigModule():
     # parameters
     ENV_NAME = "TwoWheeled-v0"
     TYPE = "Nonlinear"
+    N_AHEAD = 1
     TASK_HORIZON = 1000
     PRED_LEN = 20
     STATE_SIZE = 3
     INPUT_SIZE = 2
     DT = 0.01
     # cost parameters
+    # for Const goal
+    """
     R = np.diag([0.1, 0.1])
     Q = np.diag([1., 1., 0.01])
     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])
+    
     # bounds
-    INPUT_LOWER_BOUND = np.array([-1.5, 3.14])
+    INPUT_LOWER_BOUND = np.array([-1.5, -3.14])
     INPUT_UPPER_BOUND = np.array([1.5, 3.14])
+    # parameters
+    CAR_SIZE = 0.2
+    WHEELE_SIZE = (0.075, 0.015)
 
     def __init__(self):
         """ 
@@ -78,6 +94,27 @@ class TwoWheeledConfigModule():
         """
         return (u**2) * np.diag(TwoWheeledConfigModule.R)
     
+    @staticmethod
+    def fit_diff_in_range(diff_x):
+        """ fit difference state in range(angle)
+
+        Args:
+            diff_x (numpy.ndarray): 
+                shape(pop_size, pred_len, state_size) or
+                shape(pred_len, state_size) or
+                shape(state_size, )
+        Returns:
+            fitted_diff_x (numpy.ndarray): same shape as diff_x
+        """
+        if len(diff_x.shape) == 3:
+            diff_x[:, :, -1] = fit_angle_in_range(diff_x[:, :, -1]) 
+        elif len(diff_x.shape) == 2:
+            diff_x[:, -1] = fit_angle_in_range(diff_x[:, -1])
+        elif len(diff_x.shape) == 1:
+            diff_x[-1] = fit_angle_in_range(diff_x[-1])
+
+        return diff_x
+
     @staticmethod
     def state_cost_fn(x, g_x):
         """ state cost function
@@ -90,7 +127,8 @@ class TwoWheeledConfigModule():
             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)
+        diff = TwoWheeledConfigModule.fit_diff_in_range(x - g_x)
+        return ((diff)**2) * np.diag(TwoWheeledConfigModule.Q)
 
     @staticmethod
     def terminal_state_cost_fn(terminal_x, terminal_g_x):
@@ -104,8 +142,10 @@ class TwoWheeledConfigModule():
             cost (numpy.ndarray): cost of state, shape(pred_len, ) or
                 shape(pop_size, pred_len)
         """
-        return ((terminal_x - terminal_g_x)**2) \
+        terminal_diff = TwoWheeledConfigModule.fit_diff_in_range(terminal_x \
-                * np.diag(TwoWheeledConfigModule.Sf)
+                                                        - terminal_g_x)
+        
+        return ((terminal_diff)**2) * np.diag(TwoWheeledConfigModule.Sf)
     
     @staticmethod
     def gradient_cost_fn_with_state(x, g_x, terminal=False):
@@ -119,10 +159,12 @@ class TwoWheeledConfigModule():
             l_x (numpy.ndarray): gradient of cost, shape(pred_len, state_size)
                 or shape(1, state_size)
         """
-        if not terminal:
+        diff = TwoWheeledConfigModule.fit_diff_in_range(x - g_x)
-            return 2. * (x - g_x) * np.diag(TwoWheeledConfigModule.Q)
         
-        return (2. * (x - g_x) \
+        if not terminal:
+            return 2. * (diff) * np.diag(TwoWheeledConfigModule.Q)
+        
+        return (2. * (diff) \
             * np.diag(TwoWheeledConfigModule.Sf))[np.newaxis, :]
 
     @staticmethod

PythonLinearNonlinearControl/controllers/make_controllers.py

@@ -21,4 +21,6 @@ def make_controller(args, config, model):
     elif args.controller_type == "iLQR":
         return iLQR(config, model)
     elif args.controller_type == "DDP":
-        return DDP(config, model)
+        return DDP(config, model)
+    
+    raise ValueError("No controller: {}".format(args.controller_type))

PythonLinearNonlinearControl/controllers/mpc.py

@@ -1,7 +1,8 @@
 from logging import getLogger
 
 import numpy as np
-from cvxopt import matrix, solvers
+from scipy.optimize import minimize
+from scipy.optimize import LinearConstraint
 
 from .controller import Controller
 from ..envs.cost import calc_cost
@@ -61,6 +62,7 @@ class LinearMPC(Controller):
         self.F = None
         self.f = None
         self.setup()
+        self.prev_sol = np.zeros(self.input_size*self.pred_len)
 
         # history
         self.history_u = [np.zeros(self.input_size)]
@@ -183,6 +185,22 @@ class LinearMPC(Controller):
 
         ub = np.array(b).flatten()
 
+        # using cvxopt
+        def optimized_func(dt_us):
+            return (np.dot(dt_us, np.dot(H, dt_us.reshape(-1, 1))) \
+                    - np.dot(G.T, dt_us.reshape(-1, 1)))[0]
+
+        # constraint
+        lb = np.array([-np.inf for _ in range(len(ub))])  # one side cons
+        cons = LinearConstraint(A, lb, ub)
+        # solve
+        opt_sol = minimize(optimized_func, self.prev_sol.flatten(),\
+                           constraints=[cons])
+        opt_dt_us = opt_sol.x
+
+        """ using cvxopt ver,
+        if you want to solve more quick please use cvxopt instead of scipy
+        
         # make cvxpy problem formulation
         P = 2*matrix(H)
         q = matrix(-1 * G)
@@ -190,12 +208,15 @@ class LinearMPC(Controller):
         b = matrix(ub)
 
         # solve the problem
-        opt_result = solvers.qp(P, q, G=A, h=b)
+        opt_sol = solvers.qp(P, q, G=A, h=b)
-        opt_dt_us = np.array(list(opt_result['x']))
+        opt_dt_us = np.array(list(opt_sol['x']))
+        """
+
         # to dt form
         opt_dt_u_seq = np.cumsum(opt_dt_us.reshape(self.pred_len,\
                                                    self.input_size),
                                  axis=0)
+        self.prev_sol = opt_dt_u_seq.copy()
         
         opt_u_seq = opt_dt_u_seq + self.history_u[-1]
         

PythonLinearNonlinearControl/envs/cartpole.py

@@ -1,6 +1,8 @@
 import numpy as np
+from matplotlib.axes import Axes
 
 from .env import Env
+from ..plotters.plot_objs import square
 
 class CartPoleEnv(Env):
     """ Cartpole Environment
@@ -24,6 +26,7 @@ class CartPoleEnv(Env):
                        "mc": 1.,
                        "l": 0.5,
                        "g": 9.81,
+                       "cart_size": (0.15, 0.1),
                        }
 
         super(CartPoleEnv, self).__init__(self.config)
@@ -112,4 +115,64 @@ class CartPoleEnv(Env):
 
         return next_x.flatten(), costs, \
                self.step_count > self.config["max_step"], \
-               {"goal_state" : self.g_x}
+               {"goal_state" : self.g_x}
+    
+    def plot_func(self, to_plot, i=None, history_x=None, history_g_x=None):
+        """ plot cartpole object function
+        
+        Args:
+            to_plot (axis or imgs): plotted objects
+            i (int): frame count 
+            history_x (numpy.ndarray): history of state, shape(iters, state)
+            history_g_x (numpy.ndarray): history of goal state,
+                                         shape(iters, state)
+        Returns:
+            None or imgs : imgs order is ["cart_img", "pole_img"]
+        """
+        if isinstance(to_plot, Axes):
+            imgs = {}  # create new imgs
+            
+            imgs["cart"] = to_plot.plot([], [], c="k")[0]
+            imgs["pole"] = to_plot.plot([], [], c="k", linewidth=5)[0]
+            imgs["center"] = to_plot.plot([], [], marker="o", c="k",\
+                                          markersize=10)[0]
+            # centerline
+            to_plot.plot(np.linspace(-1., 1., num=50), np.zeros(50),\
+                         c="k", linestyle="dashed")
+            
+            # set axis
+            to_plot.set_xlim([-1., 1.])
+            to_plot.set_ylim([-0.55, 1.5])
+            
+            return imgs
+
+        # set imgs
+        cart_x, cart_y, pole_x, pole_y = \
+            self._plot_cartpole(history_x[i])
+        
+        to_plot["cart"].set_data(cart_x, cart_y)
+        to_plot["pole"].set_data(pole_x, pole_y)
+        to_plot["center"].set_data(history_x[i][0], 0.)
+        
+    def _plot_cartpole(self, curr_x):
+        """ plot cartpole fucntions
+
+        Args:
+            curr_x (numpy.ndarray): current catpole state
+        Returns:
+            cart_x (numpy.ndarray): x data of cart
+            cart_y (numpy.ndarray): y data of cart 
+            pole_x (numpy.ndarray): x data of pole 
+            pole_y (numpy.ndarray): y data of pole 
+        """
+        # cart
+        cart_x, cart_y = square(curr_x[0], 0.,\
+                                self.config["cart_size"], 0.)
+    
+        # pole    
+        pole_x = np.array([curr_x[0], curr_x[0] + self.config["l"] \
+                                      * np.cos(curr_x[2]-np.pi/2)])
+        pole_y = np.array([0., self.config["l"] \
+                               * np.sin(curr_x[2]-np.pi/2)])
+
+        return cart_x, cart_y, pole_x, pole_y

PythonLinearNonlinearControl/envs/env.py

@@ -1,8 +1,9 @@
 import numpy as np
 
 class Env():
-    """
+    """ Environments class
     Attributes:
+    
         curr_x (numpy.ndarray): current state 
         history_x (list[numpy.ndarray]): historty of state, shape(step_count*state_size)
         step_count (int): step count
@@ -48,7 +49,7 @@ class Env():
         """
         raise NotImplementedError("Implement step function")
 
-    def __str__(self):
+    def __repr__(self):
         """
         """
         return self.config

PythonLinearNonlinearControl/envs/first_order_lag.py

@@ -112,4 +112,9 @@ class FirstOrderLagEnv(Env):
 
         return next_x.flatten(), cost, \
                self.step_count > self.config["max_step"], \
-               {"goal_state" : self.g_x}
+               {"goal_state" : self.g_x}
+    
+    def plot_func(self, to_plot, i=None, history_x=None, history_g_x=None):
+        """
+        """
+        raise ValueError("FirstOrderLag does not have animation")

PythonLinearNonlinearControl/envs/make_envs.py

@@ -1,5 +1,6 @@
 from .first_order_lag import FirstOrderLagEnv
 from .two_wheeled import TwoWheeledConstEnv
+from .two_wheeled import TwoWheeledTrackEnv
 from .cartpole import CartPoleEnv
 
 def make_env(args):
@@ -8,6 +9,8 @@ def make_env(args):
         return FirstOrderLagEnv()
     elif args.env == "TwoWheeledConst":
         return TwoWheeledConstEnv()
+    elif args.env == "TwoWheeledTrack":
+        return TwoWheeledTrackEnv()
     elif args.env == "CartPole":
         return CartPoleEnv()
     

PythonLinearNonlinearControl/envs/two_wheeled.py

@@ -1,6 +1,9 @@
 import numpy as np
+from matplotlib.axes import Axes
+import matplotlib.pyplot as plt
 
 from .env import Env
+from ..plotters.plot_objs import circle_with_angle, square, circle
 
 def step_two_wheeled_env(curr_x, u, dt, method="Oylar"):
     """ step two wheeled enviroment
@@ -34,9 +37,11 @@ class TwoWheeledConstEnv(Env):
         self.config = {"state_size" : 3,\
                        "input_size" : 2,\
                        "dt" : 0.01,\
-                       "max_step" : 1000,\
+                       "max_step" : 500,\
-                       "input_lower_bound": [-1.5, -3.14],\
+                       "input_lower_bound": (-1.5, -3.14),\
-                       "input_upper_bound": [1.5, 3.14],
+                       "input_upper_bound": (1.5, 3.14),\
+                       "car_size": 0.2,\
+                       "wheel_size": (0.075, 0.015)
                        }
 
         super(TwoWheeledConstEnv, self).__init__(self.config)
@@ -99,4 +104,279 @@ class TwoWheeledConstEnv(Env):
 
         return next_x.flatten(), costs, \
                self.step_count > self.config["max_step"], \
-               {"goal_state" : self.g_x}
+               {"goal_state" : self.g_x}
+    
+    def plot_func(self, to_plot, i=None, history_x=None, history_g_x=None):
+        """ plot cartpole object function
+        
+        Args:
+            to_plot (axis or imgs): plotted objects
+            i (int): frame count 
+            history_x (numpy.ndarray): history of state, shape(iters, state)
+            history_g_x (numpy.ndarray): history of goal state,
+                                         shape(iters, state)
+        
+        Returns:
+            None or imgs : imgs order is ["cart_img", "pole_img"]
+        """
+        if isinstance(to_plot, Axes):
+            imgs = {}  # create new imgs
+            
+            imgs["car"] = to_plot.plot([], [], c="k")[0]
+            imgs["car_angle"] = to_plot.plot([], [], c="k")[0]
+            imgs["left_tire"] = to_plot.plot([], [], c="k", linewidth=5)[0]
+            imgs["right_tire"] = to_plot.plot([], [], c="k", linewidth=5)[0]
+            imgs["goal"] = to_plot.plot([], [], marker="*",
+                                        c="b", markersize=10)[0]
+            imgs["traj"] = to_plot.plot([], [], c="b", linestyle="dashed")[0]
+
+            # set axis
+            to_plot.set_xlim([-1., 3.])
+            to_plot.set_ylim([-1., 3.])
+
+            return imgs
+
+        # set imgs
+        # car imgs
+        car_x, car_y, car_angle_x, car_angle_y, \
+        left_tire_x, left_tire_y, right_tire_x, right_tire_y = \
+            self._plot_car(history_x[i])
+        
+        to_plot["car"].set_data(car_x, car_y)
+        to_plot["car_angle"].set_data(car_angle_x, car_angle_y)
+        to_plot["left_tire"].set_data(left_tire_x, left_tire_y,)
+        to_plot["right_tire"].set_data(right_tire_x, right_tire_y)
+
+        # goal and trajs
+        to_plot["goal"].set_data(history_g_x[i, 0], history_g_x[i, 1])
+        to_plot["traj"].set_data(history_x[:i, 0], history_x[:i, 1])
+        
+    def _plot_car(self, curr_x):
+        """ plot car fucntions
+        """
+        # cart
+        car_x, car_y, car_angle_x, car_angle_y = \
+            circle_with_angle(curr_x[0], curr_x[1],
+                              self.config["car_size"], curr_x[2])
+        
+        # left tire
+        center_x = (self.config["car_size"] \
+                    + self.config["wheel_size"][1]) \
+                   * np.cos(curr_x[2]-np.pi/2.) + curr_x[0]
+        center_y = (self.config["car_size"] \
+                    + self.config["wheel_size"][1]) \
+                   * np.sin(curr_x[2]-np.pi/2.) + curr_x[1]
+        
+        left_tire_x, left_tire_y = \
+            square(center_x, center_y,                  
+                   self.config["wheel_size"], curr_x[2])
+
+        # right tire
+        center_x = (self.config["car_size"] \
+                    + self.config["wheel_size"][1]) \
+                   * np.cos(curr_x[2]+np.pi/2.) + curr_x[0]
+        center_y = (self.config["car_size"] \
+                    + self.config["wheel_size"][1]) \
+                   * np.sin(curr_x[2]+np.pi/2.) + curr_x[1]
+        
+        right_tire_x, right_tire_y = \
+            square(center_x, center_y,                  
+                   self.config["wheel_size"], curr_x[2])
+        
+        return car_x, car_y, car_angle_x, car_angle_y,\
+               left_tire_x, left_tire_y,\
+               right_tire_x, right_tire_y
+
+class TwoWheeledTrackEnv(Env):
+    """ Two wheeled robot with constant goal Env
+    """
+    def __init__(self):
+        """
+        """
+        self.config = {"state_size" : 3,\
+                       "input_size" : 2,\
+                       "dt" : 0.01,\
+                       "max_step" : 1000,\
+                       "input_lower_bound": (-1.5, -3.14),\
+                       "input_upper_bound": (1.5, 3.14),\
+                       "car_size": 0.2,\
+                       "wheel_size": (0.075, 0.015)
+                       }
+
+        super(TwoWheeledTrackEnv, self).__init__(self.config)
+    
+    @staticmethod
+    def make_road(linelength=3., circle_radius=1.):
+        """ make track
+
+        Returns:
+            road (numpy.ndarray): road info, shape(n_point, 3) x, y, angle
+        """
+        # line
+        # not include start points
+        line = np.linspace(-1.5, 1.5, num=51, endpoint=False)[1:]
+        line_1 = np.stack((line, np.zeros(50)), axis=1)
+        line_2 = np.stack((line[::-1], np.zeros(50)+circle_radius*2.), axis=1)
+
+        # circle
+        circle_1_x, circle_1_y = circle(linelength/2., circle_radius,
+            circle_radius, start=-np.pi/2., end=np.pi/2., n_point=50)
+        circle_1 = np.stack((circle_1_x , circle_1_y), axis=1)
+        
+        circle_2_x, circle_2_y = circle(-linelength/2., circle_radius,
+            circle_radius, start=np.pi/2., end=3*np.pi/2., n_point=50)
+        circle_2 = np.stack((circle_2_x , circle_2_y), axis=1)
+
+        road_pos = np.concatenate((line_1, circle_1, line_2, circle_2), axis=0)
+
+        # calc road angle
+        road_diff = road_pos[1:] - road_pos[:-1]
+        road_angle = np.arctan2(road_diff[:, 1], road_diff[:, 0]) 
+        road_angle = np.concatenate((np.zeros(1), road_angle))
+
+        road = np.concatenate((road_pos, road_angle[:, np.newaxis]), axis=1)
+
+        return np.tile(road, (3, 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.g_traj = self.make_road()
+        
+        # clear memory
+        self.history_x = []
+        self.history_g_x = []
+
+        return self.curr_x, {"goal_state": self.g_traj}
+
+    def step(self, u):
+        """ step environments
+
+        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_upper_bound"])
+
+        # step
+        next_x = step_two_wheeled_env(self.curr_x, u, self.config["dt"])
+
+        costs = 0.
+        costs += 0.1 * np.sum(u**2)
+        costs += np.min(np.linalg.norm(self.curr_x - self.g_traj, axis=1))
+
+        # save history
+        self.history_x.append(next_x.flatten())
+        
+        # update
+        self.curr_x = next_x.flatten()
+        # update costs
+        self.step_count += 1
+
+        return next_x.flatten(), costs, \
+               self.step_count > self.config["max_step"], \
+               {"goal_state" : self.g_traj}
+    
+    def plot_func(self, to_plot, i=None, history_x=None, history_g_x=None):
+        """ plot cartpole object function
+        
+        Args:
+            to_plot (axis or imgs): plotted objects
+            i (int): frame count 
+            history_x (numpy.ndarray): history of state, shape(iters, state)
+            history_g_x (numpy.ndarray): history of goal state,
+                                         shape(iters, state)
+        
+        Returns:
+            None or imgs : imgs order is ["cart_img", "pole_img"]
+        """
+        if isinstance(to_plot, Axes):
+            imgs = {}  # create new imgs
+            
+            imgs["car"] = to_plot.plot([], [], c="k")[0]
+            imgs["car_angle"] = to_plot.plot([], [], c="k")[0]
+            imgs["left_tire"] = to_plot.plot([], [], c="k", linewidth=5)[0]
+            imgs["right_tire"] = to_plot.plot([], [], c="k", linewidth=5)[0]
+            imgs["goal"] = to_plot.plot([], [], marker="*",
+                                        c="b", markersize=10)[0]
+            imgs["traj"] = to_plot.plot([], [], c="b", linestyle="dashed")[0]
+
+            # set axis
+            to_plot.set_xlim([-3., 3.])
+            to_plot.set_ylim([-1., 3.])
+
+            # plot road
+            to_plot.plot(self.g_traj[:, 0], self.g_traj[:, 1],
+                         c="k", linestyle="dashed")
+
+            return imgs
+
+        # set imgs
+        # car imgs
+        car_x, car_y, car_angle_x, car_angle_y, \
+        left_tire_x, left_tire_y, right_tire_x, right_tire_y = \
+            self._plot_car(history_x[i])
+        
+        to_plot["car"].set_data(car_x, car_y)
+        to_plot["car_angle"].set_data(car_angle_x, car_angle_y)
+        to_plot["left_tire"].set_data(left_tire_x, left_tire_y,)
+        to_plot["right_tire"].set_data(right_tire_x, right_tire_y)
+
+        # goal and trajs
+        to_plot["goal"].set_data(history_g_x[i, 0], history_g_x[i, 1])
+        to_plot["traj"].set_data(history_x[:i, 0], history_x[:i, 1])
+        
+    def _plot_car(self, curr_x):
+        """ plot car fucntions
+        """
+        # cart
+        car_x, car_y, car_angle_x, car_angle_y = \
+            circle_with_angle(curr_x[0], curr_x[1],
+                              self.config["car_size"], curr_x[2])
+        
+        # left tire
+        center_x = (self.config["car_size"] \
+                    + self.config["wheel_size"][1]) \
+                   * np.cos(curr_x[2]-np.pi/2.) + curr_x[0]
+        center_y = (self.config["car_size"] \
+                    + self.config["wheel_size"][1]) \
+                   * np.sin(curr_x[2]-np.pi/2.) + curr_x[1]
+        
+        left_tire_x, left_tire_y = \
+            square(center_x, center_y,                  
+                   self.config["wheel_size"], curr_x[2])
+
+        # right tire
+        center_x = (self.config["car_size"] \
+                    + self.config["wheel_size"][1]) \
+                   * np.cos(curr_x[2]+np.pi/2.) + curr_x[0]
+        center_y = (self.config["car_size"] \
+                    + self.config["wheel_size"][1]) \
+                   * np.sin(curr_x[2]+np.pi/2.) + curr_x[1]
+        
+        right_tire_x, right_tire_y = \
+            square(center_x, center_y,                  
+                   self.config["wheel_size"], curr_x[2])
+        
+        return car_x, car_y, car_angle_x, car_angle_y,\
+               left_tire_x, left_tire_y,\
+               right_tire_x, right_tire_y

PythonLinearNonlinearControl/models/make_models.py

@@ -6,7 +6,7 @@ def make_model(args, config):
     
     if args.env == "FirstOrderLag":
         return FirstOrderLagModel(config)
-    elif args.env == "TwoWheeledConst" or args.env == "TwoWheeled":
+    elif args.env == "TwoWheeledConst" or args.env == "TwoWheeledTrack":
         return TwoWheeledModel(config)
     elif args.env == "CartPole":
         return CartPoleModel(config)

PythonLinearNonlinearControl/planners/closest_point_planner.py

@@ -0,0 +1,39 @@
+import numpy as np
+from .planner import Planner
+
+class ClosestPointPlanner(Planner):
+    """ This planner make goal state according to goal path
+    """
+    def __init__(self, config):
+        """
+        """
+        super(ClosestPointPlanner, self).__init__()
+        self.pred_len = config.PRED_LEN
+        self.state_size = config.STATE_SIZE
+        self.n_ahead = config.N_AHEAD
+
+    def plan(self, curr_x, g_traj):
+        """
+        Args:
+            curr_x (numpy.ndarray): current state, shape(state_size)
+            g_x (numpy.ndarray): goal state, shape(state_size),
+                this state should be obtained from env
+        Returns:
+            g_xs (numpy.ndarrya): goal state, shape(pred_len+1, state_size)
+        """
+        min_idx = np.argmin(np.linalg.norm(curr_x[:-1] - g_traj[:, :-1],
+                                           axis=1))
+
+        start = (min_idx+self.n_ahead) 
+        if start > len(g_traj):
+            start = len(g_traj)
+
+        end = min_idx+self.n_ahead+self.pred_len+1
+
+        if (min_idx+self.n_ahead+self.pred_len+1) > len(g_traj):
+            end = len(g_traj)
+        
+        if abs(start - end) != self.pred_len + 1:
+            return np.tile(g_traj[-1], (self.pred_len+1, 1))
+
+        return g_traj[start:end]

PythonLinearNonlinearControl/planners/const_planner.py

@@ -1,7 +1,7 @@
 import numpy as np
 from .planner import Planner
 
-class ConstantPlanner():
+class ConstantPlanner(Planner):
     """ This planner make constant goal state
     """
     def __init__(self, config):

PythonLinearNonlinearControl/planners/make_planners.py

@@ -1,8 +1,15 @@
 from .const_planner import ConstantPlanner
+from .closest_point_planner import ClosestPointPlanner
 
 def make_planner(args, config):
     
-    if args.planner_type == "const":
+    if args.env == "FirstOrderLag":
+        return ConstantPlanner(config)
+    elif args.env == "TwoWheeledConst":
+        return ConstantPlanner(config)
+    elif args.env == "TwoWheeledTrack":
+        return ClosestPointPlanner(config)
+    elif args.env == "CartPole":
         return ConstantPlanner(config)
     
     raise NotImplementedError("There is not {} Planner".format(args.planner_type))

PythonLinearNonlinearControl/plotters/animator.py

@@ -0,0 +1,74 @@
+import os
+from logging import getLogger
+
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib.animation import FuncAnimation
+
+logger = getLogger(__name__)
+
+class Animator():
+    """ animation class
+    """
+    def __init__(self, args, env):
+        """
+        """
+        self.env_name = args.env
+        self.result_dir = args.result_dir
+        self.controller_type = args.controller_type
+
+        self.interval = env.config["dt"] * 1000.  # to ms
+        self.plot_func = env.plot_func
+
+        self.imgs = []
+
+    def _setup(self):
+        """ set up figure of animation
+        """
+        # make fig
+        self.anim_fig = plt.figure()
+
+        # axis        
+        self.axis = self.anim_fig.add_subplot(111)
+        self.axis.set_aspect('equal', adjustable='box')
+
+        self.imgs = self.plot_func(self.axis)
+
+    def _update_img(self, i, history_x, history_g_x):
+        """ update animation
+
+        Args:
+            i (int): frame count
+            history_x (numpy.ndarray): history of state,
+                                       shape(iters, state_size)
+            history_g (numpy.ndarray): history of goal state,
+                                       shape(iters, input_size)
+        """
+        self.plot_func(self.imgs, i, history_x, history_g_x)
+
+    def draw(self, history_x, history_g_x=None):
+        """draw the animation and save
+
+        Args:
+            history_x (numpy.ndarray): history of state,
+                                       shape(iters, state_size)
+            history_g (numpy.ndarray): history of goal state,
+                                       shape(iters, input_size)
+        Returns:
+            None
+        """
+        # set up animation figures
+        self._setup()
+        _update_img = lambda i: self._update_img(i, history_x, history_g_x)
+
+        # call funcanimation
+        animation = FuncAnimation(
+            self.anim_fig,
+            _update_img, interval=self.interval, frames=len(history_x)-1)
+
+        # save animation
+        path = os.path.join(self.result_dir, self.controller_type,
+                            "animation-" + self.env_name + ".mp4")
+        logger.info("Saved Animation to {} ...".format(path))
+        
+        animation.save(path, writer="ffmpeg")

PythonLinearNonlinearControl/plotters/plot_objs.py

@@ -0,0 +1,99 @@
+import os
+
+import numpy as np
+import matplotlib.pyplot as plt
+
+from ..common.utils import rotate_pos
+
+def circle(center_x, center_y, radius, start=0., end=2*np.pi, n_point=100):
+    """ Create circle matrix
+    
+    Args:
+        center_x (float): the center x position of the circle
+        center_y (float): the center y position of the circle
+        radius (float): in meters
+        start (float): start angle
+        end (float): end angle
+    Returns:
+        circle x : numpy.ndarray
+        circle y : numpy.ndarray
+    """
+    diff = end - start
+
+    circle_xs = []
+    circle_ys = []
+
+    for i in range(n_point + 1):
+        circle_xs.append(center_x + radius * np.cos(i*diff/n_point + start))
+        circle_ys.append(center_y + radius * np.sin(i*diff/n_point + start))
+
+    return np.array(circle_xs), np.array(circle_ys)
+
+def circle_with_angle(center_x, center_y, radius, angle):
+    """ Create circle matrix with angle line matrix
+
+    Args:    
+        center_x (float): the center x position of the circle
+        center_y (float): the center y position of the circle
+        radius (float): in meters
+        angle (float): in radians
+    Returns: 
+        circle_x (numpy.ndarray): x data of circle 
+        circle_y (numpy.ndarray): y data of circle
+        angle_x (numpy.ndarray): x data of circle angle
+        angle_y (numpy.ndarray): y data of circle angle
+    """
+    circle_x, circle_y = circle(center_x, center_y, radius)
+
+    angle_x = np.array([center_x, center_x + np.cos(angle) * radius])
+    angle_y = np.array([center_y, center_y + np.sin(angle) * radius])
+
+    return circle_x, circle_y, angle_x, angle_y
+
+def square(center_x, center_y, shape, angle):
+    """ Create square
+
+    Args:    
+        center_x (float): the center x position of the square
+        center_y (float): the center y position of the square
+        shape (tuple): the square's shape(width/2, height/2)
+        angle (float): in radians
+    Returns: 
+        square_x (numpy.ndarray): shape(5, ), counterclockwise from right-up
+        square_y (numpy.ndarray): shape(5, ), counterclockwise from right-up
+    """
+    # start with the up right points
+    # create point in counterclockwise, local
+    square_xy = np.array([[shape[0], shape[1]],
+                          [-shape[0], shape[1]],
+                          [-shape[0], -shape[1]],
+                          [shape[0], -shape[1]],
+                          [shape[0], shape[1]]])
+    # translate position to world
+    # rotation
+    trans_points = rotate_pos(square_xy, angle)
+    # translation
+    trans_points += np.array([center_x, center_y])
+    
+    return trans_points[:, 0], trans_points[:, 1]
+
+def square_with_angle(center_x, center_y, shape, angle):
+    """ Create square with angle line
+
+    Args:    
+        center_x (float): the center x position of the square
+        center_y (float): the center y position of the square
+        shape (tuple): the square's shape(width/2, height/2)
+        angle (float): in radians
+    Returns: 
+        square_x (numpy.ndarray): shape(5, ), counterclockwise from right-up
+        square_y (numpy.ndarray): shape(5, ), counterclockwise from right-up
+        angle_x (numpy.ndarray): x data of square angle
+        angle_y (numpy.ndarray): y data of square angle
+    """
+    square_x, square_y = square(center_x, center_y, shape, angle)
+
+    angle_x = np.array([center_x, center_x + np.cos(angle) * shape[0]])
+    angle_y = np.array([center_y, center_y + np.sin(angle) * shape[1]])
+
+    return square_x, square_y, angle_x, angle_y

PythonLinearNonlinearControl/runners/runner.py

@@ -29,7 +29,7 @@ class ExpRunner():
         while not done:
             logger.debug("Step = {}".format(step_count))
             # plan
-            g_xs = planner.plan(curr_x, g_x=info["goal_state"])
+            g_xs = planner.plan(curr_x, info["goal_state"])
 
             # obtain sol
             u = controller.obtain_sol(curr_x, g_xs)

README.md

@@ -5,8 +5,11 @@
 # PythonLinearNonLinearControl
 
 PythonLinearNonLinearControl is a library implementing the linear and nonlinear control theories in python.
+Due to use only basic libralies (scipy, numpy), this library is also easily to extend for your own situations.
 
-<img src="assets/concept.png" width="500">
+<div><img src="assets/concept.png" width="500"/></div>
+
+<img src="assets/cartpole.gif" width="350"> <img src="assets/twowheeledconst.gif" width="350"> <img src="assets/twowheeledtrack.gif" width="350">
 
 # Algorithms
 
@@ -24,7 +27,6 @@ PythonLinearNonLinearControl is a library implementing the linear and nonlinear
 | Constrained Nonlinear Model Predictive Control Newton (NMPC-Newton) | x | ✓ | x | x | x |
 
 "Need Gradient" means that you have to implement the gradient of the model or the gradient of hamiltonian.  
-This library is also easily to extend for your own situations.
 
 Following algorithms are implemented in PythonLinearNonlinearControl
 
@@ -61,11 +63,13 @@ Following algorithms are implemented in PythonLinearNonlinearControl
 
 # Environments
 
+There are 4 example environments, "FirstOrderLag", "TwoWheeledConst", "TwoWheeledTrack" and "Cartpole".
+
 | Name | Linear | Nonlinear | State Size | Input size |
 |:----------|:---------------:|:----------------:|:----------------:|:----------------:|
 | First Order Lag System | ✓ | x | 4 | 2 | 
 | Two wheeled System (Constant Goal) | x | ✓ | 3 | 2 | 
-| Two wheeled System (Moving Goal) (Coming soon) | x | ✓ | 3 | 2 | 
+| Two wheeled System (Moving Goal) | x | ✓ | 3 | 2 | 
 | Cartpole (Swing up) | x | ✓ | 4 | 1 | 
 
 All states and inputs of environments are continuous.
@@ -104,7 +108,7 @@ pip install -e .
 You can run the experiments as follows:
 
 ```
-python scripts/simple_run.py --env first-order_lag --controller CEM
+python scripts/simple_run.py --env FirstOrderLag --controller CEM
 ```
 
 **figures and animations are saved in the ./result folder.**
@@ -147,9 +151,9 @@ Coming soon !!
 # Requirements
 
 - numpy
-- matplotlib
-- cvxopt
 - scipy
+- matplotlib (for figures and animations)
+- ffmpeg (for animations)
 
 # License
 

scripts/simple_run.py

@@ -9,6 +9,7 @@ from PythonLinearNonlinearControl.envs.make_envs import make_env
 from PythonLinearNonlinearControl.runners.make_runners import make_runner
 from PythonLinearNonlinearControl.plotters.plot_func import plot_results, \
                                                             save_plot_data
+from PythonLinearNonlinearControl.plotters.animator import Animator
 
 def run(args):
     # logger
@@ -39,12 +40,16 @@ def run(args):
     plot_results(args, history_x, history_u, history_g=history_g)
     save_plot_data(args, history_x, history_u, history_g=history_g)
 
+    if args.save_anim:
+        animator = Animator(args, env)
+        animator.draw(history_x, history_g)
+
 def main():
     parser = argparse.ArgumentParser()
 
-    parser.add_argument("--controller_type", type=str, default="DDP")
+    parser.add_argument("--controller_type", type=str, default="CEM")
-    parser.add_argument("--planner_type", type=str, default="const")
+    parser.add_argument("--env", type=str, default="TwoWheeledTrack")
-    parser.add_argument("--env", type=str, default="CartPole")
+    parser.add_argument("--save_anim", type=bool_flag, default=1)
     parser.add_argument("--result_dir", type=str, default="./result")
 
     args = parser.parse_args()