From f49ed382a4e9771fdd8a33869d78d7c03343e802 Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Sat, 13 Feb 2021 17:11:33 +0900 Subject: [PATCH 1/5] Add runge kutta --- PythonLinearNonlinearControl/common/utils.py | 58 ++++++++++- .../configs/nonlinear_system_sample.py | 0 .../envs/nonlinear_sample_system.py | 98 +++++++++++++++++++ 3 files changed, 155 insertions(+), 1 deletion(-) create mode 100644 PythonLinearNonlinearControl/configs/nonlinear_system_sample.py create mode 100644 PythonLinearNonlinearControl/envs/nonlinear_sample_system.py diff --git a/PythonLinearNonlinearControl/common/utils.py b/PythonLinearNonlinearControl/common/utils.py index 8010737..3645507 100644 --- a/PythonLinearNonlinearControl/common/utils.py +++ b/PythonLinearNonlinearControl/common/utils.py @@ -41,4 +41,60 @@ 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): + """ 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 + + 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. + """ + 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) + + inputs = np.concatenate([state, u]) + + for i, func in enumerate(functions): + k0[i] = dt * func(*inputs) + + add_state = state + k0 / 2. + inputs = np.concatenate([add_state, u]) + + for i, func in enumerate(functions): + k1[i] = dt * func(*inputs) + + add_state = state + k1 / 2. + inputs = np.concatenate([add_state, u]) + + for i, func in enumerate(functions): + k2[i] = dt * func(*inputs) + + add_state = state + k2 + inputs = np.concatenate([add_state, u]) + + for i, func in enumerate(functions): + k3[i] = dt * func(*inputs) + + return (k0 + 2. * k1 + 2. * k2 + k3) / 6. \ No newline at end of file diff --git a/PythonLinearNonlinearControl/configs/nonlinear_system_sample.py b/PythonLinearNonlinearControl/configs/nonlinear_system_sample.py new file mode 100644 index 0000000..e69de29 diff --git a/PythonLinearNonlinearControl/envs/nonlinear_sample_system.py b/PythonLinearNonlinearControl/envs/nonlinear_sample_system.py new file mode 100644 index 0000000..9e66e6e --- /dev/null +++ b/PythonLinearNonlinearControl/envs/nonlinear_sample_system.py @@ -0,0 +1,98 @@ +import numpy as np +import scipy +from scipy import integrate +from .env import Env +from ..common.utils import update_state_with_Runge_Kutta + +class NonlinearSampleEnv(Env): + """ Nonlinear Sample Env + """ + def __init__(self): + """ + """ + self.config = {"state_size" : 2,\ + "input_size" : 1,\ + "dt" : 0.01,\ + "max_step" : 250,\ + "input_lower_bound": [-0.5],\ + "input_upper_bound": [0.5], + } + + super(NonlinearSampleEnv, 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"]) + + funtions = [self._func_x_1, self._func_x_2] + + next_x = update_state_with_Runge_Kutta(self._curr_x, u, + functions, self.config["dt"]) + + # 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_1, x_2, u): + """ + """ + x_dot = x_2 + return x_dot + + def _func_x_2(self, x_1, x_2, u): + """ + """ + x_dot = (1. - x_1**2 - x_2**2) * x_2 - x_1 + u + return x_dot + + def plot_func(self, to_plot, i=None, history_x=None, history_g_x=None): + """ + """ + raise ValueError("NonlinearSampleEnv does not have animation") \ No newline at end of file From d64a799edab7c23b18280991c6feb115186da726 Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Sat, 13 Feb 2021 17:18:42 +0900 Subject: [PATCH 2/5] Fix format --- PythonLinearNonlinearControl/common/utils.py | 25 +-- .../configs/cartpole.py | 109 +++++------ .../configs/first_order_lag.py | 63 +++---- .../configs/make_configs.py | 3 +- .../configs/two_wheeled.py | 71 ++++---- .../controllers/cem.py | 32 ++-- .../controllers/controller.py | 18 +- .../controllers/ddp.py | 48 ++--- .../controllers/ilqr.py | 44 ++--- .../controllers/make_controllers.py | 5 +- .../controllers/mpc.py | 38 ++-- .../controllers/mppi.py | 36 ++-- .../controllers/mppi_williams.py | 28 +-- .../controllers/random.py | 12 +- PythonLinearNonlinearControl/envs/__init__.py | 2 +- PythonLinearNonlinearControl/envs/cartpole.py | 92 +++++----- PythonLinearNonlinearControl/envs/cost.py | 10 +- PythonLinearNonlinearControl/envs/env.py | 10 +- .../envs/first_order_lag.py | 46 ++--- .../envs/make_envs.py | 5 +- .../envs/nonlinear_sample_system.py | 34 ++-- .../envs/two_wheeled.py | 169 +++++++++--------- PythonLinearNonlinearControl/helper.py | 14 +- .../models/cartpole.py | 114 ++++++------ .../models/first_order_lag.py | 24 +-- .../models/make_models.py | 7 +- PythonLinearNonlinearControl/models/model.py | 60 ++++--- .../models/two_wheeled.py | 24 +-- .../planners/closest_point_planner.py | 8 +- .../planners/const_planner.py | 4 +- .../planners/planner.py | 4 +- .../plotters/animator.py | 10 +- .../plotters/plot_func.py | 21 ++- .../plotters/plot_objs.py | 10 +- .../runners/__init__.py | 2 +- .../runners/make_runners.py | 3 +- .../runners/runner.py | 6 +- 37 files changed, 648 insertions(+), 563 deletions(-) diff --git a/PythonLinearNonlinearControl/common/utils.py b/PythonLinearNonlinearControl/common/utils.py index 3645507..27d67ce 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 @@ -43,6 +45,7 @@ def fit_angle_in_range(angles, min_angle=-np.pi, max_angle=np.pi): output = np.minimum(max_angle, np.maximum(min_angle, output)) return output.reshape(output_shape) + def update_state_with_Runge_Kutta(state, u, functions, dt=0.01): """ update state in Runge Kutta methods Args: @@ -52,23 +55,23 @@ def update_state_with_Runge_Kutta(state, u, functions, dt=0.01): each function will be called like func(*state, *u) We expect that this function returns differential of each state dt (float): float in seconds - + 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. """ 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) @@ -84,10 +87,10 @@ def update_state_with_Runge_Kutta(state, u, functions, dt=0.01): for i, func in enumerate(functions): k1[i] = dt * func(*inputs) - + add_state = state + k1 / 2. inputs = np.concatenate([add_state, u]) - + for i, func in enumerate(functions): k2[i] = dt * func(*inputs) @@ -95,6 +98,6 @@ def update_state_with_Runge_Kutta(state, u, functions, dt=0.01): inputs = np.concatenate([add_state, u]) for i, func in enumerate(functions): - k3[i] = dt * func(*inputs) + k3[i] = dt * func(*inputs) - return (k0 + 2. * k1 + 2. * k2 + k3) / 6. \ No newline at end of file + return (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..4bb0873 100644 --- a/PythonLinearNonlinearControl/configs/make_configs.py +++ b/PythonLinearNonlinearControl/configs/make_configs.py @@ -2,6 +2,7 @@ from .first_order_lag import FirstOrderLagConfigModule from .two_wheeled import TwoWheeledConfigModule from .cartpole import CartPoleConfigModule + def make_config(args): """ Returns: @@ -12,4 +13,4 @@ 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() 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..c35e11a 100644 --- a/PythonLinearNonlinearControl/envs/make_envs.py +++ b/PythonLinearNonlinearControl/envs/make_envs.py @@ -3,6 +3,7 @@ from .two_wheeled import TwoWheeledConstEnv from .two_wheeled import TwoWheeledTrackEnv from .cartpole import CartPoleEnv + def make_env(args): if args.env == "FirstOrderLag": @@ -13,5 +14,5 @@ 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 + + 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 index 9e66e6e..a17390b 100644 --- a/PythonLinearNonlinearControl/envs/nonlinear_sample_system.py +++ b/PythonLinearNonlinearControl/envs/nonlinear_sample_system.py @@ -4,22 +4,24 @@ from scipy import integrate from .env import Env from ..common.utils import update_state_with_Runge_Kutta + class NonlinearSampleEnv(Env): """ Nonlinear Sample Env """ + def __init__(self): """ """ - self.config = {"state_size" : 2,\ - "input_size" : 1,\ - "dt" : 0.01,\ - "max_step" : 250,\ - "input_lower_bound": [-0.5],\ + self.config = {"state_size": 2, + "input_size": 1, + "dt": 0.01, + "max_step": 250, + "input_lower_bound": [-0.5], "input_upper_bound": [0.5], } super(NonlinearSampleEnv, self).__init__(self.config) - + def reset(self, init_x=np.array([2., 0.])): """ reset state Returns: @@ -27,7 +29,7 @@ class NonlinearSampleEnv(Env): info (dict): information """ self.step_count = 0 - + self.curr_x = np.zeros(self.config["state_size"]) if init_x is not None: @@ -35,7 +37,7 @@ class NonlinearSampleEnv(Env): # goal self.g_x = np.array([0., 0.]) - + # clear memory self.history_x = [] self.history_g_x = [] @@ -60,7 +62,7 @@ class NonlinearSampleEnv(Env): funtions = [self._func_x_1, self._func_x_2] next_x = update_state_with_Runge_Kutta(self._curr_x, u, - functions, self.config["dt"]) + functions, self.config["dt"]) # cost cost = 0 @@ -70,29 +72,29 @@ class NonlinearSampleEnv(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 _func_x_1(self, x_1, x_2, u): """ """ x_dot = x_2 return x_dot - + def _func_x_2(self, x_1, x_2, u): """ """ x_dot = (1. - x_1**2 - x_2**2) * x_2 - x_1 + u return x_dot - + def plot_func(self, to_plot, i=None, history_x=None, history_g_x=None): """ """ - raise ValueError("NonlinearSampleEnv does not have animation") \ No newline at end of file + raise ValueError("NonlinearSampleEnv 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..cc42a45 100644 --- a/PythonLinearNonlinearControl/models/make_models.py +++ b/PythonLinearNonlinearControl/models/make_models.py @@ -2,13 +2,14 @@ from .first_order_lag import FirstOrderLagModel from .two_wheeled import TwoWheeledModel from .cartpole import CartPoleModel + 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 + + 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/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/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) From faa3b92a77ae8839870de47d91b2542bb739dd60 Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Sat, 13 Feb 2021 17:58:58 +0900 Subject: [PATCH 3/5] Add model of nonlinear sample system --- Environments.md | 14 +++- PythonLinearNonlinearControl/common/utils.py | 65 ++++++++++++------- .../envs/nonlinear_sample_system.py | 12 ++-- 3 files changed, 59 insertions(+), 32 deletions(-) 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 27d67ce..bcbde9f 100644 --- a/PythonLinearNonlinearControl/common/utils.py +++ b/PythonLinearNonlinearControl/common/utils.py @@ -46,15 +46,16 @@ def fit_angle_in_range(angles, min_angle=-np.pi, max_angle=np.pi): return output.reshape(output_shape) -def update_state_with_Runge_Kutta(state, u, functions, dt=0.01): +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) + 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 @@ -68,36 +69,50 @@ def update_state_with_Runge_Kutta(state, u, functions, dt=0.01): Note that the function return x_dot. """ - state_size = len(state) - assert state_size == len(functions), \ - "Invalid functions length, You need to give the state size functions" + 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) + k0 = np.zeros(state_size) + k1 = np.zeros(state_size) + k2 = np.zeros(state_size) + k3 = np.zeros(state_size) - inputs = np.concatenate([state, u]) + for i, func in enumerate(functions): + k0[i] = dt * func(state, u) - for i, func in enumerate(functions): - k0[i] = dt * func(*inputs) + for i, func in enumerate(functions): + k1[i] = dt * func(state + k0 / 2., u) - add_state = state + k0 / 2. - inputs = np.concatenate([add_state, u]) + for i, func in enumerate(functions): + k2[i] = dt * func(state + k1 / 2., u) - for i, func in enumerate(functions): - k1[i] = dt * func(*inputs) + for i, func in enumerate(functions): + k3[i] = dt * func(state + k2, u) - add_state = state + k1 / 2. - inputs = np.concatenate([add_state, u]) + return (k0 + 2. * k1 + 2. * k2 + k3) / 6. - for i, func in enumerate(functions): - k2[i] = dt * func(*inputs) + else: + batch_size, state_size = state.shape + assert state_size == len(functions), \ + "Invalid functions length, You need to give the state size functions" - add_state = state + k2 - inputs = np.concatenate([add_state, u]) + 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): - k3[i] = dt * func(*inputs) + for i, func in enumerate(functions): + k0[:, i] = dt * func(state, u) - return (k0 + 2. * k1 + 2. * k2 + k3) / 6. + 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 (k0 + 2. * k1 + 2. * k2 + k3) / 6. diff --git a/PythonLinearNonlinearControl/envs/nonlinear_sample_system.py b/PythonLinearNonlinearControl/envs/nonlinear_sample_system.py index a17390b..01f7cd5 100644 --- a/PythonLinearNonlinearControl/envs/nonlinear_sample_system.py +++ b/PythonLinearNonlinearControl/envs/nonlinear_sample_system.py @@ -59,9 +59,9 @@ class NonlinearSampleEnv(Env): self.config["input_lower_bound"], self.config["input_upper_bound"]) - funtions = [self._func_x_1, self._func_x_2] + functions = [self._func_x_1, self._func_x_2] - next_x = update_state_with_Runge_Kutta(self._curr_x, u, + next_x = update_state_with_Runge_Kutta(self.curr_x, u, functions, self.config["dt"]) # cost @@ -82,16 +82,16 @@ class NonlinearSampleEnv(Env): self.step_count > self.config["max_step"], \ {"goal_state": self.g_x} - def _func_x_1(self, x_1, x_2, u): + def _func_x_1(self, x, u): """ """ - x_dot = x_2 + x_dot = x[1] return x_dot - def _func_x_2(self, x_1, x_2, u): + def _func_x_2(self, x, u): """ """ - x_dot = (1. - x_1**2 - x_2**2) * x_2 - x_1 + u + x_dot = (1. - x[0]**2 - x[1]**2) * x[1] - x[0] + u return x_dot def plot_func(self, to_plot, i=None, history_x=None, history_g_x=None): From 8f4ba9a12b8669993ea2d3f175e970af6abf56d2 Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Sat, 13 Feb 2021 17:59:33 +0900 Subject: [PATCH 4/5] Update Readme --- README.md | 1 + 1 file changed, 1 insertion(+) 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.** From 8c28ff328abab5d57157df6dc515cf9f30c505f1 Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Sat, 13 Feb 2021 21:19:49 +0900 Subject: [PATCH 5/5] Add nonlinear sample system --- PythonLinearNonlinearControl/common/utils.py | 12 +- .../configs/make_configs.py | 3 + .../configs/nonlinear_sample_system.py | 219 ++++++++++++++++++ .../configs/nonlinear_system_sample.py | 0 .../envs/make_envs.py | 3 + .../envs/nonlinear_sample_system.py | 17 +- .../models/make_models.py | 3 + .../models/nonlinear_sample_system.py | 164 +++++++++++++ .../planners/make_planners.py | 10 +- assets/nonlinear_sample_system.png | Bin 0 -> 20599 bytes assets/nonlinear_sample_system_score.png | Bin 0 -> 29068 bytes scripts/show_result.py | 15 +- scripts/simple_run.py | 23 +- 13 files changed, 434 insertions(+), 35 deletions(-) create mode 100644 PythonLinearNonlinearControl/configs/nonlinear_sample_system.py delete mode 100644 PythonLinearNonlinearControl/configs/nonlinear_system_sample.py create mode 100644 PythonLinearNonlinearControl/models/nonlinear_sample_system.py create mode 100644 assets/nonlinear_sample_system.png create mode 100644 assets/nonlinear_sample_system_score.png diff --git a/PythonLinearNonlinearControl/common/utils.py b/PythonLinearNonlinearControl/common/utils.py index bcbde9f..e7f6d82 100644 --- a/PythonLinearNonlinearControl/common/utils.py +++ b/PythonLinearNonlinearControl/common/utils.py @@ -91,17 +91,17 @@ def update_state_with_Runge_Kutta(state, u, functions, dt=0.01, batch=True): for i, func in enumerate(functions): k3[i] = dt * func(state + k2, u) - return (k0 + 2. * k1 + 2. * k2 + k3) / 6. + 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) + 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) @@ -115,4 +115,4 @@ def update_state_with_Runge_Kutta(state, u, functions, dt=0.01, batch=True): for i, func in enumerate(functions): k3[:, i] = dt * func(state + k2, u) - return (k0 + 2. * k1 + 2. * k2 + k3) / 6. + return state + (k0 + 2. * k1 + 2. * k2 + k3) / 6. diff --git a/PythonLinearNonlinearControl/configs/make_configs.py b/PythonLinearNonlinearControl/configs/make_configs.py index 4bb0873..1267470 100644 --- a/PythonLinearNonlinearControl/configs/make_configs.py +++ b/PythonLinearNonlinearControl/configs/make_configs.py @@ -1,6 +1,7 @@ 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): @@ -14,3 +15,5 @@ def make_config(args): return TwoWheeledConfigModule() elif args.env == "CartPole": 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/nonlinear_system_sample.py b/PythonLinearNonlinearControl/configs/nonlinear_system_sample.py deleted file mode 100644 index e69de29..0000000 diff --git a/PythonLinearNonlinearControl/envs/make_envs.py b/PythonLinearNonlinearControl/envs/make_envs.py index c35e11a..5603b45 100644 --- a/PythonLinearNonlinearControl/envs/make_envs.py +++ b/PythonLinearNonlinearControl/envs/make_envs.py @@ -2,6 +2,7 @@ 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): @@ -14,5 +15,7 @@ def make_env(args): return TwoWheeledTrackEnv() elif args.env == "CartPole": return CartPoleEnv() + 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 index 01f7cd5..38a827f 100644 --- a/PythonLinearNonlinearControl/envs/nonlinear_sample_system.py +++ b/PythonLinearNonlinearControl/envs/nonlinear_sample_system.py @@ -5,7 +5,7 @@ from .env import Env from ..common.utils import update_state_with_Runge_Kutta -class NonlinearSampleEnv(Env): +class NonlinearSampleSystemEnv(Env): """ Nonlinear Sample Env """ @@ -15,12 +15,12 @@ class NonlinearSampleEnv(Env): self.config = {"state_size": 2, "input_size": 1, "dt": 0.01, - "max_step": 250, + "max_step": 2000, "input_lower_bound": [-0.5], "input_upper_bound": [0.5], } - super(NonlinearSampleEnv, self).__init__(self.config) + super(NonlinearSampleSystemEnv, self).__init__(self.config) def reset(self, init_x=np.array([2., 0.])): """ reset state @@ -62,7 +62,8 @@ class NonlinearSampleEnv(Env): functions = [self._func_x_1, self._func_x_2] next_x = update_state_with_Runge_Kutta(self.curr_x, u, - functions, self.config["dt"]) + functions, self.config["dt"], + batch=False) # cost cost = 0 @@ -83,18 +84,14 @@ class NonlinearSampleEnv(Env): {"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 + 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("NonlinearSampleEnv does not have animation") + raise ValueError("NonlinearSampleSystemEnv does not have animation") diff --git a/PythonLinearNonlinearControl/models/make_models.py b/PythonLinearNonlinearControl/models/make_models.py index cc42a45..50f11f0 100644 --- a/PythonLinearNonlinearControl/models/make_models.py +++ b/PythonLinearNonlinearControl/models/make_models.py @@ -1,6 +1,7 @@ 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): @@ -11,5 +12,7 @@ def make_model(args, config): return TwoWheeledModel(config) elif args.env == "CartPole": return CartPoleModel(config) + elif args.env == "NonlinearSample": + return NonlinearSampleSystemModel(config) raise NotImplementedError("There is not {} Model".format(args.env)) 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/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/assets/nonlinear_sample_system.png b/assets/nonlinear_sample_system.png new file mode 100644 index 0000000000000000000000000000000000000000..95e0eb66d15d088580cb52413c8e074cba1bb563 GIT binary patch literal 20599 zcmdqJQ*>le_b-}sY&)HF+_7!jwylnByJK4&+cqne zgn!-RIE!jHE8CknyBRu~f|%Rc+nUlk89SPq+BsR+J70tK@PUB*0Fe|CRB_L`*fe!l z5y$Gi`P}?TB=C*2ZToViwnaQ@_&yP(ue`i{?8DiO0ht>|1}YbVCjE`Kv%@)DgSrQMTr^8|2(VT z>i<8}H`n28|%hBZ%e5~5b^U)q*4Iewy_kWg1z6ml4;%yRLW?gA?{ zHUgYc&-{MZ_F&y;My2a8i5|NwOK3ZwbDOc$dUpKVpc}jCrH8_+QJ-MZa|>;%d)<+H zIULgxk#G^3r64&K=(uXV-TPClSuP&3Rrj(HPwKtUQ%?5j>_g{TPOUI=7x%!~aoJbw z6QXV%UboQs{MnZsivvl*uIYk*Y^kKbSp7bg#rGZHMeJ4oI=-2R9HDywmHj#MOGZBn zx43{Hy}p4`R1ELDsXv3|jmCY^+TU11h1o8DgRec4Ij)dBhhi(pOd4}~4SfQA5lVu@ zbWFx!iS!@)MF2$BFt{uWjG_+FOk0O1vC7Y4_B#5|@x7XiC&81VZ}mn#P+65PKW;{M zbb{h5tVBGbm5|ajJFq`oc_Iqcd_a%CpckYMsS8g32{v}vbYH2#k)6IYXsCbVygQolStpNEXy)*r$NB#%3d(wd z$&4k#L5UbZXT`GSb0ieL$y#YaG3spkKXNZL$h7`!=kG#Vcxz#wLWTzj^of(uu4E1B zv5&U)W|#hts_-#MvYdbe_^@cZKa(+UHj)3MTF>ERWA|GB)iao&{{#J#{2LOSbVCHy zb=+_uPguQmKXj20b?c%%U#IMITjJdS5eeHlnW4#FXY*!B0Lh&|XYOFdW-Ff=8sZQ? zqXkWb>aTG1U9YcmyV>D(oHhqnIexim_r-5Lh8UK-*VC*_JL+$Z z7(j@YyG~D3GjB$8%C?=r_eUO!!0&t`lv>k;fFo!N`Q1o1df&Ysn=Vg}JS{ z)sUN^AHCu9R69K$KVziS3!W0BgTJ+(;!1O=7xcOD71eAe#-oYPR~U*-0XG85bFwub z+%&xTG5&2|XDK-M{L9`72QC{hd1Kq|$a#9MwHq<6LM7iH7!FH<$&^>djq{{tE;?W1 zF=qn7EuKt*f$Z(TH?MwiAZCEfw#8rDYR??%#W{F9k0KNvmjXQdS;%KO2H8@x>f6RW zq}-EJRVh>~gqz$Y#?a%0H)pmj($-{)?Uc_*jfhy-7!^w38o^;bTy2*-z(q~xs>PV9u^yAP zzx74ml5S03W)v$saQweHUy{-1WbPkFKobp$%aw&yUMxl|j`bKPKVjKJWbGzkR7(SR zrm$tr)3>_lx{f&J-ENSjIxPIBjIR~FJ6>V)VFf65PT`8gvA#qeH5Hs)qjEuc2?1yP zg?N8umR0@$!MS!sa9&|Nm-<*~&dC<*b|tjtTBY-#&i>b60InSl-mPStiNm7z*Cv0? z+-x!g=4Zk&UU^eEu2{z78^?7CyZgV~?upj7=t_^gyQ1_cce=kPE%UZl5kaYHCyU*x z{agMZSsz69Fx*{Q?ObxKIN#7fKToCaHDBB$tUrq-?o82R<~MXmTC7d-xV`Heyc=27 zP&r{pB>-7H-uJKPE)|_0zj$njIXi= z|Lm$@`J+l*{;2BwWh<^aux5w%p89CpzQ3SYcQmC}?~4Kn86kOKUj}o|y?!>5G^;tF zW!j;S9o%2#@vux|cRQW3nNIIMvmwQI)}6=HcQQV?=l1Q$C#wzkJ2juJNFqDlduUv# z%hEa{X_}^?=!+2|JQ4=hT(%Ord&;5FcCwIgw94R)w}Y(xaC2Nrd5)ZXG#*RQpPR<- zjJ6;#QptrMaoVkp_qg&&uXzoyM1vz!OGAl>^I>zuiH9hiy@{o5CJUf33#QZxOO)j4 zlw8wtlO-;7{VX_jiFq{1maAPzyxgpL;pe(b)9;~67+ zgLlh{C(`{hc1E_8)BBwhwzd?6tfF=3rz0XKPpDQlrsKyAn72OX@68IX^k~5oMw_-T zzWOyxEG7xI6>t$JB|^!g<$>6YDb{_nG@i+a%1W1udR3NKvSg$fULe_DpD*|6F+SNM zw0JoE`FNQvR4S$J(d`A14aeF22?q1eu85(o$Q*a1SAX9d?fP4fT1}vr1VC$OU8OVE zg^bM}-g^jZyE&j#N21Ad*;1)nRr&gj;>^SO1-~~Tvq7QhaDxaOxS@&aGW3RZ{(9u81{?@hVDpxVI< zLfoV?7Mokg1wnv9a+}kdoM!#qzZ$X3&DG<%i`8KE&#Lcrk(H+boL%{$2t_K7Wt2l5 zUua8*LbGy&_96Kl8D*9(zvdMxthl@2MDEU+7l4dZ1sK=WXWMZNAt7Yk%xmuZ`5tQ+ zf}g9&7(D-Zq1k>%5WOJIrBwxYJl=xqKR@8wiDwnAZkB(^fU&kWj9nj&5TO!;%qCYhA=GDX zdBQ&^mjuNv>l)uZwV=eHPV<4obW$z;GAsz;UvkM|z$EG^Y}DSB`=R~~R3bk)Pla+K zQy=2;Ge)!n-E!`eon}!&E}t9>hx5XT%B;m3anl}Y9f@BURF2KGOpE*BjSr0|I2?2S zu1e+uDDIonj^GD30)CY>{kNg5~x8GJ`$WU{!vh&LlbYipahmX`iag03;SW7CO2cjY0fc-7Y#a}yfTF#r0s z!Gt-XxNUwetmiGSK<12^^VH{$r~O^gU62kIyViBFoa9^YCvfsBI*IGu#X98F?!=o{ z@7K7uC;`(Sna9Y!`1(V8dBy+mppkwc443MOW^C?V6f_m9j-!_%kF2{!*#a%5H*Xll z;+hhD5O3bT*RdRF@lA}5_FAT|wd3vPbZ)#VWPJ!S@f4*H$E8!qa)`-gQztrgBG+D^ z@zHSL?qtuY=o@4@BS`+!wF7&??EPC;(TFft0+V%x_wTiar&SBB>KrwdOja_QNc zv=&QjQ>D@Rf6C%OmezyxJ{(T3HCrp@5s5_=Zk}%Cy=F`^@woA$xBPUB3rXEi@-lQ2 z=Wc?HjyJ{=p^oX*(Q04bd$p0Tumx(!N*Yy0ql6Q^z(UWQEt?^s!$@3g5?4qP3c<7; z(BAhw&Al3z84*DKnHzwA-JWqbaa3zPgKs6CM5R(gSLZnQBNT`#G`(nTfiZxG{zVX> z*g$;$kfTA%Bgz=FYRUYcboif{@C=T|^KOkFSKnmKE{z<^zVmQz1=%ilX|^`eE912r zf}jJl^qE5ff-v`2PTeqFCQ&TSCOS@rCnm2~QV0>dixh&&Xtb8X_9)4Az5p>wH^%;9 z)x?S?H@$7UDp58@$=)|Q`eOaC8cok}iCxd^leJPTvIesK#ce(toqOgkt7sF8h@AIU z6MWfQ4Pz3C@7og;FV_3M&GuO%;d+*o3f08Lut(%huM0)L7NBqZ`l8I^+FV{}C}MN> ztM#!@ogPdrB_~`^jfnfevmu31Ac0J=6PKZN=SMJV)A+-Rmwe`rm&dVAa3xn65{1!- zf2QCx2=_#e?Xl+D?`8Qc6{1xH**WizeFN`Yjpn))S*9>L=o7xI!;@juv46Tu_d*0p zrrj^+5BP5G3WujjU&L&A(GHwUoI6YMi_<~-Orht!hWtr8)s!f}xe|v913RMa@gIBN zcA1T#T6w2kg$n*gx21sZ@qu4$p*$QE`8Gk73UB1>|PB{yIzHkM+o8c#T?Jpoyn5G`yQq-tfd1Sz=4X*(}0*QSLb{Q%-#j+@5ARzIdd);POob@p9w) zzLeFFl;OQtz&Ax_ON8r8BNEhVnevan zX1!!?tZPTH?ZV488$Y$4m?M`y{)#UIP2VV2kVkXsMwfYE%{94?e{%Pac1vcjCK%%H zubaL6h58cn_`Gy|U9*7;N}2RiK)zRiMW_mT4{!|-@al& zCX|jzudV@3=UONLQrh<4=)a)xKP)11B50nrRZq!G$Z9&)NBAfEv?rBAchhKlt&8W9 zY!UEf$|&Ky_6BZo64SZNdChY}3s&0|OGN#;aW_&+$To~rsDE(j+2#&6cEu^KB(#`X zOL2*P7r?cNy0vT&lHqjvz0Rkc-rUUWq&0X|5?6MC%W%EMGtS#F^}vnf@%~J9bYLl& z%Lw&yXWq|=C^aGVFSbKGI}av*yI)$RA;!OSbtS9_hhYVH`rNB&xKg3t!5*RgXBe=) z*L1kG?B4AHam*cMaC^YYEV6zTIIwe66>^Kh89n3t_N zwg`}vyuidj7mN%O2|U9_%V;uKnc_~vz+d$~&h|@W)aSb$ek3ONGS@e~qg^ChlW8o; z`fOQ;AutIY$$0~LpztTlS;UrmYzvGp_`@U9;QnBC?nP00s5LR-V3Aqpe``1>>6 zKlrR)K3QQ>7XEzNq^!E!fh8-N8 zgYiZGP3ZVKDkLPnvB{b`i}d2~;!-*ul5}KWEPwGiBAGb+oEwwDbWbMbs#`8^?iWBO zM9&B6R`sd<;8$!M-_|5R8Ot5jL9kwWSE&4i4xhDXkSv6jhrJ+)Fx7|7&`OhB62eOI zw5<~>6a8}aV41Hw^B=4PE3&ziRj|H(whKiT($HO*FQmy~%dHZpyVGzh6}g-~-EJXR zP3+?Ch3)ps>MHI3sj(R~Im^M9uk`99SM`!UmUvwMy22a1vKRk^_Ug+T-u<2GH=W*b zShNm@ADr@Fk4?5R^5%;AywdNv7Z(k)z1d>xVB-IVD~V$}|E9}CkHaNYhEZLolZvCuhkA6y-D^bB(;%8I7y|cZ@)#qJmfvICN_3j0d&8M@tfNIn8 z4Mc-!N;ek2^#UCo`1z&D!_C6<_F?+S9yX^I?mqb^~4jTgMpl+5JJwLX^jgFaP%SIHSoex&fWRFa)1P8ioOGnR``#?`9=kjLxwEjZz=Ee9 z$%-%XS<_Qw`T`%%x9nc}euKo&_0m$c%lr6^)u7gcS=;vZX6ty|)Aca>Tkgo| zetVm1>E<1$!)x0Aa6wXabc|DGH7)(hJhQ9GzA!@i&BxtD$v?EKFlcB&l=q(DEw)Pl zG#0(UeP~b-h4rA9#S=k)*fz0$D*p#BX3g=48Yi0bPC*jPymmukMj!G=k`e%Ny#;Y; zpr2t_se>g5GT%%JX>Huz2Tq*p#GrOs`gj#>#i@~Y%dLg6a|^48`+*w3G`vNf^Lv{c zMHrG6Y7?uH=s2Du>$K5@Sa`Qw&F_C=vf*(7#Nj=@FG0U=KP)r$0cQfb{3GQ}SkPeb zT3nH8Q|33JTPIHb1N8j^Sj#pTKXb_(HeR{autx(W1H3=?9TB&>dMjOvIZif1e1l$R z%Y}x6Ip-y@4ee8vmRSn)>m~OugH$cB>k@UXrAy|TUTj7UhVNr}cHWOi8xc-cOMdvt zaP-|#P-Cx_@?oeo^^v5^rJ{{c$ak`*ZX{dgbU1pm3;&N;5Bl42jfe8Ab4}C{1uaF0DEBB>Pok`mED5U@^gv@ zYk^Y{FWH$I`F>L323vYty&Wu^JiEEX%?iv7+UT72Kb89uS8H55l@pH8Ex=s&Lk|(Y zR(JflA#5#){KS$wrx<|*)!_sXOq~G~ckHK1opXW4^a~q+h*`B=*6c2X8vnzGG&e5fPO23aYF8P^8M;=5CTEl#&+A2eiD{<&ChnYvIj;9;jwve#C5JY1TJBj zk7t9myul{T@8{B-H`#xZGQ@ZWKXyZhIa!+ zL*4Gr1@S;j>X^QN0Tn**>-FAds6PKt@%&*HItvd9iT#@(d@me=$D%34h{#pc&vLGq zgM*6sI1g_6UoX8vkZhD9D^;{IXR6skZB(=SVJ9F!&av4>f`7YPoq+`@AIkaUqF9@L6md< zk3whHWk_gfA2N3k2C6f;U?d2l%=O4K!txCg>j)r)(pAocay8pDF+=NPHn>(~nf^w3 zTQZmp7{kiH(0j-)zVf^KwuL6sA$jgH<@WEG*syDk;Hq%Y=w|>)Bry{?-6Za0C0>k% zqc=;k^5+w4vs1++!!@N2-D*7l)u#ETI&ZqJCDsS5wj2)HS9(BkB}N#(4G-Qwssc+d zgyt#Ha})*fU*xP>YEN~MGgb@7FRMYVa5~!R9@`cFI{hDJ|E_)PJ=P4dPr1AY_v*J9 zH7q)>!3dbo`Ih71FekTD^CLR76I$gr4ek?G-?Td}GGF1JU5RSc5Pplls+9V`HAhtb zkB6RI2cro^p?0$YrK&~4l+)^gQ$-RlPWz+2u9t0ce+bymi&y}$ zZ}j7!k`yN@ed-#lws>XS4<(-M&k~hb*VGpmgf!fH@&+^0GMe!*D6~$Zb7MH8Go9?V zVm0RD5Po)VGzP2re9>h|Gz3~%ji;}iovHW()6WT<`)ctoVMhnyq+JtJg)kp$UlE4q zeQ-Ra2BR&M?leutIk;-z*$=1Ets!eYOv;m$f2^}>ziRWq-SAi~0kP1x^fHgyCgY7n*^KVE_Z9_T zi!qtlg}-z=kKR(3GEvpcS2>A3u5=&v5i%3zkO;{x{l{g;fcQLMeFF{->G6V6V78u; zzmh0}WDFnPLPSQ#D%ONH<8w2h@&-r#V;el15)QrCf<_i~B4dhFVxDPc_Kiv~zYiP= z64l}%*C0?Ru+76$#GC&?!}xZyZ%2z2yJ;jAp6-@al18XTXV*;QM@wvtsogZ?MT0`t zuz_-#V{yMYH{c!cLK3&voKFa2+BtrxE%?0sWOx_{mfl3Y(jWhbPOBcR0$GcBXY}7% z01z}JZ*O;#{O7gDq<-_V=U;rd!U7z?$DOn5g?Bd3GK;lm&4 ziFnDWJ())Kss-}>-L1y_X}+kZa*$k94;LY8VygmcmDm1<%k9c9yE}9))hX%xaox1$ zUB|{H&TgdjXctQba#mb}xhuyf&wDD2;Em3{O$G>yB7lAvmj|DXo&pJHn^T){E){i- zY5A<(46zwJ-TTz4*#6Lj4qS}JK-p@1g&x@Lpb~l7hAf34q&5_OcGj=@*Eo{cM@e(? znS+iPpv!vg@Ip{XMfVRsbdm^S!@7yI`1@u^fq}jO-pFx#NF@$#QJP2f&aWu6y6pQO zi)NhBq$yB@S~6P84I2F`8zqiJ9;40hv1>Bm(I^x=i?Hh&okpL!X7fyy75kFJ4rTB& z$hdoyi*d(nk*~66w8l`Ah@zzSzz|?yPqtHOnghEo2MvGpTs2{tsDwBij9YF;t<5T{ zk%edmpGmp+oRlrQXS@!h&_Z=BnfncErpE#0)~ng%B4dTxsMJ!^@M z3U|E(o73rl1{9Vh>*s3S_ZM1BAEw)P)Lpmz?E9tvc0QLPiOrCrHqZaWl7NJB17KQ^2 zZ3LD?qoqHaf2u>NP!}&!Zb_DSR^qPJMi{ICfzf#Umgwgho3C>9DcOLC4&{6Ov zHXmf!CGTU6cfgIc+R#E}Dn-Ufe|h%r=75s(w3%PiD*@fSj)s<#JSPjJ8LR|-V*<>z z$wBLczu))1oryXkIG1mQxV~83%%}M*Wa@E7rr#`Ia!l7q#~P^YG8Y&3;0ms5Y?x8> z`4k&Pjk&RtWF(U|h>P)9BO1-i;virtDvzhe==)5ye#kXu-;u~N54}f~{5>9|-z-zA z1e3NOGpDb%AT3o$42Z^m zZdao$4pT%5AjX|fEbP-OWFWj)$(SsKE7fU9@<~Ew!}7Cv&j&GD%l(>u0xfj|j`~-_ z9hQiQ2p(^Z2i(6O5<{%Z7`lVCVCRlKA3!s&s~1SMc;NFmWmO;wk)d=^(4!_%*3z(VY(3;Z7}Y68=IWQ@t~EErs#wk~44tZCN8Wd~OLh?&&~Y!u)t7 z#h&|S;HorUr_ecvAYGueX8BHOLSTSVI{mNZ@Xu<$W)rFeq?VQ?4X(8VGJ88|)o6sG zDy~TV5iAqyr?W=v_K5Yq&ul{VS7X;$JGnc?H<;d>z$S`XcT=}puE8sFQpqh#$`3XQ zd47*`z@mr85j=ZLB+V@uT08@<=Jf9xb(E8ta6z>5#3NoV!p)PB0c~{-CvW;p^^d*` ze)+#YIo{_}NqY<;vi^69MEi#5ouhl4TUjpn;pA2I=stTgU)>BpdwkA(OoB>`yJ|B& zK_9XBVQ;t+hb=Hh7HfW#~OD z2y4uN3T{(mvMcWivn8{>Dh;5STy-p+Cw$%f$zdk}j#jMdIF*1*LKH%6%2Ol?*wgSw z9qs+xWbbjR3S1>;Dl(^yw!}ViB_SHsN6q2qHInl+TTZ-7WZm^05&Vf-oKjnshwlS8 zck0Jz+G>Y;dL{mLwO;>UvDGb)?Gr6O(x_10-HBY+o58xDm%~7jV;x0X#*}E(uCCP- zGw983E5Uftp+C!w1Q3<;D@`<#TWEOj zZDoqgSZMNuaer=2g+td^DXBn9!-I)`ckPSS?^UP626dN6zMlBdP6li>j^1;ge+15A zSg?(aX;mUty_y&fEU<3COT!OA^U-qQ1ot1-9sza>YobEq5t?LB*+wT9RicP@I(I%5j|kr2z^_b!wzd6-3tHjfJ4 z0q>Gt+eC*30&S@LwD)dh9|K@93NsrO_&~w?v-tMS%k}No%hLB82#P|xnpl}C^_p85ai9RJIM$r$nDmWO&Vj_fz)6V1MRqqP4c!ga zjUc<8neRyR`>(%bA-FQ8v-N!FEv z*%x!@MP^Zf3*I9RvFV)_s{m@bHBw^p{e(ErO)=$~^%Hm7Fk4)CEyUZR!#sD-_h%1{ z^Z5&>*}XS?OO$N{6c`RJ;F}!)UNj9;3a=abap&0i)VcU&WytE_aNC3$B0j65)XemjJy@v; zi6+-YQd)YLIpHbh4p|aNTWQhlLnw{6An>dk>a_VgzCzy`{;``ECf5tx4?$3K4yf@a zYbjr^AwZLprh{^%TSSqd>w?~ny|^Lx2&|d(#w0b4GP2Y(?tr% z0T*IRNC@s$uf4EeBugX$g_zkN?e^}wu39_I31a((6SY*C81Lpxr>eC8F@3_L#?;%E zL?ZX#xmkU!E^Vh?^?x?-DL@R_o0;7nYjmKSfMyzW&3!7bOwXgH@39-gt%YL21;}BCmrtAw|=PqUQ7s>^&3q}z8N1dB7JB{UL zbr1$7ddW$t+DmmHBK+KW-EL#z{mYS<$!fl$AGycs#@M-8{fS@bj5fRjlC#A%0_er{ zHgqsPjFTQM9H7bV2>w1o?=-<4&BEDbG1HEA$w%3{-h}@_S95%vF1_OMN+2fg=7?1z z*noIhroHmOzfJOGi%XkJZkD+nsPtlJ3)dPBKM6HX)1v!2J$<8`C;}~)_{5{>X{L_VXz?J161@zR!!i*&k{p&S;eSAzx2S78>_3`dpF6>iUNFb^UzcClrW-Y;%87 z(1mvMe*8s5!>>AF8Z&%zBYfKO+$3$z*5*601{#6}jJXjX@Ne)!yloqC;})@zoJk!$ zEuw8w(|x zd3SuUiS2)nwb|+oMfm(!ZMB5+yILEG$fW-@o*LIl=%FeRLO62d8)^!3cYK-$+i)4B)x5|h071gPg!h0+&5L zICL`P?N18=meQj1+BWV4ujC!VJzdc--&|^2OD`#Ga{!CgT5dD#gl-q|5 z=t;De?Ki3d3U(5P@Iz6s7C8^-yQg;$`Se^$HLv$D=P5$0$@d5>5V|q9q&OF-ta^GY z<--oRf)6QHZ0ExINEqZ0(dgfBa$cA;OG8&E)=k=U=XKLxmJ2AioxhiVGj`8879T0U zJ-z!A%N$pJeAMEHRv8;p{^x>K)^wa(u5Yf}rCm?^Y`Tc^ceqN(PVo1!5}WVixp-@m$B(dq9@D>#!?}PKLPXyOcR2YB-sh%_+LYI${3#Vgs6B8FP*r0}?X!_1B;<9BqYMhG7AA5K&)b_WZs*5i5 zis@0Ys_OlFuXmKFh2Wg?mY$s%dR0dWwu53vPjoToJzA#{@sNvSwop$sQw7#lb~dtK zdA`t8GK}3!N_s-BNEdc61^?=p5w1)yAUUE(*FXVk$|Ald$< z^gz~zl=>!i!FHjt6X1!?+R72h7M1uh@QnVHWivhA4=A=gX9-LO8r9Ove5|$AsrKP# z&Bt)*VkY?P=s>2xZ9_0$tV)IYG-UJ$_rvi9^ZJcPwQM2djVx8(v3HNW<-3=u%@Ui! z_rzsDrO%NG3!^wcP%TF*(t{`bQ>aUzposp;K0h|g^N&SxK%Q>jG6oVlkiMV)8rVm{ z;mbF;x1E{$-DF#nAAP41qflhb#9{UVd|)Rd{+{yELFzcR*!*heDzu^x=ZVG+rgpBG zI(@G6M|u~OWj4+I$fKQm>slg@vDrqEMP_&?cxrp~|7)pns$V%emEP?NJI9^?LQ6El zOVP&2;%bDOa!JR#4?i};&c$x5k2m{CVcvd+A-FI*meIzG9OhiwMV%XTPk^__4e{!F zb+IvUxokQTD=zo;b=}r_RB%zP*tF*!$-uKa-X7|rXzzfeaT@KVrMHjo9Rm@rb&#Y( zPUa&0Iv0=hp+A*E5G5IcmD`cUUaZW$@>`Xm^lP+b?JhwdJ@Dv}8Uk~TWH}>P>@_TS zVDu(^o+I;6y4f$|CO6=xZcjgO0Eq$2m1F0`fKvr_%MT=104;AFRD2pD!?*Tox>2!B<(OH}9}u{Xp@Z*O#Qb zY<$LYRG_>~?=Gtb{EfNn&AK7YOW-Z#|=ap)9iEV}=xs#CYD;-}j@PMeB z$gCF<3n>nqV?6EtR0WC-~Z3m)rsV2 z=6*T-g3AOpR*75v-~5=Lu3u0I)Yg7iNm7@0slu$#nVXZ#cJ~#RvfMDIY_}k%>(si!R=<-2a z0#7KEt*4yE)y&zJ=<^jpkj%cio~{U=-u}lV{LRhPuR_AKPoqt;sWgWLY^Wkk}kTVJ290Con6A1k8=$mSpdmEQ7bf({c^Z%P*Jy25O~x$1IF-O~*3q`%-@p2$N% zf-GsUgoR)!LYcJp`0v-MF$2Q`O_b3wv}NM*63%}m>QOJZDG*NHR(Kh*&?O@Y%HG=i zcRh5DAEAiG%PX#pKwqfL(x$^}X?e}2YN!`{zNyTBA_*jTat(F=P+g|CK|JRSF!$RP zq(aRRt91%In_s`Af;!H_$jVpS-RBHFb+tqo55BwKvZO|H<;Az#K#Xf?NhcVjL%Z)q z<<3Uib+Lovy&V+Y{CcTf5DbDdU5n252$R8_NUO-3&+5E~^*<=*D5fU6-?TyC;Nr|f z;q$^kwXu{v7I&r!w_j>i6PPZrll;(B!Nyy>kvS zcF7FP?$AKTM$F@JzkeSer{$*3uW-RbL_ilycIi=x;2L{eY1sE+uLn~Ze_D^S&HTzC z;K=&{G88=|$WEn{Qh8-jHXISE!AyG1>W^DdhhRG{!0!IEqCYRhPtBprt|uRuE~YSG zq22sroDY1(%eK!3OAz;B7;t<+I{D*ubhFG#CPjre%k||=BX8wL5`v>B0njRa>xZhR}u=1 zAm+%+*@s7G6&L@!ehAVGbJdA9J4VW zhC{ht1d6ahfP&k=oo>PRJJhE>ONA1D@hl0c^n7{M@0ri6S8O8MJXt5g=J!a-q~3$Y z+7AeMVHfJH6YH-45v4-SP6yJ*5Ay1(r`>K!o`2vt@<0Y01J*@XTojzVneZ>D)WtIH zDIsyGB|*|_k);j$^>-!ZMqlHMvh(L2`RzQ2yQf)UohP?Ki6-J!@i*e){8g^_Hj|e$ z)+heJ)fO|XI2l+LZdew3LB1%AZDG#3Yj7Z!KkaI{8j*BC$5)4%hA{Ctth4tM;a(sk zkBEeF^S7L94@0$J$HO_@Xd)zzMxgj`QJLT-L=&+rS{=PoXhQX)&LhI%;m3ZKyFrP~ zqsU`Igal3k=ZEsb96By;PfP@n>|NfLTT~g=HbMm15k(lsFCv~~=3-gO@ub+Uekup$ zpMwISTHE!FDHw9!k-n<>K7oIoxn!j!|@@Mj_iImJ%s% z870*$1*-uj;)yV+wnIg{b6N2nY870*x&c3%{vEBt128pq%$lp0A3(WfdWN}*Z}UFm zj3%2@d@(o2F9T`9NVUG}-zWWpsDu`eJZK=_+kKOLP=df90YQ6`OR9f_VtPNt6Os?(1hj^eL-lRr4vGt z``}j_^W%@dnxBnr=Su87hl1R9i39lU=-oNYWTuq#>(DSn#H8h*KQ|8sa)jk)LAjH2 z_mVe_ky*JFTpHQ82)1WLPanARwOwqPSh4C&|pE*f$-Aq@NIf8$SmKcqY+Xs{0_aEzn` zWY+y?<=ryxE;T!9d7JI_X1Ad))NVGt(C@(BZiZyfwKI085n{|zUG#XQ;ZbNNLnzxc zUNWC(BF~TL`f}pv(byje_j4pc+1yqBAd6bjf`8|^pZ1(uz@Sp*3xP+3Ch8|Ew_E>D z^GTiLSM!NtbaTjmdrvNUoHR0&eKY)cBkkMO$fVH^PxUW0hT{Dh1O^I~c*{#9Rp?X0 zBb_{%T30~1fN{ARHT8wzn7Y~K(*fa0G)xMWRs>P(C+(-`{45s3^5!w|K%h(hZydNK zr;B#_IQwgJy$&I!KY#UPvR0i)y%Dh3is;qjH!$s4V(>E;lA6(`#Z){$o;?0jA&}N% zc>np;dUvrw#XRW+3t-|jV%z?o`T!p~n$SzO{kIcNuAOiwoVg(y_kWHoYUW3fJAb75 zGd^6+US)ii5QdbPA}eB8k?_17uJKx zjV-6&6(xTM%NFzw|2DP)DgN8&LK2D6_UGgb7S7!6Fdfk~)SHEHHkJnFb`g~^fA#2x zxf+E4#R-r%M|uH=U8Y2WS|r2n+Wa$F07S`x?5FCuqm4iao>gL=Y3e8(pQ{{@JPXd#NZa%h+yYu9cI5!n(Wp z-SzM?oralx*={$O@!gf#lt?~OtF=b)48{7$X0TY59aqA46AJohp?~_uSzdCyG312n zdc4Q(QMQGx{-%}it9*xs0QTtt=SPs zn>CVCa2Ods4q~tu5Qh|J#PH}(-~a-Gbo1|j0g#LC@pQUS!i7jgyWbC*72d_GSz)5? zks^Xitpqw7%vWC}@3T@b7705hPOq1w|Qz@5A+q)HOymjv`}p z?X#v?cCBGg(J;Oq&KwLJ#-+i&h={~JwRHnNfo*Jvpcrl@>$>|*545DOHgHdJ9%pkp zAqs{0#Q4eC-A~FvrBmZAl&ODDf&Kl+KycH8_bY_03Rd?j0|EIYk9ld&-b^^k=`Pf0 zkl*;`2Q{8swaws??$amsTxB^{o$ICKJN=V3XGcLlbIOss^Iz#n4(#wgw2XmPIF=+E zO$SDIw-c%?$w`6XeGYt$CZq_zU>@4NC7b?;$qhdpYL5Of+Rwf*t6M>t%v)Yuxp!#5 zMJ}Itq^YQT+qwX#0Sp#L2jAOiqUej`&x$eLP_|RX1Q*dB6jB`ZU#Fg2Akr$Zu;WjD z!L~7Fh93sw=rP=#yf*nD3h^<7Q@aN*MV+B~0m6&O3rm3#j&`*#XSPDUV zHs6Ip3M^%>weHs% z8WHjZeu5KQD8H6LR$HUCtNC@;BiTSJAQ6ZDuX3*Qn+?VbquSbPmr@$ER;bv!h`kzu zcqyq>Ra@++EupG{)E+fz@4Z)y7qw@tqJ71vEuuz${*T}D>3(=VJolV??s?96&b^bR zCWiqkUC4;Efm+J1ZWnBw4`o>N=v=yoa>-beyL|4``?RS4 z>!I4peA!QK*2TG0<~jPW24PISy%F#t61ia?KHM*vu6jC|rnufv)AJv0IYB@Rd4;}y zz&QtN%ZG2mgzg4C8A<8xT5YiXS6nxuHOFD9ES^ljSPF?PU^33kg^iwtv}ymFNO5%TBLNZ0Wu> zPNC1?Dme`#Bd=*|Yiob>C+Qe=keHFdDJR#0feFXT2BL0+TAXS+Om-WG2e~GpLQ2~n z_q!$=^_J;;dulTjV}BMkl0VFAG5yV4GE_i740FO*fHy5Ps~mkRI7fvm(KlL5PYh|% zb+)wYgsCWgF!WP>O0F;@j2GE8wz-m@ZsXim8A=Tt%X`6lW3V;UANMy!V|FElcXFjb zGwL45b4_Ydp}i6&_`p0x)r;p93q#iO)qh+^KsHgmN_)0#@45rgM8`%}GT6wY>9Z0h z^iXoDKAxFDNB$a%phYk|8-EbP`P?d+c2DSw)N+5-&4WeeoF@Q4sp9ASkOM&}n;K@V zwddPX_60QC^N{fNO#a*pr$x@3O7j*IQIXGd*gaeu0ltWoQm?Z6iTW~qNW&s;s*~gz z;f%h^eB}Dv+>P~Ux!6nHvB`t2?U-%@zbE9j|JZnP+C)*Q12?j36q232v26iSTwcM= zF6waZ{Vwpj7USzI7MzdUo_e2%FaEv(Tealy;pFMlqDdaAPTzn$0V`WEiCF|JDJizR zT%|!U3NHywA1Llppfhr&NGS(zxv851IQ;GOlqTv*!I!PD!L817>fHFw~58 zwk?9i2EB+p<&w8rJzx_wxO#;vvbM2!d@0a0YQbi|zSK}qK49qFh!;LMbLF44B6B#L zr_|_d>7CGl)(QY@C)lA-DLNAl>45^KPY@|2|JdG+X@N$AMxy7%1RbYhRw%9>RLnWU z!PWb&=P1V0SdLy4K28Xh4Fa7}aXdR_2>JC&jCgq!n>y$7+{Fff#(;AuX~`Y%)bGzwNoC#q z$@n13utnI%g(XpHH&liSkv1>a)IukIjo*Q{^}=s3`ZE4u{SAQC7f4hZNt~NYRhgqVGM>#jve330 z{Y2ZBI791S=i*6kFuQRxuH_*hO*ik(2UwcB6Gm&9@55x1d_Cf8(d_B+$TuW}l3vo< zX{J+Ht?YB~fOrmYYt^=%a7}^&i5w1`>MfjNs+Dh;q>9`w8_$xv(ao6LWGuV;K+vNJ-Sk-YQ_TJs|| zc+o;-K4#?Dzu@@LidQKX-%CbhTDOtTXJ?B?qT>KeDq`HBO+}p18vXI9As*ac!T$JJ zftcmKxrVhTIxY}Dc!*>@aQ~TFNPlDWX&$WGIR8bM#`b>sTs*Iw)FF^XJ|2c@o- z6B)Yb9cTQnEbJ~)+4w2^p)aDuWz3-m-@#Fgx zW`j0PcBuo8Np4x)i{)NUD!+N_eL-1ca!VX01%QjDcg-1jQRzwXsHSnd-E@-)vX|J5 zg*t!1on5sbTaNVasPz`)1HL5cc~$@|?)5};_LkcIK>z^iLOx$i5~AIU#{baarS`t&|*O zn=xuvHyP6KtTXQYqwJ{Wi=;o(YRHy#`Sysf@SOzbrzgott{>kr^4VPn865Rw4_4RI zL}Rg1<>eyl>)Ur<^#8t|o(M6Q_VV&dz!s`U^q@TbeIqtRr5P}b!alAU;kEdVb-F+{ zKh>cEC*}~E*tAftkJ0e6YB{u`G{c!}X3P+r{4MdVDm^0GpT1-M@xYtk9mkYt`K#j< z-^>;z3c4tYOy@E7(Z}s1<`WbqDKuKSl(c%cbDYjBoBQ{(0^Vl%?h}BJgDg5h;8R0L zh{&sjJH!1JM31RY+QR!}4A=+ZP~6x-bdw{g-TD5&4D!x1p5}<_`=WgvFV*!WJOc>= zqjImK3@1mP*H>1oQyYT8;CqmrYy_iH#yxAyBuwE<`g_P9x`7#$$ka;fb_qLzzF%I# z)Qg1JP4D~(&(Ze_v($ZK|0>m-lzfCb)k!2A<2lB1hF{o^Z_$dIV*KHXfktvs=34GO z;iNZDCCWde(dg`f37yktRKs~asJ{d0^_BS5BI@N#Ke$z@L`0E=cu|a+uCyvBek@k4 z=#$@ir(r0Qg6D6!OV}4tJs*Tpqy{a#+4nXd}($W@1S}z zs{QwNJ^tgmwE}>o!E8y0GV?DMNzQiGnk@GxgCA33Sv7e^K2Ak+B zJ)rgzyC4682LJ#u?HMq!1LET*usQ!aQ@&l|OCz7Sgih16ER3-pc@5yS_e$ci*K zYrK7zQp`kdwFNe!mXyIoZ~vPF(&loqVF6e<q98qzdHs%LmdV;(Ltc zBC6q{Y;We`Zs=qRFt@X}HKlVlb}}`!bGEQ|xdQ3_0RRvJBt-;OJhIO>+&z>Ra3HR) zWM?;KyT)i6LzSS51;!{Lg9xL}i+@oy{ zuk!!i`e%sJ8%X_sH_eba6$bzBzcRoO75_KNklYqP`yX9Q7AV{@qkm_?G$s!OQT@Ns zIH|+0sQ;TL6ZHQxX>!m1?wr?VjjY@_IO4D z`>#&n0?MQY%#+QGXEKdKB0w08Gzl`6c09m%e)NXaQ`~4Ziuf!~2$dR_?JH`ukU`0| zPY%EGUFA3CDEzHV;dFjRbrJ<6{lH}kVQ2G2HFb~* zfCx=-COsdN5bY;1{Ka@F$ z3>ayBxOW*0n26x3dJX0%(cYlJf5DKYo)BG8+*uHCZc22#xX`e=M~zC|+H3{1ap>`F zw${i0`awn{2`b^?v?V2^gUu!g-%m__|749ih%@&r!|QpVy4pk6+h@vwBN`w`2Us{^ z_p=IfDWn5pK4EzKE%eU&V5T8_#edyod?0dE1UG65SP;5pTH!eh%U?5_mC{Yu%8{EH2uli>PrwF{k) zmlx7}AT`g3;1IoC>Oc^t2;P|TQ7JQwpv2MXo$^2K{(o!L0GAm`LQ4h$gz#2j;|17? z1I3r@)gMpEPp^QceOe0RW5j~W`caqGBicGT7~VsxJ*W5Kj&S=(2zVaPl`t>yCNg6 z!d#v+>v%(a@~{4lPpyH>2U+>h)we1#3^Og%dzbN~iiQ~apD%YZKF0)g;`riSQcvXU z&Hr(J?*!!ZFF!2bx2d6KYocfJN9;-Q_b4nr=sF)4_n5CoOsUGZGnV=YX}UlFj>|D# zP*{UU93_^bqU7njibdS7KNA%b@}{swqXP2?Q$(!iWbgQ%xaRVuj{ksROicPG6AFn? zZhBl1421)y5ya<QT7 z^>DFeypi@L{;?szNE7XoJu!lhgs3}P2;YDbM}r6ips4rWaO_+wJ_GTWrxbA6r=oc}Sx}y*xXo@awH7#4slU1fjjB2=574@MJ%AqbAa&W7OEcIeOathjTSNapPW$ z4a1A=uoBzgX|gi1?D162WCz6KQLH;+u+U=@cSpJfcszi zFvlW9@Alq|#*qRF#D4rNqQ8z~#vJZ`&B#ljv!RH}ndATu4b1B+jCAVkE{B{C8UEu7 z^`@rQC=kh+UsV(z=Uem4Wp~OCtXBUe4D&DZb1Y2u7>k#!P)Gq}+{F7#=~`D*;wkYd z^S}{^26H8NTB%2+$ce5FKQ?tPf->KRdgu1pE0wp81367900LoZhJ1=2dZFLqUe^5= zx@4y8uAVU-LdlEpjiBw2ZM$lNKQt&6NyMlib%DU@?!G}sr58fA#cS6E zd6~J=dfqB&(J5J)lq!^j4DGRJrS7|Hsig}%>@WhG)!(o`7G4<3R#j!YeY$`u_d2;T ziD`VE|G(txKEeTqheMQD9~1zkBP@($kUuudfyLcjkEySh5fr~THC%@Tv6JD3i*X0c zc=5E6fp)2Qrm6K>I&i}fRxtdvOQfOEJ39+}$V^3wtnTp!i73$8eBm0iNhoe<2_=Qb z9MX*udhrFbkAw8BnAF|R!2j4i6CZLkf}y$yxCj8aYUYo*`|(dH9ius{+8XW{$GiC5 zUu(mTy^YwA! zDZPU&G_P-qT{hps%_wKdG-vt;V2xwge4t6sLCXfHiTQqZ$;^Wew; zw>TST&OGZRMbzjRCZ8n`@`xA4MB>#mCel)E9?kIOWf#`hUI!aL9VW=9%ZYO2PmIT+j!*c%&|Nq zMptJjBaE2=X-gJTEIg24M?uf{9`6NNFOv%^n45>t@(sC@tDfTB=gs)C{f?5&%@E7j zmApcZg!L6dg{p970@J#;=8~FMJOTDafqG=<37eO|>C!Cd#EsIYpkjxU5@Q zF@0vD{7M&0PwFM5+s{j=mzT2g<12D4mjv(0NlNhazx3?PKq2?Hxl{c3RE=VcelWMj zVM0um8I6bhWuP6lzN2W^rIFt`s)m{Fz6q{8pV09(-ux+wd1(xlN{yAJpBHCZIvS&(Hk9!Pbzo>G zoI@IK**1<7K%uAhmm$Qw>p>j(8RM^kq?N3s3Uvua4i>)jo$x)mE(3Oh3K zjzPfxg*Qp%sTykNfX9UXZRKVSDV+6Qc*qAY5qhs&?D}dduhfFq1J_Gpg{heUA)vVT zI$$WL%PLG3bVc4!HX4+K(un?B`A`nP>27W zMs4sqh(&mt+lY7FmpQo<^=cAR&2DYnBS*>LVZxk61Mv}dOyOTBcP;-R6}mp7e9xw zqhNUuh!oib;sweh3@i|8&54(;)(z$>@-0RDZz(2=QT9DlVjX~V&o<$-YHGw^={o_P z@$1I;T0ZbwAQ1+;8>>rz0fJ!RFX>gUa7lj$SI547RVW0!ueZ0v4Dq^`@YEAxj~&(y zl$mn4Bn$SU=-XP>H{? zA7@`%{y_6i$ZX-@HAMyh)2?r9D;;d>1M_HGmvtiv3CijDqTM7KMQ}Uuyo&oY?{Yrl zRO&6{u^K#?rN+OO)C8Q@u6%~5do2o0lz;m}%On8vlFuEa}$DnUcT9{A; z1E_r3V|(XxaUZnsO^5J1Io>_boBS~RyUk)UYQXw1s(P`X*jmpq;~Y`ARdo%c?RM3g zz37_Acy}(8TPkedoz2^kZ8{CSJ+X7o(Za8q{i5?H#$$Ag#N$L3QD8V~L!M4qu8(=( z3whM}N(-*dm>X0<1p-ptQ9OPwkmbcj>~|3u>_79*m~6U@?^diCK-LAO@0oTkUonw> zJ|q0WoasD;Kv}>GKkJ?u*+X%tI=&>vvu+Z~a^uY#o!`$}4N^to$PbEWNb)f$)f-IY z)2^qun9O!X!f+cG$rJRx(MV6bcCiDM&IAbV22MNnBOU*m$~GRdZ;V~iudmPu^NpA| zTd(_UBEipMga=afnG7LbZ%qY33IM>gay(RQ1^pm~RTQ`Bx^O&w5yZsq0HsNX7e|R3 zQJGv`S;y)Sc65dcU0X}a2`1s{Xip9=#qjj@PHu@ucu{%2d3+?dXOO>ja3?dTfUq*L zl@>wzIulU)OyqsT2gH%sLapd@lKF(3`0k- z6T6%sp9!mkHpU276)ak7GShKR0+9B^h52WRw5#i)ZLB_kyEeI(^b!mz1mchVJ(Z@) zdET+St+SD3sFm_ENLROY{NM;~EOS=cqUkSZS%a9CY-WnA?iO$4%~^ zUJB;wZSG%O_)c-o4U2!+>*eC_U!Kay!N-mWqKoP0Et&gTY1lUVz0s5)jIP76j&1}2 z5@MV9KT__u?Odup=bPhD^R&f4_;3`jpV?eCj`eerq@0*3dGwbO!wntY<1%4c^01ck zc>0$I12nQD2xfn^`U>#E5l!?6}XWCYQfwiUWO;>XKozZvUsulYOZew_BAK9++z0={%VPy^I z;wF(tXFNmI*w+FuUdcxoe=U4p1cq48;06D<|9IQt@ZqceR|In+qW^E9S!1a4)z?C> zDNZIP)HsFPsmBmXy~HJ1GVgSa^5t-<9(A8mV?-_;~)w)%ae zY)jMj*DAbEX*BG9a|o}xI4WFr;0eV8>3SPUhUdE$`7x94j49cqgo1UYKv&`??WQm%pM2=_!#0%?mIC&?qcIq|A# zZ8^F2#?m#L46`*dxxM)`iO?2vFx8Su2oLB0&)*rsk*VXYim9ZtoroAtPI#hcHK(5$ zsb~HlifJ~*P(L%_P6=2}zdj{%%m?ekwS}!4A6E34J`(h`#6EP#ArllDe<4uCsS9AH zUb+@#7#%$k^OPgVeir(fQKgxFNjJW^75@_Ow>q(uR@#y4CM&#TAvwzjUELE%B`O~+ zixH=Si-@wT%8}4h!g9^6%WS~z>G|<=Jv_f5C>CTOzth;F{BzQDaj%Vkcd1Llv+0vd z|6!trkZg%P*y7ZCYqp{qg7R~KtKI1Wy|}1YSxgPx)YMe*H1_LwAwcYagA=(oTFf6s zGTn|;*-DGs)Wk%=ON+XAU7`S{6vg7IMBm~w+IM74NC+?jFFzec-Cc(qr9T~QLpsU& z7*4}0yCo8#A}|BO@~M8okBy8L z=OOP}CrDgh3J+2`-D$5r+YI99vb&Df?nV zxi8wDm@Q>_f|Zh(C}&wtO|V68uh2JOS?1Fklf>E^i9Rm&TiQm=V8ASu9lzJIR+Y0!nod}*bKIz; zmx$^gO)l{zGW(Ony0#ET4g}}ES`ZX3UOQ5wbsUEdseP-(u4eAUKvGtID8=RG$b3W- z`1U$iRIymKTTk_xK=$T0ImHoF6IUz*%aFQIsUMzj>;&2V@f|B&{8V*+y-G^w4cKj( z)QJ6vR1U~OMML9#Kc#;|D!0Nj50Z*fI5L~b3JyhbvO7a;5d3JIrllhB-VA9%C*A($J{YMIx-tdwd>4le}gfo9%g_(uo z;J-qxJld1M8^@K6*#t2g^8B`LA2a6ugZF}D{NiKcO~fp{LBL_OB_y7_$)m0J%Hnok zPta`1e<>a2y++H1-2^ooQirU~K-PAkvPoS)L5+~E9#>MtO%8eE^;;! zO@g={JbC_%WJ-pFveR1X%64*wj=h~{c*~RAr~O^> z$KJ99wvqD&f|*(6SI!fEd=4Jx7hozH7!D2@G!;RZT`2c@qlzyZJZY0!?m9#v~>kG7Uy21aRT@Plx zd}h+6WU2OxzT3tMDB}(4f>0&`W-MHUgmh$fOVofHP5SH^?#>I09Yo1srhzQ!%y-$z z%wzQ1rX}U(jZbytVX{w%ms`u&%CB|i$T`y!In|%a4|PU76it~w<3Vw%x9%6zVD-g< z;Xlt8by7h~BH44$^rWzDBzyM?Rg#Om*c9p*XaaG&me zy|JT27iXs-g7xfc9wwga)NTH>G8^VeTjpHg`-978PR_4ll4vEI=37Y)Oem4$UD zbAi_tW83=i_|5FPJg?OxLDR5Pf0$8gX2&#G&a823)29?eiy}C6Xh;Ce&mj*YESZ)M z+PG!RfP)vzf#M1rd5G{DNK1ROedPMnRKnX10!PaD>GMoUkI^v|?mY;!Y`r+9;LV{} zIei1|eRq%5r_I2)vZ%qMLcRX5tWoMX2mX)mP^j}n%v?iJs30Rc%4W+C!Pn*diEp#6 z9k6LXDHI_)5z9w}pr~lBP;wGKz7tNrW2ePFE|pSFdTqG@&gMI-bmr&brh*euVXW~s z*gZ0H`Wz;l5_!e}t>|;cZMSfxZh{(OgsaUCB>%?^bw~eVu-TMk=KN3A@@%VU2BR}w z(jYaw6M-Ur=v8;L021%?)1cw?v}>vS4KT5>ay~F2()vujI}c_h80(>HVxu!mf%DsT zXiJAR8YTLjF`_STw~x7i_?JRRyQSySFIo_)to%0phj=j1Wuu;=(it7Ks6y#Ky>T`h z;59m5aMG!csF9L`XrH3crEZ_s`$+7S0{XVi`B(+rvkZH&H!-5LT%lxzc|%`uE(07$ zZ`TKuTW|Q(G!DmOP#KM;;B2pZ5e(_MD7gmja;KKPQdbOtAdXkLW9tTO z(4#z^sF~nTh?nc`I^TX+&}BXkU8>}QI#X@v?XnHHFZr(aE@K$@IYwTN z8S%;?Ra$8Q6Q9S8EZz9a;~A|EMxe-*k@05Hy=Ho?&U7#@84vJ_aVW_wzkznry?D|Q z$gOxL#3%=szaG4N9B5P3d=XpvP!&bF@J4AhZU^h0~{^R7i+H^W@vh^)ron({hMS~6?YXikd_lOKg0?_ zIZF>uco8YG3TG((CLHx-xy6O828R=oHf@OL)&U+e0FIlawbXIgKXpeSH5UmQ3C9&0 z8kjJn&^C`T!_0kPUZluYD>~=IskIe$Xxd>8R`~$$4Zbi}3pSji*>H|SxFX|{W_K|c zvWV3>njL)=SJnTYP%JRg7ZerJC1G~KT!xsK2-q*{u0K)IobXqIxFu1H=&F{Z-o`|X z!lx&1NNWO~)7_gFh#cLEh?<(+Kd}s^-`V8VU@{ard3HC3g_aB==j)esdeq)bv}fA= z%N3CfR`3+~g6Y5gsg5X83`{24lIXIFS>+Wg?#x1$mUD8m{?ZPSveHrKn8ychcU59K zT+~-B?AWSA#IM@1$@lBx4tC+U^$6pWs5G5Lgkc#y%&|7u0^2z6wT8W%4%kpJtYya0uBP( z#=WksnhARr`$+rWwJWx>6&SsEt-;WU7+Yf+%sGa)Me7OwE?4BfKRiEC%RsY5$)%~zOUN-Rm_k^+;TMW~v^TJJo% z0;BP)ewHm}*m-S+-9Uke51S9@As!*7H7=YqV5n4K~tqN{%!@8@K^s&YRBP-E%Rw>b zz{a;a9M9t}v-3uaBEgWJ8OUJ*(BAuhOB?CtIU!>h4kTag2>b*N@v!OEoYT+9uT4Yp z@y+GYe*)N5PJ7IsWh{$!5S4NBFRXaqDuY?)`)ANU) z?|IkrKiD?ejq3&wA>JarEoi`IHKJfq10%a_TfzAohsjx7)^5yAcly2|X4Qtkxe9X7 zp?Ii^p)!y#vuh^jje_L##ir+tdO&hftMlGn-a*b2_aIo@D(6&XPd!nthbIq#bXV8M0?>EwDs^6a+#^~2<^}TF( zC$P8Rb;4Z(5-CB#i5b2NLr`(#nR(DHi`NX86wgW#W5;>Dy@HmuV3>HW5=zzUM;2Q9l|NQ&$gU}h(M5tKBTRGJnYF6_L`W|XnSL%CpQ&1m`2VI9x|HU z@3ojazJxSGH zss2QdUf{18$Q`(A{iz6UzGa8W?8Z#adjin`T|36p4aV=&jqDx271TEiR~>}ke{D`- zqyRDOeHyRPj;BAqRfs=r@fVwJy1kekXcJlFN?rg6DA%^5pfXMQw{$W}9mOGal&U+6 zWW=q`mae*F;+-6BY370FSwx&w_kcszSP=H00bEQ|v%+VJ*tq7EWJ`!uG8Z4w$oF~bX8JWf}{D-hVo zRw)SzIb)4Y{OOPGw&vObkBI{H=#K}C3GC&#b5kWJYn~J{y60Rq+YM<5H~RwpA9JtP9UmnBzwJ%ncadU)*L^OHn`HF3V~(f?Vl~anK2KQlc;O&k`Y!?oy@t zWO8q9WWvH4h6|Fmz^z(p>IKVJ!sWp88`x@^`L4}{REaj!?srYAvu!~>avu>I_N&5= z4z*cvlJ~z_b*0lsYO|rpi4prQZV#xpPp1OLN1qZnU(vsOwfzcO!kbBW?$lUqO_8b2 z%Eze}n7kOK0|9}n@VeCpY|2tRdUn#YS_|3<)z_dOxjOc-Nq==P7Lt|kJ9H&VUSdFd zGxfBR&9|Ru^Q#$}iJ=58FIlJxR9dFO&bmKMKiPiTghoAxi5WH;#XPPs&lPHCef$br z*2fAviMdhn6u0>M3~+LTD>>qSO>I8s7vTBS5|D4P6**+bvmDAZFHeb3F~vBE|C>{m zxh3&>JPg-FVZ?9y`l5@y-j3A9=9{*tU;To=oPyOMi$N7%@W&$vyW{DXWnRs%i8a4M zsvKMiV#iXXt=y7RaV9CLS7+EZNw6g7iQ|c{sMGR!RhrcoW={zLrF^iiuF}>=e}7Ms zF0MMZ6{(3b%@Gr==RL30HglGb`yGi}=Z8Y9vm{FYFIp>F<*n~$*}8zqbk&cpzxEde zU$Lh$lLt?<-0zTP^G`V>zkgr^gH?zu$i6=H_s$PU2{GTP-JcggN|o<__fDl-47R$> zH(xQgYfYpUNxa1$^35dCf(7Ds=KW%LEash_u0TCHej}2V1cqXR%NA$yWK%~UoQP#D z&*59);uF&sHB&Twnk^uX{LDTUw|%9}FM>9ur5P`1YEqGsRsAB}S`|qCseuGw7N&S5 z+%<>EmzJJNWQVF+cap*+=m_5vIr=R~Iq?0bD)=YR0I=oR0sV$p zT}Cq@7rRCpi?WYY!;B$J`BVaNf=4TL}hPVdiDb+F1i`ed(9_O?F~+}Y)@WcpParFBc!FvZMu@+ z3CRl{tJ&JFnJT+rube6yrP9+Wo6ENE0;%t-EvQyRM39H~^GA}uTRA9;8~wY5a&pGM zO}a3j0I%rY`%^=dszZ;=KON|P6eedx=FXvbZ49E)r!~p@9Kx$TRQ4r9e2JiZKAQZ? zt#C0fR50yR@r3K0-IUa<<_Ac_dB}=2$6lkMEWXBLvim*7cs4u17p`oe$Z!0Pxl)6Q zr(jX)KhGE?ylf!WQy4wJS5V*4yI_N9Rvu-H7SS1eSzs!kxFa;|KMGuR80(3zmC-k9gXg9^erU*|ELK6YSj@$x81O*&n z2C-p?@bJ35;Cy84tV9GEK)FwRK3&_djEB+~1d^&EKmxF(d=qr(WmvM>4gj5xq17qp!)0BATilPz}Yy_xA_+d}oAxeCiYfjt9EC z{tT$exL#vJ1PD3^4FAZXo{(2{bH*!y_Xg3fm7K0vGKul2fOya1?w9g9@>1V@o@%VW z_wo06Ur=_abm1ftNYLu5wq!V-OC(7-IM+oh3~DZUCl8 z-WsBw7&L?;L%oHFS9ddJ%RT!-W?!caOqJ!_T)Va{7Odbg`6J9o>Of>WGeWS#GE-F*kLSl)C0U;xEZ&Z!a$y?KX61LB&WyM3XQRXDK3( z#5V3CN)8+|KxJn%&zbQvwu|w+B=so?GL~uVgZB^q{q_1t!Yz3XSvnx0XK|x?QF1?` zq3&g)jT7%0>ao+qKS8R$4;MWMP~fA(xd1uGAxpJo8JQ4$AA>vr1J>$=Fqq7DS{hh& z*0!apzx2^BnlennyyF)7uJYZ}mAVs732btG?-qvIqLAbaw*&*}0ou)QaN&QmiQ_$U zSIH@3qx-U8Z&-tmo`RMu{By;`hpEfflV6MrYlL?3T$~{ze=uSPvZi!O(Q}VGDHSww z8$jW{VO>Mb2d;Po4Fq^hLUcJZd+*g0RzOCH?FU12BA`i z2EuFZ-hQpsFW3c*%cbu;+Bz@#Jf2P?0i4&gH?P38b$f9}jx4_t9opH)%gf385lR%B z!8^maDo{V2YqAn_LnV*8Ta)byujsUNh;++EI*z2?h>abr1d}Z@k$m>*$D$ z-USmGsvscF=mkzgR~?3vA}(}jm4Ffm5H@fRVP*E)Ks3R>c4`DTMJISaP(ue|*B~He zdLJ4b!_Xv>Lilt{^tTX%Y+@A6;%YneqLB3Ov_5i}B$M;2PC*Q#6x`yX^M7$Sqqz-f z)1(_-hCCw3yd$ZGBk{EW@O8s?R}AH?6Je?z@4d`LD8=4O84Moz)ALE8R^hVaYie$mn+JIMB50CFIb3(Hc;zAWL0cZ&KCoPIs zwd@zDyswK0Nyox%y}<{PO)0UT;owOvl#lPj5_jzWDlnu3-9~-Y9mx$=8qBK&kMM z_wpiUPUQQb5<9>c@*5r3L1h-WG-?QwxWfI@{X@^eEOVtjk;0K9?aosa%Z0{Qzr0qc z#CI>@F(LzKNQr5}Ls0s?}K)kB`uAFc)B1|mTBBLRe>$(vo<#?W)Cq;}U1Oc3Riwa#$hd3a1>t8O?txO- zFQ05aj;(a)mm4uFy64n;sRgx59%E0DsBwe^gAg}K;Lo%n>7OX5|G;&k* z0KAxG#H0O#(wPcCBkgS5QRt~^iB!{x(gpF8<|6C)lvZH<=* zY1e0HQLmX(#63f+(7=FHg@B;-S4}G`DUk)HDouAJ-I5xiENZj92p13vxym;PK?jlP z0J|sNiSJ_+ArsF)hAgMWBWTiL@4OE#ZcsJ`#_#)VoRG3C_gc5s`BIf5ux^uHdcp7yCoQ&oMmC_}9U2zlG+>z!P! z8*$wH9~_bhbmLc zRrP{-(dg71=q&a3(TT4=Z|tA#O0YqovTyQ(MR5xgwn{rr>yAY<3wiPG8uCr8wYG`} zwu2PWlg1Er}1J5&%eILLyz_Q->9cVN$5rS66a>BdD>eJ z%eUCtd|sFrYjmv>jHhz>aGIMJjv^7Gr4)#f0YGMIhreMLC3faLQ8)2;(e+g4<%E#p z@Y_Ji06oZlUEcG`j0I-jrqXe`*RGCkN<`8ET49BK>K!>ks}vwW`B!>yBCAcSJ6*&G z9tT66Qv>r-E;u20WTf#sRJ{~Rw_5XoJ(JcoH3tA5`U7T>eP#Zt6fm-YcotJzyrD&o zh$vzSicmQz+3%JtHvT0|$XM$cTma(s7e$5$iP4TgntD}v$yB~^#N|^50P6Ay>AuhL zFc3#cNwIP89o8`}RUby}g5l1}M5p&b2>NPa6LN@wbkH{mw0$RZnhqTNs>SleaKc8G zU@(#GhojNbRzALH|G*W8Q#O~EUb68aJj}D(X^*sr%y=%JHzEN+?bBX+YpcO(lOmsgih+NKsN^4lA(NsW3De9_M zb2j1<)c&c0QsEGKsu!-*IRF8OWJbiO`2Psn4hZ;xFbSQ$9J<>T`!?Na-B_`P*B+SD zA>SL*-pfB7(4|*BSF4^+xeI5NzjBBega-F^YK7-kN1gB62r}xfR4-<)<)qp1ftX_I zeNs%m_>=Yy_%~KVMU;BQ@sNH0-_KmzP_~Zdk@K>8e*$d&Gn`dVxhy;~6&KgP1{0TX z$hY2JbZ8d7HSsmJYt0Lm5P_$DNy*99K3}`S_6jVS0s&mH&+MXCN2RpUzp(zWg9ePx zg#O^S6J>gMNcj99VAqH3=+UKDmEA9^O3vN@1=`slP7kW#B)Va zXg3G1(xx2uhe~>BTDjQ_RAR(`9y}+$|3RN$q+KrhwS8Tg9qaw1@Ow`p-O~Q?d
zeXZHHaDuQrUZnrnM=NKq3=&j`w zP)?0m_}JQ69+`M#JFINbk3t&qhKB+P(I%r-3Mc@VB*k>vZpCDNhMU|IO?>*YA!D^f z^(Se{AdY2IB^c08-DqVO9!t?%C+L>cME9|$lMEntq=h-*dE|5bF1RH&Wl&lw$gbS` zMbEGRDg3|%9D3`LB^U=d{1`#m-!v7cuTa@C3=8k%p~l8n)Ou8bPXqBcD~~81u6w2` z*2c6B#h_AJj=xwzYg<>5n>yf7t`poBlk1pOCa>y+p3icKls1FgVgL zu_zP$2z{Z+@VDVNi2!l3rI|n!qUeTJbAS%8BoZxpMGQ zPVJ9u$yflXsOv~nEj)PeT1!yyjw+Yk#gW)f_ddP3g^C_m7xjHyHt5>ngJP_%vC>LX zM)cLPvC?exLsb@kk|~!`Pt3%o!E2~t8Z+m$X`Tzg=EM5f)(Z;Wc0HVx3@8ZeT`Wdq zjoO**ujBqS(tb36P`L5>hlLSyizTs0q5uGtMRP$~<{~R z;rHY>-F_fW6L;n5%z#rbHAy-Xhl}RmDEIwyz!&&Rg(D|EZwkTnF-3u!U==Nvub)u*U(J2_b5tH zZ#Xc?i5Q89>CNQn*l}I5Z?K_m3t)5BAD;T#fBmMfJYAKxAWLwhN3hhTdxTMMvl0FK zorX+~_B^`gE^eoOiDT#Ks0mAE5d*D8d`NY~TIow#NSbY{1t6_4&m=(9gT0~mitpd8 z($S4F$nvb;!btmL9puXH6QWRtGLb`3EYxx)7E$5IWJRio{5=@Wx|!tk*|g{&MaXOq zHjh0J@`4A}SnNI^eUXPz(x6zgolFQy)H2pz0g4M59-nSXexbm8N(+|y_Z_1i2Jwb~ z%Dn1?k{t}atIT>#^i>V}u~;0htePZo!?7A11)#cLfRK0#-2*n7 zC=5pl6onMXOUTouUH}!Miz;-RnQq(s5VUGFt|~51#0d0FlG?|sYG$06Y~&r%jRe({ zeDPY%V2IB7h9{;dsc55IAo$9~6?_-0yeij`!{bm6f)w!4FM8LAn#d6@XM7IU#zs)i zUWTGrH4FqIf;ab}REo&vL>KYAK=_5vSi%Q^<-0tiyP2$!RzK8^CEDKb+88knXKxAC zQiT%ka93x)AyLzVdeeh8je*k5Os+XRzSWG$;>il~WsP1X1@kjXI{L-Wzk%qGp%L63 z1wPG+9hmVrIn}IMl=?Rj*8QOkK=G4BelI#<5pX$6!)Ns8c z>Dd2s#8gBwQs^TEJgv?ufFmM|9DR6K!H6C*fX!9N!5$|kzIa@Ez#S!M#&BDlGi3?W zSNuTE|-5)ZQd#a1!yQihmzO+YWrQ&1-0bwOU3PoosNpe(kiio zJB=iZGj4;f#(%nLrY@fMv9J-0zN^(IM(eeQ72<*ye&t57z5W8*6_-e07MiHY>100S z)KbSiu7RHDYl1Zd?$%R>@Kn$$X&;mO3SAiwI+9b|;I0GG^lKoWu4s|y;EoR#P(lwEE36>6 z-cT~e6P})KE85bnRDMNxt}x=b!4YczI7LCw1HV;4OSyg zKf25aHi{oYUqfj8XX-bfoshJceE0+e%i&W){9#2^x*AM{zHq*!s5N$S2>=CXuGXA# zP=yY5%~bs8jJuz`nME{0La;Bi=R1o3*|quZtC{CHm_I_sVR~bCVhyB*P*>ND|E@+d zpmI8&zjr~|fw4S;gDp0&H-zhNteu}8S9SxVMr81VGJN=9pHQ7pGT8^!#J2<#XoP%< z4Sm-scM)*K0?oKI@9G$H+~?_d>oG|n7=jk|(Ks8fp*+;zvGsfjoJoERW+>h{QW_zV zKeb1!y)Es$JjOfu|39_8WpErpv!**@w3wNhnOPPyTgVeb|$PHCA0PRM9U%Ac$42IP34v^~C5yldJ zzI%xaviP2z*|w(e9G8nzP)uWeeN>AZeW!qdi8$dbZOhB0Aw#U>pIHKwg@(Qmw`@!U zS4VEL`h(hnCadky+VRbMHd)k(i7{k~M2L<|{zc>PK!D3_`(S;OUC>{}HI5%-Y+O}C zP3re9-(6&2VsqFLADKbvc{3J*&=VYMnIg{Hg-1I3O2u3~yejft&Mmo6G+ghU9W6?J zpja6Zg(j%@W4rL(EuOv?{G+pvE{C^pVk0u6|G{)*>XNlcF;!IJBej2}`I1CcR^$FQ zR2;>~R3mt+j4Yh1DuKI_x+(X{nFS&mb3|#nQf=qZe=IpEva10<*9F#F9et5munX4o zFZ`3)yvkpcn2^=;CZm^=Ov)P8s)n0fCUtZ{#WAe9a_a48TGBnGy zu8?A_+`h2^sH1arnNIifI4>KlUEiNF+5zA z0Rz*hx`C~@PWmO_iZUt#4IS*tk!}0oJHVun38B5`SulZE+Wm5BvygICQ|~JAcZifdD2M9kSQ9PF<%K`8G$yEV@(0{f z?fVH>m2vWZC^mek!IhMwo;Ak8BG_dYIRrnxwq@Z|)Sh6afiF&tBp?{I{PB~pP-xXc zHC8z4vKue9m0xkU?>R^NV}H9Puh&@!fC34lUi}B0HqfNF{FiE-E8pEoRtrwz+%a}{l5VY?F<3wHj-V3#k8>}N-jH@Dgidt``bIYOtA(IzBZpDwtp(Q?{CF2gXq zqe2!(E2yU4-u(cPAA4j?)g6$MZ8g5rJ%t<_UnogrMbuYL37|7DTl{w|)~p7_K`t#hd-}aSEDK_o`qD>D&Q9f90*BH@5@Lke)!xM zLjfGhR;21;2XvBNToB{Z7i+d8HJ4hn6pjzyLln=y6&UxJR8vDoOB6KE&0QA9b*7XGYHZEiB%FV>cTWa!_hLY})2Bu?%rzLj=;Y=ji? zbT;SYGMI5jYXlD+IP|;`pmiH^q)n;FcDi6w@eOss&MW>~S}!HJ{Rk#0#-Mn{6KQ6u zV98_?BmkcTDpnUc(r8hIh_ce!+A_?SOs9{?_6W{5%F%7`-4c_{PbyLF-6w7G+^-?( zD2!ofXgTgYsR2oQ%2hE_ruTc@t02oM{<8%;Q-9>3)V7CB9}_P>5bh zi5t)xY8EOO6;6XwaHd75!kdF2Lg!}ObImz_H z6b%9=E4_Du;qo&h9S3tPsvE8M3uZ?pDOlnY2_$LsnPKyF^XFG3VoU8z-U7wu?yH@U z_DJdcm4-)X^|I0EO&j$PN$6pfujhl&Iips*1)G))pEs)ZQ-m|SZia!VokPzqk8^wk z;lMz==gd5Kl50{iBC0@=p1BHvF|5{uOqrICM}WEuO}NQmC##kDl~j&`#t}#!0uKV* z2v$kGGD$^BN@Ae8+mWJ<(A({$gIzotWdlJ$;_Q}wh06gvEmvCh5C>osO6OM5Ckqas z3pH#F#?}!Sl-}&v`ECa(5@*@lJ2Gm-t%6~{;R}rkfHMQGUETg^kw{v3UjGK0Gn5|z z!)yi)xX+5=A;!siXpUDqjjj zK72Ddp6QsJ%6r8e7h>f_p?_Hl?hNDI==3)7yNc*8!Ao!_mJqSF&o|jxRHh8c3q68z zXWWK4t$`_2;4758*)bAOr6RdIv9H7t!u}XNew?M8lLhL|BcTDT1%}S$n4P`C!$QDp zjZU+jF2B9MpfN!f|HcLkzjD<~K0tO_t&epP=Y-bvDf0;8hQsM8A% zqVu?Pu1x|20?MLfN@AiVy#VfKaKUJSB-(xLAFn#Qp%U4=Sb)RuIAyvRT(6tJex45s z)Icy<^@{R?%`M^5R7(m`N2z6lFUFK$5~SeB_7WB@10wM-K0^iKpW?ue*RjU3gzyW( z+0G1f3c#M>_F&G{-QLnAEm37}ViNwfJQRkkrRG%kIgAhQGdjjbkJ>cTCl@)S}2+VgJiGB6dm z%5%cGFL$V0djqk;D5~aYLro(B$K4O#-7hY$-11yZZX~mxFBd`mjEY@X_aob|M3iAW zTl)+6u?fg`HZ|ED)ZOhk;!-vJaYqysot-i0r}3nG=N83%o0IL1yu8IsIDODf@y9Ua z11rf-ZZEt~iaDV}#}kd1h|y z7)eA1DiV!4CVQ_0W?wsCsAv#s%JZNFBsv862-)L3O*if|^S(S_XviE+?K!`-b8P{b z?oSF`g_vqcD9CtdD0{Rk#W>ZOOLm>2J>QqH7SBmGwEVNBH)l(!^t?pEi^b=;_`5H( z7~0iaej9Og=?6eDAHWMBuSRu76wKv%NV3A}Ht7wvI~j`e`|y}K=6U+vc)U5)lYLeE zo@vi-OOxEYDgrJNSXT%UZOOw#7E~Ojb5e;Xwvm;tmu=WfC7r$V*EX}QzXbdSk!lhJ z&C%&YHe0c?RJTn-A`CgrevoK8<~I7P)!&MC-1?~AUeC&+6oBh_tiw}@|K#k<77jp^ z=owt)Sg8{tYP!~D0)?R~hi%6~+FZH%<`pFw#|P0z5&p_9OG6d{6C62L0{DX@h;U4U zIpYvrr;#I_IY{?0kENtZ5K?t#P59JFE7!|13oWG1PSu9v$Pd~d0co!$A9KEo0L=b~ z(+#IwS2l^_EkZD0bIg%f!gquU@#KjRsZ+Uk(T8FwTR)__crJk!Z@DrgQWL2Rj6^k{RP>xgT%C%yR3;@g=;#*oo^CdwTPYk)}L!o*oJBOWHKfiT+sgVw-8 zvEa_oD8gMVq+%E6t}zLTOiN5p#ZBclc;KIAM@|+hJBwVmRX(8oPgRX5u{!TPX9@1( zAZGnoKx8C5f}G`AHTWZU>_ED{`PF>AkmK|tQnkfl?t^{Tg^RSrpDsckqWcdYF8V{b zY2V!0U$AX)za%h=93_9dTFhg(k8)Sr14bP8cAE$u}}BF}ax+3?5! zpbSX%t`9kXswdRiUs>rf@DsAb6m|DU5Bw6cID3X^@XrlOrFJ*URvK#lcq*N~md&Y` zUs2ZtpVB!?r|&^^J4p$~2yMY%b|jDSIhVS`%vPxn)v*RlB9`Paj4Jp)A%`Ohf_I1A z&M2mQ{7-un;Gf*QrvVi>oSL+T;zKBlsC|aswEpVX3f$PcAJ#=K^%i#(nbGmA;etIy2 z@|_<50|&rO7^QLLOAR!f<2&}&Lhx~q2;fH1K=1GDqR@b){}P~E8i{Bp`mQn;61T8f zfg=McXU*b^EU17b?4#gp2C&N!OuGO^BO3|0$08z=%`u_$ zm8t~OYNT95cI;1N0gl4@O9~~eNcBPK9HFEvuEet`A4>zd*I4HK8v?6NO3ZPztXFh+ z8V7k#BA!Bv*qCjHR8^NuvY)LqxXVqYa4^@VY%81Of$17&HBsx&BPpL;O0}yK#Xlrq ztK-mfyH*-&uY)V?jpoiaH;(qQw;U~865a~*YdmNGXC~exCi?oei}}*g*3wkbni?O< z&2)pjSR+xt<)Oc))G;THe9h*VjJoxqSj^v~Zx1I54E^!M{nnSsu+`weq+h8y=|tnY za21nF-edzN^hIK@m`f<-v}UkASyav6Rq&KmMmqkQQ3Tp5c+YO}OjhIDDOWhmr(`af zh1FQu_zL^8C_pl1;)b)9rTCC;51HR%fBaR_27|womGD?X9TdG&oWk*C~msM(?X! z8bgUB!r^?j&SxR<&|PWM?~M_O9vC;RUnbS}Wy4od90$w7Ru{pNjh@$N-*GN>s?aq%Ce;}_plP~<*hQMrN!kahEY+P;YGAuJ$ zQl)371%p<1I13}VN4-s}e({uTZ1ic9W_7hW#IROrgZ!OUo%33Ga;|uCVkHk4(Ob*b6q=i?DPp#3 z?r}#5#LSWRvhXZ>13`*v2o)7B9Bvrxl7>uGpWNMgC1FO@@&dl^!Ua0Ep%$}d+g(5fXg8Ic)YT0A6jkO2C-#7K- z_&RPc1-b!voc?4A*(_<{59bnbR<2M6^aEt>X%8?y9C6Ov(iY&do#;_6N~l3 zxR*ei=K%FOSsULK#kk~#Tt$l@*SH5_ico;+=g$~vlL)kbU-A};O`Pge7xNWEhUU_H z^?^x0(Nn8gqun=^WCb5|+gcj2Q=8dHvQ!m-k3c65YY~_mr{9I}3e+;fYzZ?Ce`g6P^uMqB#FIC2^?>2$?ZKV?)Gx=Mh*1iFU^@i2SavfFc zT1tugwtcCT%o=TjfEn0gLkb#6qRVLApS;NkIE^`DJmtPvl62+Q)%?}2Pt;<_j07bc z8O5(?&cKzR@OT^%Lrw-Mrs_DWz3}!7q=`SAn{Mul7;OcDnCvVh;-+eB&5dOE{65Z( zt`yXkmd;6|(tl56tJ!cz^|D7tN0SK7&PRsYaCFi$5|i@5aBJ})vjgh7=yhO8 z@RJ>^JklVXkN00{2(9lKGxhpk3PoXFO(2dV)7Bt0dA`qCoWA%E29-~jp;ylqIFu*G z72`j<;YgTv6Uyggrchf(@ZD|xeZ6CXd_Fm0JNY)7hNv*E++aX{yv_;%)K}nkc6AUH zw(C?no=P5J#oWwY4xWHQfg~^Ie=|!WFndWM+k<`IXd}pggZ`tQ&v{QQm==zFByleo zo$Pb}arcbAnRQwrBF&BE=PO0Enywd|4e3o(1}OJj{f^-?kLJd_b51=N2_oBQ$FK<%Z2o2YQi z{QU~pwXo73b@H&TXuQgEJYZTVZ9#$p2~ocK>6j}};VVm0kPXV4sJ!m%vIX3c9{UV- zs`@aR9^xHokH2)k2sz472dHC7^~dzg^SD27BuZ>|!2UvuY9uN{lQ8 z`{408KqBh)jG>qX{MC%7Io9~%cDfwsQN*BPHLk!S)r3ZYbOEiKZa>eiLr3_%q?hZGe5IfFu!Fe?_OE@L0PhS~%I ze`qf*!|hvTEJnbnMFft= zQLDpLTVJLvBaQVNoO)rjDN1S+hY<&0#-MY4td;l0ck|itQ-iQ9_RL>Pe@>6OxCm@s z9I3DX%ERXp9tlJ%sOBN0>%p~^a{h;sK1vvuI{|Jd#=EIh+PK{i`nSArb$ncq)Q@>R zlTu02T#P}PjQqbcn$M}n$ zj^IH^9kO)qmY)FYP**I)dAET9eQCe$WIwKjcaXLR^7WIV^CPO$pd5!SN);^Sl2K

)!SWeLcmMqoZd-)-d~LGx;kq@ zGFw8h$*U1xw>?Z%+clpVO*v(wL;X9II$OK>6L|3Q?A%pk`MKMZga8`_*zjF1P*Y2= z6>Lu8LmW=4>1|EFdGdC}Rv&Q^kb-zdK{RDpmx#`F`sbwqi-(u-9Y_5fmbNEl|)Nd8}{PO%K3$`mZ>7Cel;cNFV%7P{Z0W}BD(N)K1-PuZe*o{>b*l)VAzfM`b53lih`6q zAC57gQrL8}Rb`qr)#j-!gcWXVz?U zRf@Y8u77@)6_olRB1Ty*|EqAfEgM3;cHPyFELx?psR3WcNW?8XckUR`!>{*ye8v_@ zbWc~5VZrk+eHI69VoPzN{h?OzmA1dkm78tI*kXN-ro#zV6zt(_7eww;r%@Je(56z- zU&|gEq{ynmgTzmI<)sJi8TnB%d#mBXRCWbCz*HgN0JKHfyci;~Y=fP;QzYT?Dt1>Q zPnjp4jdF>7B4i?g>bm^i7$l=Dw~r*gg6|wfOoX*pe^})m&a?L~dyBjc5v`8`Df9+{ zCKD1H{P^$Hle(DZ$jO|ik+MCjLTysJj?@erTFdo5e^Y;4=!g0zlIR37(k(nJBxg`D z@T(_9uylFJ#jcAUHSJ&o_uKb=&Tv&e+b}j=@+a(A2Cck_BkxaLJdb}c?pz4EUc;u2Kmj4FG41O7(n^8b!BW#Uo?@Z6+$19<1a(lp0j39I7&ith) zAlWa-I6|UTxb!TZ#aztn1})pw7{0X%)YyHtC}g;*#a&}9d!I%XU(q6&!|{;u&&L&P{3cpNkz-MAG8U^lQXsz{R3uO;8NBZV4? z2z5Rl)T>q;U$@ztQ70v8RdmyeCE$OvgDHofYi-CzAGBKAXh4*KqCa7I%vDU5z_N>S z^^?SQ=B&>T7jISpV!6wq4wl|6Yf(zmt*B0GOv8`P>8FV)FuUJ@dWS*<-mqr;zT^$J zeApyN&o&lRsg2dwE(9|2w2`H0{$7 zbH*>G&ZrFMhmy$J=wE> zHRb7QhJ^Jx{C7_qNh27FZlJRUolUi*t~5&-P7} zCTz=;uV;|t;89~9{Vd~!I#@lkxnaYCT_I8?NPDgd;saY5>UKK2=d9Y?=x&xC<0@|x zecJ(XUIk9)f@1fiz-tBFI5b>%vR_2$+;&Y32P8;;oDJ-Xe~%EqW^>|()~tv|BIhQj z;JwXiUMLb;<8qN{4@{;7NHuMZYw%J8!-O;B1`jKN;Vm@tG(n~L?wq`JmqOy2^W9gG~WVFHdSKyexr@4aO$*GQ{&H?}^FaH<;O!rwn zu-*^GU)V*xy1!*h6$K{35cJIs!?iRA(D>3d_YK5}CR$z-HG~@$l!Z8*7=~hVKkRJ( z+MvAs!w6_=bg8{_%X#Ll)K-@QB+Hdg1P3o+;PS3NZbvA1*gZ ze7)|G%6qsA0}?=WjYqpA=V5>=iWD74)_w9gMtQ4csE|k=0_qL!hm2MBC0Bs8GQ}hf zi&TDFLKmAM@O>PIqWJR?2_^N0MpFYCaLoqzt!U>AArm8)y9gv-toBxsl(Jm}hyFGT~#Hn}6l zR;gbkV8~Th2(bYNZI(bm23`= zhT%~g76hQ7Incj3l4eZ#3cjouQ)@oN-uPa|Otr)lHmI+6UqLQQ2V5-M{`u{T8&Vp) zc{L!Lf1C<+mYS+?`VO#=Sqmcv?RE7d&-ZKOaKLT1XhhVov1TV6lHzGtNM5IOsBBVb z%W^&}qO@44K{00=EkZCt8`CQca{rpK@gpnH*@<#(^y$F>rEH66_)#!GTw)Y%rM6C3 z_WK<>=!q_bs^TGbCHCG`V`6t6^6kJY>T53v87nCqu`6d%WmklcsO&F!i|%~Fv-4Pz z9eoovUW0B=s2(n@Vp>?cCZOv6<^far;>GGM@$z!x1m33p(m3{MzYiBd7a@B zm>4qGp163CQgD+jl?=^NP;j$01n>SNfAu_upWXusf5LqF=grk)p(n36DOaZvZgY~2ZDjvJ(nIjOdp&XzPJgD*(2{43uxDD~%oVGaG+Jt) z-*KQqk|ud;M`lOJw*3JbaK*L<`|(W$JDShj*l+??6#^Z@!!4pJ+%T*whkssPZwJkH ze%}G(vKb8Df^%pbodgPR92zqAOv=DPcR%9u8PU&&`)6@l-_8Hj<)6( zGHX}T_jiC}q50rSLX?bZb0F9rqE_oHPD83BKR3>Tyh88uHWNoSOEsx65xudd9InC7D3JsSH!~66lOEJhd1?Lrh~fAwEw86&+_bvvjC~b zm*{puoADD{;2DON1vaXs3CR6YgT`t*F7&p+ss|9ThyLzTWt%z7jJ-MmTSv3o>M~it&SK-WDWlbA%P_@8Mykj52wGF7`Hm?}plsG}jH&K`AW~4Fj1!3F#hoW} zSP0U}O*jqMLKz(9Xs_FZaa&64dJ6MUVbD&wyXrwanJp0FwC4`i%#9gyUKae#mq@i- ztN6MJ)teS>8?|g-c2_j%-&&23fuE6>QH-gxjztY4^)G)P(V!y7wv=+Fv6oxYOf$1L zkzh->Jf^#J&#*M%@>3`gdw4|-6fv;lQq8VLn5t*=-e$RBxTc)s&2i&bPl1yd&`H(_ z62}NjJuf#}`M{=as5OeOmJ_Qr4~ zYd1XmK?SFkmNg5~a505)vj{x{{C0%@)S7-`pBe}GEBY9&qm4yYC86=YK}Mx`T@;K17iJ)2K8~KAn?8Jr9QeepUZ39bpb;-w zP^x^|DX8~j8vmr!1NBZe>7kjjyf7S@nu%#cLgL*AaJavHenTxf8s9lPrSh7f2zuUX zHcH2|ioB}P!xIxZnnESQKQ2`ckEW+gg#hH96yJyl&v@x3|H*Hkt8T#cK^(N}Nzy)- zS?f6am-oqIgw#FMutzEpqH?l6NPg1h7!4~a#C}dSvs<4AK!4^b0F_mwJb>8TF$2nQ(hFU3* z?UWVZ(Tt3`K4iHqt%B7;CBZ5Dnvkl^)rH0*55nI(M?h#)0#L6{?p$NPITOmF8SO8i zHXt)e!f0JE{m0y^G-B^$JF6#m{unr?a)S`H3Ug)T*3LkRleRxM76nd$bJpOKrG{m$ z=rG7!Y?i*J5lx=$P~w)Hqmp86s@qVoDgI0;nFrRti%sLEVM1vp8Yums>}e|}ezR?P z)#E9c$|uLl>EZ;3tL95$X-<0Tlo8;qVm zon6lgQ3vUZbVM;NnehbefZzk0XyIoW2RVehluvg5ihBSv+fXrzdprLxbbLCd3Hu<9)#gu=k zaI7v6qj~P-yE}cOuO86;HQKCO=cDhDviDXBS_^iNj=p!?5!6H6J6@r?(AQOq0Hedv z?O8FZMYpU2Bg2UPv67hzoZZk5|_8tLig_Qjvznw(2NVfy8OVY{pV$w+>f z_35#dk?-9dmjCc~IJ+<@NIPBe`j2@@Gi)8trw#I*K2dPF;O9_ys-SXssS!J9of(KD z_xvC?JX15J^#kpy!mW&cHo%4hyP~&W;s-bQqkxP}B8S5G*Rc$OWJB)}$2l-2n-E%v zIt}Qz-d=s$%VGZ{+}X?1yMF9noQNKAF#cfLAY?gzRmD5|SQ{3K*Rk*OFJj9NAarqs}eQr%8bB zPS{gpr_bg`gSHrSC5#5$OI0c-4Dz<`!4MkqmmO^^5IL{*Qpa`tb>|6Kjw5??n8&pC z8{SYlUej;1B%oy)g6G8yX5JncU-1G~QU3fz|2KU*7IG2^IZBn&`mU!8sW*EH&zBxf-p*I5XdMvp?mxEfeXe! z^7jW?jd*dnG2;e}zZOoT!F8>7V+(WA=Eg;kq9#$lp;~J~6HO7=n#I<=^^7iLY$*$E zhZz%g^>bz|;gAT%QL-yqzL}^%6^q4(uKe_LeP&@AXxkr;4u96v1kG?Pra{3TJQHLG z7#Iqq+=A6Iq^3@X6s|igLRy|!;|~$Qz*82_lK@;GMieRx%X2$B08;vk{;bIpEQiy< zFR2gJ{p)JhBaR110zSluw>vhotwnV)Ql*~tp9xOSJH|NP@*$;QF!;Ep48s3%5h#rb z+_$?$F#ZPr%05AU~q`>YHtUeB8p@Bf@O6P}csi9{U~M0vRF zBGQm_JTesM9{Yd%eg^Jl^G>9EBwDD+PxE}HxKn>B!Re5UNs>Y|kdltG@(cPocCvXEhWQXdxJhjF zcrPeSw0nO2-v|Z(fK$xlVsKCblLXZ8I4xwA-M{$(Kt(-dBGr~*5#?O4w#}iY1ksm< z;ORlP>c1(fBNG{zH)X$8*vA7NFGI0EgzwGAN)~t`XL0F=X9MK=v2b{{u)p%YVTch- z<$Vba2CTFB15bw#{`2STo#KFhzHVAMqt?9{cHLQG>{CP_0uXB(+5Pau_HOOCL>kWf z7Iwe96%0wPj{l6__H)8ZOGjNF)DSr~o$z0rCEfw{|01(#=a<5FCqy`GQ!gm03V}8U zKkBxkgqchduU~0Ht*pBBw?zFmt{gqwXb+Uzl|eBI8jIuPhyE9aQ~sNqf$0rRIo3Zu z#B$FJ7N1)++p*W)z3ACuLFO+=1SZ~3htuXIw8n2;Z>A)%@SO6dUIGd?U#f&&y32ut zINCn2A8~~n+3^2N0{=ggsxizg{=N{}a+SNw9#HOiS@CNn-~i5C!kpNP?ekt6(Qw6E zZX?--?902KVnKo9qRJ>nYD38W)t4dqp^#6XLI|I$%aRH%21p~IDK`&w`R}>!!C4$~ z=s`MQLHyXqMhx)7{X)M=%JjtXaM=xCe5#e+>_YmL!#TXjYYS zrk$6nDx91c5{kO88{h(4;xh$n;(N^B5z7fMEUReTK|7oc#X1QKap#8 zUp8rQ_9?pNx<^4bCe*f=I`U$g}s>nxRC+7mfDFS396(y?0 HjDr6cgEi)# literal 0 HcmV?d00001 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()