MRAC controller rearrangement

Author: mattia-gramuglia

acsl_pychrono/config/config.py

@@ -8,7 +8,7 @@ class MissionConfig:
   # Run the simulator in Wrapper mode (more simulations automatically run sequentially)
   wrapper_flag: bool = False
   # If True, perform real-time rendering of the simulation with Irrlicht
-  visualization_flag: bool = True
+  visualization_flag: bool = False
   # Dynamic camera options:
   # "fixed"
   # "default",

acsl_pychrono/control/MRAC/m_mrac.py

@@ -8,53 +8,30 @@ def computeAdaptiveLaw(Gamma_gain, pi_vector, eTranspose_P_B):
 
     return K_hat_state_dot
 
-  def reshapeAdaptiveGainsToMatrices(self):
-    """
-    Reshapes all gain parameters to their correct (row, col) shape and converts them to np.matrix.
-    This is intended to be called once after loading or updating gains stored as flat arrays.
-    """
-    self.K_hat_x_tran = np.matrix(self.K_hat_x_tran.reshape(6,3))
-    self.K_hat_r_tran = np.matrix(self.K_hat_r_tran.reshape(3,3))
-    self.Theta_hat_tran = np.matrix(self.Theta_hat_tran.reshape(6,3))
-    self.K_hat_x_rot = np.matrix(self.K_hat_x_rot.reshape(3,3))
-    self.K_hat_r_rot = np.matrix(self.K_hat_r_rot.reshape(3,3))
-    self.Theta_hat_rot = np.matrix(self.Theta_hat_rot.reshape(6,3))
+  @ staticmethod
+  def compute_eTransposePB(e, P, B):
+    eTranspose_P_B = e.T * P * B
+    return eTranspose_P_B
 
-  def compute_eTransposePB_OuterLoop(self):
-    eTranspose_P_B_tran = self.e_tran.T * self.gains.P_tran * self.gains.B_tran
+  @staticmethod
+  def computeAllAdaptiveLaws(
+    Gamma_x,
+    x,
+    Gamma_r,
+    r,
+    Gamma_Theta,
+    Phi_regressor_vector,
+    eTranspose_P_B
+    ) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
+    """
+    Compute all the classical MRAC adaptive laws
 
-    return eTranspose_P_B_tran
+    Returns:
+      - (K_hat_x_dot, K_hat_r_dot, Theta_hat_dot)
+    """
 
-  def computeAllAdaptiveLawsOuterLoop(self, eTranspose_P_B_tran):
-    self.K_hat_x_tran_dot = self.computeAdaptiveLaw(
-      -self.gains.Gamma_x_tran,
-      self.x_tran,
-      eTranspose_P_B_tran
-    )
-    self.K_hat_r_tran_dot = self.computeAdaptiveLaw(
-      -self.gains.Gamma_r_tran,
-      self.r_tran,
-      eTranspose_P_B_tran
-    )
-    self.Theta_hat_tran_dot = self.computeAdaptiveLaw(
-      self.gains.Gamma_Theta_tran,
-      self.Phi_adaptive_tran_augmented,
-      eTranspose_P_B_tran
-    )
+    K_hat_x_dot = M_MRAC.computeAdaptiveLaw(-Gamma_x, x, eTranspose_P_B)
+    K_hat_r_dot = M_MRAC.computeAdaptiveLaw(-Gamma_r, r, eTranspose_P_B)
+    Theta_hat_dot = M_MRAC.computeAdaptiveLaw(Gamma_Theta, Phi_regressor_vector, eTranspose_P_B)
 
-  def computeAllAdaptiveLawsInnerLoop(self, eTranspose_P_B_rot):
-    self.K_hat_x_rot_dot = self.computeAdaptiveLaw(
-      -self.gains.Gamma_x_rot,
-      self.odein.angular_velocity,
-      eTranspose_P_B_rot
-    )
-    self.K_hat_r_rot_dot = self.computeAdaptiveLaw(
-      -self.gains.Gamma_r_rot,
-      self.r_rot,
-      eTranspose_P_B_rot
-    )
-    self.Theta_hat_rot_dot = self.computeAdaptiveLaw(
-      self.gains.Gamma_Theta_rot,
-      self.Phi_adaptive_rot_augmented,
-      eTranspose_P_B_rot
-    )
+    return (K_hat_x_dot, K_hat_r_dot, Theta_hat_dot)

acsl_pychrono/control/MRAC/mrac.py

@@ -8,7 +8,7 @@
 from acsl_pychrono.control.base_mrac import BaseMRAC
 from acsl_pychrono.control.MRAC.m_mrac import M_MRAC
 
-class MRAC(M_MRAC, BaseMRAC, Control):
+class MRAC(BaseMRAC, Control):
   def __init__(self, gains: MRACGains, ode_input: OdeInput, flight_params: FlightParams, timestep: float):
     super().__init__(odein=ode_input)
     self.gains = gains
@@ -65,10 +65,18 @@ def computeControlAlgorithm(self, ode_input: OdeInput):
     self.mu_tran_raw = self.computeMuRawOuterLoop()
 
     # Precompute e^T*P*B for outer loop
-    eTranspose_P_B_tran = self.compute_eTransposePB_OuterLoop()
+    eTranspose_P_B_tran = M_MRAC.compute_eTransposePB(self.e_tran, self.gains.P_tran, self.gains.B_tran)
 
     # Outer Loop Adaptive Laws
-    self.computeAllAdaptiveLawsOuterLoop(eTranspose_P_B_tran)
+    (self.K_hat_x_tran_dot,
+     self.K_hat_r_tran_dot,
+     self.Theta_hat_tran_dot
+    ) = M_MRAC.computeAllAdaptiveLaws(
+      self.gains.Gamma_x_tran, self.x_tran,
+      self.gains.Gamma_r_tran, self.r_tran,
+      self.gains.Gamma_Theta_tran, self.Phi_adaptive_tran_augmented,
+      eTranspose_P_B_tran
+    )
 
     # Outer Loop Safety Mechanism
     self.mu_x, self.mu_y, self.mu_z = self.safety_mechanism.apply(self.mu_tran_raw)
@@ -129,10 +137,18 @@ def computeControlAlgorithm(self, ode_input: OdeInput):
     ) = self.computeRegressorVectorInnerLoop()
 
     # Precompute e^T*P*B for inner loop
-    eTranspose_P_B_rot = self.compute_eTransposePB_InnerLoop()
+    eTranspose_P_B_rot = M_MRAC.compute_eTransposePB(self.e_rot, self.gains.P_rot, self.gains.B_rot)
 
     # Inner Loop Adaptive Laws
-    self.computeAllAdaptiveLawsInnerLoop(eTranspose_P_B_rot)
+    (self.K_hat_x_rot_dot,
+     self.K_hat_r_rot_dot,
+     self.Theta_hat_rot_dot
+    ) = M_MRAC.computeAllAdaptiveLaws(
+      self.gains.Gamma_x_rot, self.odein.angular_velocity,
+      self.gains.Gamma_r_rot, self.r_rot,
+      self.gains.Gamma_Theta_rot, self.Phi_adaptive_rot_augmented,
+      eTranspose_P_B_rot
+    )
 
     self.Moment_baseline = self.computeMomentBaselineInnerLoop()
 

acsl_pychrono/control/MRAC/mrac_gains.py

@@ -32,23 +32,23 @@ def __init__(self, flight_params: FlightParams):
     self.KD_tran_PD_baseline = np.matrix(1 * np.diag([8,8,3]))
 
     # **Rotational** baseline parameters
-    # self.KP_rot = np.matrix(1e2 * np.diag([1,1,0.5]))
-    # self.KP_rot = np.matrix(5e1 * np.diag([0.1,0.1,0.5]))
-    self.KP_rot = np.matrix(5e1 * np.diag([0.05,0.05,0.5]))
+    # self.KP_rot = np.matrix(5e1 * np.diag([0.05,0.05,0.5]))
     # self.KI_rot = np.matrix(1e2 * np.diag([1,1,1]))
-    self.KI_rot = np.matrix(1e2 * np.diag([1,1,1]))
+    self.KP_rot = np.matrix(1 * np.diag([100,100,50]))
+    self.KI_rot = np.matrix(0 * np.diag([0,0,0]))
 
     # **Rotational** parameters for the PI baseline controller (Moment_baseline_PI)       
-    # self.KP_rot_PI_baseline = np.matrix(40 * np.diag([1,1,1]))
+    # self.KP_rot_PI_baseline = np.matrix(200 * np.diag([1,1,0.3]))
     # self.KD_rot_PI_baseline = np.matrix(0 * np.diag([1,1,0.5]))
-    # self.KI_rot_PI_baseline = np.matrix(1e-1 * np.diag([1,1,0.5]))
-    self.KP_rot_PI_baseline = np.matrix(200 * np.diag([1,1,0.3]))
+    # self.KI_rot_PI_baseline = np.matrix(1e2 * np.diag([10,10,0.5]))
+    self.KP_rot_PI_baseline = np.matrix(40 * np.diag([1,1,0.5]))
     self.KD_rot_PI_baseline = np.matrix(0 * np.diag([1,1,0.5]))
-    self.KI_rot_PI_baseline = np.matrix(1e2 * np.diag([10,10,0.5]))
+    self.KI_rot_PI_baseline = np.matrix(0.1 * np.diag([1,1,0.5]))
 
-    # self.K_P_omega_ref = np.matrix(1.5e-1 * np.diag([5,5,10]))
-    self.K_P_omega_ref = np.matrix(1.5e0 * np.diag([50,50,10]))
-    self.K_I_omega_ref = np.matrix(1e2 * np.diag([1,1,1]))
+    # self.K_P_omega_ref = np.matrix(1.5e0 * np.diag([50,50,10]))
+    # self.K_I_omega_ref = np.matrix(1e2 * np.diag([1,1,1]))
+    self.K_P_omega_ref = np.matrix(1.5e-1 * np.diag([5,5,10]))
+    self.K_I_omega_ref = np.matrix(0 * np.diag([0,0,0]))
 
     # ----------------------------------------------------------------
     #                   Translational Parameters MRAC
@@ -99,17 +99,17 @@ def __init__(self, flight_params: FlightParams):
     self.B_ref_rot = np.matrix(np.eye(3))
 
     # **Rotational** parameters Lyapunov equation
-    # self.Q_rot = np.matrix(7e-3 * np.diag([1,1,2]))
-    self.Q_rot = np.matrix(7e-3 * np.diag([2,2,2]))
+    # self.Q_rot = np.matrix(7e-3 * np.diag([2,2,2]))
+    self.Q_rot = np.matrix(7e-3 * np.diag([1,1,1]))
     self.P_rot = np.matrix(linalg.solve_continuous_lyapunov(self.A_ref_rot.T, -self.Q_rot))
 
     # **Rotational** adaptive parameters
-    # self.Gamma_x_rot = np.matrix(1e1 * np.diag([1,1,10])) # Adaptive rates
-    # self.Gamma_r_rot = np.matrix(1e-4 * np.diag([1,1,1])) # Adaptive rates
-    # self.Gamma_Theta_rot = np.matrix(1e0 * np.diag([1,1,1,1,1,1])) # Adaptive rates
-    self.Gamma_x_rot = np.matrix(1e4 * np.diag([1,1,1])) # Adaptive rates
-    self.Gamma_r_rot = np.matrix(1e1 * np.diag([1,1,1])) # Adaptive rates
-    self.Gamma_Theta_rot = np.matrix(1e2 * np.diag([1,1,1,1,1,1])) # Adaptive rates
+    # self.Gamma_x_rot = np.matrix(1e4 * np.diag([1,1,1])) # Adaptive rates
+    # self.Gamma_r_rot = np.matrix(1e1 * np.diag([1,1,1])) # Adaptive rates
+    # self.Gamma_Theta_rot = np.matrix(1e2 * np.diag([1,1,1,1,1,1])) # Adaptive rates
+    self.Gamma_x_rot = np.matrix(1e1 * np.diag([1,1,1])) # Adaptive rates
+    self.Gamma_r_rot = np.matrix(1e-4 * np.diag([1,1,1])) # Adaptive rates
+    self.Gamma_Theta_rot = np.matrix(1e0 * np.diag([1,1,1,1,1,1])) # Adaptive rates
 
     # ----------------------------------------------------------------
     #                   Safety Mechanism Parameters

acsl_pychrono/control/base_mrac.py

@@ -7,6 +7,18 @@
 from acsl_pychrono.control.control import Control
 
 class BaseMRAC():  
+  def reshapeAdaptiveGainsToMatrices(self):
+    """
+    Reshapes all gain parameters to their correct (row, col) shape and converts them to np.matrix.
+    This is intended to be called once after loading or updating gains stored as flat arrays.
+    """
+    self.K_hat_x_tran = np.matrix(self.K_hat_x_tran.reshape(6,3))
+    self.K_hat_r_tran = np.matrix(self.K_hat_r_tran.reshape(3,3))
+    self.Theta_hat_tran = np.matrix(self.Theta_hat_tran.reshape(6,3))
+    self.K_hat_x_rot = np.matrix(self.K_hat_x_rot.reshape(3,3))
+    self.K_hat_r_rot = np.matrix(self.K_hat_r_rot.reshape(3,3))
+    self.Theta_hat_rot = np.matrix(self.Theta_hat_rot.reshape(6,3))
+
   def computeTrajectoryTrackingErrors(self, odein: OdeInput):
     """
     Computes translational and rotational tracking errors and extracts the reference position.
@@ -160,11 +172,6 @@ def computeRegressorVectorInnerLoop(self):
 
     return Phi_adaptive_rot, Phi_adaptive_rot_augmented
 
-  def compute_eTransposePB_InnerLoop(self):
-    eTranspose_P_B_rot = self.e_rot.T * self.gains.P_rot * self.gains.B_rot
-
-    return eTranspose_P_B_rot
-  
   def computeMomentBaselineInnerLoop(self):
     Moment_baseline = np.cross(
       self.odein.angular_velocity.ravel(),