diff --git a/PythonLinearNonlinearControl/configs/make_configs.py b/PythonLinearNonlinearControl/configs/make_configs.py index 7f07d20..fefcaab 100644 --- a/PythonLinearNonlinearControl/configs/make_configs.py +++ b/PythonLinearNonlinearControl/configs/make_configs.py @@ -1,5 +1,5 @@ from .first_order_lag import FirstOrderLagConfigModule -from .two_wheeled import TwoWheeledConfigModule +from .two_wheeled import TwoWheeledConfigModule, TwoWheeledExtendConfigModule from .cartpole import CartPoleConfigModule from .nonlinear_sample_system import NonlinearSampleSystemConfigModule, NonlinearSampleSystemExtendConfigModule @@ -12,6 +12,8 @@ def make_config(args): if args.env == "FirstOrderLag": return FirstOrderLagConfigModule() elif args.env == "TwoWheeledConst" or args.env == "TwoWheeledTrack": + if args.controller_type == "NMPCCGMRES": + return TwoWheeledExtendConfigModule() return TwoWheeledConfigModule() elif args.env == "CartPole": return CartPoleConfigModule() diff --git a/PythonLinearNonlinearControl/configs/nonlinear_sample_system.py b/PythonLinearNonlinearControl/configs/nonlinear_sample_system.py index 6d74d49..f4b9ff6 100644 --- a/PythonLinearNonlinearControl/configs/nonlinear_sample_system.py +++ b/PythonLinearNonlinearControl/configs/nonlinear_sample_system.py @@ -290,8 +290,6 @@ class NonlinearSampleSystemConfigModule(): class NonlinearSampleSystemExtendConfigModule(NonlinearSampleSystemConfigModule): - INPUT_SIZE = 1 # include dummy input - def __init__(self): super().__init__() self.opt_config = { diff --git a/PythonLinearNonlinearControl/configs/two_wheeled.py b/PythonLinearNonlinearControl/configs/two_wheeled.py index b293318..536e6b6 100644 --- a/PythonLinearNonlinearControl/configs/two_wheeled.py +++ b/PythonLinearNonlinearControl/configs/two_wheeled.py @@ -22,17 +22,16 @@ class TwoWheeledConfigModule(): Q = np.diag([1., 1., 0.01]) Sf = np.diag([5., 5., 1.]) """ - # for track goal """ + # for track goal R = np.diag([0.01, 0.01]) Q = np.diag([2.5, 2.5, 0.01]) Sf = np.diag([2.5, 2.5, 0.01]) """ # for track goal to NMPC - R = np.diag([0.1, 0.1]) - Q = np.diag([0.1, 0.1, 0.1]) - Sf = np.diag([0.25, 0.25, 0.25]) - + R = np.diag([1., 1.]) + Q = np.diag([0.001, 0.001, 0.001]) + Sf = np.diag([1., 1., 0.001]) # bounds INPUT_LOWER_BOUND = np.array([-1.5, -3.14]) INPUT_UPPER_BOUND = np.array([1.5, 3.14]) @@ -326,3 +325,83 @@ class TwoWheeledConfigModule(): return lam_dot else: raise NotImplementedError + + +class TwoWheeledExtendConfigModule(TwoWheeledConfigModule): + PRED_LEN = 20 + + def __init__(self): + super().__init__() + self.opt_config = { + "NMPCCGMRES": { + "threshold": 1e-3, + "zeta": 5., + "delta": 0.01, + "alpha": 0.5, + "tf": 1., + "constraint": True + }, + "NMPCNewton": { + "threshold": 1e-3, + "max_iteration": 500, + "learning_rate": 1e-3 + } + } + + @staticmethod + def gradient_hamiltonian_input_with_constraint(x, lam, u, g_x, dummy_u, raw): + """ + + Args: + x (numpy.ndarray): shape(pred_len+1, state_size) + lam (numpy.ndarray): shape(pred_len, state_size) + u (numpy.ndarray): shape(pred_len, input_size) + g_xs (numpy.ndarray): shape(pred_len, state_size) + dummy_u (numpy.ndarray): shape(pred_len, input_size) + raw (numpy.ndarray): shape(pred_len, input_size), Lagrangian for constraints + + Returns: + F (numpy.ndarray), shape(pred_len, 3) + """ + if len(x.shape) == 1: + vanilla_F = np.zeros(2) + extend_F = np.zeros(2) # 1 is the same as input size + extend_C = np.zeros(2) + + vanilla_F[0] = u[0] + lam[0] * \ + np.cos(x[2]) + lam[1] * np.sin(x[2]) + 2. * raw[0] * u[0] + vanilla_F[1] = u[1] + lam[2] + 2 * raw[1] * u[1] + + extend_F[0] = -0.01 + 2. * raw[0] * dummy_u[0] + extend_F[1] = -0.01 + 2. * raw[1] * dummy_u[1] + + extend_C[0] = u[0]**2 + dummy_u[0]**2 - \ + TwoWheeledConfigModule.INPUT_LOWER_BOUND[0]**2 + extend_C[1] = u[1]**2 + dummy_u[1]**2 - \ + TwoWheeledConfigModule.INPUT_LOWER_BOUND[1]**2 + + F = np.concatenate([vanilla_F, extend_F, extend_C]) + + elif len(x.shape) == 2: + pred_len, _ = u.shape + vanilla_F = np.zeros((pred_len, 2)) + extend_F = np.zeros((pred_len, 2)) # 1 is the same as input size + extend_C = np.zeros((pred_len, 2)) + + for i in range(pred_len): + vanilla_F[i, 0] = u[i, 0] + lam[i, 0] * \ + np.cos(x[i, 2]) + lam[i, 1] * \ + np.sin(x[i, 2]) + 2. * raw[i, 0] * u[i, 0] + vanilla_F[i, 1] = u[i, 1] + lam[i, 2] + 2 * raw[i, 1] * u[i, 1] + + extend_F[i, 0] = -0.01 + 2. * raw[i, 0] * dummy_u[i, 0] + extend_F[i, 1] = -0.01 + 2. * raw[i, 1] * dummy_u[i, 1] + + extend_C[i, 0] = u[i, 0]**2 + dummy_u[i, 0]**2 - \ + TwoWheeledConfigModule.INPUT_LOWER_BOUND[0]**2 + extend_C[i, 1] = u[i, 1]**2 + dummy_u[i, 1]**2 - \ + TwoWheeledConfigModule.INPUT_LOWER_BOUND[1]**2 + + F = np.concatenate([vanilla_F, extend_F, extend_C], axis=1) + + return F diff --git a/PythonLinearNonlinearControl/envs/two_wheeled.py b/PythonLinearNonlinearControl/envs/two_wheeled.py index 5a8cb69..07c87a1 100644 --- a/PythonLinearNonlinearControl/envs/two_wheeled.py +++ b/PythonLinearNonlinearControl/envs/two_wheeled.py @@ -59,7 +59,7 @@ class TwoWheeledConstEnv(Env): self.step_count = 0 noise = np.clip(np.random.randn(3), -0.1, 0.1) - noise *= 0.01 + noise *= 0.1 self.curr_x = np.zeros(self.config["state_size"]) + noise if init_x is not None: diff --git a/PythonLinearNonlinearControl/models/two_wheeled.py b/PythonLinearNonlinearControl/models/two_wheeled.py index 0a89860..2f1e0a5 100644 --- a/PythonLinearNonlinearControl/models/two_wheeled.py +++ b/PythonLinearNonlinearControl/models/two_wheeled.py @@ -56,6 +56,20 @@ class TwoWheeledModel(Model): return next_x + def x_dot(self, curr_x, u): + """ compute x dot + Args: + curr_x (numpy.ndarray): current state, shape(state_size, ) + u (numpy.ndarray): input, shape(input_size, ) + Returns: + x_dot (numpy.ndarray): next state, shape(state_size, ) + """ + 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]) + return x_dot.flatten() + def predict_adjoint_state(self, lam, x, u, g_x=None, t=None): """ predict adjoint states diff --git a/scripts/simple_run.py b/scripts/simple_run.py index 29b89f2..8d12a0f 100644 --- a/scripts/simple_run.py +++ b/scripts/simple_run.py @@ -41,7 +41,7 @@ def main(): parser = argparse.ArgumentParser() parser.add_argument("--controller_type", type=str, default="NMPCCGMRES") - parser.add_argument("--env", type=str, default="NonlinearSample") + parser.add_argument("--env", type=str, default="TwoWheeledConst") parser.add_argument("--save_anim", type=bool_flag, default=0) parser.add_argument("--result_dir", type=str, default="./result")