Merge pull request #6 from Shunichi09/develop

Add: TwoWheeledTrack Env

Author: Shunichi09

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,16 +3,17 @@
 class CartPoleConfigModule():
     # parameters
     ENV_NAME = "CartPole-v0"
+    PLANNER_TYPE = "Const"
     TYPE = "Nonlinear"
     TASK_HORIZON = 500
     PRED_LEN = 50
     STATE_SIZE = 4
     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 @@ def __init__(self):
     @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 @@ def input_cost_fn(u):
     @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 @@ def state_cost_fn(x, g_x):
     @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 @@ def terminal_state_cost_fn(terminal_x, terminal_g_x):
                    + 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 @@ def gradient_cost_fn_with_state(x, g_x, terminal=False):
         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 @@ def gradient_cost_fn_with_input(x, u):
         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 @@ def hessian_cost_fn_with_state(x, g_x, terminal=False):
         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 @@ def hessian_cost_fn_with_state(x, g_x, terminal=False):
                           * -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 @@ def hessian_cost_fn_with_input(x, u):
         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 @@ def hessian_cost_fn_with_input_state(x, u):
         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 @@ def input_cost_fn(u):
         """
         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 @@ def state_cost_fn(x, g_x):
             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 @@ def terminal_state_cost_fn(terminal_x, terminal_g_x):
             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)
+        terminal_diff = TwoWheeledConfigModule.fit_diff_in_range(terminal_x \
+                                                        - 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 @@ def gradient_cost_fn_with_state(x, g_x, terminal=False):
             l_x (numpy.ndarray): gradient of cost, shape(pred_len, state_size)
                 or shape(1, state_size)
         """
+        diff = TwoWheeledConfigModule.fit_diff_in_range(x - g_x)
+        
         if not terminal:
-            return 2. * (x - g_x) * np.diag(TwoWheeledConfigModule.Q)
+            return 2. * (diff) * np.diag(TwoWheeledConfigModule.Q)
 
-        return (2. * (x - g_x) \
+        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 @@ def __init__(self, config, model):
         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,19 +185,38 @@ def obtain_sol(self, curr_x, g_xs):
 
         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)
         A = matrix(A)
         b = matrix(ub)
 
         # solve the problem
-        opt_result = solvers.qp(P, q, G=A, h=b)
-        opt_dt_us = np.array(list(opt_result['x']))
+        opt_sol = solvers.qp(P, q, G=A, h=b)
+        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]