reset mpc on fall too

config/base.gin

@@ -22,7 +22,6 @@ MPCBalancer.terminal_cost_weight = 1.0
 MPCBalancer.warm_start = True
 
 PIBalancer.air_return_period = 1.0                   # [s]
-PIBalancer.fall_pitch = 1.0                          # [rad]
 PIBalancer.max_integral_error_velocity = 10.0        # [m] / [s]
 PIBalancer.max_target_distance = 1.0                 # [m]
 
@@ -34,6 +33,7 @@ RemoteControl.max_linear_velocity = 0.5              # [m] / [s]
 RemoteControl.max_yaw_accel = 10.0                   # [rad] / [s]²
 RemoteControl.max_yaw_velocity = 1.0                 # [rad] / [s]
 
+SagittalBalancer.fall_pitch = 1.0                    # [rad]
 SagittalBalancer.max_ground_accel = 10.0             # [m] / [s]²
 SagittalBalancer.max_ground_velocity = 1.0           # [m] / [s]
 

pink_balancer/sagittal_balance/mpc_balancer.py

@@ -11,6 +11,7 @@
 from qpmpc import MPCQP, Plan
 from qpmpc.systems import WheeledInvertedPendulum
 from qpsolvers import solve_problem
+from upkie.exceptions import FallDetected
 from upkie.utils.clamp import clamp_and_warn
 from upkie.utils.filters import low_pass_filter
 from upkie.utils.spdlog import logging
@@ -114,6 +115,10 @@ def compute_ground_velocity(
         ground_position = observation["wheel_odometry"]["position"]
         ground_velocity = observation["wheel_odometry"]["velocity"]
 
+        if abs(base_pitch) > self.fall_pitch:
+            raise FallDetected(
+                    f"Base angle {base_pitch=:.3} rad denotes a fall")
+
         # NB: state structure comes from WheeledInvertedPendulum
         cur_state = np.array(
             [

pink_balancer/sagittal_balance/pi_balancer.py

@@ -44,7 +44,6 @@ class PIBalancer(SagittalBalancer):
     def __init__(
         self,
         air_return_period: float,
-        fall_pitch: float,
         max_integral_error_velocity: float,
         max_target_distance: float,
     ):
@@ -53,7 +52,6 @@ def __init__(
         Args:
             air_return_period: Cutoff period for resetting integrators while
                 the robot is in the air, in [s].
-            fall_pitch: Fall pitch threshold, in radians.
             max_integral_error_velocity: Maximum integral error velocity, in
                 [m] / [s].
             max_target_distance: Maximum distance from the current ground
@@ -62,7 +60,6 @@ def __init__(
         super().__init__()
         self.air_return_period = air_return_period
         self.error = np.zeros(2)
-        self.fall_pitch = fall_pitch
         self.gains = PIBalancerGains()
         self.integral_error_velocity = 0.0
         self.max_integral_error_velocity = max_integral_error_velocity

pink_balancer/sagittal_balance/sagittal_balancer.py

@@ -15,17 +15,20 @@ class SagittalBalancer(abc.ABC):
 
     def __init__(
         self,
+        fall_pitch: float,
         max_ground_accel: float,
         max_ground_velocity: float,
     ):
         """Initialize balancer.
 
         Args:
+            fall_pitch: Fall pitch threshold, in radians.
             max_ground_accel: Maximum commanded ground acceleration no matter
                 what, in [m] / [s]².
             max_ground_velocity: Maximum commanded ground velocity no matter
                 what, in [m] / [s].
         """
+        self.fall_pitch = fall_pitch
         self.max_ground_accel = max_ground_accel
         self.max_ground_velocity = max_ground_velocity