diff --git a/Environments.md b/Environments.md index 412b41c..d1a010b 100644 --- a/Environments.md +++ b/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) @@ -53,4 +55,14 @@ mc = 1, mp = 0.2, l = 0.5, g = 9.81 ### Cost. - \ No newline at end of file + + +## [Nonlinear Sample System Env](PythonLinearNonlinearControl/envs/nonlinear_sample_system.py) + +## System equation. + + + +### Cost. + + diff --git a/PythonLinearNonlinearControl/common/utils.py b/PythonLinearNonlinearControl/common/utils.py index 8010737..e7f6d82 100644 --- a/PythonLinearNonlinearControl/common/utils.py +++ b/PythonLinearNonlinearControl/common/utils.py @@ -1,8 +1,9 @@ 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 @@ -14,9 +15,10 @@ def rotate_pos(pos, 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 @@ -29,7 +31,7 @@ def fit_angle_in_range(angles, min_angle=-np.pi, max_angle=np.pi): 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 @@ -41,4 +43,76 @@ def fit_angle_in_range(angles, min_angle=-np.pi, max_angle=np.pi): output += min_angle output = np.minimum(max_angle, np.maximum(min_angle, output)) - return output.reshape(output_shape) \ No newline at end of file + return output.reshape(output_shape) + + +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) + 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 + + Notes: + sample of function is as follows: + + def func_x(self, x_1, x_2, u): + x_dot = (1. - x_1**2 - x_2**2) * x_2 - x_1 + u + return x_dot + + 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" + + k0 = np.zeros(state_size) + k1 = np.zeros(state_size) + k2 = np.zeros(state_size) + k3 = np.zeros(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 state + (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 state + (k0 + 2. * k1 + 2. * k2 + k3) / 6. diff --git a/PythonLinearNonlinearControl/configs/cartpole.py b/PythonLinearNonlinearControl/configs/cartpole.py index bbcf99b..e8d10a6 100644 --- a/PythonLinearNonlinearControl/configs/cartpole.py +++ b/PythonLinearNonlinearControl/configs/cartpole.py @@ -1,5 +1,6 @@ import numpy as np + class CartPoleConfigModule(): # parameters ENV_NAME = "CartPole-v0" @@ -12,7 +13,7 @@ class CartPoleConfigModule(): DT = 0.02 # cost parameters R = np.diag([0.01]) # 0.01 is worked for MPPI and CEM and MPPIWilliams - # 1. is worked for iLQR + # 1. is worked for iLQR TERMINAL_WEIGHT = 1. Q = None Sf = None @@ -39,41 +40,41 @@ class CartPoleConfigModule(): "num_elites": 50, "max_iters": 15, "alpha": 0.3, - "init_var":9., - "threshold":0.001 + "init_var": 9., + "threshold": 0.001 }, - "MPPI":{ - "beta" : 0.6, + "MPPI": { + "beta": 0.6, "popsize": 5000, "kappa": 0.9, "noise_sigma": 0.5, }, - "MPPIWilliams":{ + "MPPIWilliams": { "popsize": 5000, "lambda": 1., "noise_sigma": 0.9, }, - "iLQR":{ + "iLQR": { "max_iter": 500, "init_mu": 1., "mu_min": 1e-6, "mu_max": 1e10, "init_delta": 2., "threshold": 1e-6, - }, - "DDP":{ + }, + "DDP": { "max_iter": 500, "init_mu": 1., "mu_min": 1e-6, "mu_max": 1e10, "init_delta": 2., "threshold": 1e-6, - }, - "NMPC-CGMRES":{ - }, - "NMPC-Newton":{ - }, - } + }, + "NMPC-CGMRES": { + }, + "NMPC-Newton": { + }, + } @staticmethod def input_cost_fn(u): @@ -87,7 +88,7 @@ class CartPoleConfigModule(): shape(pop_size, pred_len, input_size) """ return (u**2) * np.diag(CartPoleConfigModule.R) - + @staticmethod def state_cost_fn(x, g_x): """ state cost function @@ -103,21 +104,21 @@ class CartPoleConfigModule(): """ if len(x.shape) > 2: - return (6. * (x[:, :, 0]**2) \ - + 12. * ((np.cos(x[:, :, 2]) + 1.)**2) \ - + 0.1 * (x[:, :, 1]**2) \ - + 0.1 * (x[:, :, 3]**2))[:, :, np.newaxis] + return (6. * (x[:, :, 0]**2) + + 12. * ((np.cos(x[:, :, 2]) + 1.)**2) + + 0.1 * (x[:, :, 1]**2) + + 0.1 * (x[:, :, 3]**2))[:, :, np.newaxis] elif len(x.shape) > 1: - return (6. * (x[:, 0]**2) \ - + 12. * ((np.cos(x[:, 2]) + 1.)**2) \ - + 0.1 * (x[:, 1]**2) \ - + 0.1 * (x[:, 3]**2))[:, np.newaxis] - + return (6. * (x[:, 0]**2) + + 12. * ((np.cos(x[:, 2]) + 1.)**2) + + 0.1 * (x[:, 1]**2) + + 0.1 * (x[:, 3]**2))[:, np.newaxis] + return 6. * (x[0]**2) \ - + 12. * ((np.cos(x[2]) + 1.)**2) \ - + 0.1 * (x[1]**2) \ - + 0.1 * (x[3]**2) + + 12. * ((np.cos(x[2]) + 1.)**2) \ + + 0.1 * (x[1]**2) \ + + 0.1 * (x[3]**2) @staticmethod def terminal_state_cost_fn(terminal_x, terminal_g_x): @@ -134,18 +135,18 @@ class CartPoleConfigModule(): """ if len(terminal_x.shape) > 1: - 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))[:, np.newaxis] \ - * 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)) \ + 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))[:, np.newaxis] \ * 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 + @staticmethod def gradient_cost_fn_with_state(x, g_x, terminal=False): """ gradient of costs with respect to the state @@ -153,26 +154,26 @@ 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_x (numpy.ndarray): gradient of cost, shape(pred_len, state_size) or shape(1, state_size) """ if not terminal: - cost_dx0 = 12. * x[:, 0] + cost_dx0 = 12. * x[:, 0] cost_dx1 = 0.2 * x[:, 1] cost_dx2 = 24. * (1 + np.cos(x[:, 2])) * -np.sin(x[:, 2]) cost_dx3 = 0.2 * x[:, 3] - cost_dx = np.stack((cost_dx0, cost_dx1,\ + cost_dx = np.stack((cost_dx0, cost_dx1, cost_dx2, cost_dx3), axis=1) return cost_dx - - cost_dx0 = 12. * x[0] + + cost_dx0 = 12. * x[0] cost_dx1 = 0.2 * x[1] cost_dx2 = 24. * (1 + np.cos(x[2])) * -np.sin(x[2]) cost_dx3 = 0.2 * x[3] cost_dx = np.array([[cost_dx0, cost_dx1, cost_dx2, cost_dx3]]) - + return cost_dx * CartPoleConfigModule.TERMINAL_WEIGHT @staticmethod @@ -206,21 +207,21 @@ class CartPoleConfigModule(): hessian[:, 0, 0] = 12. hessian[:, 1, 1] = 0.2 hessian[:, 2, 2] = 24. * -np.sin(x[:, 2]) \ - * (-np.sin(x[:, 2])) \ - + 24. * (1. + np.cos(x[:, 2])) \ - * -np.cos(x[:, 2]) + * (-np.sin(x[:, 2])) \ + + 24. * (1. + np.cos(x[:, 2])) \ + * -np.cos(x[:, 2]) hessian[:, 3, 3] = 0.2 return hessian - + state_size = len(x) hessian = np.eye(state_size) hessian[0, 0] = 12. hessian[1, 1] = 0.2 hessian[2, 2] = 24. * -np.sin(x[2]) \ - * (-np.sin(x[2])) \ - + 24. * (1. + np.cos(x[2])) \ - * -np.cos(x[2]) + * (-np.sin(x[2])) \ + + 24. * (1. + np.cos(x[2])) \ + * -np.cos(x[2]) hessian[3, 3] = 0.2 return hessian[np.newaxis, :, :] * CartPoleConfigModule.TERMINAL_WEIGHT @@ -239,7 +240,7 @@ class CartPoleConfigModule(): (pred_len, _) = u.shape return np.tile(2.*CartPoleConfigModule.R, (pred_len, 1, 1)) - + @staticmethod def hessian_cost_fn_with_input_state(x, u): """ hessian costs with respect to the state and input @@ -254,4 +255,4 @@ class CartPoleConfigModule(): (_, state_size) = x.shape (pred_len, input_size) = u.shape - return np.zeros((pred_len, input_size, state_size)) \ No newline at end of file + return np.zeros((pred_len, input_size, state_size)) diff --git a/PythonLinearNonlinearControl/configs/first_order_lag.py b/PythonLinearNonlinearControl/configs/first_order_lag.py index 2c9d268..fef097d 100644 --- a/PythonLinearNonlinearControl/configs/first_order_lag.py +++ b/PythonLinearNonlinearControl/configs/first_order_lag.py @@ -1,5 +1,6 @@ import numpy as np + class FirstOrderLagConfigModule(): # parameters ENV_NAME = "FirstOrderLag-v0" @@ -34,43 +35,43 @@ class FirstOrderLagConfigModule(): "num_elites": 50, "max_iters": 15, "alpha": 0.3, - "init_var":1., - "threshold":0.001 + "init_var": 1., + "threshold": 0.001 }, - "MPPI":{ - "beta" : 0.6, + "MPPI": { + "beta": 0.6, "popsize": 5000, "kappa": 0.9, "noise_sigma": 0.5, }, - "MPPIWilliams":{ + "MPPIWilliams": { "popsize": 5000, "lambda": 1., "noise_sigma": 0.9, }, - "MPC":{ - }, - "iLQR":{ + "MPC": { + }, + "iLQR": { "max_iter": 500, "init_mu": 1., "mu_min": 1e-6, "mu_max": 1e10, "init_delta": 2., "threshold": 1e-6, - }, - "DDP":{ + }, + "DDP": { "max_iter": 500, "init_mu": 1., "mu_min": 1e-6, "mu_max": 1e10, "init_delta": 2., "threshold": 1e-6, - }, - "NMPC-CGMRES":{ - }, - "NMPC-Newton":{ - }, - } + }, + "NMPC-CGMRES": { + }, + "NMPC-Newton": { + }, + } @staticmethod def input_cost_fn(u): @@ -83,7 +84,7 @@ class FirstOrderLagConfigModule(): shape(pop_size, pred_len, input_size) """ return (u**2) * np.diag(FirstOrderLagConfigModule.R) - + @staticmethod def state_cost_fn(x, g_x): """ state cost function @@ -111,8 +112,8 @@ class FirstOrderLagConfigModule(): shape(pop_size, pred_len) """ return ((terminal_x - terminal_g_x)**2) \ - * np.diag(FirstOrderLagConfigModule.Sf) - + * np.diag(FirstOrderLagConfigModule.Sf) + @staticmethod def gradient_cost_fn_with_state(x, g_x, terminal=False): """ gradient of costs with respect to the state @@ -120,16 +121,16 @@ class FirstOrderLagConfigModule(): Args: x (numpy.ndarray): state, shape(pred_len, state_size) g_x (numpy.ndarray): goal state, shape(pred_len, state_size) - + Returns: l_x (numpy.ndarray): gradient of cost, shape(pred_len, state_size) or shape(1, state_size) """ if not terminal: return 2. * (x - g_x) * np.diag(FirstOrderLagConfigModule.Q) - - return (2. * (x - g_x) \ - * np.diag(FirstOrderLagConfigModule.Sf))[np.newaxis, :] + + return (2. * (x - g_x) + * np.diag(FirstOrderLagConfigModule.Sf))[np.newaxis, :] @staticmethod def gradient_cost_fn_with_input(x, u): @@ -138,7 +139,7 @@ class FirstOrderLagConfigModule(): 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) """ @@ -151,7 +152,7 @@ class FirstOrderLagConfigModule(): 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 @@ -159,9 +160,9 @@ class FirstOrderLagConfigModule(): """ if not terminal: (pred_len, _) = x.shape - return np.tile(2.*FirstOrderLagConfigModule.Q, (pred_len, 1, 1)) - - return np.tile(2.*FirstOrderLagConfigModule.Sf, (1, 1, 1)) + return np.tile(2.*FirstOrderLagConfigModule.Q, (pred_len, 1, 1)) + + return np.tile(2.*FirstOrderLagConfigModule.Sf, (1, 1, 1)) @staticmethod def hessian_cost_fn_with_input(x, u): @@ -170,7 +171,7 @@ class FirstOrderLagConfigModule(): 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) @@ -178,7 +179,7 @@ class FirstOrderLagConfigModule(): (pred_len, _) = u.shape return np.tile(2.*FirstOrderLagConfigModule.R, (pred_len, 1, 1)) - + @staticmethod def hessian_cost_fn_with_input_state(x, u): """ hessian costs with respect to the state and input @@ -186,7 +187,7 @@ class FirstOrderLagConfigModule(): 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) diff --git a/PythonLinearNonlinearControl/configs/make_configs.py b/PythonLinearNonlinearControl/configs/make_configs.py index a48aedc..1267470 100644 --- a/PythonLinearNonlinearControl/configs/make_configs.py +++ b/PythonLinearNonlinearControl/configs/make_configs.py @@ -1,6 +1,8 @@ from .first_order_lag import FirstOrderLagConfigModule from .two_wheeled import TwoWheeledConfigModule from .cartpole import CartPoleConfigModule +from .nonlinear_sample_system import NonlinearSampleSystemConfigModule + def make_config(args): """ @@ -12,4 +14,6 @@ def make_config(args): elif args.env == "TwoWheeledConst" or args.env == "TwoWheeledTrack": return TwoWheeledConfigModule() elif args.env == "CartPole": - return CartPoleConfigModule() \ No newline at end of file + return CartPoleConfigModule() + elif args.env == "NonlinearSample": + return NonlinearSampleSystemConfigModule() diff --git a/PythonLinearNonlinearControl/configs/nonlinear_sample_system.py b/PythonLinearNonlinearControl/configs/nonlinear_sample_system.py new file mode 100644 index 0000000..62d9c9f --- /dev/null +++ b/PythonLinearNonlinearControl/configs/nonlinear_sample_system.py @@ -0,0 +1,219 @@ +import numpy as np + + +class NonlinearSampleSystemConfigModule(): + # parameters + ENV_NAME = "NonlinearSampleSystem-v0" + PLANNER_TYPE = "Const" + TYPE = "Nonlinear" + TASK_HORIZON = 2500 + PRED_LEN = 10 + STATE_SIZE = 2 + INPUT_SIZE = 1 + DT = 0.01 + R = np.diag([0.01]) + Q = None + Sf = None + # bounds + INPUT_LOWER_BOUND = np.array([-0.5]) + INPUT_UPPER_BOUND = np.array([0.5]) + + def __init__(self): + """ + """ + # opt configs + self.opt_config = { + "Random": { + "popsize": 5000 + }, + "CEM": { + "popsize": 500, + "num_elites": 50, + "max_iters": 15, + "alpha": 0.3, + "init_var": 9., + "threshold": 0.001 + }, + "MPPI": { + "beta": 0.6, + "popsize": 5000, + "kappa": 0.9, + "noise_sigma": 0.5, + }, + "MPPIWilliams": { + "popsize": 5000, + "lambda": 1., + "noise_sigma": 0.9, + }, + "iLQR": { + "max_iter": 500, + "init_mu": 1., + "mu_min": 1e-6, + "mu_max": 1e10, + "init_delta": 2., + "threshold": 1e-6, + }, + "DDP": { + "max_iter": 500, + "init_mu": 1., + "mu_min": 1e-6, + "mu_max": 1e10, + "init_delta": 2., + "threshold": 1e-6, + }, + "NMPC-CGMRES": { + }, + "NMPC-Newton": { + }, + } + + @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) + Returns: + cost (numpy.ndarray): cost of input, shape(pred_len, input_size) or + shape(pop_size, pred_len, input_size) + """ + return (u**2) * np.diag(NonlinearSampleSystemConfigModule.R) + + @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) + g_x (numpy.ndarray): goal state, shape(pred_len, state_size) + or shape(pop_size, pred_len, state_size) + Returns: + cost (numpy.ndarray): cost of state, shape(pred_len, 1) or + shape(pop_size, pred_len, 1) + """ + + if len(x.shape) > 2: + return (0.5 * (x[:, :, 0]**2) + + 0.5 * (x[:, :, 1]**2))[:, :, np.newaxis] + + elif len(x.shape) > 1: + return (0.5 * (x[:, 0]**2) + 0.5 * (x[:, 1]**2))[:, np.newaxis] + + return 0.5 * (x[0]**2) + 0.5 * (x[1]**2) + + @ 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) + terminal_g_x (numpy.ndarray): terminal goal state, + shape(state_size, ) or shape(pop_size, state_size) + Returns: + cost (numpy.ndarray): cost of state, shape(pred_len, ) or + shape(pop_size, pred_len) + """ + + if len(terminal_x.shape) > 1: + return (0.5 * (terminal_x[:, 0]**2) + + 0.5 * (terminal_x[:, 1]**2))[:, np.newaxis] + + return 0.5 * (terminal_x[0]**2) + 0.5 * (terminal_x[1]**2) + + @ staticmethod + def gradient_cost_fn_with_state(x, g_x, terminal=False): + """ gradient of costs with respect to the state + + Args: + x (numpy.ndarray): state, shape(pred_len, state_size) + g_x (numpy.ndarray): goal state, shape(pred_len, state_size) + + Returns: + l_x (numpy.ndarray): gradient of cost, shape(pred_len, state_size) + or shape(1, state_size) + """ + if not terminal: + cost_dx0 = x[:, 0] + cost_dx1 = x[:, 1] + cost_dx = np.stack((cost_dx0, cost_dx1), axis=1) + return cost_dx + + cost_dx0 = x[0] + cost_dx1 = x[1] + cost_dx = np.array([[cost_dx0, cost_dx1]]) + + return cost_dx + + @ staticmethod + def gradient_cost_fn_with_input(x, u): + """ gradient of costs with respect to the input + + 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) + """ + return 2. * u * np.diag(NonlinearSampleSystemConfigModule.R) + + @ staticmethod + def hessian_cost_fn_with_state(x, g_x, terminal=False): + """ hessian costs with respect to the state + + 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 + shape(1, state_size, state_size) or + """ + if not terminal: + (pred_len, state_size) = x.shape + hessian = np.eye(state_size) + hessian = np.tile(hessian, (pred_len, 1, 1)) + hessian[:, 0, 0] = 1. + hessian[:, 1, 1] = 1. + + return hessian + + state_size = len(x) + hessian = np.eye(state_size) + hessian[0, 0] = 1. + hessian[1, 1] = 1. + + return hessian[np.newaxis, :, :] + + @ staticmethod + def hessian_cost_fn_with_input(x, u): + """ hessian costs with respect to the input + + 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) + """ + (pred_len, _) = u.shape + + return np.tile(NonlinearSampleSystemConfigModule.R, (pred_len, 1, 1)) + + @ staticmethod + def hessian_cost_fn_with_input_state(x, u): + """ hessian costs with respect to the state and input + + 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) + """ + (_, state_size) = x.shape + (pred_len, input_size) = u.shape + + return np.zeros((pred_len, input_size, state_size)) diff --git a/PythonLinearNonlinearControl/configs/two_wheeled.py b/PythonLinearNonlinearControl/configs/two_wheeled.py index 93de72a..56de209 100644 --- a/PythonLinearNonlinearControl/configs/two_wheeled.py +++ b/PythonLinearNonlinearControl/configs/two_wheeled.py @@ -4,6 +4,7 @@ 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" @@ -25,7 +26,7 @@ class TwoWheeledConfigModule(): 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_UPPER_BOUND = np.array([1.5, 3.14]) @@ -46,41 +47,41 @@ class TwoWheeledConfigModule(): "num_elites": 50, "max_iters": 15, "alpha": 0.3, - "init_var":1., - "threshold":0.001 + "init_var": 1., + "threshold": 0.001 }, - "MPPI":{ - "beta" : 0.6, + "MPPI": { + "beta": 0.6, "popsize": 5000, "kappa": 0.9, "noise_sigma": 0.5, }, - "MPPIWilliams":{ + "MPPIWilliams": { "popsize": 5000, "lambda": 1, "noise_sigma": 1., }, - "iLQR":{ + "iLQR": { "max_iter": 500, "init_mu": 1., "mu_min": 1e-6, "mu_max": 1e10, "init_delta": 2., "threshold": 1e-6, - }, - "DDP":{ + }, + "DDP": { "max_iter": 500, "init_mu": 1., "mu_min": 1e-6, "mu_max": 1e10, "init_delta": 2., "threshold": 1e-6, - }, - "NMPC-CGMRES":{ - }, - "NMPC-Newton":{ - }, - } + }, + "NMPC-CGMRES": { + }, + "NMPC-Newton": { + }, + } @staticmethod def input_cost_fn(u): @@ -93,7 +94,7 @@ class TwoWheeledConfigModule(): shape(pop_size, pred_len, input_size) """ return (u**2) * np.diag(TwoWheeledConfigModule.R) - + @staticmethod def fit_diff_in_range(diff_x): """ fit difference state in range(angle) @@ -107,7 +108,7 @@ class TwoWheeledConfigModule(): 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]) + 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: @@ -142,11 +143,11 @@ class TwoWheeledConfigModule(): cost (numpy.ndarray): cost of state, shape(pred_len, ) or shape(pop_size, pred_len) """ - terminal_diff = TwoWheeledConfigModule.fit_diff_in_range(terminal_x \ - - terminal_g_x) - + 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): """ gradient of costs with respect to the state @@ -154,18 +155,18 @@ class TwoWheeledConfigModule(): Args: x (numpy.ndarray): state, shape(pred_len, state_size) g_x (numpy.ndarray): goal state, shape(pred_len, state_size) - + Returns: 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. * (diff) * np.diag(TwoWheeledConfigModule.Q) - - return (2. * (diff) \ - * np.diag(TwoWheeledConfigModule.Sf))[np.newaxis, :] + + return (2. * (diff) + * np.diag(TwoWheeledConfigModule.Sf))[np.newaxis, :] @staticmethod def gradient_cost_fn_with_input(x, u): @@ -174,7 +175,7 @@ class TwoWheeledConfigModule(): 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) """ @@ -187,7 +188,7 @@ class TwoWheeledConfigModule(): 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 @@ -195,9 +196,9 @@ class TwoWheeledConfigModule(): """ if not terminal: (pred_len, _) = x.shape - return np.tile(2.*TwoWheeledConfigModule.Q, (pred_len, 1, 1)) - - return np.tile(2.*TwoWheeledConfigModule.Sf, (1, 1, 1)) + return np.tile(2.*TwoWheeledConfigModule.Q, (pred_len, 1, 1)) + + return np.tile(2.*TwoWheeledConfigModule.Sf, (1, 1, 1)) @staticmethod def hessian_cost_fn_with_input(x, u): @@ -206,7 +207,7 @@ class TwoWheeledConfigModule(): 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) @@ -214,7 +215,7 @@ class TwoWheeledConfigModule(): (pred_len, _) = u.shape return np.tile(2.*TwoWheeledConfigModule.R, (pred_len, 1, 1)) - + @staticmethod def hessian_cost_fn_with_input_state(x, u): """ hessian costs with respect to the state and input @@ -222,7 +223,7 @@ class TwoWheeledConfigModule(): 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) @@ -230,4 +231,4 @@ class TwoWheeledConfigModule(): (_, state_size) = x.shape (pred_len, input_size) = u.shape - return np.zeros((pred_len, input_size, state_size)) \ No newline at end of file + return np.zeros((pred_len, input_size, state_size)) diff --git a/PythonLinearNonlinearControl/controllers/cem.py b/PythonLinearNonlinearControl/controllers/cem.py index 238ff39..176607e 100644 --- a/PythonLinearNonlinearControl/controllers/cem.py +++ b/PythonLinearNonlinearControl/controllers/cem.py @@ -8,6 +8,7 @@ from ..envs.cost import calc_cost logger = getLogger(__name__) + class CEM(Controller): """ Cross Entropy Method for linear and nonlinear method @@ -19,6 +20,7 @@ class CEM(Controller): using probabilistic dynamics models. In Advances in Neural Information Processing Systems (pp. 4754-4765). """ + def __init__(self, config, model): super(CEM, self).__init__(config, model) @@ -38,7 +40,7 @@ class CEM(Controller): self.init_var = config.opt_config["CEM"]["init_var"] self.opt_dim = self.input_size * self.pred_len - # get bound + # get bound self.input_upper_bounds = np.tile(config.INPUT_UPPER_BOUND, self.pred_len) self.input_lower_bounds = np.tile(config.INPUT_LOWER_BOUND, @@ -50,18 +52,18 @@ class CEM(Controller): self.input_cost_fn = config.input_cost_fn # init mean - self.init_mean = np.tile((config.INPUT_UPPER_BOUND \ + self.init_mean = np.tile((config.INPUT_UPPER_BOUND + config.INPUT_LOWER_BOUND) / 2., self.pred_len) self.prev_sol = self.init_mean.copy() # init variance var = np.ones_like(config.INPUT_UPPER_BOUND) \ - * config.opt_config["CEM"]["init_var"] + * config.opt_config["CEM"]["init_var"] self.init_var = np.tile(var, self.pred_len) # save self.history_u = [] - + def clear_sol(self): """ clear prev sol """ @@ -77,21 +79,21 @@ class CEM(Controller): Returns: opt_input (numpy.ndarray): optimal input, shape(input_size, ) """ - # initialize + # initialize opt_count = 0 # get configuration - mean = self.prev_sol.flatten().copy() + mean = self.prev_sol.flatten().copy() var = self.init_var.flatten().copy() - # make distribution + # make distribution X = stats.truncnorm(-1, 1, - loc=np.zeros_like(mean),\ + loc=np.zeros_like(mean), scale=np.ones_like(mean)) - + while (opt_count < self.max_iters) and np.max(var) > self.epsilon: # constrained - lb_dist = mean - self.input_lower_bounds + lb_dist = mean - self.input_lower_bounds ub_dist = self.input_upper_bounds - mean constrained_var = np.minimum(np.minimum(np.square(lb_dist), np.square(ub_dist)), @@ -99,15 +101,15 @@ class CEM(Controller): # sample samples = X.rvs(size=[self.pop_size, self.opt_dim]) \ - * np.sqrt(constrained_var) \ - + mean + * np.sqrt(constrained_var) \ + + mean # calc cost # samples.shape = (pop_size, opt_dim) costs = self.calc_cost(curr_x, samples.reshape(self.pop_size, - self.pred_len, - self.input_size), + self.pred_len, + self.input_size), g_xs) # sort cost @@ -124,7 +126,7 @@ class CEM(Controller): logger.debug("Var = {}".format(np.max(var))) logger.debug("Costs = {}".format(np.mean(costs))) opt_count += 1 - + sol = mean.copy() self.prev_sol = np.concatenate((mean[self.input_size:], np.zeros(self.input_size))) diff --git a/PythonLinearNonlinearControl/controllers/controller.py b/PythonLinearNonlinearControl/controllers/controller.py index 50abb01..35c2297 100644 --- a/PythonLinearNonlinearControl/controllers/controller.py +++ b/PythonLinearNonlinearControl/controllers/controller.py @@ -2,9 +2,11 @@ import numpy as np from ..envs.cost import calc_cost + class Controller(): """ Controller class """ + def __init__(self, config, model): """ """ @@ -15,7 +17,7 @@ class Controller(): self.state_cost_fn = config.state_cost_fn self.terminal_state_cost_fn = config.terminal_state_cost_fn self.input_cost_fn = config.input_cost_fn - + def obtain_sol(self, curr_x, g_xs): """ calculate the optimal inputs Args: @@ -26,7 +28,7 @@ class Controller(): """ raise NotImplementedError("Implement the algorithm to \ get optimal input") - + def calc_cost(self, curr_x, samples, g_xs): """ calculate the cost of input samples @@ -46,22 +48,24 @@ class Controller(): # calc cost, pred_xs.shape = (pop_size, pred_len+1, state_size) pred_xs = self.model.predict_traj(curr_x, samples) - + # get particle cost costs = calc_cost(pred_xs, samples, g_xs, - self.state_cost_fn, self.input_cost_fn, \ + self.state_cost_fn, self.input_cost_fn, self.terminal_state_cost_fn) - + return costs @staticmethod def gradient_hamiltonian_x(x, u, lam): """ gradient of hamitonian with respect to the state, """ - raise NotImplementedError("Implement gradient of hamitonian with respect to the state") + raise NotImplementedError( + "Implement gradient of hamitonian with respect to the state") @staticmethod def gradient_hamiltonian_u(x, u, lam): """ gradient of hamitonian with respect to the input """ - raise NotImplementedError("Implement gradient of hamitonian with respect to the input") \ No newline at end of file + raise NotImplementedError( + "Implement gradient of hamitonian with respect to the input") diff --git a/PythonLinearNonlinearControl/controllers/ddp.py b/PythonLinearNonlinearControl/controllers/ddp.py index 4abb229..2ef8099 100644 --- a/PythonLinearNonlinearControl/controllers/ddp.py +++ b/PythonLinearNonlinearControl/controllers/ddp.py @@ -8,6 +8,7 @@ from ..envs.cost import calc_cost logger = getLogger(__name__) + class DDP(Controller): """ Differential Dynamic Programming @@ -18,11 +19,12 @@ class DDP(Controller): https://github.com/studywolf/control, and https://github.com/anassinator/ilqr """ + def __init__(self, config, model): """ """ super(DDP, self).__init__(config, model) - + # model self.model = model @@ -56,7 +58,7 @@ class DDP(Controller): self.Q = config.Q self.R = config.R self.Sf = config.Sf - + # initialize self.prev_sol = np.zeros((self.pred_len, self.input_size)) @@ -65,7 +67,7 @@ class DDP(Controller): """ logger.debug("Clear Sol") self.prev_sol = np.zeros((self.pred_len, self.input_size)) - + def obtain_sol(self, curr_x, g_xs): """ calculate the optimal inputs @@ -89,26 +91,26 @@ class DDP(Controller): while opt_count < self.max_iter: accepted_sol = False - # forward + # forward if update_sol == True: pred_xs, cost, f_x, f_u, f_xx, f_ux, f_uu,\ - l_x, l_xx, l_u, l_uu, l_ux = \ + l_x, l_xx, l_u, l_uu, l_ux = \ self.forward(curr_x, g_xs, sol) update_sol = False - + try: # backward - k, K = self.backward(f_x, f_u, f_xx, f_ux, f_uu, \ + k, K = self.backward(f_x, f_u, f_xx, f_ux, f_uu, l_x, l_xx, l_u, l_uu, l_ux) # line search for alpha in alphas: new_pred_xs, new_sol = \ self.calc_input(k, K, pred_xs, sol, alpha) - + new_cost = calc_cost(new_pred_xs[np.newaxis, :, :], new_sol[np.newaxis, :, :], - g_xs[np.newaxis, :, :], + g_xs[np.newaxis, :, :], self.state_cost_fn, self.input_cost_fn, self.terminal_state_cost_fn) @@ -131,15 +133,15 @@ class DDP(Controller): # accept the solution accepted_sol = True break - + except np.linalg.LinAlgError as e: logger.debug("Non ans : {}".format(e)) - + if not accepted_sol: # increase regularization term. self.delta = max(1.0, self.delta) * self.init_delta self.mu = max(self.mu_min, self.mu * self.delta) - logger.debug("Update regularization term to {}"\ + logger.debug("Update regularization term to {}" .format(self.mu)) if self.mu >= self.mu_max: logger.debug("Reach Max regularization term") @@ -156,7 +158,7 @@ class DDP(Controller): self.prev_sol[-1] = sol[-1] # last use the terminal input return sol[0] - + def calc_input(self, k, K, pred_xs, sol, alpha): """ calc input trajectory by using k and K @@ -183,8 +185,8 @@ class DDP(Controller): for t in range(pred_len): new_sol[t] = sol[t] \ - + alpha * k[t] \ - + np.dot(K[t], (new_pred_xs[t] - pred_xs[t])) + + alpha * k[t] \ + + np.dot(K[t], (new_pred_xs[t] - pred_xs[t])) new_pred_xs[t+1] = self.model.predict_next_state(new_pred_xs[t], new_sol[t]) @@ -227,7 +229,7 @@ class DDP(Controller): g_xs) # calc gradinet in batch - f_x = self.model.calc_f_x(pred_xs[:-1], sol, self.dt) + f_x = self.model.calc_f_x(pred_xs[:-1], sol, self.dt) f_u = self.model.calc_f_u(pred_xs[:-1], sol, self.dt) # calc hessian in batch f_xx = self.model.calc_f_xx(pred_xs[:-1], sol, self.dt) @@ -237,13 +239,13 @@ class DDP(Controller): # gradint of costs l_x, l_xx, l_u, l_uu, l_ux = \ self._calc_gradient_hessian_cost(pred_xs, g_xs, sol) - + return pred_xs, cost, f_x, f_u, f_xx, f_ux, f_uu, \ l_x, l_xx, l_u, l_uu, l_ux def _calc_gradient_hessian_cost(self, pred_xs, g_x, sol): """ calculate gradient and hessian of model and cost fn - + Args: pred_xs (numpy.ndarray): predict traj, shape(pred_len+1, state_size) @@ -268,7 +270,7 @@ class DDP(Controller): self.gradient_cost_fn_with_state(pred_xs[-1], g_x[-1], terminal=True) - l_x = np.concatenate((l_x, terminal_l_x), axis=0) + l_x = np.concatenate((l_x, terminal_l_x), axis=0) # l_u.shape = (pred_len, input_size) l_u = self.gradient_cost_fn_with_input(pred_xs[:-1], sol) @@ -281,7 +283,7 @@ class DDP(Controller): g_x[-1], terminal=True) l_xx = np.concatenate((l_xx, terminal_l_xx), axis=0) - + # l_uu.shape = (pred_len, input_size, input_size) l_uu = self.hessian_cost_fn_with_input(pred_xs[:-1], sol) @@ -321,7 +323,7 @@ class DDP(Controller): # get size (_, state_size, _) = f_x.shape - # initialzie + # initialzie V_x = l_x[-1] V_xx = l_xx[-1] k = np.zeros((self.pred_len, self.input_size)) @@ -388,7 +390,7 @@ class DDP(Controller): """ # get size state_size = len(l_x) - + Q_x = l_x + np.dot(f_x.T, V_x) Q_u = l_u + np.dot(f_u.T, V_x) Q_xx = l_xx + np.dot(np.dot(f_x.T, V_xx), f_x) @@ -402,4 +404,4 @@ class DDP(Controller): Q_ux += np.tensordot(V_x, f_ux, axes=1) Q_uu += np.tensordot(V_x, f_uu, axes=1) - return Q_x, Q_u, Q_xx, Q_ux, Q_uu \ No newline at end of file + return Q_x, Q_u, Q_xx, Q_ux, Q_uu diff --git a/PythonLinearNonlinearControl/controllers/ilqr.py b/PythonLinearNonlinearControl/controllers/ilqr.py index 9a80dfa..7bb81cd 100644 --- a/PythonLinearNonlinearControl/controllers/ilqr.py +++ b/PythonLinearNonlinearControl/controllers/ilqr.py @@ -8,6 +8,7 @@ from ..envs.cost import calc_cost logger = getLogger(__name__) + class iLQR(Controller): """ iterative Liner Quadratique Regulator @@ -16,11 +17,12 @@ class iLQR(Controller): Intelligent Robots and Systems (pp. 4906-4913). and Study Wolf, https://github.com/studywolf/control """ + def __init__(self, config, model): """ """ super(iLQR, self).__init__(config, model) - + # model self.model = model @@ -58,7 +60,7 @@ class iLQR(Controller): """ logger.debug("Clear Sol") self.prev_sol = np.zeros((self.pred_len, self.input_size)) - + def obtain_sol(self, curr_x, g_xs): """ calculate the optimal inputs @@ -82,12 +84,12 @@ class iLQR(Controller): while opt_count < self.max_iter: accepted_sol = False - # forward + # forward if update_sol == True: pred_xs, cost, f_x, f_u, l_x, l_xx, l_u, l_uu, l_ux = \ self.forward(curr_x, g_xs, sol) update_sol = False - + try: # backward k, K = self.backward(f_x, f_u, l_x, l_xx, l_u, l_uu, l_ux) @@ -96,10 +98,10 @@ class iLQR(Controller): for alpha in alphas: new_pred_xs, new_sol = \ self.calc_input(k, K, pred_xs, sol, alpha) - + new_cost = calc_cost(new_pred_xs[np.newaxis, :, :], new_sol[np.newaxis, :, :], - g_xs[np.newaxis, :, :], + g_xs[np.newaxis, :, :], self.state_cost_fn, self.input_cost_fn, self.terminal_state_cost_fn) @@ -122,15 +124,15 @@ class iLQR(Controller): # accept the solution accepted_sol = True break - + except np.linalg.LinAlgError as e: logger.debug("Non ans : {}".format(e)) - + if not accepted_sol: # increase regularization term. self.delta = max(1.0, self.delta) * self.init_delta self.mu = max(self.mu_min, self.mu * self.delta) - logger.debug("Update regularization term to {}"\ + logger.debug("Update regularization term to {}" .format(self.mu)) if self.mu >= self.mu_max: logger.debug("Reach Max regularization term") @@ -147,7 +149,7 @@ class iLQR(Controller): self.prev_sol[-1] = sol[-1] # last use the terminal input return sol[0] - + def calc_input(self, k, K, pred_xs, sol, alpha): """ calc input trajectory by using k and K @@ -174,8 +176,8 @@ class iLQR(Controller): for t in range(pred_len): new_sol[t] = sol[t] \ - + alpha * k[t] \ - + np.dot(K[t], (new_pred_xs[t] - pred_xs[t])) + + alpha * k[t] \ + + np.dot(K[t], (new_pred_xs[t] - pred_xs[t])) new_pred_xs[t+1] = self.model.predict_next_state(new_pred_xs[t], new_sol[t]) @@ -212,18 +214,18 @@ class iLQR(Controller): g_xs) # calc gradinet in batch - f_x = self.model.calc_f_x(pred_xs[:-1], sol, self.dt) + f_x = self.model.calc_f_x(pred_xs[:-1], sol, self.dt) f_u = self.model.calc_f_u(pred_xs[:-1], sol, self.dt) # gradint of costs l_x, l_xx, l_u, l_uu, l_ux = \ self._calc_gradient_hessian_cost(pred_xs, g_xs, sol) - + return pred_xs, cost, f_x, f_u, l_x, l_xx, l_u, l_uu, l_ux def _calc_gradient_hessian_cost(self, pred_xs, g_x, sol): """ calculate gradient and hessian of model and cost fn - + Args: pred_xs (numpy.ndarray): predict traj, shape(pred_len+1, state_size) @@ -248,7 +250,7 @@ class iLQR(Controller): self.gradient_cost_fn_with_state(pred_xs[-1], g_x[-1], terminal=True) - l_x = np.concatenate((l_x, terminal_l_x), axis=0) + l_x = np.concatenate((l_x, terminal_l_x), axis=0) # l_u.shape = (pred_len, input_size) l_u = self.gradient_cost_fn_with_input(pred_xs[:-1], sol) @@ -261,7 +263,7 @@ class iLQR(Controller): g_x[-1], terminal=True) l_xx = np.concatenate((l_xx, terminal_l_xx), axis=0) - + # l_uu.shape = (pred_len, input_size, input_size) l_uu = self.hessian_cost_fn_with_input(pred_xs[:-1], sol) @@ -287,7 +289,7 @@ class iLQR(Controller): shape(pred_len, input_size, input_size) l_ux (numpy.ndarray): hessian of cost with respect to state and input, shape(pred_len, input_size, state_size) - + Returns: k (numpy.ndarray): gain, shape(pred_len, input_size) K (numpy.ndarray): gain, shape(pred_len, input_size, state_size) @@ -295,7 +297,7 @@ class iLQR(Controller): # get size (_, state_size, _) = f_x.shape - # initialzie + # initialzie V_x = l_x[-1] V_xx = l_xx[-1] k = np.zeros((self.pred_len, self.input_size)) @@ -352,7 +354,7 @@ class iLQR(Controller): """ # get size state_size = len(l_x) - + Q_x = l_x + np.dot(f_x.T, V_x) Q_u = l_u + np.dot(f_u.T, V_x) Q_xx = l_xx + np.dot(np.dot(f_x.T, V_xx), f_x) @@ -361,4 +363,4 @@ class iLQR(Controller): Q_ux = l_ux + np.dot(np.dot(f_u.T, (V_xx + reg)), f_x) Q_uu = l_uu + np.dot(np.dot(f_u.T, (V_xx + reg)), f_u) - return Q_x, Q_u, Q_xx, Q_ux, Q_uu \ No newline at end of file + return Q_x, Q_u, Q_xx, Q_ux, Q_uu diff --git a/PythonLinearNonlinearControl/controllers/make_controllers.py b/PythonLinearNonlinearControl/controllers/make_controllers.py index d5ec8df..1bc6138 100644 --- a/PythonLinearNonlinearControl/controllers/make_controllers.py +++ b/PythonLinearNonlinearControl/controllers/make_controllers.py @@ -6,6 +6,7 @@ from .mppi_williams import MPPIWilliams from .ilqr import iLQR from .ddp import DDP + def make_controller(args, config, model): if args.controller_type == "MPC": @@ -22,5 +23,5 @@ def make_controller(args, config, model): return iLQR(config, model) elif args.controller_type == "DDP": return DDP(config, model) - - raise ValueError("No controller: {}".format(args.controller_type)) \ No newline at end of file + + raise ValueError("No controller: {}".format(args.controller_type)) diff --git a/PythonLinearNonlinearControl/controllers/mpc.py b/PythonLinearNonlinearControl/controllers/mpc.py index ddf5840..c1e0db3 100644 --- a/PythonLinearNonlinearControl/controllers/mpc.py +++ b/PythonLinearNonlinearControl/controllers/mpc.py @@ -9,6 +9,7 @@ from ..envs.cost import calc_cost logger = getLogger(__name__) + class LinearMPC(Controller): """ Model Predictive Controller for linear model @@ -21,6 +22,7 @@ class LinearMPC(Controller): Ref: Maciejowski, J. M. (2002). Predictive control: with constraints. """ + def __init__(self, config, model): """ Args: @@ -55,7 +57,7 @@ class LinearMPC(Controller): self.dt_input_upper_bound = config.DT_INPUT_UPPER_BOUND self.input_lower_bound = config.INPUT_LOWER_BOUND self.input_upper_bound = config.INPUT_UPPER_BOUND - + # setup controllers self.W = None self.omega = None @@ -66,7 +68,7 @@ class LinearMPC(Controller): # history self.history_u = [np.zeros(self.input_size)] - + def setup(self): """ setup Model Predictive Control as a quadratic programming @@ -77,11 +79,11 @@ class LinearMPC(Controller): for _ in range(self.pred_len - 1): temp_mat = np.matmul(A_factorials[-1], self.A) self.phi_mat = np.vstack((self.phi_mat, temp_mat)) - A_factorials.append(temp_mat) # after we use this factorials - + A_factorials.append(temp_mat) # after we use this factorials + self.gamma_mat = self.B.copy() gammma_mat_temp = self.B.copy() - + for i in range(self.pred_len - 1): temp_1_mat = np.matmul(A_factorials[i], self.B) gammma_mat_temp = temp_1_mat + gammma_mat_temp @@ -91,8 +93,8 @@ class LinearMPC(Controller): for i in range(self.pred_len - 1): temp_mat = np.zeros_like(self.gamma_mat) - temp_mat[int((i + 1)*self.state_size): , :] =\ - self.gamma_mat[:-int((i + 1)*self.state_size) , :] + temp_mat[int((i + 1)*self.state_size):, :] =\ + self.gamma_mat[:-int((i + 1)*self.state_size), :] self.theta_mat = np.hstack((self.theta_mat, temp_mat)) @@ -114,12 +116,12 @@ class LinearMPC(Controller): for i in range(self.pred_len - 1): for j in range(self.input_size): - temp_F[j * 2: (j + 1) * 2,\ + temp_F[j * 2: (j + 1) * 2, ((i+1) * self.input_size) + j] = np.array([1., -1.]) self.F = np.vstack((self.F, temp_F)) self.F1 = self.F[:, :self.input_size] - + temp_f = [] for i in range(self.input_size): temp_f.append(-1 * self.input_upper_bound[i]) @@ -168,7 +170,7 @@ class LinearMPC(Controller): H = H * 0.5 # constraints - A = [] + A = [] b = [] if self.W is not None: @@ -187,14 +189,14 @@ class LinearMPC(Controller): # using cvxopt def optimized_func(dt_us): - return (np.dot(dt_us, np.dot(H, dt_us.reshape(-1, 1))) \ + 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(),\ + opt_sol = minimize(optimized_func, self.prev_sol.flatten(), constraints=[cons]) opt_dt_us = opt_sol.x @@ -213,21 +215,21 @@ class LinearMPC(Controller): """ # to dt form - opt_dt_u_seq = np.cumsum(opt_dt_us.reshape(self.pred_len,\ + 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] - + # save self.history_u.append(opt_u_seq[0]) # check costs costs = self.calc_cost(curr_x, opt_u_seq.reshape(1, - self.pred_len, - self.input_size), + self.pred_len, + self.input_size), g_xs) logger.debug("Cost = {}".format(costs)) @@ -235,4 +237,4 @@ class LinearMPC(Controller): return opt_u_seq[0] def __str__(self): - return "LinearMPC" \ No newline at end of file + return "LinearMPC" diff --git a/PythonLinearNonlinearControl/controllers/mppi.py b/PythonLinearNonlinearControl/controllers/mppi.py index fc8d887..3b0be45 100644 --- a/PythonLinearNonlinearControl/controllers/mppi.py +++ b/PythonLinearNonlinearControl/controllers/mppi.py @@ -8,6 +8,7 @@ from ..envs.cost import calc_cost logger = getLogger(__name__) + class MPPI(Controller): """ Model Predictive Path Integral for linear and nonlinear method @@ -18,6 +19,7 @@ class MPPI(Controller): Deep Dynamics Models for Learning Dexterous Manipulation. arXiv preprint arXiv:1909.11652. """ + def __init__(self, config, model): super(MPPI, self).__init__(config, model) @@ -35,7 +37,7 @@ class MPPI(Controller): self.noise_sigma = config.opt_config["MPPI"]["noise_sigma"] self.opt_dim = self.input_size * self.pred_len - # get bound + # get bound self.input_upper_bounds = np.tile(config.INPUT_UPPER_BOUND, (self.pred_len, 1)) self.input_lower_bounds = np.tile(config.INPUT_LOWER_BOUND, @@ -47,14 +49,14 @@ class MPPI(Controller): self.input_cost_fn = config.input_cost_fn # init mean - self.prev_sol = np.tile((config.INPUT_UPPER_BOUND \ + self.prev_sol = np.tile((config.INPUT_UPPER_BOUND + config.INPUT_LOWER_BOUND) / 2., self.pred_len) self.prev_sol = self.prev_sol.reshape(self.pred_len, self.input_size) # save self.history_u = [np.zeros(self.input_size)] - + def clear_sol(self): """ clear prev sol """ @@ -74,24 +76,24 @@ class MPPI(Controller): """ # get noised inputs noise = np.random.normal( - loc=0, scale=1.0, size=(self.pop_size, self.pred_len, - self.input_size)) * self.noise_sigma + loc=0, scale=1.0, size=(self.pop_size, self.pred_len, + self.input_size)) * self.noise_sigma noised_inputs = noise.copy() - + for t in range(self.pred_len): if t > 0: noised_inputs[:, t, :] = self.beta \ - * (self.prev_sol[t, :] \ - + noise[:, t, :]) \ - + (1 - self.beta) \ - * noised_inputs[:, t-1, :] + * (self.prev_sol[t, :] + + noise[:, t, :]) \ + + (1 - self.beta) \ + * noised_inputs[:, t-1, :] else: noised_inputs[:, t, :] = self.beta \ - * (self.prev_sol[t, :] \ - + noise[:, t, :]) \ - + (1 - self.beta) \ - * self.history_u[-1] - + * (self.prev_sol[t, :] + + noise[:, t, :]) \ + + (1 - self.beta) \ + * self.history_u[-1] + # clip actions noised_inputs = np.clip( noised_inputs, self.input_lower_bounds, self.input_upper_bounds) @@ -108,7 +110,7 @@ class MPPI(Controller): # weight actions weighted_inputs = exp_rewards[:, np.newaxis, np.newaxis] \ - * noised_inputs + * noised_inputs sol = np.sum(weighted_inputs, 0) / denom # update @@ -121,4 +123,4 @@ class MPPI(Controller): return sol[0] def __str__(self): - return "MPPI" \ No newline at end of file + return "MPPI" diff --git a/PythonLinearNonlinearControl/controllers/mppi_williams.py b/PythonLinearNonlinearControl/controllers/mppi_williams.py index 1fd0102..3ffaaa1 100644 --- a/PythonLinearNonlinearControl/controllers/mppi_williams.py +++ b/PythonLinearNonlinearControl/controllers/mppi_williams.py @@ -8,6 +8,7 @@ from ..envs.cost import calc_cost logger = getLogger(__name__) + class MPPIWilliams(Controller): """ Model Predictive Path Integral for linear and nonlinear method @@ -19,6 +20,7 @@ class MPPIWilliams(Controller): 2017 IEEE International Conference on Robotics and Automation (ICRA), Singapore, 2017, pp. 1714-1721. """ + def __init__(self, config, model): super(MPPIWilliams, self).__init__(config, model) @@ -35,7 +37,7 @@ class MPPIWilliams(Controller): self.noise_sigma = config.opt_config["MPPIWilliams"]["noise_sigma"] self.opt_dim = self.input_size * self.pred_len - # get bound + # get bound self.input_upper_bounds = np.tile(config.INPUT_UPPER_BOUND, (self.pred_len, 1)) self.input_lower_bounds = np.tile(config.INPUT_LOWER_BOUND, @@ -47,14 +49,14 @@ class MPPIWilliams(Controller): self.input_cost_fn = config.input_cost_fn # init mean - self.prev_sol = np.tile((config.INPUT_UPPER_BOUND \ + self.prev_sol = np.tile((config.INPUT_UPPER_BOUND + config.INPUT_LOWER_BOUND) / 2., self.pred_len) self.prev_sol = self.prev_sol.reshape(self.pred_len, self.input_size) # save self.history_u = [np.zeros(self.input_size)] - + def clear_sol(self): """ clear prev sol """ @@ -62,7 +64,7 @@ class MPPIWilliams(Controller): self.prev_sol = \ (self.input_upper_bounds + self.input_lower_bounds) / 2. self.prev_sol = self.prev_sol.reshape(self.pred_len, self.input_size) - + def calc_cost(self, curr_x, samples, g_xs): """ calculate the cost of input samples by using MPPI's eq @@ -82,12 +84,12 @@ class MPPIWilliams(Controller): # calc cost, pred_xs.shape = (pop_size, pred_len+1, state_size) pred_xs = self.model.predict_traj(curr_x, samples) - + # get particle cost costs = calc_cost(pred_xs, samples, g_xs, - self.state_cost_fn, None, \ + self.state_cost_fn, None, self.terminal_state_cost_fn) - + return costs def obtain_sol(self, curr_x, g_xs): @@ -101,9 +103,9 @@ class MPPIWilliams(Controller): """ # get noised inputs noise = np.random.normal( - loc=0, scale=1.0, size=(self.pop_size, self.pred_len, - self.input_size)) * self.noise_sigma - + loc=0, scale=1.0, size=(self.pop_size, self.pred_len, + self.input_size)) * self.noise_sigma + noised_inputs = self.prev_sol + noise # clip actions @@ -120,7 +122,7 @@ class MPPIWilliams(Controller): # mppi update beta = np.min(costs) eta = np.sum(np.exp(- 1. / self.lam * (costs - beta)), axis=0) \ - + 1e-10 + + 1e-10 # weight # eta.shape = (pred_len, input_size) @@ -128,7 +130,7 @@ class MPPIWilliams(Controller): # update inputs sol = self.prev_sol \ - + np.sum(weights[:, np.newaxis, np.newaxis] * noise, axis=0) + + np.sum(weights[:, np.newaxis, np.newaxis] * noise, axis=0) # update self.prev_sol[:-1] = sol[1:] @@ -140,4 +142,4 @@ class MPPIWilliams(Controller): return sol[0] def __str__(self): - return "MPPIWilliams" \ No newline at end of file + return "MPPIWilliams" diff --git a/PythonLinearNonlinearControl/controllers/random.py b/PythonLinearNonlinearControl/controllers/random.py index 53a7622..e82fbdf 100644 --- a/PythonLinearNonlinearControl/controllers/random.py +++ b/PythonLinearNonlinearControl/controllers/random.py @@ -8,6 +8,7 @@ from ..envs.cost import calc_cost logger = getLogger(__name__) + class RandomShooting(Controller): """ Random Shooting Method for linear and nonlinear method @@ -19,6 +20,7 @@ class RandomShooting(Controller): using probabilistic dynamics models. In Advances in Neural Information Processing Systems (pp. 4754-4765). """ + def __init__(self, config, model): super(RandomShooting, self).__init__(config, model) @@ -33,7 +35,7 @@ class RandomShooting(Controller): self.pop_size = config.opt_config["Random"]["popsize"] self.opt_dim = self.input_size * self.pred_len - # get bound + # get bound self.input_upper_bounds = np.tile(config.INPUT_UPPER_BOUND, self.pred_len) self.input_lower_bounds = np.tile(config.INPUT_LOWER_BOUND, @@ -46,7 +48,7 @@ class RandomShooting(Controller): # save self.history_u = [] - + def obtain_sol(self, curr_x, g_xs): """ calculate the optimal inputs @@ -65,8 +67,8 @@ class RandomShooting(Controller): # calc cost costs = self.calc_cost(curr_x, samples.reshape(self.pop_size, - self.pred_len, - self.input_size), + self.pred_len, + self.input_size), g_xs) # solution sol = samples[np.argmin(costs)] @@ -74,4 +76,4 @@ class RandomShooting(Controller): return sol.reshape(self.pred_len, self.input_size).copy()[0] def __str__(self): - return "RandomShooting" \ No newline at end of file + return "RandomShooting" diff --git a/PythonLinearNonlinearControl/envs/__init__.py b/PythonLinearNonlinearControl/envs/__init__.py index 837b5d8..e649609 100644 --- a/PythonLinearNonlinearControl/envs/__init__.py +++ b/PythonLinearNonlinearControl/envs/__init__.py @@ -5,4 +5,4 @@ from PythonLinearNonlinearControl.envs.first_order_lag \ from PythonLinearNonlinearControl.envs.two_wheeled \ import TwoWheeledConstEnv # NOQA from PythonLinearNonlinearControl.envs.two_wheeled \ - import TwoWheeledTrackEnv # NOQA \ No newline at end of file + import TwoWheeledTrackEnv # NOQA diff --git a/PythonLinearNonlinearControl/envs/cartpole.py b/PythonLinearNonlinearControl/envs/cartpole.py index fd70d47..f64b0df 100644 --- a/PythonLinearNonlinearControl/envs/cartpole.py +++ b/PythonLinearNonlinearControl/envs/cartpole.py @@ -4,6 +4,7 @@ from matplotlib.axes import Axes from .env import Env from ..plotters.plot_objs import square + class CartPoleEnv(Env): """ Cartpole Environment @@ -13,13 +14,14 @@ class CartPoleEnv(Env): 6-832-underactuated-robotics-spring-2009/readings/ MIT6_832s09_read_ch03.pdf """ + def __init__(self): """ """ - self.config = {"state_size" : 4, - "input_size" : 1, - "dt" : 0.02, - "max_step" : 500, + self.config = {"state_size": 4, + "input_size": 1, + "dt": 0.02, + "max_step": 500, "input_lower_bound": [-3.], "input_upper_bound": [3.], "mp": 0.2, @@ -30,7 +32,7 @@ class CartPoleEnv(Env): } super(CartPoleEnv, self).__init__(self.config) - + def reset(self, init_x=None): """ reset state @@ -39,7 +41,7 @@ class CartPoleEnv(Env): info (dict): information """ self.step_count = 0 - + theta = np.random.randn(1) self.curr_x = np.array([0., 0., theta[0], 0.]) @@ -48,7 +50,7 @@ class CartPoleEnv(Env): # goal self.g_x = np.array([0., 0., -np.pi, 0.]) - + # clear memory self.history_x = [] self.history_g_x = [] @@ -76,50 +78,50 @@ class CartPoleEnv(Env): # x d_x0 = self.curr_x[1] # v_x - d_x1 = (u[0] + self.config["mp"] * np.sin(self.curr_x[2]) \ - * (self.config["l"] * (self.curr_x[3]**2) \ - + self.config["g"] * np.cos(self.curr_x[2]))) \ - / (self.config["mc"] + self.config["mp"] \ - * (np.sin(self.curr_x[2])**2)) + d_x1 = (u[0] + self.config["mp"] * np.sin(self.curr_x[2]) + * (self.config["l"] * (self.curr_x[3]**2) + + self.config["g"] * np.cos(self.curr_x[2]))) \ + / (self.config["mc"] + self.config["mp"] + * (np.sin(self.curr_x[2])**2)) # theta d_x2 = self.curr_x[3] - + # v_theta - d_x3 = (-u[0] * np.cos(self.curr_x[2]) \ - - self.config["mp"] * self.config["l"] * (self.curr_x[3]**2) \ - * np.cos(self.curr_x[2]) * np.sin(self.curr_x[2]) \ - - (self.config["mc"] + self.config["mp"]) * self.config["g"] \ - * np.sin(self.curr_x[2])) \ - / (self.config["l"] * (self.config["mc"] + self.config["mp"] \ - * (np.sin(self.curr_x[2])**2))) - + d_x3 = (-u[0] * np.cos(self.curr_x[2]) + - self.config["mp"] * self.config["l"] * (self.curr_x[3]**2) + * np.cos(self.curr_x[2]) * np.sin(self.curr_x[2]) + - (self.config["mc"] + self.config["mp"]) * self.config["g"] + * np.sin(self.curr_x[2])) \ + / (self.config["l"] * (self.config["mc"] + self.config["mp"] + * (np.sin(self.curr_x[2])**2))) + next_x = self.curr_x +\ - np.array([d_x0, d_x1, d_x2, d_x3]) * self.config["dt"] + np.array([d_x0, d_x1, d_x2, d_x3]) * self.config["dt"] # TODO: costs costs = 0. costs += 0.1 * np.sum(u**2) costs += 6. * self.curr_x[0]**2 \ - + 12. * (np.cos(self.curr_x[2]) + 1.)**2 \ - + 0.1 * self.curr_x[1]**2 \ - + 0.1 * self.curr_x[3]**2 + + 12. * (np.cos(self.curr_x[2]) + 1.)**2 \ + + 0.1 * self.curr_x[1]**2 \ + + 0.1 * self.curr_x[3]**2 # save history self.history_x.append(next_x.flatten()) self.history_g_x.append(self.g_x.flatten()) - + # update self.curr_x = next_x.flatten().copy() # update costs self.step_count += 1 return next_x.flatten(), costs, \ - self.step_count > self.config["max_step"], \ - {"goal_state" : self.g_x} - + self.step_count > self.config["max_step"], \ + {"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 @@ -131,29 +133,29 @@ class CartPoleEnv(Env): """ 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",\ + 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),\ + 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 @@ -166,13 +168,13 @@ class CartPoleEnv(Env): pole_y (numpy.ndarray): y data of pole """ # cart - cart_x, cart_y = square(curr_x[0], 0.,\ + 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 \ No newline at end of file + # 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 diff --git a/PythonLinearNonlinearControl/envs/cost.py b/PythonLinearNonlinearControl/envs/cost.py index 117d5f2..3247dec 100644 --- a/PythonLinearNonlinearControl/envs/cost.py +++ b/PythonLinearNonlinearControl/envs/cost.py @@ -4,6 +4,7 @@ import numpy as np logger = getLogger(__name__) + def calc_cost(pred_xs, input_sample, g_xs, state_cost_fn, input_cost_fn, terminal_state_cost_fn): """ calculate the cost @@ -24,20 +25,21 @@ def calc_cost(pred_xs, input_sample, g_xs, # state cost state_cost = 0. if state_cost_fn is not None: - state_pred_par_cost = state_cost_fn(pred_xs[:, 1:-1, :], g_xs[:, 1:-1, :]) + state_pred_par_cost = state_cost_fn( + pred_xs[:, 1:-1, :], g_xs[:, 1:-1, :]) state_cost = np.sum(np.sum(state_pred_par_cost, axis=-1), axis=-1) # terminal cost terminal_state_cost = 0. if terminal_state_cost_fn is not None: terminal_state_par_cost = terminal_state_cost_fn(pred_xs[:, -1, :], - g_xs[:, -1, :]) + g_xs[:, -1, :]) terminal_state_cost = np.sum(terminal_state_par_cost, axis=-1) - + # act cost act_cost = 0. if input_cost_fn is not None: act_pred_par_cost = input_cost_fn(input_sample) act_cost = np.sum(np.sum(act_pred_par_cost, axis=-1), axis=-1) - return state_cost + terminal_state_cost + act_cost \ No newline at end of file + return state_cost + terminal_state_cost + act_cost diff --git a/PythonLinearNonlinearControl/envs/env.py b/PythonLinearNonlinearControl/envs/env.py index 4f63903..9f59141 100644 --- a/PythonLinearNonlinearControl/envs/env.py +++ b/PythonLinearNonlinearControl/envs/env.py @@ -1,13 +1,15 @@ 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 """ + def __init__(self, config): """ """ @@ -25,12 +27,12 @@ class Env(): 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 - + # clear memory self.history_x = [] self.history_g_x = [] @@ -52,4 +54,4 @@ class Env(): def __repr__(self): """ """ - return self.config \ No newline at end of file + return self.config diff --git a/PythonLinearNonlinearControl/envs/first_order_lag.py b/PythonLinearNonlinearControl/envs/first_order_lag.py index 5597a07..f7f06bc 100644 --- a/PythonLinearNonlinearControl/envs/first_order_lag.py +++ b/PythonLinearNonlinearControl/envs/first_order_lag.py @@ -3,25 +3,27 @@ import scipy from scipy import integrate from .env import Env + class FirstOrderLagEnv(Env): """ First Order Lag System Env """ + def __init__(self, tau=0.63): """ """ - self.config = {"state_size" : 4,\ - "input_size" : 2,\ - "dt" : 0.05,\ - "max_step" : 500,\ - "input_lower_bound": [-0.5, -0.5],\ + self.config = {"state_size": 4, + "input_size": 2, + "dt": 0.05, + "max_step": 500, + "input_lower_bound": [-0.5, -0.5], "input_upper_bound": [0.5, 0.5], } super(FirstOrderLagEnv, self).__init__(self.config) # to get discrete system matrix - self.A, self.B = self._to_state_space(tau, dt=self.config["dt"]) - + self.A, self.B = self._to_state_space(tau, dt=self.config["dt"]) + @staticmethod def _to_state_space(tau, dt=0.05): """ @@ -34,13 +36,13 @@ class FirstOrderLagEnv(Env): """ # continuous Ac = np.array([[-1./tau, 0., 0., 0.], - [0., -1./tau, 0., 0.], - [1., 0., 0., 0.], - [0., 1., 0., 0.]]) + [0., -1./tau, 0., 0.], + [1., 0., 0., 0.], + [0., 1., 0., 0.]]) Bc = np.array([[1./tau, 0.], - [0., 1./tau], - [0., 0.], - [0., 0.]]) + [0., 1./tau], + [0., 0.], + [0., 0.]]) # to discrete system A = scipy.linalg.expm(dt*Ac) # B = np.matmul(np.matmul(scipy.linalg.expm(Ac*dt) - @@ -55,7 +57,7 @@ class FirstOrderLagEnv(Env): B[m, n] = sol[0] return A, B - + def reset(self, init_x=None): """ reset state Returns: @@ -63,7 +65,7 @@ class FirstOrderLagEnv(Env): info (dict): information """ self.step_count = 0 - + self.curr_x = np.zeros(self.config["state_size"]) if init_x is not None: @@ -71,7 +73,7 @@ class FirstOrderLagEnv(Env): # goal self.g_x = np.array([0., 0, -2., 3.]) - + # clear memory self.history_x = [] self.history_g_x = [] @@ -94,7 +96,7 @@ class FirstOrderLagEnv(Env): self.config["input_upper_bound"]) next_x = np.matmul(self.A, self.curr_x[:, np.newaxis]) \ - + np.matmul(self.B, u[:, np.newaxis]) + + np.matmul(self.B, u[:, np.newaxis]) # cost cost = 0 @@ -104,17 +106,17 @@ class FirstOrderLagEnv(Env): # save history self.history_x.append(next_x.flatten()) self.history_g_x.append(self.g_x.flatten()) - + # update self.curr_x = next_x.flatten() # update costs self.step_count += 1 return next_x.flatten(), cost, \ - self.step_count > self.config["max_step"], \ - {"goal_state" : self.g_x} - + self.step_count > self.config["max_step"], \ + {"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") \ No newline at end of file + raise ValueError("FirstOrderLag does not have animation") diff --git a/PythonLinearNonlinearControl/envs/make_envs.py b/PythonLinearNonlinearControl/envs/make_envs.py index 253f3ed..5603b45 100644 --- a/PythonLinearNonlinearControl/envs/make_envs.py +++ b/PythonLinearNonlinearControl/envs/make_envs.py @@ -2,6 +2,8 @@ from .first_order_lag import FirstOrderLagEnv from .two_wheeled import TwoWheeledConstEnv from .two_wheeled import TwoWheeledTrackEnv from .cartpole import CartPoleEnv +from .nonlinear_sample_system import NonlinearSampleSystemEnv + def make_env(args): @@ -13,5 +15,7 @@ def make_env(args): return TwoWheeledTrackEnv() elif args.env == "CartPole": return CartPoleEnv() - - raise NotImplementedError("There is not {} Env".format(args.env)) \ No newline at end of file + elif args.env == "NonlinearSample": + return NonlinearSampleSystemEnv() + + raise NotImplementedError("There is not {} Env".format(args.env)) diff --git a/PythonLinearNonlinearControl/envs/nonlinear_sample_system.py b/PythonLinearNonlinearControl/envs/nonlinear_sample_system.py new file mode 100644 index 0000000..38a827f --- /dev/null +++ b/PythonLinearNonlinearControl/envs/nonlinear_sample_system.py @@ -0,0 +1,97 @@ +import numpy as np +import scipy +from scipy import integrate +from .env import Env +from ..common.utils import update_state_with_Runge_Kutta + + +class NonlinearSampleSystemEnv(Env): + """ Nonlinear Sample Env + """ + + def __init__(self): + """ + """ + self.config = {"state_size": 2, + "input_size": 1, + "dt": 0.01, + "max_step": 2000, + "input_lower_bound": [-0.5], + "input_upper_bound": [0.5], + } + + super(NonlinearSampleSystemEnv, self).__init__(self.config) + + def reset(self, init_x=np.array([2., 0.])): + """ 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_x = np.array([0., 0.]) + + # clear memory + self.history_x = [] + self.history_g_x = [] + + return self.curr_x, {"goal_state": self.g_x} + + def step(self, u): + """ + 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"]) + + functions = [self._func_x_1, self._func_x_2] + + next_x = update_state_with_Runge_Kutta(self.curr_x, u, + functions, self.config["dt"], + batch=False) + + # cost + cost = 0 + cost = np.sum(u**2) + cost += np.sum((self.curr_x - self.g_x)**2) + + # save history + self.history_x.append(next_x.flatten()) + self.history_g_x.append(self.g_x.flatten()) + + # update + self.curr_x = next_x.flatten() + # update costs + self.step_count += 1 + + return next_x.flatten(), cost, \ + self.step_count > self.config["max_step"], \ + {"goal_state": self.g_x} + + def _func_x_1(self, x, u): + x_dot = x[1] + return x_dot + + def _func_x_2(self, x, u): + x_dot = (1. - x[0]**2 - x[1]**2) * x[1] - x[0] + u[0] + return x_dot + + def plot_func(self, to_plot, i=None, history_x=None, history_g_x=None): + """ + """ + raise ValueError("NonlinearSampleSystemEnv does not have animation") diff --git a/PythonLinearNonlinearControl/envs/two_wheeled.py b/PythonLinearNonlinearControl/envs/two_wheeled.py index 43f5f0c..be59d3e 100644 --- a/PythonLinearNonlinearControl/envs/two_wheeled.py +++ b/PythonLinearNonlinearControl/envs/two_wheeled.py @@ -5,47 +5,50 @@ 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 - + Args: curr_x (numpy.ndarray): current state, shape(state_size, ) u (numpy.ndarray): input, shape(input_size, ) dt (float): sampling time Returns: next_x (numpy.ndarray): next state, shape(state_size. ) - + Notes: TODO: deal with another method, like Runge Kutta """ B = np.array([[np.cos(curr_x[-1]), 0.], [np.sin(curr_x[-1]), 0.], [0., 1.]]) - + x_dot = np.matmul(B, u[:, np.newaxis]) next_x = x_dot.flatten() * dt + curr_x return next_x + class TwoWheeledConstEnv(Env): """ Two wheeled robot with constant goal Env """ + def __init__(self): """ """ - self.config = {"state_size" : 3,\ - "input_size" : 2,\ - "dt" : 0.01,\ - "max_step" : 500,\ - "input_lower_bound": (-1.5, -3.14),\ - "input_upper_bound": (1.5, 3.14),\ - "car_size": 0.2,\ + self.config = {"state_size": 3, + "input_size": 2, + "dt": 0.01, + "max_step": 500, + "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(TwoWheeledConstEnv, self).__init__(self.config) - + def reset(self, init_x=None): """ reset state @@ -54,7 +57,7 @@ class TwoWheeledConstEnv(Env): info (dict): information """ self.step_count = 0 - + self.curr_x = np.zeros(self.config["state_size"]) if init_x is not None: @@ -62,7 +65,7 @@ class TwoWheeledConstEnv(Env): # goal self.g_x = np.array([2.5, 2.5, 0.]) - + # clear memory self.history_x = [] self.history_g_x = [] @@ -96,32 +99,32 @@ class TwoWheeledConstEnv(Env): # save history self.history_x.append(next_x.flatten()) self.history_g_x.append(self.g_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_x} - + self.step_count > self.config["max_step"], \ + {"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] @@ -139,9 +142,9 @@ class TwoWheeledConstEnv(Env): # 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 = \ + 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,) @@ -150,7 +153,7 @@ class TwoWheeledConstEnv(Env): # 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 """ @@ -158,53 +161,55 @@ class TwoWheeledConstEnv(Env): 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"] \ + 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"] \ + * 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] - + * np.sin(curr_x[2]-np.pi/2.) + curr_x[1] + left_tire_x, left_tire_y = \ - square(center_x, center_y, + square(center_x, center_y, self.config["wheel_size"], curr_x[2]) # right tire - center_x = (self.config["car_size"] \ + 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"] \ + * 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] - + * np.sin(curr_x[2]+np.pi/2.) + curr_x[1] + right_tire_x, right_tire_y = \ - square(center_x, center_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 + 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,\ + 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 @@ -220,23 +225,23 @@ class TwoWheeledTrackEnv(Env): # 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_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) + 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.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)) + return np.tile(road, (3, 1)) def reset(self, init_x=None): """ reset state @@ -246,7 +251,7 @@ class TwoWheeledTrackEnv(Env): info (dict): information """ self.step_count = 0 - + self.curr_x = np.zeros(self.config["state_size"]) if init_x is not None: @@ -254,7 +259,7 @@ class TwoWheeledTrackEnv(Env): # goal self.g_traj = self.make_road() - + # clear memory self.history_x = [] self.history_g_x = [] @@ -286,32 +291,32 @@ class TwoWheeledTrackEnv(Env): # 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} - + 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] @@ -333,9 +338,9 @@ class TwoWheeledTrackEnv(Env): # 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 = \ + 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,) @@ -344,7 +349,7 @@ class TwoWheeledTrackEnv(Env): # 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 """ @@ -352,31 +357,31 @@ class TwoWheeledTrackEnv(Env): 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"] \ + 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"] \ + * 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] - + * np.sin(curr_x[2]-np.pi/2.) + curr_x[1] + left_tire_x, left_tire_y = \ - square(center_x, center_y, + square(center_x, center_y, self.config["wheel_size"], curr_x[2]) # right tire - center_x = (self.config["car_size"] \ + 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"] \ + * 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] - + * np.sin(curr_x[2]+np.pi/2.) + curr_x[1] + right_tire_x, right_tire_y = \ - square(center_x, center_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 \ No newline at end of file + left_tire_x, left_tire_y,\ + right_tire_x, right_tire_y diff --git a/PythonLinearNonlinearControl/helper.py b/PythonLinearNonlinearControl/helper.py index 7fa2058..26844ef 100644 --- a/PythonLinearNonlinearControl/helper.py +++ b/PythonLinearNonlinearControl/helper.py @@ -7,6 +7,7 @@ import six import pickle from logging import DEBUG, basicConfig, getLogger, FileHandler, StreamHandler, Formatter, Logger + def make_logger(save_dir): """ Args: @@ -21,7 +22,7 @@ def make_logger(save_dir): # mypackage log level logger = getLogger("PythonLinearNonlinearControl") logger.setLevel(DEBUG) - + # file handler log_path = os.path.join(save_dir, "log.txt") file_handler = FileHandler(log_path) @@ -33,6 +34,7 @@ def make_logger(save_dir): # sh_handler = StreamHandler() # logger.addHandler(sh_handler) + def int_tuple(s): """ transform str to tuple Args: @@ -42,6 +44,7 @@ def int_tuple(s): """ return tuple(int(i) for i in s.split(',')) + def bool_flag(s): """ transform str to bool flg Args: @@ -54,6 +57,7 @@ def bool_flag(s): msg = 'Invalid value "%s" for bool flag (should be 0 or 1)' raise ValueError(msg % s) + def file_exists(path): """ Check file existence on given path Args: @@ -63,6 +67,7 @@ def file_exists(path): """ return os.path.exists(path) + def create_dir_if_not_exist(outdir): """ Check directory existence and creates new directory if not exist Args: @@ -77,6 +82,7 @@ def create_dir_if_not_exist(outdir): return os.makedirs(outdir) + def write_text_to_file(file_path, data): """ Write given text data to file Args: @@ -86,6 +92,7 @@ def write_text_to_file(file_path, data): with open(file_path, 'w') as f: f.write(data) + def read_text_from_file(file_path): """ Read given file as text Args: @@ -96,6 +103,7 @@ def read_text_from_file(file_path): with open(file_path, 'r') as f: return f.read() + def save_pickle(file_path, data): """ pickle given data to file Args: @@ -105,6 +113,7 @@ def save_pickle(file_path, data): with open(file_path, 'wb') as f: pickle.dump(data, f) + def load_pickle(file_path): """ load pickled data from file Args: @@ -118,6 +127,7 @@ def load_pickle(file_path): else: return pickle.load(f, encoding='bytes') + def prepare_output_dir(base_dir, args, time_format='%Y-%m-%d-%H%M%S'): """ prepare a directory with current datetime as name. created directory contains the command and args when the script was called as text file. @@ -144,4 +154,4 @@ def prepare_output_dir(base_dir, args, time_format='%Y-%m-%d-%H%M%S'): argv = ' '.join(sys.argv) write_text_to_file(argv_file_path, argv) - return outdir \ No newline at end of file + return outdir diff --git a/PythonLinearNonlinearControl/models/cartpole.py b/PythonLinearNonlinearControl/models/cartpole.py index db8ef61..cc9bfda 100644 --- a/PythonLinearNonlinearControl/models/cartpole.py +++ b/PythonLinearNonlinearControl/models/cartpole.py @@ -2,9 +2,11 @@ import numpy as np from .model import Model + class CartPoleModel(Model): """ cartpole model """ + def __init__(self, config): """ """ @@ -17,7 +19,7 @@ class CartPoleModel(Model): def predict_next_state(self, curr_x, u): """ predict next state - + Args: curr_x (numpy.ndarray): current state, shape(state_size, ) or shape(pop_size, state_size) @@ -31,59 +33,59 @@ class CartPoleModel(Model): # x d_x0 = curr_x[1] # v_x - d_x1 = (u[0] + self.mp * np.sin(curr_x[2]) \ - * (self.l * (curr_x[3]**2) \ - + self.g * np.cos(curr_x[2]))) \ - / (self.mc + self.mp * (np.sin(curr_x[2])**2)) + d_x1 = (u[0] + self.mp * np.sin(curr_x[2]) + * (self.l * (curr_x[3]**2) + + self.g * np.cos(curr_x[2]))) \ + / (self.mc + self.mp * (np.sin(curr_x[2])**2)) # theta d_x2 = curr_x[3] # v_theta - d_x3 = (-u[0] * np.cos(curr_x[2]) \ - - self.mp * self.l * (curr_x[3]**2) \ - * np.cos(curr_x[2]) * np.sin(curr_x[2]) \ + d_x3 = (-u[0] * np.cos(curr_x[2]) + - self.mp * self.l * (curr_x[3]**2) + * np.cos(curr_x[2]) * np.sin(curr_x[2]) - (self.mc + self.mp) * self.g * np.sin(curr_x[2])) \ - / (self.l * (self.mc + self.mp * (np.sin(curr_x[2])**2))) - + / (self.l * (self.mc + self.mp * (np.sin(curr_x[2])**2))) + next_x = curr_x +\ - np.array([d_x0, d_x1, d_x2, d_x3]) * self.dt - + np.array([d_x0, d_x1, d_x2, d_x3]) * self.dt + return next_x elif len(u.shape) == 2: # x d_x0 = curr_x[:, 1] # v_x - d_x1 = (u[:, 0] + self.mp * np.sin(curr_x[:, 2]) \ - * (self.l * (curr_x[:, 3]**2) \ - + self.g * np.cos(curr_x[:, 2]))) \ - / (self.mc + self.mp * (np.sin(curr_x[:, 2])**2)) + d_x1 = (u[:, 0] + self.mp * np.sin(curr_x[:, 2]) + * (self.l * (curr_x[:, 3]**2) + + self.g * np.cos(curr_x[:, 2]))) \ + / (self.mc + self.mp * (np.sin(curr_x[:, 2])**2)) # theta d_x2 = curr_x[:, 3] # v_theta - d_x3 = (-u[:, 0] * np.cos(curr_x[:, 2]) \ - - self.mp * self.l * (curr_x[:, 3]**2) \ - * np.cos(curr_x[:, 2]) * np.sin(curr_x[:, 2]) \ + d_x3 = (-u[:, 0] * np.cos(curr_x[:, 2]) + - self.mp * self.l * (curr_x[:, 3]**2) + * np.cos(curr_x[:, 2]) * np.sin(curr_x[:, 2]) - (self.mc + self.mp) * self.g * np.sin(curr_x[:, 2])) \ - / (self.l * (self.mc + self.mp * (np.sin(curr_x[:, 2])**2))) - + / (self.l * (self.mc + self.mp * (np.sin(curr_x[:, 2])**2))) + next_x = curr_x +\ - np.stack((d_x0, d_x1, d_x2, d_x3), axis=1) * self.dt + np.stack((d_x0, d_x1, d_x2, d_x3), axis=1) * self.dt return next_x - + def calc_f_x(self, xs, us, dt): """ gradient of model with respect to the state in batch form Args: xs (numpy.ndarray): state, shape(pred_len+1, state_size) us (numpy.ndarray): input, shape(pred_len, input_size,) - + Return: f_x (numpy.ndarray): gradient of model with respect to x, shape(pred_len, state_size, state_size) Notes: This should be discrete form !! - """ + """ # get size (_, state_size) = xs.shape (pred_len, _) = us.shape @@ -95,36 +97,36 @@ class CartPoleModel(Model): # f_theta tmp = ((self.mc + self.mp * np.sin(xs[:, 2])**2)**(-2)) \ - * self.mp * 2. * np.sin(xs[:, 2]) * np.cos(xs[:, 2]) + * self.mp * 2. * np.sin(xs[:, 2]) * np.cos(xs[:, 2]) tmp2 = 1. / (self.mc + self.mp * (np.sin(xs[:, 2])**2)) f_x[:, 1, 2] = - us[:, 0] * tmp \ - - tmp * (self.mp * np.sin(xs[:, 2]) \ - * (self.l * xs[:, 3]**2 \ + - tmp * (self.mp * np.sin(xs[:, 2]) + * (self.l * xs[:, 3]**2 + self.g * np.cos(xs[:, 2]))) \ - + tmp2 * (self.mp * np.cos(xs[:, 2]) * self.l \ - * xs[:, 3]**2 \ - + self.mp * self.g * (np.cos(xs[:, 2])**2 \ - - np.sin(xs[:, 2])**2)) + + tmp2 * (self.mp * np.cos(xs[:, 2]) * self.l + * xs[:, 3]**2 + + self.mp * self.g * (np.cos(xs[:, 2])**2 + - np.sin(xs[:, 2])**2)) f_x[:, 3, 2] = - 1. / self.l * tmp \ - * (-us[:, 0] * np.cos(xs[:, 2]) \ - - self.mp * self.l * (xs[:, 3]**2) \ - * np.cos(xs[:, 2]) * np.sin(xs[:, 2]) \ - - (self.mc + self.mp) * self.g * np.sin(xs[:, 2])) \ - + 1. / self.l * tmp2 \ - * (us[:, 0] * np.sin(xs[:, 2]) \ - - self.mp * self.l * xs[:, 3]**2 \ - * (np.cos(xs[:, 2])**2 - np.sin(xs[:, 2])**2) \ - - (self.mc + self.mp) \ - * self.g * np.cos(xs[:, 2])) + * (-us[:, 0] * np.cos(xs[:, 2]) + - self.mp * self.l * (xs[:, 3]**2) + * np.cos(xs[:, 2]) * np.sin(xs[:, 2]) + - (self.mc + self.mp) * self.g * np.sin(xs[:, 2])) \ + + 1. / self.l * tmp2 \ + * (us[:, 0] * np.sin(xs[:, 2]) + - self.mp * self.l * xs[:, 3]**2 + * (np.cos(xs[:, 2])**2 - np.sin(xs[:, 2])**2) + - (self.mc + self.mp) + * self.g * np.cos(xs[:, 2])) # f_theta_dot - f_x[:, 1, 3] = tmp2 * (self.mp * np.sin(xs[:, 2]) \ - * self.l * 2 * xs[:, 3]) + f_x[:, 1, 3] = tmp2 * (self.mp * np.sin(xs[:, 2]) + * self.l * 2 * xs[:, 3]) f_x[:, 2, 3] = np.ones(pred_len) f_x[:, 3, 3] = 1. / self.l * tmp2 \ - * (-2. * self.mp * self.l * xs[:, 3] \ - * np.cos(xs[:, 2]) * np.sin(xs[:, 2])) + * (-2. * self.mp * self.l * xs[:, 3] + * np.cos(xs[:, 2]) * np.sin(xs[:, 2])) return f_x * dt + np.eye(state_size) # to discrete form @@ -133,25 +135,25 @@ class CartPoleModel(Model): Args: xs (numpy.ndarray): state, shape(pred_len+1, state_size) us (numpy.ndarray): input, shape(pred_len, input_size,) - + Return: f_u (numpy.ndarray): gradient of model with respect to x, shape(pred_len, state_size, input_size) Notes: This should be discrete form !! - """ + """ # get size (_, state_size) = xs.shape (pred_len, input_size) = us.shape f_u = np.zeros((pred_len, state_size, input_size)) - + f_u[:, 1, 0] = 1. / (self.mc + self.mp * (np.sin(xs[:, 2])**2)) - + f_u[:, 3, 0] = -np.cos(xs[:, 2]) \ - / (self.l * (self.mc \ - + self.mp * (np.sin(xs[:, 2])**2))) + / (self.l * (self.mc + + self.mp * (np.sin(xs[:, 2])**2))) return f_u * dt # to discrete form @@ -161,7 +163,7 @@ class CartPoleModel(Model): Args: xs (numpy.ndarray): state, shape(pred_len+1, state_size) us (numpy.ndarray): input, shape(pred_len, input_size,) - + Return: f_xx (numpy.ndarray): gradient of model with respect to x, shape(pred_len, state_size, state_size, state_size) @@ -180,7 +182,7 @@ class CartPoleModel(Model): Args: xs (numpy.ndarray): state, shape(pred_len+1, state_size) us (numpy.ndarray): input, shape(pred_len, input_size,) - + Return: f_ux (numpy.ndarray): gradient of model with respect to x, shape(pred_len, state_size, input_size, state_size) @@ -199,7 +201,7 @@ class CartPoleModel(Model): Args: xs (numpy.ndarray): state, shape(pred_len+1, state_size) us (numpy.ndarray): input, shape(pred_len, input_size,) - + Return: f_uu (numpy.ndarray): gradient of model with respect to x, shape(pred_len, state_size, input_size, input_size) @@ -210,4 +212,4 @@ class CartPoleModel(Model): f_uu = np.zeros((pred_len, state_size, input_size, input_size)) - raise NotImplementedError \ No newline at end of file + raise NotImplementedError diff --git a/PythonLinearNonlinearControl/models/first_order_lag.py b/PythonLinearNonlinearControl/models/first_order_lag.py index a4a97fb..f3ccec5 100644 --- a/PythonLinearNonlinearControl/models/first_order_lag.py +++ b/PythonLinearNonlinearControl/models/first_order_lag.py @@ -3,6 +3,7 @@ import scipy.linalg from scipy import integrate from .model import LinearModel + class FirstOrderLagModel(LinearModel): """ first order lag model Attributes: @@ -10,13 +11,15 @@ class FirstOrderLagModel(LinearModel): u (numpy.ndarray): history_pred_xs (numpy.ndarray): """ + def __init__(self, config, tau=0.63): """ Args: tau (float): time constant - """ + """ # param - self.A, self.B = self._to_state_space(tau, dt=config.DT) # discrete system + self.A, self.B = self._to_state_space( + tau, dt=config.DT) # discrete system super(FirstOrderLagModel, self).__init__(self.A, self.B) @staticmethod @@ -31,21 +34,22 @@ class FirstOrderLagModel(LinearModel): """ # continuous Ac = np.array([[-1./tau, 0., 0., 0.], - [0., -1./tau, 0., 0.], - [1., 0., 0., 0.], - [0., 1., 0., 0.]]) + [0., -1./tau, 0., 0.], + [1., 0., 0., 0.], + [0., 1., 0., 0.]]) Bc = np.array([[1./tau, 0.], - [0., 1./tau], - [0., 0.], - [0., 0.]]) + [0., 1./tau], + [0., 0.], + [0., 0.]]) # to discrete system A = scipy.linalg.expm(dt*Ac) # B = np.matmul(np.matmul(scipy.linalg.expm(Ac*dt)-scipy.linalg.expm(Ac*0.), np.linalg.inv(Ac)), Bc) B = np.zeros_like(Bc) for m in range(Bc.shape[0]): for n in range(Bc.shape[1]): - integrate_fn = lambda tau: np.matmul(scipy.linalg.expm(Ac*tau), Bc)[m, n] + def integrate_fn(tau): return np.matmul( + scipy.linalg.expm(Ac*tau), Bc)[m, n] sol = integrate.quad(integrate_fn, 0, dt) B[m, n] = sol[0] - return A, B \ No newline at end of file + return A, B diff --git a/PythonLinearNonlinearControl/models/make_models.py b/PythonLinearNonlinearControl/models/make_models.py index fb628ad..50f11f0 100644 --- a/PythonLinearNonlinearControl/models/make_models.py +++ b/PythonLinearNonlinearControl/models/make_models.py @@ -1,14 +1,18 @@ from .first_order_lag import FirstOrderLagModel from .two_wheeled import TwoWheeledModel from .cartpole import CartPoleModel +from .nonlinear_sample_system import NonlinearSampleSystemModel + def make_model(args, config): - + if args.env == "FirstOrderLag": return FirstOrderLagModel(config) elif args.env == "TwoWheeledConst" or args.env == "TwoWheeledTrack": return TwoWheeledModel(config) elif args.env == "CartPole": return CartPoleModel(config) - - raise NotImplementedError("There is not {} Model".format(args.env)) \ No newline at end of file + elif args.env == "NonlinearSample": + return NonlinearSampleSystemModel(config) + + raise NotImplementedError("There is not {} Model".format(args.env)) diff --git a/PythonLinearNonlinearControl/models/model.py b/PythonLinearNonlinearControl/models/model.py index 5eb2cb7..7e43234 100644 --- a/PythonLinearNonlinearControl/models/model.py +++ b/PythonLinearNonlinearControl/models/model.py @@ -1,8 +1,10 @@ import numpy as np + class Model(): """ base class of model """ + def __init__(self): """ """ @@ -22,17 +24,17 @@ class Model(): or shape(pop_size, pred_len+1, state_size) """ if len(us.shape) == 3: - pred_xs =self._predict_traj_alltogether(curr_x, us) + pred_xs = self._predict_traj_alltogether(curr_x, us) elif len(us.shape) == 2: pred_xs = self._predict_traj(curr_x, us) else: raise ValueError("Invalid us") - + return pred_xs - + def _predict_traj(self, curr_x, us): """ predict trajectories - + Args: curr_x (numpy.ndarray): current state, shape(state_size, ) us (numpy.ndarray): inputs, shape(pred_len, input_size) @@ -53,10 +55,10 @@ class Model(): x = next_x return pred_xs - + def _predict_traj_alltogether(self, curr_x, us): """ predict trajectories for all samples - + Args: curr_x (numpy.ndarray): current state, shape(pop_size, state_size) us (numpy.ndarray): inputs, shape(pop_size, pred_len, input_size) @@ -75,12 +77,12 @@ class Model(): # next_x.shape = (pop_size, state_size) next_x = self.predict_next_state(x, us[t]) # update - pred_xs = np.concatenate((pred_xs, next_x[np.newaxis, :, :]),\ - axis=0) + pred_xs = np.concatenate((pred_xs, next_x[np.newaxis, :, :]), + axis=0) x = next_x return np.transpose(pred_xs, (1, 0, 2)) - + def predict_next_state(self, curr_x, u): """ predict next state """ @@ -99,23 +101,23 @@ class Model(): # get size (pred_len, input_size) = us.shape # pred final adjoint state - lam = self.predict_terminal_adjoint_state(xs[-1],\ + lam = self.predict_terminal_adjoint_state(xs[-1], terminal_g_x=g_xs[-1]) lams = lam[np.newaxis, :] - for t in range(pred_len-1, 0, -1): + for t in range(pred_len-1, 0, -1): prev_lam = \ - self.predict_adjoint_state(lam, xs[t], us[t],\ + self.predict_adjoint_state(lam, xs[t], us[t], goal=g_xs[t], t=t) # update lams = np.concatenate((prev_lam[np.newaxis, :], lams), axis=0) lam = prev_lam - + return lams def predict_adjoint_state(self, lam, x, u, goal=None, t=None): """ predict adjoint states - + Args: lam (numpy.ndarray): adjoint state, shape(state_size, ) x (numpy.ndarray): state, shape(state_size, ) @@ -129,7 +131,7 @@ class Model(): def predict_terminal_adjoint_state(self, terminal_x, terminal_g_x=None): """ predict terminal adjoint state - + Args: terminal_x (numpy.ndarray): terminal state, shape(state_size, ) terminal_g_x (numpy.ndarray): terminal goal state, @@ -143,7 +145,7 @@ class Model(): @staticmethod def calc_f_x(xs, us, dt): """ gradient of model with respect to the state in batch form - """ + """ raise NotImplementedError("Implement gradient of model \ with respect to the state") @@ -153,11 +155,11 @@ class Model(): """ raise NotImplementedError("Implement gradient of model \ with respect to the input") - + @staticmethod def calc_f_xx(xs, us, dt): """ hessian of model with respect to the state in batch form - """ + """ raise NotImplementedError("Implement hessian of model \ with respect to the state") @@ -171,27 +173,29 @@ class Model(): @staticmethod def calc_f_uu(xs, us, dt): """ hessian of model with respect to the state in batch form - """ + """ raise NotImplementedError("Implement hessian of model \ with respect to the input") + class LinearModel(Model): """ discrete linear model, x[k+1] = Ax[k] + Bu[k] - + Attributes: A (numpy.ndarray): shape(state_size, state_size) B (numpy.ndarray): shape(state_size, input_size) """ + def __init__(self, A, B): """ """ super(LinearModel, self).__init__() self.A = A self.B = B - + def predict_next_state(self, curr_x, u): """ predict next state - + Args: curr_x (numpy.ndarray): current state, shape(state_size, ) or shape(pop_size, state_size) @@ -203,7 +207,7 @@ class LinearModel(Model): """ if len(u.shape) == 1: next_x = np.matmul(self.A, curr_x[:, np.newaxis]) \ - + np.matmul(self.B, u[:, np.newaxis]) + + np.matmul(self.B, u[:, np.newaxis]) return next_x.flatten() @@ -211,7 +215,7 @@ class LinearModel(Model): next_x = np.matmul(curr_x, self.A.T) + np.matmul(u, self.B.T) return next_x - + def calc_f_x(self, xs, us, dt): """ gradient of model with respect to the state in batch form @@ -223,7 +227,7 @@ class LinearModel(Model): shape(pred_len, state_size, state_size) Notes: This should be discrete form !! - """ + """ # get size (pred_len, _) = us.shape @@ -240,7 +244,7 @@ class LinearModel(Model): shape(pred_len, state_size, input_size) Notes: This should be discrete form !! - """ + """ # get size (pred_len, input_size) = us.shape @@ -283,7 +287,7 @@ class LinearModel(Model): f_ux = np.zeros((pred_len, state_size, input_size, state_size)) return f_ux - + @staticmethod def calc_f_uu(xs, us, dt): """ hessian of model with respect to input in batch form @@ -301,4 +305,4 @@ class LinearModel(Model): f_uu = np.zeros((pred_len, state_size, input_size, input_size)) - return f_uu + return f_uu diff --git a/PythonLinearNonlinearControl/models/nonlinear_sample_system.py b/PythonLinearNonlinearControl/models/nonlinear_sample_system.py new file mode 100644 index 0000000..490d2ba --- /dev/null +++ b/PythonLinearNonlinearControl/models/nonlinear_sample_system.py @@ -0,0 +1,164 @@ +import numpy as np + +from .model import Model +from ..common.utils import update_state_with_Runge_Kutta + + +class NonlinearSampleSystemModel(Model): + """ nonlinear sample system model + """ + + def __init__(self, config): + """ + """ + super(NonlinearSampleSystemModel, self).__init__() + self.dt = config.DT + + def predict_next_state(self, curr_x, u): + """ predict next state + + Args: + curr_x (numpy.ndarray): current state, shape(state_size, ) or + shape(pop_size, state_size) + u (numpy.ndarray): input, shape(input_size, ) or + shape(pop_size, input_size) + Returns: + next_x (numpy.ndarray): next state, shape(state_size, ) or + shape(pop_size, state_size) + """ + if len(u.shape) == 1: + func_1 = self._func_x_1 + func_2 = self._func_x_2 + functions = [func_1, func_2] + next_x = update_state_with_Runge_Kutta( + curr_x, u, functions, batch=False) + return next_x + + elif len(u.shape) == 2: + def func_1(xs, us): return self._func_x_1(xs, us, batch=True) + def func_2(xs, us): return self._func_x_2(xs, us, batch=True) + functions = [func_1, func_2] + next_x = update_state_with_Runge_Kutta( + curr_x, u, functions, batch=True) + + return next_x + + def _func_x_1(self, x, u, batch=False): + if not batch: + x_dot = x[1] + else: + x_dot = x[:, 1] + return x_dot + + def _func_x_2(self, x, u, batch=False): + if not batch: + x_dot = (1. - x[0]**2 - x[1]**2) * x[1] - x[0] + u[0] + else: + x_dot = (1. - x[:, 0]**2 - x[:, 1]**2) * \ + x[:, 1] - x[:, 0] + u[:, 0] + return x_dot + + def calc_f_x(self, xs, us, dt): + """ gradient of model with respect to the state in batch form + Args: + xs (numpy.ndarray): state, shape(pred_len+1, state_size) + us (numpy.ndarray): input, shape(pred_len, input_size,) + + Return: + f_x (numpy.ndarray): gradient of model with respect to x, + shape(pred_len, state_size, state_size) + + Notes: + This should be discrete form !! + """ + # get size + (_, state_size) = xs.shape + (pred_len, _) = us.shape + + f_x = np.zeros((pred_len, state_size, state_size)) + f_x[:, 0, 1] = 1. + f_x[:, 1, 0] = 2. * xs[:, 0] * xs[:, 1] - 1. + f_x[:, 1, 1] = - 2. * xs[:, 1] * xs[:, 1] + \ + (1. - xs[:, 0]**2 - xs[:, 1]**2) + + return f_x * dt + np.eye(state_size) # to discrete form + + def calc_f_u(self, xs, us, dt): + """ gradient of model with respect to the input in batch form + Args: + xs (numpy.ndarray): state, shape(pred_len+1, state_size) + us (numpy.ndarray): input, shape(pred_len, input_size,) + + Return: + f_u (numpy.ndarray): gradient of model with respect to x, + shape(pred_len, state_size, input_size) + + Notes: + This should be discrete form !! + """ + # get size + (_, state_size) = xs.shape + (pred_len, input_size) = us.shape + + f_u = np.zeros((pred_len, state_size, input_size)) + + f_u[:, 1, 0] = 1. + + return f_u * dt # to discrete form + + def calc_f_xx(self, xs, us, dt): + """ hessian of model with respect to the state in batch form + + Args: + xs (numpy.ndarray): state, shape(pred_len+1, state_size) + us (numpy.ndarray): input, shape(pred_len, input_size,) + + Return: + f_xx (numpy.ndarray): gradient of model with respect to x, + shape(pred_len, state_size, state_size, state_size) + """ + # get size + (_, state_size) = xs.shape + (pred_len, _) = us.shape + + f_xx = np.zeros((pred_len, state_size, state_size, state_size)) + + raise NotImplementedError + + def calc_f_ux(self, xs, us, dt): + """ hessian of model with respect to state and input in batch form + + Args: + xs (numpy.ndarray): state, shape(pred_len+1, state_size) + us (numpy.ndarray): input, shape(pred_len, input_size,) + + Return: + f_ux (numpy.ndarray): gradient of model with respect to x, + shape(pred_len, state_size, input_size, state_size) + """ + # get size + (_, state_size) = xs.shape + (pred_len, input_size) = us.shape + + f_ux = np.zeros((pred_len, state_size, input_size, state_size)) + + raise NotImplementedError + + def calc_f_uu(self, xs, us, dt): + """ hessian of model with respect to input in batch form + + Args: + xs (numpy.ndarray): state, shape(pred_len+1, state_size) + us (numpy.ndarray): input, shape(pred_len, input_size,) + + Return: + f_uu (numpy.ndarray): gradient of model with respect to x, + shape(pred_len, state_size, input_size, input_size) + """ + # get size + (_, state_size) = xs.shape + (pred_len, input_size) = us.shape + + f_uu = np.zeros((pred_len, state_size, input_size, input_size)) + + raise NotImplementedError diff --git a/PythonLinearNonlinearControl/models/two_wheeled.py b/PythonLinearNonlinearControl/models/two_wheeled.py index 3bd60d7..3a81ff5 100644 --- a/PythonLinearNonlinearControl/models/two_wheeled.py +++ b/PythonLinearNonlinearControl/models/two_wheeled.py @@ -2,9 +2,11 @@ import numpy as np from .model import Model + class TwoWheeledModel(Model): """ two wheeled model """ + def __init__(self, config): """ """ @@ -13,7 +15,7 @@ class TwoWheeledModel(Model): def predict_next_state(self, curr_x, u): """ predict next state - + Args: curr_x (numpy.ndarray): current state, shape(state_size, ) or shape(pop_size, state_size) @@ -50,21 +52,21 @@ class TwoWheeledModel(Model): next_x = x_dot[:, :, 0] * self.dt + curr_x return next_x - + @staticmethod def calc_f_x(xs, us, dt): """ gradient of model with respect to the state in batch form Args: xs (numpy.ndarray): state, shape(pred_len+1, state_size) us (numpy.ndarray): input, shape(pred_len, input_size,) - + Return: f_x (numpy.ndarray): gradient of model with respect to x, shape(pred_len, state_size, state_size) Notes: This should be discrete form !! - """ + """ # get size (_, state_size) = xs.shape (pred_len, _) = us.shape @@ -81,14 +83,14 @@ class TwoWheeledModel(Model): Args: xs (numpy.ndarray): state, shape(pred_len+1, state_size) us (numpy.ndarray): input, shape(pred_len, input_size,) - + Return: f_u (numpy.ndarray): gradient of model with respect to x, shape(pred_len, state_size, input_size) Notes: This should be discrete form !! - """ + """ # get size (_, state_size) = xs.shape (pred_len, input_size) = us.shape @@ -107,7 +109,7 @@ class TwoWheeledModel(Model): Args: xs (numpy.ndarray): state, shape(pred_len+1, state_size) us (numpy.ndarray): input, shape(pred_len, input_size,) - + Return: f_xx (numpy.ndarray): gradient of model with respect to x, shape(pred_len, state_size, state_size, state_size) @@ -130,7 +132,7 @@ class TwoWheeledModel(Model): Args: xs (numpy.ndarray): state, shape(pred_len+1, state_size) us (numpy.ndarray): input, shape(pred_len, input_size,) - + Return: f_ux (numpy.ndarray): gradient of model with respect to x, shape(pred_len, state_size, input_size, state_size) @@ -145,7 +147,7 @@ class TwoWheeledModel(Model): f_ux[:, 1, 0, 2] = np.cos(xs[:, 2]) return f_ux * dt - + @staticmethod def calc_f_uu(xs, us, dt): """ hessian of model with respect to input in batch form @@ -153,7 +155,7 @@ class TwoWheeledModel(Model): Args: xs (numpy.ndarray): state, shape(pred_len+1, state_size) us (numpy.ndarray): input, shape(pred_len, input_size,) - + Return: f_uu (numpy.ndarray): gradient of model with respect to x, shape(pred_len, state_size, input_size, input_size) @@ -164,4 +166,4 @@ class TwoWheeledModel(Model): f_uu = np.zeros((pred_len, state_size, input_size, input_size)) - return f_uu * dt \ No newline at end of file + return f_uu * dt diff --git a/PythonLinearNonlinearControl/planners/closest_point_planner.py b/PythonLinearNonlinearControl/planners/closest_point_planner.py index 291aff3..cf24a97 100644 --- a/PythonLinearNonlinearControl/planners/closest_point_planner.py +++ b/PythonLinearNonlinearControl/planners/closest_point_planner.py @@ -1,9 +1,11 @@ import numpy as np from .planner import Planner + class ClosestPointPlanner(Planner): """ This planner make goal state according to goal path """ + def __init__(self, config): """ """ @@ -24,7 +26,7 @@ class ClosestPointPlanner(Planner): min_idx = np.argmin(np.linalg.norm(curr_x[:-1] - g_traj[:, :-1], axis=1)) - start = (min_idx+self.n_ahead) + start = (min_idx+self.n_ahead) if start > len(g_traj): start = len(g_traj) @@ -32,8 +34,8 @@ class ClosestPointPlanner(Planner): 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] \ No newline at end of file + return g_traj[start:end] diff --git a/PythonLinearNonlinearControl/planners/const_planner.py b/PythonLinearNonlinearControl/planners/const_planner.py index a0569a2..caa8110 100644 --- a/PythonLinearNonlinearControl/planners/const_planner.py +++ b/PythonLinearNonlinearControl/planners/const_planner.py @@ -1,9 +1,11 @@ import numpy as np from .planner import Planner + class ConstantPlanner(Planner): """ This planner make constant goal state """ + def __init__(self, config): """ """ @@ -20,4 +22,4 @@ class ConstantPlanner(Planner): Returns: g_xs (numpy.ndarrya): goal state, shape(pred_len, state_size) """ - return np.tile(g_x, (self.pred_len+1, 1)) \ No newline at end of file + return np.tile(g_x, (self.pred_len+1, 1)) diff --git a/PythonLinearNonlinearControl/planners/make_planners.py b/PythonLinearNonlinearControl/planners/make_planners.py index 9f18e51..eb77082 100644 --- a/PythonLinearNonlinearControl/planners/make_planners.py +++ b/PythonLinearNonlinearControl/planners/make_planners.py @@ -1,8 +1,9 @@ from .const_planner import ConstantPlanner from .closest_point_planner import ClosestPointPlanner + def make_planner(args, config): - + if args.env == "FirstOrderLag": return ConstantPlanner(config) elif args.env == "TwoWheeledConst": @@ -11,5 +12,8 @@ def make_planner(args, config): return ClosestPointPlanner(config) elif args.env == "CartPole": return ConstantPlanner(config) - - raise NotImplementedError("There is not {} Planner".format(args.planner_type)) \ No newline at end of file + elif args.env == "NonlinearSample": + return ConstantPlanner(config) + + raise NotImplementedError( + "There is not {} Planner".format(args.planner_type)) diff --git a/PythonLinearNonlinearControl/planners/planner.py b/PythonLinearNonlinearControl/planners/planner.py index 7e20b4f..c524ea7 100644 --- a/PythonLinearNonlinearControl/planners/planner.py +++ b/PythonLinearNonlinearControl/planners/planner.py @@ -1,8 +1,10 @@ import numpy as np + class Planner(): """ """ + def __init__(self): """ """ @@ -15,4 +17,4 @@ class Planner(): Returns: g_xs (numpy.ndarrya): goal state, shape(pred_len, state_size) """ - raise NotImplementedError("Implement plan func") \ No newline at end of file + raise NotImplementedError("Implement plan func") diff --git a/PythonLinearNonlinearControl/plotters/animator.py b/PythonLinearNonlinearControl/plotters/animator.py index 2260707..49a9d05 100644 --- a/PythonLinearNonlinearControl/plotters/animator.py +++ b/PythonLinearNonlinearControl/plotters/animator.py @@ -8,9 +8,11 @@ import matplotlib.animation as animation logger = getLogger(__name__) + class Animator(): """ animation class """ + def __init__(self, env, args=None): """ """ @@ -34,7 +36,7 @@ class Animator(): # make fig self.anim_fig = plt.figure() - # axis + # axis self.axis = self.anim_fig.add_subplot(111) self.axis.set_aspect('equal', adjustable='box') @@ -65,12 +67,12 @@ class Animator(): """ # set up animation figures self._setup() - _update_img = lambda i: self._update_img(i, history_x, history_g_x) + def _update_img(i): return self._update_img(i, history_x, history_g_x) # Set up formatting for the movie files Writer = animation.writers['ffmpeg'] writer = Writer(fps=15, metadata=dict(artist='Me'), bitrate=1800) - + # call funcanimation ani = FuncAnimation( self.anim_fig, @@ -79,6 +81,6 @@ class Animator(): # save animation path = os.path.join(self.result_dir, self.controller_type, "animation-" + self.env_name + ".mp4") - logger.info("Saved Animation to {} ...".format(path)) + logger.info("Saved Animation to {} ...".format(path)) ani.save(path, writer=writer) diff --git a/PythonLinearNonlinearControl/plotters/plot_func.py b/PythonLinearNonlinearControl/plotters/plot_func.py index 3d2a7de..d25e1c5 100644 --- a/PythonLinearNonlinearControl/plotters/plot_func.py +++ b/PythonLinearNonlinearControl/plotters/plot_func.py @@ -5,6 +5,7 @@ import matplotlib.pyplot as plt from ..helper import save_pickle, load_pickle + def plot_result(history, history_g=None, ylabel="x", save_dir="./result", name="state_history"): """ @@ -28,14 +29,14 @@ def plot_result(history, history_g=None, ylabel="x", def plot(axis, history, history_g=None): axis.plot(range(iters), history, c="r", linewidth=3) if history_g is not None: - axis.plot(range(iters), history_g,\ + axis.plot(range(iters), history_g, c="b", linewidth=3, label="goal") if i < size: plot(axis1, history[:, i], history_g=history_g[:, i]) if i+1 < size: plot(axis2, history[:, i+1], history_g=history_g[:, i+1]) - if i+2 < size: + if i+2 < size: plot(axis3, history[:, i+2], history_g=history_g[:, i+2]) # save @@ -47,6 +48,7 @@ def plot_result(history, history_g=None, ylabel="x", axis1.legend(ncol=1, bbox_to_anchor=(0., 1.02, 1., 0.102), loc=3) figure.savefig(path, bbox_inches="tight", pad_inches=0.05) + def plot_results(history_x, history_u, history_g=None, args=None): """ @@ -64,12 +66,13 @@ def plot_results(history_x, history_u, history_g=None, args=None): controller_type = args.controller_type plot_result(history_x, history_g=history_g, ylabel="x", - name= env + "-state_history", + name=env + "-state_history", save_dir="./result/" + controller_type) plot_result(history_u, history_g=np.zeros_like(history_u), ylabel="u", - name= env + "-input_history", + name=env + "-input_history", save_dir="./result/" + controller_type) + def save_plot_data(history_x, history_u, history_g=None, args=None): """ save plot data @@ -98,6 +101,7 @@ def save_plot_data(history_x, history_u, history_g=None, args=None): env + "-history_g.pkl") save_pickle(path, history_g) + def load_plot_data(env, controller_type, result_dir="./result"): """ Args: @@ -123,6 +127,7 @@ def load_plot_data(env, controller_type, result_dir="./result"): return history_x, history_u, history_g + def plot_multi_result(histories, histories_g=None, labels=None, ylabel="x", save_dir="./result", name="state_history"): """ @@ -130,7 +135,7 @@ def plot_multi_result(histories, histories_g=None, labels=None, ylabel="x", history (numpy.ndarray): history, shape(iters, size) """ (_, iters, size) = histories.shape - + for i in range(0, size, 2): figure = plt.figure() @@ -146,17 +151,17 @@ def plot_multi_result(histories, histories_g=None, labels=None, ylabel="x", axis.plot(range(iters), history, linewidth=3, label=label, alpha=0.7, linestyle="dashed") if history_g is not None: - axis.plot(range(iters), history_g,\ + axis.plot(range(iters), history_g, c="b", linewidth=3) if i < size: for j, (history, history_g) \ - in enumerate(zip(histories, histories_g)): + in enumerate(zip(histories, histories_g)): plot(axis1, history[:, i], history_g=history_g[:, i], label=labels[j]) if i+1 < size: for j, (history, history_g) in \ - enumerate(zip(histories, histories_g)): + enumerate(zip(histories, histories_g)): plot(axis2, history[:, i+1], history_g=history_g[:, i+1], label=labels[j]) diff --git a/PythonLinearNonlinearControl/plotters/plot_objs.py b/PythonLinearNonlinearControl/plotters/plot_objs.py index 911cb5f..1e14bcb 100644 --- a/PythonLinearNonlinearControl/plotters/plot_objs.py +++ b/PythonLinearNonlinearControl/plotters/plot_objs.py @@ -5,9 +5,10 @@ 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 @@ -29,6 +30,7 @@ def circle(center_x, center_y, radius, start=0., end=2*np.pi, n_point=100): 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 @@ -50,6 +52,7 @@ def circle_with_angle(center_x, center_y, radius, angle): return circle_x, circle_y, angle_x, angle_y + def square(center_x, center_y, shape, angle): """ Create square @@ -74,9 +77,10 @@ def square(center_x, center_y, shape, angle): 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 @@ -96,4 +100,4 @@ def square_with_angle(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 \ No newline at end of file + return square_x, square_y, angle_x, angle_y diff --git a/PythonLinearNonlinearControl/runners/__init__.py b/PythonLinearNonlinearControl/runners/__init__.py index 8fd5521..0406fea 100644 --- a/PythonLinearNonlinearControl/runners/__init__.py +++ b/PythonLinearNonlinearControl/runners/__init__.py @@ -1,2 +1,2 @@ from PythonLinearNonlinearControl.runners.runner \ - import ExpRunner # NOQA \ No newline at end of file + import ExpRunner # NOQA diff --git a/PythonLinearNonlinearControl/runners/make_runners.py b/PythonLinearNonlinearControl/runners/make_runners.py index be08186..133ae06 100644 --- a/PythonLinearNonlinearControl/runners/make_runners.py +++ b/PythonLinearNonlinearControl/runners/make_runners.py @@ -1,4 +1,5 @@ from .runner import ExpRunner + def make_runner(args): - return ExpRunner() \ No newline at end of file + return ExpRunner() diff --git a/PythonLinearNonlinearControl/runners/runner.py b/PythonLinearNonlinearControl/runners/runner.py index 4ef0c6b..b83789d 100644 --- a/PythonLinearNonlinearControl/runners/runner.py +++ b/PythonLinearNonlinearControl/runners/runner.py @@ -4,9 +4,11 @@ import numpy as np logger = getLogger(__name__) + class ExpRunner(): """ experiment runner """ + def __init__(self): """ """ @@ -46,6 +48,6 @@ class ExpRunner(): score += cost step_count += 1 - logger.debug("Controller type = {}, Score = {}"\ + logger.debug("Controller type = {}, Score = {}" .format(controller, score)) - return np.array(history_x), np.array(history_u), np.array(history_g) \ No newline at end of file + return np.array(history_x), np.array(history_u), np.array(history_g) diff --git a/README.md b/README.md index d03d02b..8016bdc 100644 --- a/README.md +++ b/README.md @@ -71,6 +71,7 @@ There are 4 example environments, "FirstOrderLag", "TwoWheeledConst", "TwoWheele | Two wheeled System (Constant Goal) | x | ✓ | 3 | 2 | | Two wheeled System (Moving Goal) | x | ✓ | 3 | 2 | | Cartpole (Swing up) | x | ✓ | 4 | 1 | +| Nonlinear Sample System Env | x | ✓ | 2 | 1 | All states and inputs of environments are continuous. **It should be noted that the algorithms for linear model could be applied to nonlinear enviroments if you have linealized the model of nonlinear environments.** diff --git a/assets/nonlinear_sample_system.png b/assets/nonlinear_sample_system.png new file mode 100644 index 0000000..95e0eb6 Binary files /dev/null and b/assets/nonlinear_sample_system.png differ diff --git a/assets/nonlinear_sample_system_score.png b/assets/nonlinear_sample_system_score.png new file mode 100644 index 0000000..26e8310 Binary files /dev/null and b/assets/nonlinear_sample_system_score.png differ diff --git a/scripts/show_result.py b/scripts/show_result.py index e54b9dc..f1fd300 100644 --- a/scripts/show_result.py +++ b/scripts/show_result.py @@ -6,7 +6,8 @@ import numpy as np import matplotlib.pyplot as plt from PythonLinearNonlinearControl.plotters.plot_func import load_plot_data, \ - plot_multi_result + plot_multi_result + def run(args): @@ -17,7 +18,7 @@ def run(args): history_gs = None # load data - for controller in controllers: + for controller in controllers: history_x, history_u, history_g = \ load_plot_data(args.env, controller, result_dir=args.result_dir) @@ -27,19 +28,20 @@ def run(args): history_us = history_u[np.newaxis, :] history_gs = history_g[np.newaxis, :] continue - + history_xs = np.concatenate((history_xs, history_x[np.newaxis, :]), axis=0) history_us = np.concatenate((history_us, history_u[np.newaxis, :]), axis=0) history_gs = np.concatenate((history_gs, history_g[np.newaxis, :]), axis=0) - + plot_multi_result(history_xs, histories_g=history_gs, labels=controllers, ylabel="x") plot_multi_result(history_us, histories_g=np.zeros_like(history_us), - labels=controllers, ylabel="u", name="input_history") + labels=controllers, ylabel="u", name="input_history") + def main(): parser = argparse.ArgumentParser() @@ -51,5 +53,6 @@ def main(): run(args) + if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/scripts/simple_run.py b/scripts/simple_run.py index 2ef1fd8..d3dbffb 100644 --- a/scripts/simple_run.py +++ b/scripts/simple_run.py @@ -8,9 +8,10 @@ from PythonLinearNonlinearControl.models.make_models import make_model 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 + save_plot_data from PythonLinearNonlinearControl.plotters.animator import Animator + def run(args): # logger make_logger(args.result_dir) @@ -18,23 +19,23 @@ def run(args): # make envs env = make_env(args) - # make config + # make config config = make_config(args) # make planner planner = make_planner(args, config) - + # make model model = make_model(args, config) - + # make controller controller = make_controller(args, config, model) - + # make simulator runner = make_runner(args) # run experiment - history_x, history_u, history_g = runner.run(env, controller, planner) + history_x, history_u, history_g = runner.run(env, controller, planner) # plot results plot_results(history_x, history_u, history_g=history_g, args=args) @@ -44,17 +45,19 @@ def run(args): animator = Animator(env, args=args) animator.draw(history_x, history_g) + def main(): parser = argparse.ArgumentParser() - parser.add_argument("--controller_type", type=str, default="CEM") - parser.add_argument("--env", type=str, default="TwoWheeledTrack") - parser.add_argument("--save_anim", type=bool_flag, default=1) + parser.add_argument("--controller_type", type=str, default="DDP") + parser.add_argument("--env", type=str, default="NonlinearSample") + parser.add_argument("--save_anim", type=bool_flag, default=0) parser.add_argument("--result_dir", type=str, default="./result") args = parser.parse_args() run(args) + if __name__ == "__main__": - main() \ No newline at end of file + main()