diff --git a/mpc/with_disturbance/coordinate_trans.py b/mpc/with_disturbance/coordinate_trans.py new file mode 100755 index 0000000..ea66939 --- /dev/null +++ b/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) + diff --git a/mpc/with_disturbance/main_track.py b/mpc/with_disturbance/main_track.py index fbbafdd..03200e5 100644 --- a/mpc/with_disturbance/main_track.py +++ b/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 diff --git a/mpc/with_disturbance/mpc_func_with_cvxopt.py b/mpc/with_disturbance/mpc_func_with_cvxopt.py index 4121bb8..a46d2d2 100644 --- a/mpc/with_disturbance/mpc_func_with_cvxopt.py +++ b/mpc/with_disturbance/mpc_func_with_cvxopt.py @@ -150,6 +150,15 @@ class MpcController(): self.dist_mat = np.hstack((self.dist_mat, temp_mat)) 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)]) @@ -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