From 68fd07829daa8e7ab6db56df57759a4b4e0ea89a Mon Sep 17 00:00:00 2001 From: Federico-PizarroBejarano <federico.pizarrobejarano@mail.utoronto.ca> Date: Fri, 1 Nov 2024 13:45:32 -0400 Subject: [PATCH] Temp commit --- .../mpsc_acados_quadrotor_2D_attitude.yaml | 32 ++ .../linear_mpsc_quadrotor_2D_attitude.pkl | Bin 0 -> 447 bytes .../nl_mpsc_quadrotor_2D_attitude.pkl | Bin 0 -> 1872 bytes experiments/mpsc/mpsc_experiment.py | 6 +- experiments/mpsc/mpsc_experiment.sh | 4 +- experiments/mpsc/train_model.sh | 4 +- .../envs/gym_pybullet_drones/base_aviary.py | 128 ++++---- .../envs/gym_pybullet_drones/quadrotor.py | 53 ++-- safe_control_gym/safety_filters/__init__.py | 4 + safe_control_gym/safety_filters/mpsc/mpsc.py | 2 +- .../safety_filters/mpsc/mpsc_acados.py | 276 ++++++++++++++++++ .../safety_filters/mpsc/mpsc_utils.py | 42 +++ .../safety_filters/mpsc/nl_mpsc.py | 52 +--- 13 files changed, 457 insertions(+), 146 deletions(-) create mode 100644 experiments/mpsc/config_overrides/mpsc_acados_quadrotor_2D_attitude.yaml create mode 100644 experiments/mpsc/models/mpsc_parameters/linear_mpsc_quadrotor_2D_attitude.pkl create mode 100644 experiments/mpsc/models/mpsc_parameters/nl_mpsc_quadrotor_2D_attitude.pkl create mode 100644 safe_control_gym/safety_filters/mpsc/mpsc_acados.py diff --git a/experiments/mpsc/config_overrides/mpsc_acados_quadrotor_2D_attitude.yaml b/experiments/mpsc/config_overrides/mpsc_acados_quadrotor_2D_attitude.yaml new file mode 100644 index 000000000..1152caa0c --- /dev/null +++ b/experiments/mpsc/config_overrides/mpsc_acados_quadrotor_2D_attitude.yaml @@ -0,0 +1,32 @@ +safety_filter: nl_mpsc +sf_config: + # LQR controller parameters + q_mpc: [18, 0.1, 18, 0.5, 0.5, 0.0001] + r_mpc: [3., 3.] + + prior_info: + prior_prop: + beta_1: 18.11298 + beta_2: 3.6800 + beta_3: 0 + alpha_1: -140.8 + alpha_2: -13.4 + alpha_3: 124.8 + randomize_prior_prop: False + prior_prop_rand_info: null + + # MPC Parameters + use_acados: True + horizon: 25 + warmstart: True + integration_algo: rk4 + use_terminal_set: False + + # Cost function + cost_function: one_step_cost + mpsc_cost_horizon: 5 + decay_factor: 0.85 + + # Softening + soften_constraints: True + slack_cost: 0.1 diff --git a/experiments/mpsc/models/mpsc_parameters/linear_mpsc_quadrotor_2D_attitude.pkl b/experiments/mpsc/models/mpsc_parameters/linear_mpsc_quadrotor_2D_attitude.pkl new file mode 100644 index 0000000000000000000000000000000000000000..cb616ada5af767f92ed16874e7e4648107d9625a GIT binary patch literal 447 zcmZo*nYx9M0Ss!V^e_fY=@HE<%`K?ZOU^G!)ypl-DalMMDoU)J(!	l$xBMS6ot5 znp`qv@{}G{u)--l?0G2=C6lLkGqg@|W=xvWJ|$?1hBu=(n>SnAlnmA$)|8UUf>e+u zrZkHwogEN)<{n0yDSm!_UjKmrOn5VtOiAi=QUJPWYTDW83g4Fw&m1!y&sBbMQ2lYl zLtw%N2i+1sW`@+Y4nd;qZ42}T9q;Aq?@dZUsIxv}a%xigZwG~h!@YG+wm7so*DUIn z+3KLTs@nMRT1iL#z}o>E{v*tTsattvhT)a&9}WWAA+-Yj-yII_XWPcT^rFMz9ek&z zY*+=i7iJzz-PNL~d-GR)cQ~Um&+pv|p!&ZCGd}sPhPw}DFU&lcy7W1E(~}NcIA*?> i{FKX12<{J<`(XCM%!8>D@0a@9`sj_r6ktdd>j3~B?#PS) literal 0 HcmV?d00001 diff --git a/experiments/mpsc/models/mpsc_parameters/nl_mpsc_quadrotor_2D_attitude.pkl b/experiments/mpsc/models/mpsc_parameters/nl_mpsc_quadrotor_2D_attitude.pkl new file mode 100644 index 0000000000000000000000000000000000000000..0c5896aa8894ab242cf875e27343ce92643ab441 GIT binary patch literal 1872 zcmZuxYfuwc7$t@f2(bp0K|nOYR?(>pjzw_XunQ5<1i@B8oFZumk$}WtNCXrrDu!23 zM-~S~#l{v8%V;6iv1DB|g7{z@<RJ=EK+!4)K^|73lrG6$I!@Ui`^~-Ix#yhw-S6HE z>%u^~mHA6YJv20hR4x>wL2yHXQXl`LNkiKvl_*8%P9o>HM<q*4^%Tn$5>J^bK^Z4f zC`75K#wJNDN)Rc~0#rjYLx>ooOih$b&CJ^6i{@l!q2Hk%0;+}rp&=n5{7K?7{{&V_ zG@55?(eK7lHGvaYNo}Oe=}Fi;DbY|OP^q<sDO5<r@;ymPg-Wa>_@#6WeNT+VNCC0* zD9WQmqZT=ez(!!BMy1T005-+S%9v8V=xF~NxUG>>mVvh5`A_Fwpfc!~m-p|%*IJiy z*_7%V&hEAN$H+5`_QOrnxXc41gk1{q!uP<{JJnv??Dm8*aeg3Hr+7PgpZ6Hod9h2I zTF0mP04`VEK<M3r*F>(%AF*%3xl)IvQd<_b;abnr;jJ_-&;@(|R~b8Z$^QM5a9nBg z`?QuN-0KFb`9IltPx}M9fDhohK9mwZAHnixMfKv7?bG`J{(vsv12_XZPORe~C~*`T ziU5^5Nu8$(DRl*9oJE$A_rt)d#F*M32kyZVhG8trq<34GUo58niFYdteZp$$@I+~t zx?bnV<)%97IJ9EDecbL(H5aRJr|s_;lE5l_`}qi=slyTLtC-WJhP6;v<33%V;1zsU zyLZJ`!fM#!Vap62K0C^|PXAG1tO>r9#B&_Y>xR{VclWkOeT1~#;Vh=B8}`9;>_$V% zIJ~-u)l|jT!>37{x8~cPrpx{C!K96MpjQYMuFZQ?RW$--TT8ev>Em#~VE@<g;`{L7 zD-n(BAH0WhD-6CnM-c3Yv3xMfb^+paY3&vzy$>E!87f*HEaRSaP544x=#J%`ydPv- z=7zndIz+$zdjxVOadvNJ4@}&90ngQzO{_L}VIC7{kAxd0^#i28Tv8X>cixAw$b%cx z+q*F8nHLr{d&9o;KirUSvXXP1&>q;6+{b#--?>!=zW-TwEM{-eHrwP$y*G)gbREB2 zsjjn>(Jn@kpcDxc<YLrvp&qvRUTpmsX}M4uDsj`vMAx>#w&s^cGC@xgMvD}}U1XAX zXjs#k{R^3-B~K;ER1G$y!?l2rCKIIylgU(DRk5}6Ks33^S}crDn&D7cM3*shcp0~R z-V-fogh4Fo8a0nWOthjAPs9)m;>zmVp8U51Fo?>uW-}f{k-%gggP7FaXC8w{UE{lY zYjQ6PqPgv8u_BpFMEYkwS?Z%G8w9o5GF6WM-HXq9@(d>!7jFtj(hG1_2j-8Id*?rI z!~L<^x))={)J~|<mc3Y68Hy}_Twgxa{1OJZ1tmM@Ech}AiRY?*Exhf7U3GaA{?EGS z`t_S~RP}4OBL#v@tj8`xFyI5Y-NOyDulIx^!M$}JgAW;q@km(K=H>IT(v;TQ_Qy^` zKo{@<TxQX!P$q|k3~8(Toxg9@%d$Fz>G@Zu{Q+IT2XNGk*h4o$R7lJ5+O)-d#`Hdb zKcEZv08W0-T2kCMZ0Sj=V8%lJ_rS2BiCdSCoYKGhv|H{TZ-7iO3mw8j`|*x*YaO1^ z25>n>V{``BL4Iueii(hpdfm^3HjA%x;D=bZ0)|iY;u(CG5|_MOXoqa`hnm#mdVi-i zQ7w&Q_)4hKj=$^)Za!&-OitVlVRW2mrlz#9G^JO-Uul!eJxgz<=`Pr<$mHN#`*zq< lo}Txcm-*YBdE!C8QBGt)Nmb)b_T?)PVM~mDQ`9QYe*sPzZR`L5 literal 0 HcmV?d00001 diff --git a/experiments/mpsc/mpsc_experiment.py b/experiments/mpsc/mpsc_experiment.py index ff9d884ae..6edddf462 100644 --- a/experiments/mpsc/mpsc_experiment.py +++ b/experiments/mpsc/mpsc_experiment.py @@ -141,7 +141,7 @@ def run_multiple_models(plot, all_models): for model in all_models: print(model) - for i in range(25): + for i in range(1): X_GOAL, uncert_results, _, cert_results, _ = run(plot=plot, training=False, model=model) if i == 0: all_uncert_results, all_cert_results = uncert_results, cert_results @@ -168,5 +168,5 @@ def run_multiple_models(plot, all_models): if __name__ == '__main__': - # run(training=False) - run_multiple_models(plot=False, all_models=['True', 'False']) + # run(plot=True, training=False, model='ppo') + run_multiple_models(plot=True, all_models=['none', 'mpsf']) diff --git a/experiments/mpsc/mpsc_experiment.sh b/experiments/mpsc/mpsc_experiment.sh index eacad2068..678584020 100755 --- a/experiments/mpsc/mpsc_experiment.sh +++ b/experiments/mpsc/mpsc_experiment.sh @@ -5,7 +5,9 @@ TASK='tracking' ALGO='ppo' SAFETY_FILTER='nl_mpsc' -MPSC_COST='precomputed_cost' +# SAFETY_FILTER='mpsc_acados' +# MPSC_COST='precomputed_cost' +MPSC_COST='one_step_cost' MPSC_COST_HORIZON=25 DECAY_FACTOR=0.9 diff --git a/experiments/mpsc/train_model.sh b/experiments/mpsc/train_model.sh index 9febae2ef..7ea77edfe 100755 --- a/experiments/mpsc/train_model.sh +++ b/experiments/mpsc/train_model.sh @@ -10,9 +10,9 @@ FILTER=False SF_PEN=1 if [ "$FILTER" == 'True' ]; then - TAG=mpsf + TAG=mpsf4 else - TAG=none + TAG=none4 fi # Train the unsafe controller/agent. diff --git a/safe_control_gym/envs/gym_pybullet_drones/base_aviary.py b/safe_control_gym/envs/gym_pybullet_drones/base_aviary.py index 1d6af8781..802e8b4b0 100644 --- a/safe_control_gym/envs/gym_pybullet_drones/base_aviary.py +++ b/safe_control_gym/envs/gym_pybullet_drones/base_aviary.py @@ -14,11 +14,10 @@ from datetime import datetime from enum import Enum +import casadi as cs import numpy as np import pybullet as p import pybullet_data -import casadi as cs -from termcolor import colored from safe_control_gym.envs.benchmark_env import BenchmarkEnv from safe_control_gym.math_and_models.transformations import csRotXYZ, get_angularvelocity_rpy @@ -94,26 +93,26 @@ def __init__(self, self.RECORD = record # Load the drone properties from the .urdf file. self.MASS, \ - self.L, \ - self.THRUST2WEIGHT_RATIO, \ - self.J, \ - self.J_INV, \ - self.KF, \ - self.KM, \ - self.COLLISION_H, \ - self.COLLISION_R, \ - self.COLLISION_Z_OFFSET, \ - self.MAX_SPEED_KMH, \ - self.GND_EFF_COEFF, \ - self.PROP_RADIUS, \ - self.DRAG_COEFF, \ - self.DW_COEFF_1, \ - self.DW_COEFF_2, \ - self.DW_COEFF_3, \ - self.PWM2RPM_SCALE, \ - self.PWM2RPM_CONST, \ - self.MIN_PWM, \ - self.MAX_PWM = self._parse_urdf_parameters(self.URDF_PATH) + self.L, \ + self.THRUST2WEIGHT_RATIO, \ + self.J, \ + self.J_INV, \ + self.KF, \ + self.KM, \ + self.COLLISION_H, \ + self.COLLISION_R, \ + self.COLLISION_Z_OFFSET, \ + self.MAX_SPEED_KMH, \ + self.GND_EFF_COEFF, \ + self.PROP_RADIUS, \ + self.DRAG_COEFF, \ + self.DW_COEFF_1, \ + self.DW_COEFF_2, \ + self.DW_COEFF_3, \ + self.PWM2RPM_SCALE, \ + self.PWM2RPM_CONST, \ + self.MIN_PWM, \ + self.MAX_PWM = self._parse_urdf_parameters(self.URDF_PATH) self.GROUND_PLANE_Z = -0.05 if verbose: print( @@ -270,8 +269,6 @@ def _advance_simulation(self, clipped_action, disturbance_force=None): `_preprocess_action()` in each subclass. disturbance_force (ndarray, optional): Disturbance force, applied to all drones. ''' - time_before_stepping = time.time() - # clipped_action = np.reshape(clipped_action, (self.NUM_DRONES, 4)) clipped_action = np.expand_dims(clipped_action, axis=0) # Repeat for as many as the aggregate physics steps. @@ -280,20 +277,20 @@ def _advance_simulation(self, clipped_action, disturbance_force=None): # Between aggregate steps for certain types of update. if self.PYB_STEPS_PER_CTRL > 1 and self.PHYSICS in [ Physics.DYN, Physics.PYB_GND, Physics.PYB_DRAG, - Physics.PYB_DW, Physics.PYB_GND_DRAG_DW, Physics.RK4 #, Physics.DYN_2D + Physics.PYB_DW, Physics.PYB_GND_DRAG_DW, Physics.RK4 # , Physics.DYN_2D ]: self._update_and_store_kinematic_information() # Step the simulation using the desired physics update. for i in range(self.NUM_DRONES): - rpm = self._preprocess_control(clipped_action[i, :]) + executable_action = self._preprocess_control(clipped_action[i, :]) if self.PHYSICS == Physics.PYB: - self._physics(rpm, i) + self._physics(executable_action, i) elif self.PHYSICS == Physics.DYN: self._dynamics(clipped_action[i, :], i) elif self.PHYSICS == Physics.DYN_2D: - self._dynamics_2d(rpm, i) + self._dynamics_2d(executable_action, i) elif self.PHYSICS == Physics.DYN_SI: - self._dynamics_si(clipped_action[i, :], i, disturbance_force) + self._dynamics_si(executable_action, i, disturbance_force) elif self.PHYSICS == Physics.RK4: self._dynamics_rk4(clipped_action[i, :], i) elif self.PHYSICS == Physics.DYN_SI_3D: @@ -316,9 +313,9 @@ def _advance_simulation(self, clipped_action, disturbance_force=None): if disturbance_force is not None: pos = self._get_drone_state_vector(i)[:3] ''' - NOTE: applyExternalForce only works when explicitly + NOTE: applyExternalForce only works when explicitly stepping the simulation with p.stepSimulation(). - Therefore, + Therefore, ''' p.applyExternalForce( self.DRONE_IDS[i], @@ -484,7 +481,7 @@ def _ground_effect(self, rpm, nth_drone): ]) prop_heights = np.clip(prop_heights, self.GND_EFF_H_CLIP, np.inf) gnd_effects = np.array(rpm ** 2) * self.KF * self.GND_EFF_COEFF \ - * (self.PROP_RADIUS / (4 * prop_heights)) ** 2 + * (self.PROP_RADIUS / (4 * prop_heights)) ** 2 if np.abs(self.rpy[nth_drone, 0]) < np.pi / 2 and np.abs( self.rpy[nth_drone, 1]) < np.pi / 2: for i in range(4): @@ -642,7 +639,6 @@ def _dynamics_rk4(self, rpm, nth_drone): self.rpy_rates[nth_drone, :] = rpy_rates def setup_rk4_dynamics_expression(self): - nx, nu = 12, 4 gamma = self.KM / self.KF z = cs.MX.sym('z') z_dot = cs.MX.sym('z_dot') @@ -659,7 +655,7 @@ def setup_rk4_dynamics_expression(self): # PyBullet Euler angles use the SDFormat for rotation matrices. Rob = csRotXYZ(phi, theta, psi) # rotation matrix transforming a vector in the body frame to the world frame. - # Define state variables. + # Define state variables. X = cs.vertcat(x, x_dot, y, y_dot, z, z_dot, phi, theta, psi, p_body, q_body, r_body) # Define inputs. @@ -682,14 +678,14 @@ def setup_rk4_dynamics_expression(self): self.L / cs.sqrt(2.0) * (-f1 + f2 + f3 - f4), gamma * (-f1 + f2 - f3 + f4)) rate_dot = self.J_INV @ ( - Mb - (cs.skew(cs.vertcat(p_body, q_body, r_body)) @ self.J @ cs.vertcat(p_body, q_body, r_body))) + Mb - (cs.skew(cs.vertcat(p_body, q_body, r_body)) @ self.J @ cs.vertcat(p_body, q_body, r_body))) ang_dot = cs.blockcat([[1, cs.sin(phi) * cs.tan(theta), cs.cos(phi) * cs.tan(theta)], [0, cs.cos(phi), -cs.sin(phi)], [0, cs.sin(phi) / cs.cos(theta), cs.cos(phi) / cs.cos(theta)]]) @ cs.vertcat(p_body, q_body, r_body) X_dot = cs.vertcat(pos_dot[0], pos_ddot[0], pos_dot[1], pos_ddot[1], pos_dot[2], pos_ddot[2], ang_dot, rate_dot) - self.X_dot_fun = cs.Function("X_dot", [X, U], [X_dot]) + self.X_dot_fun = cs.Function('X_dot', [X, U], [X_dot]) self.fd_func = cs.integrator('fd', 'rk', {'x': X, 'p': U, 'ode': X_dot}, {'tf': self.PYB_TIMESTEP}) @@ -708,7 +704,6 @@ def _dynamics_2d(self, rpm, nth_drone): rpy = self.rpy[nth_drone, :] vel = self.vel[nth_drone, :] ang_v = self.ang_v[nth_drone, :] - rpy_rates = self.rpy_rates[nth_drone, :] # rotation = np.array(p.getMatrixFromQuaternion(quat)).reshape(3, 3) # Compute forces and torques. @@ -721,7 +716,7 @@ def _dynamics_2d(self, rpm, nth_drone): # update state with RK4 # next_state = self.fd_func(x0=state, p=input)['xf'].full()[:, 0] X_dot = self.X_dot_fun(state, action).full()[:, 0] - next_state = state + X_dot*self.PYB_TIMESTEP + next_state = state + X_dot * self.PYB_TIMESTEP # Updated information pos = np.array([next_state[0], 0, next_state[4]]) @@ -787,13 +782,13 @@ def setup_dynamics_2d_expression(self): self.L / cs.sqrt(2.0) * (-f1 + f2 + f3 - f4), gamma * (-f1 + f2 - f3 + f4)) rate_dot = self.J_INV @ ( - Mb - (cs.skew(cs.vertcat(p_body, q_body, r_body)) @ self.J @ cs.vertcat(p_body, q_body, r_body))) + Mb - (cs.skew(cs.vertcat(p_body, q_body, r_body)) @ self.J @ cs.vertcat(p_body, q_body, r_body))) ang_dot = (cs.blockcat([[1, cs.sin(phi) * cs.tan(theta), cs.cos(phi) * cs.tan(theta)], [0, cs.cos(phi), -cs.sin(phi)], [0, cs.sin(phi) / cs.cos(theta), cs.cos(phi) / cs.cos(theta)]]) @ cs.vertcat(p_body, q_body, r_body)) X_dot = cs.vertcat(pos_dot[0], pos_ddot[0], pos_dot[1], pos_ddot[1], pos_dot[2], pos_ddot[2], ang_dot, rate_dot) - self.X_dot_fun = cs.Function("X_dot", [X, U], [X_dot]) + self.X_dot_fun = cs.Function('X_dot', [X, U], [X_dot]) def _dynamics_si(self, action, nth_drone, disturbance_force=None): '''Explicit dynamics implementation from the identified model. @@ -810,7 +805,6 @@ def _dynamics_si(self, action, nth_drone, disturbance_force=None): # quat = self.quat[nth_drone, :] rpy = self.rpy[nth_drone, :] vel = self.vel[nth_drone, :] - ang_v = self.ang_v[nth_drone, :] rpy_rates = self.rpy_rates[nth_drone, :] # Compute forces and torques. @@ -820,7 +814,7 @@ def _dynamics_si(self, action, nth_drone, disturbance_force=None): # update state if disturbance_force is not None: d = np.array([disturbance_force[0], disturbance_force[2]]) - else: + else: d = np.array([0, 0]) # perform euler integration # next_state = state + self.PYB_TIMESTEP * self.X_dot_fun(state, action, d).full()[:, 0] @@ -829,7 +823,7 @@ def _dynamics_si(self, action, nth_drone, disturbance_force=None): k2 = self.X_dot_fun(state + 0.5 * self.PYB_TIMESTEP * k1, action, d).full()[:, 0] k3 = self.X_dot_fun(state + 0.5 * self.PYB_TIMESTEP * k2, action, d).full()[:, 0] k4 = self.X_dot_fun(state + self.PYB_TIMESTEP * k3, action, d).full()[:, 0] - next_state = state + (self.PYB_TIMESTEP / 6) * (k1 + 2*k2 + 2*k3 + k4) + next_state = state + (self.PYB_TIMESTEP / 6) * (k1 + 2 * k2 + 2 * k3 + k4) # Updated information pos = np.array([next_state[0], 0, next_state[2]]) @@ -853,19 +847,19 @@ def setup_dynamics_si_expression(self): theta_dot = cs.MX.sym('theta_dot') # Pitch X = cs.vertcat(x, x_dot, z, z_dot, theta, theta_dot) g = self.GRAVITY_ACC - d = cs.MX.sym('d', 2, 1) # disturbance force + d = cs.MX.sym('d', 2, 1) # disturbance force # Define inputs. - T = cs.MX.sym('T') # normlized thrust [N] + T = cs.MX.sym('T') # normlized thrust [N] P = cs.MX.sym('P') # desired pitch angle [rad] U = cs.vertcat(T, P) X_dot = cs.vertcat(x_dot, - (18.112984649321753 * T+ 3.6800) * cs.sin(theta) + -0.008 + d[0] / self.MASS, - z_dot, - (18.112984649321753 * T + 3.6800) * cs.cos(theta) - g + d[1] / self.MASS, - theta_dot, - -140.8 * theta - 13.4 * theta_dot + 124.8 * P) - self.X_dot_fun = cs.Function("X_dot", [X, U, d], [X_dot]) + (18.112984649321753 * T + 3.6800) * cs.sin(theta) + -0.008 + d[0] / self.MASS, + z_dot, + (18.112984649321753 * T + 3.6800) * cs.cos(theta) - g + d[1] / self.MASS, + theta_dot, + -140.8 * theta - 13.4 * theta_dot + 124.8 * P) + self.X_dot_fun = cs.Function('X_dot', [X, U, d], [X_dot]) def _dynamics_si_3d(self, action, nth_drone, disturbance_force=None): '''Explicit dynamics implementation from the identified model. @@ -882,7 +876,6 @@ def _dynamics_si_3d(self, action, nth_drone, disturbance_force=None): rpy = self.rpy[nth_drone, :] vel = self.vel[nth_drone, :] ang_v = self.ang_v[nth_drone, :] - rpy_rates = self.rpy_rates[nth_drone, :] # Compute forces and torques. # Update state with discrete time dynamics. @@ -961,21 +954,20 @@ def setup_dynamics_si_3d_expression(self): # - 61.62863740616216 * theta - 7.205874472066235 * theta_dot + 51.90335491067372 * P, # (- 12.544174350349687 * psi - 0.012945379372787613 * psi_dot + 43.839961280232046 * Y)*0) - X_dot = cs.vertcat(x_dot, - (18.112984649321753 * T + 3.6800) * (cs.cos(phi) * cs.sin(theta) * cs.cos(psi) + cs.sin(phi) * cs.sin(psi)) + d[0] / self.MASS, - y_dot, - 0.0, - z_dot, - (18.112984649321753 * T + 3.6800) * cs.cos(phi) * cs.cos(theta) - g + d[1] / self.MASS, - phi_dot, - theta_dot, - psi_dot, - -140.8 * phi - 13.4 * phi_dot + 124.8 * R, - -140.8 * theta - 13.4 * theta_dot + 124.8 * P, - 0.0) - - self.X_dot_fun = cs.Function("X_dot", [X, U, d], [X_dot]) + (18.112984649321753 * T + 3.6800) * (cs.cos(phi) * cs.sin(theta) * cs.cos(psi) + cs.sin(phi) * cs.sin(psi)) + d[0] / self.MASS, + y_dot, + 0.0, + z_dot, + (18.112984649321753 * T + 3.6800) * cs.cos(phi) * cs.cos(theta) - g + d[1] / self.MASS, + phi_dot, + theta_dot, + psi_dot, + -140.8 * phi - 13.4 * phi_dot + 124.8 * R, + -140.8 * theta - 13.4 * theta_dot + 124.8 * P, + 0.0) + + self.X_dot_fun = cs.Function('X_dot', [X, U, d], [X_dot]) def _show_drone_local_axes(self, nth_drone): '''Draws the local frame of the n-th drone in PyBullet's GUI. @@ -1047,5 +1039,5 @@ def _parse_urdf_parameters(self, file_name): MIN_PWM = float(URDF_TREE[0].attrib['pwm_min']) MAX_PWM = float(URDF_TREE[0].attrib['pwm_max']) return M, L, THRUST2WEIGHT_RATIO, J, J_INV, KF, KM, COLLISION_H, COLLISION_R, COLLISION_Z_OFFSET, MAX_SPEED_KMH, \ - GND_EFF_COEFF, PROP_RADIUS, DRAG_COEFF, DW_COEFF_1, DW_COEFF_2, DW_COEFF_3, \ - PWM2RPM_SCALE, PWM2RPM_CONST, MIN_PWM, MAX_PWM \ No newline at end of file + GND_EFF_COEFF, PROP_RADIUS, DRAG_COEFF, DW_COEFF_1, DW_COEFF_2, DW_COEFF_3, \ + PWM2RPM_SCALE, PWM2RPM_CONST, MIN_PWM, MAX_PWM diff --git a/safe_control_gym/envs/gym_pybullet_drones/quadrotor.py b/safe_control_gym/envs/gym_pybullet_drones/quadrotor.py index 7c2ba4283..f7cfcb0a9 100644 --- a/safe_control_gym/envs/gym_pybullet_drones/quadrotor.py +++ b/safe_control_gym/envs/gym_pybullet_drones/quadrotor.py @@ -869,6 +869,8 @@ def _set_action_space(self): self.hover_thrust = self.GRAVITY_ACC * self.MASS else: self.hover_thrust = self.GRAVITY_ACC * self.MASS / action_dim + self.action_scale = (self.physical_action_bounds[1] - self.physical_action_bounds[0]) / 2 + self.action_bias = (self.physical_action_bounds[1] + self.physical_action_bounds[0]) / 2 self.action_space = spaces.Box(low=-np.ones(action_dim), high=np.ones(action_dim), dtype=np.float32) @@ -998,10 +1000,10 @@ def _preprocess_control(self, action): # Identified dynamics model works with collective thrust and pitch directly # No need to compute RPMs, (save compute) self.current_clipped_action = np.clip(self.current_noisy_physical_action, - self.physical_action_bounds[0], - self.physical_action_bounds[1]) + self.action_space.low, + self.action_space.high) if self.PHYSICS == Physics.DYN_SI or self.PHYSICS == Physics.DYN_SI_3D: - return None + return self.current_clipped_action if self.QUAD_TYPE == QuadType.TWO_D_ATTITUDE or self.QUAD_TYPE == QuadType.TWO_D_ATTITUDE_5S: collective_thrust, pitch = self.current_clipped_action @@ -1036,10 +1038,11 @@ def normalize_action(self, action): normalized_action (ndarray): The action in the correct action space. """ if self.NORMALIZED_RL_ACTION_SPACE: - if self.QUAD_TYPE == QuadType.TWO_D_ATTITUDE or self.QUAD_TYPE == QuadType.TWO_D_ATTITUDE_5S: - action = np.array([(action[0] / self.hover_thrust - 1) / self.norm_act_scale, action[1]]) - else: - action = (action / self.hover_thrust - 1) / self.norm_act_scale + # if self.QUAD_TYPE in [QuadType.TWO_D_ATTITUDE, QuadType.TWO_D_ATTITUDE_5S, QuadType.TWO_D_ATTITUDE_BODY]: + # action = np.array([(action[0] / self.hover_thrust - 1) / self.norm_act_scale, action[1]]) + # else: + # action = (action / self.hover_thrust - 1) / self.norm_act_scale + action = (action - self.action_bias) / self.action_scale return action @@ -1054,20 +1057,22 @@ def denormalize_action(self, action): """ if self.NORMALIZED_RL_ACTION_SPACE: - if self.QUAD_TYPE == QuadType.TWO_D_ATTITUDE or self.QUAD_TYPE == QuadType.TWO_D_ATTITUDE_5S: - # # divided by 4 as action[0] is a collective thrust - # thrust = action[0] / 4 - # hover_pwm = (self.HOVER_RPM - self.PWM2RPM_CONST) / self.PWM2RPM_SCALE - # thrust = np.where(thrust <= 0, self.MIN_PWM + (thrust + 1) * (hover_pwm - self.MIN_PWM), - # hover_pwm + (self.MAX_PWM - hover_pwm) * thrust) - - thrust = (1 + self.norm_act_scale * action[0]) * self.hover_thrust - # thrust = self.attitude_control.thrust2pwm(thrust) - # thrust = self.HOVER_RPM * (1+0.05*action[0]) - - action = np.array([thrust, action[1]]) - else: - action = (1 + self.norm_act_scale * action) * self.hover_thrust + # if self.QUAD_TYPE in [QuadType.TWO_D_ATTITUDE, QuadType.TWO_D_ATTITUDE_5S, QuadType.TWO_D_ATTITUDE_BODY]: + # # # divided by 4 as action[0] is a collective thrust + # # thrust = action[0] / 4 + # # hover_pwm = (self.HOVER_RPM - self.PWM2RPM_CONST) / self.PWM2RPM_SCALE + # # thrust = np.where(thrust <= 0, self.MIN_PWM + (thrust + 1) * (hover_pwm - self.MIN_PWM), + # # hover_pwm + (self.MAX_PWM - hover_pwm) * thrust) + # + # thrust = (1 + self.norm_act_scale * action[0]) * self.hover_thrust + # # thrust = self.attitude_control.thrust2pwm(thrust) + # # thrust = self.HOVER_RPM * (1+0.05*action[0]) + # + # action = np.array([thrust, action[1]]) + # else: + # action = (1 + self.norm_act_scale * action) * self.hover_thrust + action = action * self.action_scale + self.action_bias + # action = np.clip(action, self.action_space.low, self.action_space.high) return action @@ -1245,9 +1250,9 @@ def _get_info(self): # Filter only relevant dimensions. state_error = state_error * self.info_mse_metric_state_weight info['mse'] = np.sum(state_error ** 2) - # if self.constraints is not None: - # info['constraint_values'] = self.constraints.get_values(self) - # info['constraint_violations'] = self.constraints.get_violations(self) + if self.constraints is not None: + info['constraint_values'] = self.constraints.get_values(self) + info['constraint_violations'] = self.constraints.get_violations(self) return info def _get_reset_info(self): diff --git a/safe_control_gym/safety_filters/__init__.py b/safe_control_gym/safety_filters/__init__.py index a283ed008..b82a4b201 100644 --- a/safe_control_gym/safety_filters/__init__.py +++ b/safe_control_gym/safety_filters/__init__.py @@ -10,6 +10,10 @@ entry_point='safe_control_gym.safety_filters.mpsc.nl_mpsc:NL_MPSC', config_entry_point='safe_control_gym.safety_filters.mpsc:mpsc.yaml') +register(idx='mpsc_acados', + entry_point='safe_control_gym.safety_filters.mpsc.mpsc_acados:MPSC_ACADOS', + config_entry_point='safe_control_gym.safety_filters.mpsc:mpsc.yaml') + register(idx='cbf', entry_point='safe_control_gym.safety_filters.cbf.cbf:CBF', config_entry_point='safe_control_gym.safety_filters.cbf:cbf.yaml') diff --git a/safe_control_gym/safety_filters/mpsc/mpsc.py b/safe_control_gym/safety_filters/mpsc/mpsc.py index e9a8dd258..251c58985 100644 --- a/safe_control_gym/safety_filters/mpsc/mpsc.py +++ b/safe_control_gym/safety_filters/mpsc/mpsc.py @@ -248,7 +248,7 @@ def solve_acados_optimization(self, 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): - self.slack_prev[i, :] = ocp_solver.get(i, 'su') + # self.slack_prev[i, :] = ocp_solver.get(i, 'su') x_val[i, :] = ocp_solver.get(i, 'x') u_val[i, :] = ocp_solver.get(i, 'u') x_val[self.horizon, :] = ocp_solver.get(self.horizon, 'x') diff --git a/safe_control_gym/safety_filters/mpsc/mpsc_acados.py b/safe_control_gym/safety_filters/mpsc/mpsc_acados.py new file mode 100644 index 000000000..44692a32e --- /dev/null +++ b/safe_control_gym/safety_filters/mpsc/mpsc_acados.py @@ -0,0 +1,276 @@ +'''Model Predictive Safety Certification using Acados.''' +import os +import shutil +from datetime import datetime + +import numpy as np +import scipy +from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver + +from safe_control_gym.controllers.mpc.mpc_utils import set_acados_constraint_bound +from safe_control_gym.safety_filters.mpsc.mpsc import MPSC +from safe_control_gym.safety_filters.mpsc.mpsc_utils import Cost_Function, box2polytopic +from safe_control_gym.utils.utils import timing + + +class MPSC_ACADOS(MPSC): + '''MPSC with full nonlinear model.''' + + def __init__( + self, + env_func, + horizon: int = 5, + q_mpc: list = [1], + r_mpc: list = [1], + integration_algo: str = 'rk4', + warmstart: bool = True, + additional_constraints: list = None, + use_terminal_set: bool = True, + soften_constraints: bool = False, + slack_cost: float = 1, + constraint_tol: float = 1e-6, + seed: int = 0, + use_RTI: bool = False, + cost_function: Cost_Function = Cost_Function.ONE_STEP_COST, + mpsc_cost_horizon: int = 5, + decay_factor: float = 0.85, + **kwargs + ): + '''Creates task and controller. + + Args: + env_func (Callable): function to instantiate task/environment. + horizon (int): mpc planning horizon. + q_mpc (list): diagonals of state cost weight. + r_mpc (list): diagonals of input/action cost weight. + warmstart (bool): if to initialize from previous iteration. + soften_constraints (bool): Formulate the constraints as soft constraints. + constraint_tol (float): Tolerance to add the the constraint as sometimes solvers are not exact. + seed (int): random seed. + use_RTI (bool): Real-time iteration for acados. + ''' + for k, v in locals().items(): + if k != 'self' and k != 'kwargs' and '__' not in k: + self.__dict__.update({k: v}) + + 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) + + # acados settings + self.use_RTI = use_RTI + + self.n = self.model.nx + self.m = self.model.nu + self.q = self.model.nx + + self.state_constraint = self.constraints.state_constraints[0] + self.input_constraint = self.constraints.input_constraints[0] + + [self.X_mid, L_x, l_x] = box2polytopic(self.state_constraint) + [self.U_mid, L_u, l_u] = box2polytopic(self.input_constraint) + + # number of constraints + p_x = l_x.shape[0] + p_u = l_u.shape[0] + self.p = p_x + p_u + + self.L_x = np.vstack((L_x, np.zeros((p_u, self.n)))) + self.L_u = np.vstack((np.zeros((p_x, self.m)), L_u)) + self.l_xu = np.concatenate([l_x, l_u]) + + # Dynamics model. + self.setup_acados_model() + # Acados optimizer. + self.setup_acados_optimizer() + + @timing + def reset(self): + '''Prepares for training or evaluation.''' + print('Resetting MPC') + super().reset() + if hasattr(self, 'acados_model'): + del self.acados_model + if hasattr(self, 'ocp'): + del self.ocp + if hasattr(self, 'acados_ocp_solver'): + del self.acados_ocp_solver + + # delete the generated c code directory + if os.path.exists('./c_generated_code'): + print('deleting the generated MPC c code directory') + shutil.rmtree('./c_generated_code') + assert not os.path.exists('./c_generated_code'), 'Failed to delete the generated c code directory' + + def setup_acados_model(self) -> AcadosModel: + '''Sets up symbolic model for acados.''' + acados_model = AcadosModel() + acados_model.x = self.model.x_sym + acados_model.u = self.model.u_sym + acados_model.name = self.env.NAME + + # set up rk4 (acados need symbolic expression of dynamics, not function) + k1 = self.model.fc_func(acados_model.x, acados_model.u) + k2 = self.model.fc_func(acados_model.x + self.dt / 2 * k1, acados_model.u) + k3 = self.model.fc_func(acados_model.x + self.dt / 2 * k2, acados_model.u) + k4 = self.model.fc_func(acados_model.x + self.dt * k3, acados_model.u) + f_disc = acados_model.x + self.dt / 6 * (k1 + 2 * k2 + 2 * k3 + k4) + + acados_model.disc_dyn_expr = f_disc + + # store meta information # NOTE: unit is missing + acados_model.x_labels = self.env.STATE_LABELS + acados_model.u_labels = self.env.ACTION_LABELS + acados_model.t_label = 'time' + # get current time stamp in $ymd_HMS format + current_time = datetime.now().strftime('%Y%m%d_%H%M%S') + acados_model.name = self.env.NAME + '_' + current_time + + self.acados_model = acados_model + + def set_dynamics(self): + pass + + def setup_casadi_optimizer(self): + pass + + def setup_acados_optimizer(self): + '''Sets up nonlinear optimization problem.''' + nx, nu = self.model.nx, self.model.nu + ny = nx + nu + ny_e = nx + + # create ocp object to formulate the OCP + ocp = AcadosOcp() + ocp.model = self.acados_model + + # set dimensions + ocp.dims.N = self.horizon # prediction horizon + + # set cost (NOTE: safe-control-gym uses quadratic cost) + ocp.cost.cost_type = 'LINEAR_LS' + ocp.cost.cost_type_e = 'LINEAR_LS' + + Q_mat = np.zeros((nx, nx)) + R_mat = np.eye(nu) + ocp.cost.W_e = np.zeros((nx, nx)) + ocp.cost.W = scipy.linalg.block_diag(Q_mat, R_mat) + + ocp.cost.Vx = np.zeros((ny, nx)) + ocp.cost.Vu = np.zeros((ny, nu)) + ocp.cost.Vu[nx:nx + nu, :] = np.eye(nu) + ocp.cost.Vx_e = np.eye(nx) + + # placeholder y_ref and y_ref_e (will be set in select_action) + ocp.cost.yref = np.zeros((ny, )) + ocp.cost.yref_e = np.zeros((ny_e, )) + + # set up solver options + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' + ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' + ocp.solver_options.integrator_type = 'DISCRETE' + ocp.solver_options.nlp_solver_type = 'SQP' if not self.use_RTI else 'SQP_RTI' + ocp.solver_options.nlp_solver_max_iter = 25 if not self.use_RTI else 1 + ocp.solver_options.tf = self.horizon * self.dt # prediction horizon + + # set constraints + ocp.constraints.constr_type = 'BGH' + ocp.constraints.constr_type_e = 'BGH' + + ocp.constraints.x0 = self.model.X_EQ + ocp.constraints.C = self.L_x + ocp.constraints.D = self.L_u + ocp.constraints.lg = -1000 * np.ones((self.p)) + ocp.constraints.ug = np.zeros((self.p)) + + # slack costs for nonlinear constraints + if self.soften_constraints: + # slack variables for all constraints + ocp.constraints.Jsg = np.eye(self.p) + # slack penalty + ocp.cost.Zu = self.slack_cost * np.ones(self.p) + ocp.cost.Zl = self.slack_cost * np.ones(self.p) + ocp.cost.zl = self.slack_cost * np.ones(self.p) + ocp.cost.zu = self.slack_cost * np.ones(self.p) + + solver_json = 'acados_ocp_mpsf.json' + ocp_solver = AcadosOcpSolver(ocp, json_file=solver_json, generate=True, build=True) + + for stage in range(self.mpsc_cost_horizon): + ocp_solver.cost_set(stage, 'W', (self.cost_function.decay_factor**stage) * ocp.cost.W) + + for stage in range(self.mpsc_cost_horizon, self.horizon): + ocp_solver.cost_set(stage, 'W', 0 * ocp.cost.W) + + g = np.zeros((self.horizon, self.p)) + for i in range(self.horizon): + for j in range(self.p): + g[i, j] = self.l_xu[j] + g[i, :] += (self.L_x @ self.X_mid) + (self.L_u @ self.U_mid) + ocp_solver.constraints_set(i, 'ug', g[i, :]) + + self.ocp_solver = ocp_solver + self.ocp = ocp + + def processing_acados_constraints_expression(self, ocp: AcadosOcp, h0_expr, h_expr, he_expr) -> AcadosOcp: + '''Preprocess the constraints to be compatible with acados. + Args: + ocp (AcadosOcp): acados ocp object + h0_expr (casadi expression): initial state constraints + h_expr (casadi expression): state and input constraints + he_expr (casadi expression): terminal state constraints + Returns: + ocp (AcadosOcp): acados ocp object with constraints set. + + An alternative way to set the constraints is to use bounded constraints of acados: + # bounded input constraints + idxbu = np.where(np.sum(self.env.constraints.input_constraints[0].constraint_filter, axis=0) != 0)[0] + ocp.constraints.Jbu = np.eye(nu) + ocp.constraints.lbu = self.env.constraints.input_constraints[0].lower_bounds + ocp.constraints.ubu = self.env.constraints.input_constraints[0].upper_bounds + ocp.constraints.idxbu = idxbu # active constraints dimension + ''' + + ub = {'h': set_acados_constraint_bound(h_expr, 'ub', self.constraint_tol), + 'h0': set_acados_constraint_bound(h0_expr, 'ub', self.constraint_tol), + 'he': set_acados_constraint_bound(he_expr, 'ub', self.constraint_tol), } + + lb = {'h': set_acados_constraint_bound(h_expr, 'lb'), + 'h0': set_acados_constraint_bound(h0_expr, 'lb'), + 'he': set_acados_constraint_bound(he_expr, 'lb'), } + + # make sure all the ub and lb are 1D numpy arrays + # (see: https://discourse.acados.org/t/infeasible-qps-when-using-nonlinear-casadi-constraint-expressions/1595/5?u=mxche) + for key in ub.keys(): + ub[key] = ub[key].flatten() if ub[key].ndim != 1 else ub[key] + lb[key] = lb[key].flatten() if lb[key].ndim != 1 else lb[key] + # check ub and lb dimensions + for key in ub.keys(): + assert ub[key].ndim == 1, f'ub[{key}] is not 1D numpy array' + assert lb[key].ndim == 1, f'lb[{key}] is not 1D numpy array' + assert ub['h'].shape == lb['h'].shape, 'h_ub and h_lb have different shapes' + + # pass the constraints to the ocp object + ocp.model.con_h_expr_0, ocp.model.con_h_expr, ocp.model.con_h_expr_e = \ + h0_expr, h_expr, he_expr + ocp.dims.nh_0, ocp.dims.nh, ocp.dims.nh_e = \ + h0_expr.shape[0], h_expr.shape[0], he_expr.shape[0] + # assign constraints upper and lower bounds + ocp.constraints.uh_0 = ub['h0'] + ocp.constraints.lh_0 = lb['h0'] + ocp.constraints.uh = ub['h'] + ocp.constraints.lh = lb['h'] + ocp.constraints.uh_e = ub['he'] + ocp.constraints.lh_e = lb['he'] + + return ocp diff --git a/safe_control_gym/safety_filters/mpsc/mpsc_utils.py b/safe_control_gym/safety_filters/mpsc/mpsc_utils.py index 0771e0f75..0a66435a4 100644 --- a/safe_control_gym/safety_filters/mpsc/mpsc_utils.py +++ b/safe_control_gym/safety_filters/mpsc/mpsc_utils.py @@ -157,3 +157,45 @@ def get_discrete_derivative(signal, ctrl_freq): ''' discrete_derivative = (signal[1:, :] - signal[:-1, :]) * ctrl_freq return discrete_derivative + + +def box2polytopic(constraint): + '''Convert constraints into an explicit polytopic form. This assumes that constraints contain the origin. + + Args: + constraint (Constraint): The constraint to be converted. + + Returns: + L (ndarray): The polytopic matrix. + l (ndarray): Whether the constraint is active. + ''' + + Limit = [] + limit_active = [] + + Z_mid = (constraint.upper_bounds + constraint.lower_bounds) / 2.0 + Z_limits = np.array([[constraint.upper_bounds[i] - Z_mid[i], constraint.lower_bounds[i] - Z_mid[i]] for i in range(constraint.upper_bounds.shape[0])]) + + dim = Z_limits.shape[0] + eye_dim = np.eye(dim) + + for constraint_id in range(0, dim): + if Z_limits[constraint_id, 0] != -float('inf'): + if Z_limits[constraint_id, 0] == 0: + limit_active += [0] + Limit += [-eye_dim[constraint_id, :]] + else: + limit_active += [1] + factor = 1 / Z_limits[constraint_id, 0] + Limit += [factor * eye_dim[constraint_id, :]] + + if Z_limits[constraint_id, 1] != float('inf'): + if Z_limits[constraint_id, 1] == 0: + limit_active += [0] + Limit += [eye_dim[constraint_id, :]] + else: + limit_active += [1] + factor = 1 / Z_limits[constraint_id, 1] + Limit += [factor * eye_dim[constraint_id, :]] + + return Z_mid, np.array(Limit), np.array(limit_active) diff --git a/safe_control_gym/safety_filters/mpsc/nl_mpsc.py b/safe_control_gym/safety_filters/mpsc/nl_mpsc.py index c9f9e199a..de1f40677 100644 --- a/safe_control_gym/safety_filters/mpsc/nl_mpsc.py +++ b/safe_control_gym/safety_filters/mpsc/nl_mpsc.py @@ -25,8 +25,7 @@ from safe_control_gym.envs.benchmark_env import Environment, Task from safe_control_gym.safety_filters.cbf.cbf_utils import cartesian_product from safe_control_gym.safety_filters.mpsc.mpsc import MPSC -from safe_control_gym.safety_filters.mpsc.mpsc_utils import \ - Cost_Function # , get_error_parameters, error_function +from safe_control_gym.safety_filters.mpsc.mpsc_utils import Cost_Function, box2polytopic class NL_MPSC(MPSC): @@ -79,8 +78,8 @@ def __init__(self, self.state_constraint = self.constraints.state_constraints[0] self.input_constraint = self.constraints.input_constraints[0] - [self.X_mid, L_x, l_x] = self.box2polytopic(self.state_constraint) - [self.U_mid, L_u, l_u] = self.box2polytopic(self.input_constraint) + [self.X_mid, L_x, l_x] = box2polytopic(self.state_constraint) + [self.U_mid, L_u, l_u] = box2polytopic(self.input_constraint) # number of constraints p_x = l_x.shape[0] @@ -368,47 +367,6 @@ def get_terminal_ingredients(self): if self.integration_algo == 'LTI' and self.use_terminal_set: self.get_terminal_constraint() - def box2polytopic(self, constraint): - '''Convert constraints into an explicit polytopic form. This assumes that constraints contain the origin. - - Args: - constraint (Constraint): The constraint to be converted. - - Returns: - L (ndarray): The polytopic matrix. - l (ndarray): Whether the constraint is active. - ''' - - Limit = [] - limit_active = [] - - Z_mid = (constraint.upper_bounds + constraint.lower_bounds) / 2.0 - Z_limits = np.array([[constraint.upper_bounds[i] - Z_mid[i], constraint.lower_bounds[i] - Z_mid[i]] for i in range(constraint.upper_bounds.shape[0])]) - - dim = Z_limits.shape[0] - eye_dim = np.eye(dim) - - for constraint_id in range(0, dim): - if Z_limits[constraint_id, 0] != -float('inf'): - if Z_limits[constraint_id, 0] == 0: - limit_active += [0] - Limit += [-eye_dim[constraint_id, :]] - else: - limit_active += [1] - factor = 1 / Z_limits[constraint_id, 0] - Limit += [factor * eye_dim[constraint_id, :]] - - if Z_limits[constraint_id, 1] != float('inf'): - if Z_limits[constraint_id, 1] == 0: - limit_active += [0] - Limit += [eye_dim[constraint_id, :]] - else: - limit_active += [1] - factor = 1 / Z_limits[constraint_id, 1] - Limit += [factor * eye_dim[constraint_id, :]] - - return Z_mid, np.array(Limit), np.array(limit_active) - def setup_tube_optimization(self, lamb): '''Sets up the optimization to find the lyapunov function. @@ -1022,8 +980,8 @@ def setup_acados_optimizer(self): ocp.constraints.Jsg = np.eye(self.p) ocp.cost.Zu = self.slack_cost * np.ones(self.p) ocp.cost.Zl = self.slack_cost * np.ones(self.p) - ocp.cost.zu = self.slack_cost * np.ones(self.p) ocp.cost.zl = self.slack_cost * np.ones(self.p) + ocp.cost.zu = self.slack_cost * np.ones(self.p) # Options ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM' @@ -1036,7 +994,7 @@ def setup_acados_optimizer(self): ocp.solver_options.tf = self.dt * self.horizon solver_json = 'acados_ocp_mpsf.json' - ocp_solver = AcadosOcpSolver(ocp, json_file=solver_json, generate=True, build=True) + ocp_solver = AcadosOcpSolver(ocp, json_file=solver_json, generate=False, build=False) for stage in range(self.mpsc_cost_horizon): ocp_solver.cost_set(stage, 'W', (self.cost_function.decay_factor**stage) * ocp.cost.W)