diff --git a/benchmarking_sim/cartpole/config_overrides/linear_mpsc_cartpole_stab_100.yaml b/benchmarking_sim/cartpole/config_overrides/linear_mpsc_cartpole_stab_100.yaml index 9485bf733..ed2df1ded 100644 --- a/benchmarking_sim/cartpole/config_overrides/linear_mpsc_cartpole_stab_100.yaml +++ b/benchmarking_sim/cartpole/config_overrides/linear_mpsc_cartpole_stab_100.yaml @@ -1,9 +1,9 @@ safety_filter: linear_mpsc sf_config: # LQR controller parameters - r_lin: + r_mpc: - 0.1 - q_lin: + q_mpc: - 1 - 0.1 - 1 diff --git a/examples/hpo/cartpole/config_overrides/linear_mpsc_cartpole_stab_100.yaml b/examples/hpo/cartpole/config_overrides/linear_mpsc_cartpole_stab_100.yaml index 9485bf733..ed2df1ded 100644 --- a/examples/hpo/cartpole/config_overrides/linear_mpsc_cartpole_stab_100.yaml +++ b/examples/hpo/cartpole/config_overrides/linear_mpsc_cartpole_stab_100.yaml @@ -1,9 +1,9 @@ safety_filter: linear_mpsc sf_config: # LQR controller parameters - r_lin: + r_mpc: - 0.1 - q_lin: + q_mpc: - 1 - 0.1 - 1 diff --git a/examples/mpsc/config_overrides/cartpole/linear_mpsc_cartpole.yaml b/examples/mpsc/config_overrides/cartpole/linear_mpsc_cartpole.yaml index 7a538c0ec..0bc1572d7 100644 --- a/examples/mpsc/config_overrides/cartpole/linear_mpsc_cartpole.yaml +++ b/examples/mpsc/config_overrides/cartpole/linear_mpsc_cartpole.yaml @@ -1,9 +1,9 @@ safety_filter: linear_mpsc sf_config: # LQR controller parameters - r_lin: + r_mpc: - 0.1 - q_lin: + q_mpc: - 1 - 1 - 1 diff --git a/examples/mpsc/config_overrides/cartpole/lqr_cartpole.yaml b/examples/mpsc/config_overrides/cartpole/lqr_cartpole.yaml index f3dcfbd44..10e11a05e 100644 --- a/examples/mpsc/config_overrides/cartpole/lqr_cartpole.yaml +++ b/examples/mpsc/config_overrides/cartpole/lqr_cartpole.yaml @@ -1,9 +1,9 @@ algo: lqr algo_config: # Cost parameters - r_lin: + r_mpc: - 0.1 - q_lin: + q_mpc: - 1 - 1 - 1 diff --git a/examples/mpsc/config_overrides/quadrotor_2D/linear_mpsc_quadrotor_2D.yaml b/examples/mpsc/config_overrides/quadrotor_2D/linear_mpsc_quadrotor_2D.yaml index 476468a46..a98c88970 100644 --- a/examples/mpsc/config_overrides/quadrotor_2D/linear_mpsc_quadrotor_2D.yaml +++ b/examples/mpsc/config_overrides/quadrotor_2D/linear_mpsc_quadrotor_2D.yaml @@ -1,9 +1,9 @@ safety_filter: linear_mpsc sf_config: # LQR controller parameters - r_lin: + r_mpc: - 0.1 - q_lin: + q_mpc: - 1 - 1 - 1 diff --git a/examples/mpsc/config_overrides/quadrotor_2D/lqr_quadrotor_2D.yaml b/examples/mpsc/config_overrides/quadrotor_2D/lqr_quadrotor_2D.yaml index 3ff048ecf..bb776b073 100644 --- a/examples/mpsc/config_overrides/quadrotor_2D/lqr_quadrotor_2D.yaml +++ b/examples/mpsc/config_overrides/quadrotor_2D/lqr_quadrotor_2D.yaml @@ -1,9 +1,9 @@ algo: lqr algo_config: # Cost parameters - r_lin: + r_mpc: - 0.1 - q_lin: + q_mpc: - 1 - 1 - 1 diff --git a/examples/mpsc/config_overrides/quadrotor_3D/linear_mpsc_quadrotor_3D.yaml b/examples/mpsc/config_overrides/quadrotor_3D/linear_mpsc_quadrotor_3D.yaml index 1d0a2b8fa..de25250fd 100644 --- a/examples/mpsc/config_overrides/quadrotor_3D/linear_mpsc_quadrotor_3D.yaml +++ b/examples/mpsc/config_overrides/quadrotor_3D/linear_mpsc_quadrotor_3D.yaml @@ -1,9 +1,9 @@ safety_filter: linear_mpsc sf_config: # LQR controller parameters - r_lin: + r_mpc: - 0.1 - q_lin: + q_mpc: - 0.01 - 0.01 - 0.01 diff --git a/safe_control_gym/safety_filters/mpsc/linear_mpsc.py b/safe_control_gym/safety_filters/mpsc/linear_mpsc.py index 6dcb7c597..2d9536744 100644 --- a/safe_control_gym/safety_filters/mpsc/linear_mpsc.py +++ b/safe_control_gym/safety_filters/mpsc/linear_mpsc.py @@ -31,8 +31,8 @@ class LINEAR_MPSC(MPSC): def __init__(self, env_func, horizon: int = 10, - q_lin: list = None, - r_lin: list = None, + q_mpc: list = None, + r_mpc: list = None, integration_algo: str = 'rk4', n_samples: int = 600, n_samples_terminal_set: int = 100, @@ -51,7 +51,7 @@ def __init__(self, Args: env_func (partial gym.Env): Environment for the task. horizon (int): The MPC horizon. - q_lin, r_lin (list): Q and R gain matrices for linear controller. + q_mpc, r_mpc (list): Q and R gain matrices for linear controller. integration_algo (str): The algorithm used for integrating the dynamics, either 'LTI', 'rk4', 'rk', or 'cvodes'. n_samples (int): Number of samples to create W set. @@ -71,7 +71,7 @@ def __init__(self, if key != 'self' and key != 'kwargs' and '__' not in key: self.__dict__[key] = value - super().__init__(env_func, horizon, q_lin, r_lin, integration_algo, warmstart, additional_constraints, use_terminal_set, cost_function, mpsc_cost_horizon, decay_factor, **kwargs) + super().__init__(env_func, horizon, q_mpc, r_mpc, integration_algo, warmstart, additional_constraints, use_terminal_set, cost_function, mpsc_cost_horizon, decay_factor, **kwargs) self.terminal_set_verts = None diff --git a/safe_control_gym/safety_filters/mpsc/mpsc.py b/safe_control_gym/safety_filters/mpsc/mpsc.py index 5f15f110d..e9a8dd258 100644 --- a/safe_control_gym/safety_filters/mpsc/mpsc.py +++ b/safe_control_gym/safety_filters/mpsc/mpsc.py @@ -27,8 +27,8 @@ class MPSC(BaseSafetyFilter, ABC): def __init__(self, env_func, horizon: int = 10, - q_lin: list = None, - r_lin: list = None, + q_mpc: list = None, + r_mpc: list = None, integration_algo: str = 'rk4', warmstart: bool = True, additional_constraints: list = None, @@ -44,7 +44,7 @@ def __init__(self, Args: env_func (partial BenchmarkEnv): Environment for the task. horizon (int): The MPC horizon. - q_lin, r_lin (list): Q and R gain matrices for linear controller. + q_mpc, r_mpc (list): Q and R gain matrices for linear controller. integration_algo (str): The algorithm used for integrating the dynamics, either 'LTI', 'rk4', 'rk', or 'cvodes'. warmstart (bool): If the previous MPC soln should be used to warmstart the next mpc step. @@ -74,8 +74,8 @@ def __init__(self, # Setup attributes. self.reset() self.dt = self.model.dt - self.Q = get_cost_weight_matrix(q_lin, self.model.nx) - self.R = get_cost_weight_matrix(r_lin, self.model.nu) + self.Q = get_cost_weight_matrix(q_mpc, self.model.nx) + self.R = get_cost_weight_matrix(r_mpc, self.model.nu) self.X_EQ = np.zeros(self.model.nx) self.U_EQ = self.model.U_EQ @@ -244,7 +244,7 @@ def solve_acados_optimization(self, try: action = ocp_solver.solve_for_x0(x0_bar=obs) self.cost_prev = ocp_solver.get_cost() - self.slack_prev = np.zeros((self.horizon, self.p)) + self.slack_prev = np.zeros((self.horizon, self.model.nx + self.model.nu)) x_val = np.zeros((self.horizon + 1, self.model.nx)) u_val = np.zeros((self.horizon, self.model.nu)) for i in range(self.horizon): diff --git a/safe_control_gym/safety_filters/mpsc/mpsc.yaml b/safe_control_gym/safety_filters/mpsc/mpsc.yaml index 2ac98fa53..9640712bd 100644 --- a/safe_control_gym/safety_filters/mpsc/mpsc.yaml +++ b/safe_control_gym/safety_filters/mpsc/mpsc.yaml @@ -1,7 +1,7 @@ # LQR controller parameters -r_lin: +r_mpc: - 1. -q_lin: +q_mpc: - 1. # MPC Parameters diff --git a/safe_control_gym/safety_filters/mpsc/nl_mpsc.py b/safe_control_gym/safety_filters/mpsc/nl_mpsc.py index 30b8d6eee..c82025d71 100644 --- a/safe_control_gym/safety_filters/mpsc/nl_mpsc.py +++ b/safe_control_gym/safety_filters/mpsc/nl_mpsc.py @@ -35,8 +35,8 @@ class NL_MPSC(MPSC): def __init__(self, env_func, horizon: int = 10, - q_lin: list = None, - r_lin: list = None, + q_mpc: list = None, + r_mpc: list = None, integration_algo: str = 'rk4', warmstart: bool = True, additional_constraints: list = None, @@ -66,7 +66,7 @@ def __init__(self, ''' self.model_bias = None - super().__init__(env_func, horizon, q_lin, r_lin, integration_algo, warmstart, additional_constraints, use_terminal_set, cost_function, mpsc_cost_horizon, decay_factor, **kwargs) + super().__init__(env_func, horizon, q_mpc, r_mpc, integration_algo, warmstart, additional_constraints, use_terminal_set, cost_function, mpsc_cost_horizon, decay_factor, **kwargs) self.n_samples = n_samples self.soften_constraints = soften_constraints