From f23262f2a9a504adca109e73d148346662caa580 Mon Sep 17 00:00:00 2001 From: Martin Schuck Date: Wed, 18 Nov 2026 18:38:30 +0100 Subject: [PATCH] Fix double clipping in chained controllers and final clipping in sim pipeline --- crazyflow/control/mellinger/control.py | 16 +------- crazyflow/sim/sim.py | 4 +- tests/fuzzing/test_mellinger.py | 53 +++++++++++++++++--------- tests/unit/test_sim.py | 2 +- 4 files changed, 41 insertions(+), 34 deletions(-) diff --git a/crazyflow/control/mellinger/control.py b/crazyflow/control/mellinger/control.py index 07ff08bd..02d5c841 100644 --- a/crazyflow/control/mellinger/control.py +++ b/crazyflow/control/mellinger/control.py @@ -162,7 +162,6 @@ def attitude2force_torque( int_err_max: Array, torque_pwm_max: Array, thrust_max: float, - pwm_min: float, pwm_max: float, L: float, thrust2torque: float, @@ -190,7 +189,6 @@ def attitude2force_torque( int_err_max: Range of the integral error with shape (3,). i_range in the firmware. torque_pwm_max: Maximum torque in PWM. thrust_max: Maximum thrust in N. - pwm_min: Minimum PWM value. pwm_max: Maximum PWM value. L: Distance from the center of the quadrotor to the center of the rotor in m. thrust2torque: Conversion factor (m). @@ -218,7 +216,6 @@ def attitude2force_torque( int_err_max=int_err_max, torque_pwm_max=torque_pwm_max, thrust_max=thrust_max, - pwm_min=pwm_min, pwm_max=pwm_max, L=L, thrust2torque=thrust2torque, @@ -242,7 +239,6 @@ def body_rate2force_torque( int_err_max: Array, torque_pwm_max: Array, thrust_max: float, - pwm_min: float, pwm_max: float, L: float, thrust2torque: float, @@ -278,7 +274,6 @@ def body_rate2force_torque( int_err_max: Range of the integral error with shape (3,). i_range in the firmware. torque_pwm_max: Maximum torque in PWM. thrust_max: Maximum thrust in N. - pwm_min: Minimum PWM value. pwm_max: Maximum PWM value. L: Distance from the center of the quadrotor to the center of the rotor in m. thrust2torque: Conversion factor (m). @@ -311,7 +306,6 @@ def body_rate2force_torque( int_err_max=int_err_max, torque_pwm_max=torque_pwm_max, thrust_max=thrust_max, - pwm_min=pwm_min, pwm_max=pwm_max, L=L, thrust2torque=thrust2torque, @@ -337,7 +331,6 @@ def _attitude2force_torque( int_err_max: Array, torque_pwm_max: Array, thrust_max: float, - pwm_min: float, pwm_max: float, L: float, thrust2torque: float, @@ -367,7 +360,6 @@ def _attitude2force_torque( int_err_max: Range of the integral error with shape (3,). i_range in the firmware. torque_pwm_max: Maximum torque in PWM. thrust_max: Maximum thrust in N. - pwm_min: Minimum PWM value. pwm_max: Maximum PWM value. L: Distance from the center of the quadrotor to the center of the rotor in m. thrust2torque: Conversion factor (m). @@ -412,11 +404,6 @@ def _attitude2force_torque( rot_err_i = xp.where((force_des > 0)[..., None], rot_err_i, 0.0) force_des_pwm = force2pwm(force_des / 4, thrust_max, pwm_max) pwms = force_torque_pwms2pwms(force_des_pwm, torque_pwm, mixing_matrix) - # 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 @@ -486,7 +473,8 @@ def force_torque2rotor_vel( assert force.shape[-1] == 1, f"Force must have shape (..., 1), but has {force.shape}" 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. + # Clip motor forces on the thrust instead of PWM level. Motors below thrust_min are turned off, + # see https://github.com/bitcraze/crazyflie-firmware/blob/d794c5ca081d19aa1e1bbcb6fd4a3711784fb8b1/src/drivers/src/motors.c#L199-L202 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) diff --git a/crazyflow/sim/sim.py b/crazyflow/sim/sim.py index e0c61af9..5180f23c 100644 --- a/crazyflow/sim/sim.py +++ b/crazyflow/sim/sim.py @@ -827,8 +827,8 @@ def rotor_vel_limits(dynamics: Dynamics, drone: Drone) -> tuple[float, float]: params = load_dynamics_params(dynamics, drone) thrust_min, thrust_max = float(params["thrust_min"]), float(params["thrust_max"]) if dynamics == Dynamics.first_principles: - rpm = motor_force2rotor_vel(np.asarray([thrust_min, thrust_max]), params["rpm2thrust"]) - return float(rpm[0]), float(rpm[1]) + rpm_max = motor_force2rotor_vel(np.asarray(thrust_max), params["rpm2thrust"]) + return 0.0, float(rpm_max) return 4 * thrust_min, 4 * thrust_max diff --git a/tests/fuzzing/test_mellinger.py b/tests/fuzzing/test_mellinger.py index 741a0293..8979f4f5 100644 --- a/tests/fuzzing/test_mellinger.py +++ b/tests/fuzzing/test_mellinger.py @@ -15,10 +15,11 @@ from hypothesis.extra import numpy as hnp from scipy.spatial.transform import Rotation as R -from crazyflow.control import load_fn_params, load_params, parametrize +from crazyflow.control import Control, load_fn_params, load_params, parametrize from crazyflow.control.mellinger import control from crazyflow.control.transform import force2pwm, motor_force2rotor_vel, pwm2force from crazyflow.drones import Drone +from crazyflow.sim import Sim if TYPE_CHECKING: from numpy.typing import NDArray @@ -240,8 +241,9 @@ def test_body_rate2force_torque( np.testing.assert_allclose(thrust, out.thrust, atol=1) # See test_attitude2force_torque for why we assume a minimum thrust for torque comparison assume(abs(out.thrust) > 1) - # The yaw is undefined at 90 degrees pitch, and both controllers level at the current yaw - assume(abs(R.from_quat(quat).as_matrix()[2, 0]) < 1 - 1e-6) + # The yaw is undefined at 90 degrees pitch + pitch = R.from_quat(quat).as_euler("xyz", suppress_warnings=True)[1] + assume(abs(pitch) < np.pi / 2 - 1e-3) np.testing.assert_allclose(torque, [out.roll, out.pitch, out.yaw], atol=2) ref_rot_err_i = [ref.i_error_m_x, -ref.i_error_m_y, ref.i_error_m_z] np.testing.assert_allclose(rot_err_i, ref_rot_err_i, atol=1e-6) @@ -264,24 +266,41 @@ 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: Use the current and previous body rate setpoint") +@pytest.fixture(scope="module") +def sim() -> Sim: + """Simulate one cf2x_L250 with the state controller at the firmware's controller rate.""" + sim = Sim(drone=Drone.cf2x_L250, control=Control.state, state_freq=cf.ATTITUDE_RATE) + sim.step() # Compile outside the examples to stay within the hypothesis deadline + return sim + + @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.""" - pos, vel, quat, ang_vel = map(jnp.asarray, state) - pos_err_i, rot_err_i, prev_ang_vel, _ = map(jnp.asarray, ctrl_state) - # Jitting the module functions keeps JAX's compilation cache across examples - state2attitude = parametrize(jax.jit(control.state2attitude), Drone.cf2x_L250, xp=jnp) - attitude2force_torque = parametrize( - jax.jit(control.attitude2force_torque), Drone.cf2x_L250, xp=jnp +def test_end_to_end( + sim: Sim, state: tuple[NDArray, ...], ctrl_state: tuple[NDArray, ...], cmd: NDArray +): + """Compare the rotor velocities of a simulated state control step.""" + # The setpoint yaw is undefined at 90 degrees pitch + pitch_des = R.from_quat(cmd[9:13]).as_euler("xyz", suppress_warnings=True)[1] + assume(abs(pitch_des) < np.pi / 2 - 1e-3) + pos, vel, quat, ang_vel = jax.device_put([x[None, None] for x in state], sim.device) + pos_err_i, rot_err_i, prev_ang_vel, prev_ang_vel_des = jax.device_put( + [x[None, None] for x in ctrl_state], sim.device ) - force_torque2rotor_vel = parametrize( - jax.jit(control.force_torque2rotor_vel), Drone.cf2x_L250, xp=jnp + sim.reset() + data = sim.data + controls = sim.data.controls + data = data.replace(states=data.states.replace(pos=pos, vel=vel, quat=quat, ang_vel=ang_vel)) + controls = controls.replace(state=data.controls.state.replace(pos_err_i=pos_err_i)) + controls = controls.replace( + attitude=data.controls.attitude.replace( + rot_err_i=rot_err_i, prev_ang_vel=prev_ang_vel, ang_vel_des=prev_ang_vel_des + ) ) + sim.data = data.replace(controls=controls) + sim.state_control(jax.device_put(cmd[None, None], sim.device)) + sim.step() + rotor_vel = sim.data.controls.rotor_vel[0, 0] params = load_params("mellinger", Drone.cf2x_L250)["core"] - rpyt, _ = state2attitude(pos, quat, vel, jnp.asarray(cmd), pos_err_i, cf.ATTITUDE_RATE) - force, torque, _ = attitude2force_torque(quat, ang_vel, rpyt, prev_ang_vel, rot_err_i) - rotor_vel = force_torque2rotor_vel(force, torque) setpoint = cf.setpoint_t() sp_pos = setpoint.position sp_pos.x, sp_pos.y, sp_pos.z = cmd[:3].tolist() diff --git a/tests/unit/test_sim.py b/tests/unit/test_sim.py index 915647e4..bc3438ff 100644 --- a/tests/unit/test_sim.py +++ b/tests/unit/test_sim.py @@ -466,7 +466,7 @@ def test_rotor_vel_clip(integrator: Integrator): device="cpu", ) lower, upper = rotor_vel_limits(Dynamics.first_principles, sim.drone) - assert 0.0 < lower < upper + assert 0.0 < upper for value, target in ((2 * upper, upper), (-upper, lower)): sim.reset()