Merge branch 'master' of github.com:Shunichi09/linear_nonlinear_control

mpc/with_disturbance/coordinate_trans.py

@@ -0,0 +1,112 @@
+import math
+import numpy as np
+import copy
+
+def coordinate_transformation_in_angle(positions, base_angle):
+    '''
+    Transformation the coordinate in the angle
+
+    Parameters
+    -------
+    positions : numpy.ndarray
+        this parameter is composed of xs, ys 
+        should have (2, N) shape 
+    base_angle : float [rad]
+    
+    Returns
+    -------
+    traslated_positions : numpy.ndarray
+        the shape is (2, N)
+    
+    '''
+    if positions.shape[0] != 2:
+        raise ValueError('the input data should have (2, N)')
+
+    positions = np.array(positions)
+    positions = positions.reshape(2, -1)
+
+    rot_matrix = [[np.cos(base_angle), np.sin(base_angle)],
+                  [-1*np.sin(base_angle), np.cos(base_angle)]]
+
+    rot_matrix = np.array(rot_matrix)
+    
+    translated_positions = np.dot(rot_matrix, positions)
+
+    return translated_positions
+
+def coordinate_transformation_in_position(positions, base_positions):
+    '''
+    Transformation the coordinate in the positions
+
+    Parameters
+    -------
+    positions : numpy.ndarray
+        this parameter is composed of xs, ys 
+        should have (2, N) shape 
+    base_positions : numpy.ndarray
+        this parameter is composed of x, y
+        shoulg have (2, 1) shape
+    
+    Returns
+    -------
+    traslated_positions : numpy.ndarray
+    
+    '''
+
+    if positions.shape[0] != 2:
+        raise ValueError('the input data should have (2, N)')
+
+    positions = np.array(positions)
+    positions = positions.reshape(2, -1)
+    base_positions = np.array(base_positions)
+    base_positions = base_positions.reshape(2, 1)
+
+    translated_positions = positions - base_positions
+
+    return translated_positions
+
+
+def coordinate_transformation_in_matrix_angles(positions, base_angles):
+    '''
+    Transformation the coordinate in the matrix angle
+
+    Parameters
+    -------
+    positions : numpy.ndarray
+        this parameter is composed of xs, ys 
+        should have (2, N) shape 
+    base_angle : float [rad]
+    
+    Returns
+    -------
+    traslated_positions : numpy.ndarray
+        the shape is (2, N)
+    
+    '''
+    if positions.shape[0] != 2:
+        raise ValueError('the input data should have (2, N)')
+
+    positions = np.array(positions)
+    positions = positions.reshape(2, -1)
+    translated_positions = np.zeros_like(positions)
+
+    for i in range(len(base_angles)):
+        rot_matrix = [[np.cos(base_angles[i]), np.sin(base_angles[i])],
+                    [-1*np.sin(base_angles[i]), np.cos(base_angles[i])]]
+
+        rot_matrix = np.array(rot_matrix)
+    
+        translated_position = np.dot(rot_matrix, positions[:, i].reshape(2, 1))
+        
+        translated_positions[:, i] = translated_position.flatten()
+
+    return translated_positions.reshape(2, -1)
+
+# def coordinate_inv_transformation
+if __name__ == '__main__':
+    positions_1 = np.array([[1.0], [2.0]])
+    base_angle = 1.25
+
+    translated_positions_1 = coordinate_transformation_in_angle(positions_1, base_angle)
+    print(translated_positions_1)
+

mpc/with_disturbance/main_track.py

@@ -6,6 +6,7 @@ import copy
 from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt
 from animation import AnimDrawer
 from control import matlab
+from coordinate_trans import coordinate_transformation_in_angle, coordinate_transformation_in_position
 
 class WheeledSystem():
     """SampleSystem, this is the simulator
@@ -124,7 +125,7 @@ class WheeledSystem():
     def _func_x_4(self, y_1, y_2, y_3, y_4, u_1, u_2):
         """
         """
-        y_dot = math.atan2(self.REAR_WHEELE_BASE / (self.REAR_WHEELE_BASE + self.FRONT_WHEELE_BASE) * math.tan(u_2))
+        y_dot = math.atan2(self.REAR_WHEELE_BASE * math.tan(u_2) ,self.REAR_WHEELE_BASE + self.FRONT_WHEELE_BASE)
 
         return y_dot
 
@@ -136,15 +137,26 @@ def main():
     # you must be care about this matrix
     # these A and B are for continuos system if you want to use discret system matrix please skip this step
     # lineared car system
-    V = 5.0
-    Ad = np.array([[1., 0., 0., 0.],
-                   [0., 1,  V, 0.],
-                   [0., 0., 1., 0.],
-                   [0., 0., 1., 0.]]) * dt
+    WHEEL_BASE = 2.2
+    tau = 0.01
 
-    Bd = np.array([[0.], [0.], [0.], [0.3]]) * dt
+    V = 5.0 # initialize
 
-    W_D = np.array([[V], [0.], [0.], [0.]]) * dt
+    
+    delta_r = 0.
+
+    A12 = (V / WHEEL_BASE) / (math.cos(delta_r)**2)
+    A22 = (1. - 1. / tau)
+
+    Ad = np.array([[1., V,   0.], 
+                   [0., 1., A12],
+                   [0., 0., A22]]) * dt
+
+    Bd = np.array([[0.], [0.], [1. / tau]]) * dt
+
+    W_D_0 = - (V / WHEEL_BASE) * delta_r / (math.cos(delta_r)**2)
+
+    W_D = np.array([[0.], [W_D_0], [0.]]) * dt
 
     # make simulator with coninuous matrix
     init_xs_lead = np.array([5., 0., 0. ,0.])
@@ -153,9 +165,9 @@ def main():
     follow_car = WheeledSystem(init_states=init_xs_follow)
 
     # evaluation function weight
-    Q = np.diag([1., 1., 1., 1.])
+    Q = np.diag([1., 1., 1.])
     R = np.diag([5.])
-    pre_step = 2
+    pre_step = 15
 
     # make controller with discreted matrix
     # please check the solver, if you want to use the scipy, set the MpcController_scipy
@@ -171,18 +183,57 @@ def main():
     follow_controller.initialize_controller()
 
     # reference
-    lead_reference = np.array([[0., 0.] for _ in range(pre_step)]).flatten()
+    lead_reference = np.array([[0., 0., 0.] for _ in range(pre_step)]).flatten()
+    ref = np.array([[0.], [0.]])
 
     for i in range(iteration_num):
         print("simulation time = {0}".format(i))
+
         # make lead car's move
         if i > int(iteration_num / 3):
-            lead_reference = np.array([[4., 0.] for _ in range(pre_step)]).flatten()
+            ref = np.array([[0.], [4.]])
 
+        ## lead
+        # world traj
         lead_states = lead_car.xs
-        lead_opt_u = lead_controller.calc_input(lead_states[1:], lead_reference)
+
+        # transformation
+        relative_ref = coordinate_transformation_in_position(ref, lead_states[:2])
+        relative_ref = coordinate_transformation_in_angle(relative_ref, lead_states[2])
+
+        # make ref
+        lead_reference = np.array([[ref[1, 0], 0., 0.] for _ in range(pre_step)]).flatten()
+
+        alpha = math.atan2(relative_ref[1], relative_ref[0])
+        R = np.linalg.norm(relative_ref) / 2 * math.sin(alpha)
+
+        print(R)
+        input()
+
+        V = 7.0 
+        delta_r = math.atan2(WHEEL_BASE, R)
+
+        A12 = (V / WHEEL_BASE) / (math.cos(delta_r)**2)
+        A22 = (1. - 1. / tau)
+
+        Ad = np.array([[1., V,   0.], 
+                    [0., 1., A12],
+                    [0., 0., A22]]) * dt
+
+        Bd = np.array([[0.], [0.], [1. / tau]]) * dt
+
+        W_D_0 = - (V / WHEEL_BASE) * delta_r / (math.cos(delta_r)**2)
+
+        W_D = np.array([[0.], [W_D_0], [0.]]) * dt
+
+        # update system matrix
+        lead_controller.update_system_model(Ad, Bd, W_D)
+
+        lead_opt_u = lead_controller.calc_input(np.zeros(3), lead_reference)
         lead_opt_u = np.hstack((np.array([V]), lead_opt_u))
 
+        
+        ## follow
         # make follow car
         follow_reference = np.array([lead_states[1:] for _ in range(pre_step)]).flatten()
         follow_states = follow_car.xs

mpc/with_disturbance/mpc_func_with_cvxopt.py

@@ -151,6 +151,15 @@ class MpcController():
 
         print("dist_mat = \n{0}".format(self.dist_mat))
         
+        W_Ds = copy.deepcopy(self.W_D)
+
+        for _ in range(self.pre_step - 1):
+            W_Ds = np.vstack((W_Ds, self.W_D))
+        
+        self.dist_mat = np.dot(self.dist_mat, W_Ds)
+
+        print("dist_mat = \n{0}".format(self.dist_mat))
+
         # evaluation function weight
         diag_Qs = np.array([np.diag(self.Q) for _ in range(self.pre_step)])
         diag_Rs = np.array([np.diag(self.R) for _ in range(self.pre_step)])
@@ -217,7 +226,7 @@ class MpcController():
 
         # about state
         print("check the matrix!! if you think rite, plese push enter")
-        input()
+        # input()
 
     def calc_input(self, states, references):
         """calculate optimal input