From 6895da87bdde64d30e446410cd1789da7038a25c Mon Sep 17 00:00:00 2001 From: Martin Schuck Date: Wed, 18 Nov 2026 15:31:58 +0100 Subject: [PATCH 1/2] Fix thrust clipping in the controllers --- crazyflow/control/mellinger/control.py | 13 ++++++++----- tests/fuzzing/test_mellinger.py | 13 +++++++------ 2 files changed, 15 insertions(+), 11 deletions(-) diff --git a/crazyflow/control/mellinger/control.py b/crazyflow/control/mellinger/control.py index 1e845f23..72c1486e 100644 --- a/crazyflow/control/mellinger/control.py +++ b/crazyflow/control/mellinger/control.py @@ -409,8 +409,11 @@ def _attitude2force_torque( torque_pwm = xp.where((force_des > 0)[..., None], torque_pwm, 0.0) force_des_pwm = force2pwm(force_des / 4, thrust_max, pwm_max) pwms = force_torque_pwms2pwms(force_des_pwm, torque_pwm, mixing_matrix) - idle = xp.all(pwms == 0, axis=-1, keepdims=True) - pwms = xp.where(idle, 0.0, xp.clip(pwms, pwm_min, pwm_max)) + # We assume battery compensation is perfect. Motors below pwm_min are turned off. See + # https://github.com/bitcraze/crazyflie-firmware/blob/d794c5ca081d19aa1e1bbcb6fd4a3711784fb8b1/src/drivers/src/motors.c#L199-L202 + pwms = xp.where(pwms < pwm_min, 0.0, pwms) + excess = xp.clip(xp.max(pwms, axis=-1, keepdims=True) - pwm_max, min=0.0) + pwms = xp.clip(pwms - excess, min=0.0) # Info: The Mellinger controller in the firmware ends here. However, we enforce a standardized # interface in the simulation from states -> attitude -> force_torque. We therefore need this @@ -481,9 +484,9 @@ def force_torque2rotor_vel( torque_forces = (torque * xp.asarray([1 / L, 1 / L, 1 / thrust2torque])) @ mixing_matrix motor_forces = (torque_forces + force) / 4 # Clip motor forces on the thrust instead of PWM level. - idle = xp.all(force == 0, axis=-1, keepdims=True) - motor_forces = xp.where(idle, 0.0, xp.clip(motor_forces, thrust_min, thrust_max)) - # Assume perfect battery compensation and calculate the desired motor speeds directly + motor_forces = xp.where(motor_forces < thrust_min, 0.0, motor_forces) + excess = xp.clip(xp.max(motor_forces, axis=-1, keepdims=True) - thrust_max, min=0.0) + motor_forces = xp.clip(motor_forces - excess, min=0.0) return motor_force2rotor_vel(motor_forces, rpm2thrust) diff --git a/tests/fuzzing/test_mellinger.py b/tests/fuzzing/test_mellinger.py index a7d9ebee..9e312ac3 100644 --- a/tests/fuzzing/test_mellinger.py +++ b/tests/fuzzing/test_mellinger.py @@ -266,11 +266,7 @@ def test_mixer(thrust: NDArray, torque: NDArray): np.testing.assert_allclose(motors, [m.m1, m.m2, m.m3, m.m4], atol=3) -@pytest.mark.xfail( - strict=True, - reason="TODO: Clip as the firmware, uniform above the maximum and at 0, and use the current " - "and previous body rate setpoint", -) +@pytest.mark.xfail(strict=True, reason="TODO: Use the current and previous body rate setpoint") @given(state=_states(), ctrl_state=_ctrl_states(), cmd=_state_cmds()) def test_end_to_end(state: tuple[NDArray, ...], ctrl_state: tuple[NDArray, ...], cmd: NDArray): """Compare the rotor velocities for state commands.""" @@ -305,7 +301,12 @@ def test_end_to_end(state: tuple[NDArray, ...], ctrl_state: tuple[NDArray, ...], assume(abs(out.thrust) > 1) uncapped, capped = cf.motors_thrust_uncapped_t(), cf.motors_thrust_pwm_t() cf.powerDistribution(out, uncapped) - cf.powerDistributionCap(uncapped, capped) # We always assume perfect battery compensation + # cffirmware does not run the battery compensation, which turns off motors below the minimum + # thrust. We assume perfect compensation + m = uncapped.motors + pwms = [m.m1, m.m2, m.m3, m.m4] + m.m1, m.m2, m.m3, m.m4 = [0 if pwm < params["pwm_min"] else pwm for pwm in (pwms)] + cf.powerDistributionCap(uncapped, capped) m = capped.motors pwms = np.array([m.m1, m.m2, m.m3, m.m4]) motor_forces = pwm2force(pwms, params["thrust_max"], params["pwm_max"]) From db95af4738497bc2ea4b340b1229c4377da37f2c Mon Sep 17 00:00:00 2001 From: Martin Schuck Date: Wed, 18 Nov 2026 15:50:18 +0100 Subject: [PATCH 2/2] Fix tests and apply review comments --- tests/fuzzing/test_mellinger.py | 4 ++-- tests/unit/control/test_mellinger.py | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/tests/fuzzing/test_mellinger.py b/tests/fuzzing/test_mellinger.py index 9e312ac3..ea50f819 100644 --- a/tests/fuzzing/test_mellinger.py +++ b/tests/fuzzing/test_mellinger.py @@ -301,8 +301,8 @@ def test_end_to_end(state: tuple[NDArray, ...], ctrl_state: tuple[NDArray, ...], assume(abs(out.thrust) > 1) uncapped, capped = cf.motors_thrust_uncapped_t(), cf.motors_thrust_pwm_t() cf.powerDistribution(out, uncapped) - # cffirmware does not run the battery compensation, which turns off motors below the minimum - # thrust. We assume perfect compensation + # cffirmware does not run the battery compensation. The battery compensation turns off motors + # below the minimum thrust. We assume perfect compensation, so we clip manually m = uncapped.motors pwms = [m.m1, m.m2, m.m3, m.m4] m.m1, m.m2, m.m3, m.m4 = [0 if pwm < params["pwm_min"] else pwm for pwm in (pwms)] diff --git a/tests/unit/control/test_mellinger.py b/tests/unit/control/test_mellinger.py index d65e3e65..426e3fe8 100644 --- a/tests/unit/control/test_mellinger.py +++ b/tests/unit/control/test_mellinger.py @@ -351,7 +351,8 @@ def test_attitude2force_torque_batch_zero_thrust(drone: str): def test_force_torque2rotor_vel_batch_zero_force(drone: str): # Drones with zero desired force must not be clipped because other drones have non-zero force. controller = parametrize(force_torque2rotor_vel, drone) - force = np.array([[0.0], [0.2]]) + thrust_min = load_fn_params(force_torque2rotor_vel, drone)["thrust_min"] + force = np.array([[0.0], [8 * thrust_min]]) torque = np.zeros((2, 3)) rotor_vel_batch = controller(force, torque) rotor_vel_single = controller(force[0], torque[0])