prepare to change to attitude interface.

Author: Bourmindge Liey

.devcontainer/devcontainer.json

@@ -19,6 +19,12 @@
                 "ms-toolsai.jupyter-keymap"
             ],
             "settings": {
+                "terminal.integrated.defaultProfile.linux": "bash",
+                "terminal.integrated.profiles.linux": {
+                    "bash": {
+                        "path": "/bin/bash"
+                    }
+                },
                 "python.defaultInterpreterPath": "/home/vscode/venv/bin/python",
                 "python.testing.pytestEnabled": true, // test framework
                 "python.testing.pytestArgs": [

tutorials/LQR_ILQR.ipynb

@@ -916,7 +916,7 @@
  ],
  "metadata": {
   "kernelspec": {
-   "display_name": "control_course_venv",
+   "display_name": "venv",
    "language": "python",
    "name": "python3"
   },
@@ -930,7 +930,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.11.10"
+   "version": "3.11.0"
   }
  },
  "nbformat": 4,

tutorials/MPC.ipynb

@@ -9,10 +9,8 @@
     "%reload_ext autoreload\n",
     "%autoreload 2\n",
     "\n",
-    "\n",
-    "\n",
     "from pathlib import Path\n",
-    "import os, sys  \n",
+    "import os, sys\n",
     "parent_dir = str(Path().resolve().parents[0])\n",
     "sys.path.insert(-1, parent_dir)\n",
     "\n",
@@ -1051,7 +1049,6 @@
     "y_ref = np.concatenate([goal, goal_u])\n",
     "y_ref_e = goal\n",
     "\n",
-    "counter = 3\n",
     "for i in range(2500):\n",
     "    counter += 1\n",
     "    if counter == 4:\n",

tutorials/MPC/mpc.py

@@ -1,28 +1,36 @@
+"""This module contains the implementation of a Model Predictive Controller (MPC)."""
+
 import numpy as np
+
 from crazyflow.sim.symbolic import SymbolicModel
 from tutorials.MPC.BaseClass import BaseController
 from tutorials.MPC.ocp_setup import create_ocp
 
+
 class ModelPredictiveController(BaseController):
+    """Model Predictive Controller (MPC) for controlling dynamic systems."""
 
-    def  __init__(self, symbolic_model: SymbolicModel, options: dict):
+    def __init__(self, symbolic_model: SymbolicModel, options: dict):
+        """Initialize the Model Predictive Controller.
 
+        Args:
+            symbolic_model (SymbolicModel): The symbolic model of the system.
+            options (dict): Configuration options for the MPC.
+        """
         self.options = options
 
         ## parameters ##
-        self.N = options['solver']['n_pred']
-        self.u_pred = np.ones((options['solver']['n_pred'], symbolic_model.u_sym.size()[0])) * 0.0662175
-        self.x_pred = np.zeros((options['solver']['n_pred']+1, symbolic_model.x_sym.size()[0]))
+        self.N = options["solver"]["n_pred"]
+        self.u_pred = (
+            np.ones((options["solver"]["n_pred"], symbolic_model.u_sym.size()[0])) * 0.0662175
+        )
+        self.x_pred = np.zeros((options["solver"]["n_pred"] + 1, symbolic_model.x_sym.size()[0]))
 
-        ## create acados ocp solver 
+        ## create acados ocp solver
         self.ocp, self.ocp_solver = create_ocp(symbolic_model, options)
-        
-       
 
     def control(self, x: np.ndarray, y_ref: np.ndarray, y_ref_e: np.ndarray) -> np.ndarray:
-        '''
-        Compute the control input for the cartpole system. 
-        '''
+        """Compute the control input for the cartpole system."""
         ## set initial state
         self.ocp_solver.set(0, "lbx", x)
         self.ocp_solver.set(0, "ubx", x)
@@ -36,22 +44,22 @@ def control(self, x: np.ndarray, y_ref: np.ndarray, y_ref_e: np.ndarray) -> np.n
         for i in range(self.N):
             self.ocp_solver.set(i, "u", self.u_pred[i, :])
 
-        ## solve ocp 
+        ## solve ocp
         status = self.ocp_solver.solve()
         if status != 0:
             print(f"Solver failed with status {status}")
             # Return safe control input (e.g., zero or previous solution)
             return np.zeros_like(self.ocp_solver.get(0, "u"))
 
-        ## get control input 
+        ## get control input
         u = self.ocp_solver.get(0, "u")
 
         ## extract predicted control inputs
         for i in range(self.N):
             self.u_pred[i, :] = self.ocp_solver.get(i, "u")
 
         ## extract predicted states
-        for i in range(self.N+1):
+        for i in range(self.N + 1):
             self.x_pred[i, :] = self.ocp_solver.get(i, "x")
 
-        return u
+        return u

tutorials/MPC/ocp_setup.py

@@ -1,38 +1,42 @@
 from acados_template import AcadosOcp, AcadosOcpSolver
 
 from crazyflow.sim.symbolic import SymbolicModel
-from tutorials.MPC.utils import create_prediction_model, create_ocp_constraints, create_ocp_costs, create_ocp_solver
+from tutorials.MPC.utils import (
+    create_ocp_constraints,
+    create_ocp_costs,
+    create_ocp_solver,
+    create_prediction_model,
+)
 
 
 def create_ocp(symbolic_model: SymbolicModel, options: dict):
-    '''
-    The Acados OCP is composed of the following key components. Please pay attention to the options that are used to configure the OCP.
-    These are also frequently used options for other OCP problems. 
-    '''
+    """The Acados OCP is composed of the following key components. Please pay attention to the options that are used to configure the OCP.
+    These are also frequently used options for other OCP problems.
+    """
     ocp = AcadosOcp()
     ############### 1. acados ocp model #############
-    '''
+    """
     The first key component of an Acados OCP is the model.
-    '''
+    """
     ocp.model = create_prediction_model(symbolic_model)
 
     ############### 2. acados ocp constraints #############
-    '''
+    """
     The second key component of an Acados OCP are the constraints.
-    '''
-    ocp = create_ocp_constraints(ocp, options['constraint'])
+    """
+    ocp = create_ocp_constraints(ocp, options["constraint"])
 
     ############### 3. acados ocp costs #############
-    '''
+    """
     Then cost is added to the ocp.
-    '''
-    ocp = create_ocp_costs(ocp, options['cost'])
+    """
+    ocp = create_ocp_costs(ocp, options["cost"])
 
     ############### 4. acados ocp solver #############
-    '''
+    """
     Finally, the ocp solver is created.
-    '''
-    ocp = create_ocp_solver(ocp, symbolic_model, options['solver'])
-    ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp_' + ocp.model.name + '.json')
+    """
+    ocp = create_ocp_solver(ocp, symbolic_model, options["solver"])
+    ocp_solver = AcadosOcpSolver(ocp, json_file="acados_ocp_" + ocp.model.name + ".json")
 
-    return ocp, ocp_solver
+    return ocp, ocp_solver

tutorials/MPC/plot_utils.py

@@ -195,6 +195,8 @@ def create_ocp_solver(ocp: AcadosOcp, symbolic_model: SymbolicModel, options: di
     ocp.solver_options.nlp_solver_tol_comp = 1e-4
     ocp.solver_options.nlp_solver_step_length = 1.0
 
+    ocp.solver_options.
+
     return ocp
 
 
@@ -233,3 +235,5 @@ def solve_eqn(symbolic_model: SymbolicModel, goal: np.ndarray):
     u_solution = sol["x"].full().flatten()
 
     return u_solution
+
+