diff --git a/config/base.gin b/config/base.gin index 2ae012d..68de46b 100644 --- a/config/base.gin +++ b/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] diff --git a/pink_balancer/sagittal_balance/mpc_balancer.py b/pink_balancer/sagittal_balance/mpc_balancer.py index dbf7f6f..5ff3345 100644 --- a/pink_balancer/sagittal_balance/mpc_balancer.py +++ b/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( [ diff --git a/pink_balancer/sagittal_balance/pi_balancer.py b/pink_balancer/sagittal_balance/pi_balancer.py index 897afea..6a27195 100644 --- a/pink_balancer/sagittal_balance/pi_balancer.py +++ b/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 diff --git a/pink_balancer/sagittal_balance/sagittal_balancer.py b/pink_balancer/sagittal_balance/sagittal_balancer.py index 4272fa6..d04f0d8 100644 --- a/pink_balancer/sagittal_balance/sagittal_balancer.py +++ b/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