Added MPC and Linear MPC examples

.pre-commit-config.yaml

@@ -26,7 +26,7 @@ repos:
     -   id: double-quote-string-fixer
 
 -   repo: https://github.com/pycqa/isort
-    rev: 5.12.0
+    rev: 5.13.2
     hooks:
       - id: isort
         name: isort
@@ -45,7 +45,7 @@ repos:
         files: (^tests/|^safe_control_gym/math_and_models/transformations.py)
 
 -   repo: https://github.com/PyCQA/flake8
-    rev: 6.1.0
+    rev: 7.0.0
     hooks:
     -   id: flake8
         name: flake8_default

examples/mpc/config_overrides/cartpole/cartpole_stabilization.yaml

@@ -0,0 +1,39 @@
+task_config:
+  seed: 42
+  info_in_reset: True
+  ctrl_freq: 15
+  pyb_freq: 750
+  physics: pyb
+
+  init_state_randomization_info:
+    init_x:
+      distrib: 'uniform'
+      low: -2
+      high: 2
+    init_x_dot:
+      distrib: 'uniform'
+      low: -0.1
+      high: 0.1
+    init_theta:
+      distrib: 'uniform'
+      low: -0.2
+      high: 0.2
+    init_theta_dot:
+      distrib: 'uniform'
+      low: -0.1
+      high: 0.1
+
+  task: stabilization
+  task_info:
+    stabilization_goal: [1.0, 0.0]
+    stabilization_goal_tolerance: 0.0
+
+  episode_len_sec: 6
+  cost: quadratic
+  done_on_out_of_bound: True
+
+  constraints:
+  - constraint_form: default_constraint
+    constrained_variable: input
+  - constraint_form: default_constraint
+    constrained_variable: state

examples/mpc/config_overrides/cartpole/cartpole_tracking.yaml

@@ -0,0 +1,42 @@
+task_config:
+  seed: 42
+  info_in_reset: True
+  ctrl_freq: 15
+  pyb_freq: 750
+  physics: pyb
+
+  init_state_randomization_info:
+    init_x:
+      distrib: 'uniform'
+      low: -1
+      high: 1
+    init_x_dot:
+      distrib: 'uniform'
+      low: -0.1
+      high: 0.1
+    init_theta:
+      distrib: 'uniform'
+      low: -0.2
+      high: 0.2
+    init_theta_dot:
+      distrib: 'uniform'
+      low: -0.1
+      high: 0.1
+
+  task: traj_tracking
+  task_info:
+    trajectory_type: 'circle'
+    num_cycles: 2
+    trajectory_plane: 'zx'
+    trajectory_position_offset: [0, 0]
+    trajectory_scale: 1
+
+  episode_len_sec: 6
+  cost: quadratic
+  done_on_out_of_bound: True
+
+  constraints:
+  - constraint_form: default_constraint
+    constrained_variable: input
+  - constraint_form: default_constraint
+    constrained_variable: state

examples/mpc/config_overrides/cartpole/linear_mpc_cartpole_stabilization.yaml

@@ -0,0 +1,17 @@
+algo: linear_mpc
+algo_config:
+  horizon: 20
+  r_mpc:
+    - 0.1
+  q_mpc:
+    - 5.0
+    - 0.1
+    - 5.0
+    - 0.1
+  # Prior info
+  prior_info:
+    prior_prop: null
+    randomize_prior_prop: False
+    prior_prop_rand_info: null
+  warmstart: True
+  solver: qrqp

examples/mpc/config_overrides/cartpole/linear_mpc_cartpole_tracking.yaml

@@ -0,0 +1,17 @@
+algo: linear_mpc
+algo_config:
+  horizon: 40
+  r_mpc:
+    - 0.1
+  q_mpc:
+    - 5.0
+    - 0.1
+    - 5.0
+    - 0.1
+  # Prior info
+  prior_info:
+    prior_prop: null
+    randomize_prior_prop: False
+    prior_prop_rand_info: null
+  warmstart: True
+  solver: qrqp

examples/mpc/config_overrides/cartpole/mpc_cartpole_stabilization.yaml

@@ -0,0 +1,17 @@
+algo: mpc
+algo_config:
+  horizon: 20
+  r_mpc:
+    - 0.1
+  q_mpc:
+    - 5.0
+    - 0.1
+    - 5.0
+    - 0.1
+  # Prior info
+  prior_info:
+    prior_prop: null
+    randomize_prior_prop: False
+    prior_prop_rand_info: null
+  warmstart: True
+  solver: ipopt

examples/mpc/config_overrides/cartpole/mpc_cartpole_tracking.yaml

@@ -0,0 +1,17 @@
+algo: mpc
+algo_config:
+  horizon: 20
+  r_mpc:
+    - 0.1
+  q_mpc:
+    - 5.0
+    - 0.1
+    - 5.0
+    - 0.1
+  # Prior info
+  prior_info:
+    prior_prop: null
+    randomize_prior_prop: False
+    prior_prop_rand_info: null
+  warmstart: True
+  solver: ipopt

examples/mpc/config_overrides/quadrotor_2D/linear_mpc_quadrotor_2D_stabilization.yaml

@@ -0,0 +1,20 @@
+algo: linear_mpc
+algo_config:
+  horizon: 20
+  r_mpc:
+    - 0.1
+    - 0.1
+  q_mpc:
+    - 5.0
+    - 0.1
+    - 5.0
+    - 0.1
+    - 0.1
+    - 0.1
+  # Prior info
+  prior_info:
+    prior_prop: null
+    randomize_prior_prop: False
+    prior_prop_rand_info: null
+  warmstart: True
+  solver: qrqp

examples/mpc/config_overrides/quadrotor_2D/linear_mpc_quadrotor_2D_tracking.yaml

@@ -0,0 +1,20 @@
+algo: linear_mpc
+algo_config:
+  horizon: 40
+  r_mpc:
+    - 0.1
+    - 0.1
+  q_mpc:
+    - 1.0
+    - 0.1
+    - 1.0
+    - 0.1
+    - 0.1
+    - 0.1
+  # Prior info
+  prior_info:
+    prior_prop: null
+    randomize_prior_prop: False
+    prior_prop_rand_info: null
+  warmstart: True
+  solver: qrqp

examples/mpc/config_overrides/quadrotor_2D/mpc_quadrotor_2D_stabilization.yaml

@@ -0,0 +1,20 @@
+algo: mpc
+algo_config:
+  horizon: 20
+  r_mpc:
+    - 0.1
+    - 0.1
+  q_mpc:
+    - 5.0
+    - 0.1
+    - 5.0
+    - 0.1
+    - 0.1
+    - 0.1
+  # Prior info
+  prior_info:
+    prior_prop: null
+    randomize_prior_prop: False
+    prior_prop_rand_info: null
+  warmstart: True
+  solver: ipopt

examples/mpc/config_overrides/quadrotor_2D/mpc_quadrotor_2D_tracking.yaml

@@ -0,0 +1,20 @@
+algo: mpc
+algo_config:
+  horizon: 20
+  r_mpc:
+    - 0.1
+    - 0.1
+  q_mpc:
+    - 5.0
+    - 0.1
+    - 5.0
+    - 0.1
+    - 0.1
+    - 0.1
+  # Prior info
+  prior_info:
+    prior_prop: null
+    randomize_prior_prop: False
+    prior_prop_rand_info: null
+  warmstart: True
+  solver: ipopt

examples/mpc/config_overrides/quadrotor_2D/quadrotor_2D_stabilization.yaml

@@ -0,0 +1,51 @@
+task_config:
+  seed: 1337
+  info_in_reset: True
+  ctrl_freq: 50
+  pyb_freq: 1000
+  gui: False
+  physics: pyb
+  quad_type: 2
+
+  init_state_randomization_info:
+    init_x:
+      distrib: 'uniform'
+      low: -1
+      high: 1
+    init_x_dot:
+      distrib: 'uniform'
+      low: -0.1
+      high: 0.1
+    init_z:
+      distrib: 'uniform'
+      low: 0.5
+      high: 1.5
+    init_z_dot:
+      distrib: 'uniform'
+      low: -0.1
+      high: 0.1
+    init_theta:
+      distrib: 'uniform'
+      low: -0.2
+      high: 0.2
+    init_theta_dot:
+      distrib: 'uniform'
+      low: -0.1
+      high: 0.1
+  randomized_init: True
+  randomized_inertial_prop: False
+
+  task: stabilization
+  task_info:
+    stabilization_goal: [0, 1]
+    stabilization_goal_tolerance: 0.0
+
+  episode_len_sec: 6
+  cost: quadratic
+  done_on_out_of_bound: True
+
+  constraints:
+  - constraint_form: default_constraint
+    constrained_variable: input
+  - constraint_form: default_constraint
+    constrained_variable: state

examples/mpc/config_overrides/quadrotor_2D/quadrotor_2D_tracking.yaml

@@ -0,0 +1,54 @@
+task_config:
+  seed: 1337
+  info_in_reset: True
+  ctrl_freq: 50
+  pyb_freq: 1000
+  gui: False
+  physics: pyb
+  quad_type: 2
+
+  init_state_randomization_info:
+    init_x:
+      distrib: 'uniform'
+      low: -1
+      high: 1
+    init_x_dot:
+      distrib: 'uniform'
+      low: -0.1
+      high: 0.1
+    init_z:
+      distrib: 'uniform'
+      low: 0.5
+      high: 1.5
+    init_z_dot:
+      distrib: 'uniform'
+      low: -0.1
+      high: 0.1
+    init_theta:
+      distrib: 'uniform'
+      low: -0.2
+      high: 0.2
+    init_theta_dot:
+      distrib: 'uniform'
+      low: -0.1
+      high: 0.1
+  randomized_init: True
+  randomized_inertial_prop: False
+
+  task: traj_tracking
+  task_info:
+    trajectory_type: figure8
+    num_cycles: 1
+    trajectory_plane: 'xz'
+    trajectory_position_offset: [0, 1]
+    trajectory_scale: 0.75
+
+  episode_len_sec: 6
+  cost: quadratic
+  done_on_out_of_bound: True
+
+  constraints:
+  - constraint_form: default_constraint
+    constrained_variable: input
+  - constraint_form: default_constraint
+    constrained_variable: state

examples/mpc/config_overrides/quadrotor_3D/linear_mpc_quadrotor_3D_stabilization.yaml

@@ -0,0 +1,28 @@
+algo: linear_mpc
+algo_config:
+  horizon: 20
+  r_mpc:
+    - 0.1
+    - 0.1
+    - 0.1
+    - 0.1
+  q_mpc:
+    - 5.0
+    - 0.1
+    - 5.0
+    - 0.1
+    - 5.0
+    - 0.1
+    - 0.1
+    - 0.1
+    - 0.1
+    - 0.1
+    - 0.1
+    - 0.1
+  # Prior info
+  prior_info:
+    prior_prop: null
+    randomize_prior_prop: False
+    prior_prop_rand_info: null
+  warmstart: True
+  solver: qrqp