Fix global mutate of casadi symbols

Author: amacati

docs/user-guide/symbolic.md

@@ -75,10 +75,7 @@ State vector layout with `model_rotor_vel=True`:
 
 Both functions return raw CasADi expressions. Wrap them in a `cs.Function` to pass to any CasADi-based solver:
 
-!!! warning
-    This is currently broken. Investigate and fix the issue.
-
-```{ .python notest }
+```python
 import casadi as cs
 from drone_models.so_rpy_rotor_drag import symbolic_dynamics_euler
 from drone_models.core import parametrize

drone_models/first_principles/model.py

@@ -231,6 +231,7 @@ def symbolic_dynamics(
             + rotor_dyn_coef[3] * (U**2 - symbols.rotor_vel**2),
         )
     else:
+        _saved_rotor_vel = symbols.rotor_vel
         symbols.rotor_vel = U
     # Creating force and torque vector
     forces_motor = (
@@ -281,4 +282,6 @@ def symbolic_dynamics(
         X_dot = cs.vertcat(pos_dot, quat_dot, vel_dot, ang_vel_dot)
     Y = cs.vertcat(symbols.pos, symbols.quat)
 
+    if not model_rotor_vel:
+        symbols.rotor_vel = _saved_rotor_vel
     return X_dot, X, U, Y

drone_models/so_rpy/model.py

@@ -164,8 +164,12 @@ def symbolic_dynamics(
         * ``Y``: Output ``[pos(3), quat(4)]``.
     """
     # We need to set the rpy and drpy symbols before building the euler model
-    symbols.rpy = rotation.cs_quat2euler(symbols.quat)
-    symbols.drpy = rotation.cs_ang_vel2rpy_rates(symbols.quat, symbols.ang_vel)
+    _saved_rpy = symbols.rpy
+    _saved_drpy = symbols.drpy
+    _rpy_quat = rotation.cs_quat2euler(symbols.quat)
+    _drpy_quat = rotation.cs_ang_vel2rpy_rates(symbols.quat, symbols.ang_vel)
+    symbols.rpy = _rpy_quat
+    symbols.drpy = _drpy_quat
     X_dot_euler, X_euler, U_euler, Y_euler = symbolic_dynamics_euler(
         model_rotor_vel=model_rotor_vel,
         mass=mass,
@@ -178,6 +182,8 @@ def symbolic_dynamics(
         rpy_rates_coef=rpy_rates_coef,
         cmd_rpy_coef=cmd_rpy_coef,
     )
+    symbols.rpy = _saved_rpy
+    symbols.drpy = _saved_drpy
 
     # States and Inputs
     X = cs.vertcat(symbols.pos, symbols.quat, symbols.vel, symbols.ang_vel)
@@ -203,7 +209,7 @@ def symbolic_dynamics(
     )
     quat_dot = 0.5 * (xi @ symbols.quat)
     ang_vel_dot = rotation.cs_rpy_rates_deriv2ang_vel_deriv(
-        symbols.quat, symbols.drpy, X_dot_euler[9:12]
+        symbols.quat, _drpy_quat, X_dot_euler[9:12]
     )
     if model_dist_t:
         # adding torque disturbances to the state

drone_models/so_rpy_rotor/model.py

@@ -184,8 +184,12 @@ def symbolic_dynamics(
         * ``Y``: Output ``[pos(3), quat(4)]``.
     """
     ## We need to set the rpy and drpy symbols before building the euler model
-    symbols.rpy = rotation.cs_quat2euler(symbols.quat)
-    symbols.drpy = rotation.cs_ang_vel2rpy_rates(symbols.quat, symbols.ang_vel)
+    _saved_rpy = symbols.rpy
+    _saved_drpy = symbols.drpy
+    _rpy_quat = rotation.cs_quat2euler(symbols.quat)
+    _drpy_quat = rotation.cs_ang_vel2rpy_rates(symbols.quat, symbols.ang_vel)
+    symbols.rpy = _rpy_quat
+    symbols.drpy = _drpy_quat
     X_dot_euler, X_euler, U_euler, Y_euler = symbolic_dynamics_euler(
         model_rotor_vel=model_rotor_vel,
         mass=mass,
@@ -199,6 +203,8 @@ def symbolic_dynamics(
         rpy_rates_coef=rpy_rates_coef,
         cmd_rpy_coef=cmd_rpy_coef,
     )
+    symbols.rpy = _saved_rpy
+    symbols.drpy = _saved_drpy
 
     # States and Inputs
     X = cs.vertcat(symbols.pos, symbols.quat, symbols.vel, symbols.ang_vel)
@@ -223,7 +229,7 @@ def symbolic_dynamics(
     )
     quat_dot = 0.5 * (xi @ symbols.quat)
     ang_vel_dot = rotation.cs_rpy_rates_deriv2ang_vel_deriv(
-        symbols.quat, symbols.drpy, X_dot_euler[9:12]
+        symbols.quat, _drpy_quat, X_dot_euler[9:12]
     )
     if model_dist_t:
         # adding torque disturbances to the state

drone_models/so_rpy_rotor_drag/model.py

@@ -199,9 +199,15 @@ def symbolic_dynamics(
         * ``U``: Input vector ``[roll_rad, pitch_rad, yaw_rad, thrust_N]``.
         * ``Y``: Output ``[pos(3), quat(4)]``.
     """
-    # We need to set the rpy and drpy symbols before building the euler model
-    symbols.rpy = rotation.cs_quat2euler(symbols.quat)
-    symbols.drpy = rotation.cs_ang_vel2rpy_rates(symbols.quat, symbols.ang_vel)
+    # Temporarily override rpy/drpy so symbolic_dynamics_euler uses quaternion-derived
+    # expressions for this call. Restore them afterwards so subsequent calls to
+    # symbolic_dynamics_euler still get the original leaf symbolic variables.
+    _saved_rpy = symbols.rpy
+    _saved_drpy = symbols.drpy
+    _rpy_quat = rotation.cs_quat2euler(symbols.quat)
+    _drpy_quat = rotation.cs_ang_vel2rpy_rates(symbols.quat, symbols.ang_vel)
+    symbols.rpy = _rpy_quat
+    symbols.drpy = _drpy_quat
     X_dot_euler, X_euler, U_euler, Y_euler = symbolic_dynamics_euler(
         model_rotor_vel=model_rotor_vel,
         mass=mass,
@@ -216,6 +222,8 @@ def symbolic_dynamics(
         cmd_rpy_coef=cmd_rpy_coef,
         drag_matrix=drag_matrix,
     )
+    symbols.rpy = _saved_rpy
+    symbols.drpy = _saved_drpy
 
     # States and Inputs
     X = cs.vertcat(symbols.pos, symbols.quat, symbols.vel, symbols.ang_vel)
@@ -240,7 +248,7 @@ def symbolic_dynamics(
     )
     quat_dot = 0.5 * (xi @ symbols.quat)
     ang_vel_dot = rotation.cs_rpy_rates_deriv2ang_vel_deriv(
-        symbols.quat, symbols.drpy, X_dot_euler[9:12]
+        symbols.quat, _drpy_quat, X_dot_euler[9:12]
     )
     if model_dist_t:
         # adding torque disturbances to the state

pixi.lock

@@ -138,7 +138,7 @@ pytest-markdown-docs = "*"
 
 [tool.pixi.feature.tests.tasks]
 tests = { cmd = "pytest -v tests", description = "Run tests" }
-test-docs = { cmd = "pytest --markdown-docs --markdown-docs-syntax=superfences drone_models/ docs/ --ignore=docs/gen_ref_pages.py", env = { SCIPY_ARRAY_API = "1" }, description = "Run doctests for docstrings and documentation" }
+test-docs = { cmd = "pytest -v --markdown-docs --markdown-docs-syntax=superfences drone_models/ docs/ --ignore=docs/gen_ref_pages.py", env = { SCIPY_ARRAY_API = "1" }, description = "Run doctests for docstrings and documentation" }
 
 [tool.pixi.feature.docs.dependencies]
 mkdocs = "*"