|
| 1 | +"""Regression tests for #104: motor count must be derived from ``mixing_matrix.shape[-1]``. |
| 2 | +
|
| 3 | +These tests exercise the changed pure functions directly with a synthetic 6-column mixing matrix, |
| 4 | +without registering a full hexacopter drone (MuJoCo asset + fitted dynamics coefficients for all |
| 5 | +four dynamics models), since none of that exists for a real non-quadcopter platform yet. |
| 6 | +""" |
| 7 | + |
| 8 | +from __future__ import annotations |
| 9 | + |
| 10 | +import jax |
| 11 | +import numpy as np |
| 12 | +import pytest |
| 13 | + |
| 14 | +from crazyflow.control import Control, load_params |
| 15 | +from crazyflow.control.mellinger import force_torque2rotor_vel, state2attitude |
| 16 | +from crazyflow.control.transform import motor_force2rotor_vel |
| 17 | +from crazyflow.dynamics import Dynamics |
| 18 | +from crazyflow.envs.drone_env import action_space |
| 19 | +from crazyflow.sim.data import SimControls, SimCore, SimData, SimParams, SimState, SimStateDeriv |
| 20 | +from crazyflow.sim.functional import rotor_vel_control |
| 21 | +from crazyflow.sim.sim import rotor_vel_limits |
| 22 | + |
| 23 | +N_MOTORS = 6 |
| 24 | + |
| 25 | +# Arbitrary, physically meaningless mixing matrix: only the motor count (last dimension) matters. |
| 26 | +HEXA_MIXING_MATRIX = np.array( |
| 27 | + [ |
| 28 | + [-1.0, -1.0, 0.0, 1.0, 1.0, 0.0], |
| 29 | + [-0.5, 0.5, 1.0, 0.5, -0.5, -1.0], |
| 30 | + [1.0, -1.0, 1.0, -1.0, 1.0, -1.0], |
| 31 | + ] |
| 32 | +) |
| 33 | + |
| 34 | + |
| 35 | +@pytest.mark.unit |
| 36 | +def test_sim_data_buffers_scale_with_n_motors(): |
| 37 | + device = jax.devices("cpu")[0] |
| 38 | + states = SimState.create(n_worlds=2, n_drones=3, n_motors=N_MOTORS, device=device) |
| 39 | + assert states.rotor_vel.shape == (2, 3, N_MOTORS) |
| 40 | + assert states.quat.shape == (2, 3, 4) # Unaffected: quaternion, not motor count |
| 41 | + |
| 42 | + states_deriv = SimStateDeriv.create(n_worlds=2, n_drones=3, n_motors=N_MOTORS, device=device) |
| 43 | + assert states_deriv.rotor_acc.shape == (2, 3, N_MOTORS) |
| 44 | + |
| 45 | + controls = SimControls.create( |
| 46 | + n_worlds=2, |
| 47 | + n_drones=3, |
| 48 | + n_motors=N_MOTORS, |
| 49 | + control=Control.rotor_vel, |
| 50 | + drone="cf2x_L250", |
| 51 | + state_freq=None, |
| 52 | + attitude_freq=None, |
| 53 | + body_rate_freq=None, |
| 54 | + force_torque_freq=None, |
| 55 | + device=device, |
| 56 | + ) |
| 57 | + assert controls.rotor_vel.shape == (2, 3, N_MOTORS) |
| 58 | + |
| 59 | + |
| 60 | +def _build_rotor_vel_sim_data(n_worlds: int, n_drones: int, n_motors: int) -> SimData: |
| 61 | + """Minimal SimData in Control.rotor_vel mode, with rotor buffers sized for n_motors.""" |
| 62 | + device = jax.devices("cpu")[0] |
| 63 | + return SimData( |
| 64 | + states=SimState.create(n_worlds, n_drones, n_motors, device), |
| 65 | + states_deriv=SimStateDeriv.create(n_worlds, n_drones, n_motors, device), |
| 66 | + controls=SimControls.create( |
| 67 | + n_worlds, |
| 68 | + n_drones, |
| 69 | + n_motors, |
| 70 | + Control.rotor_vel, |
| 71 | + "cf2x_L250", |
| 72 | + None, |
| 73 | + None, |
| 74 | + None, |
| 75 | + None, |
| 76 | + device, |
| 77 | + ), |
| 78 | + params=SimParams.create(Dynamics.first_principles, "cf2x_L250", device), |
| 79 | + core=SimCore.create(500, n_worlds, n_drones, list(range(n_drones)), 0, device), |
| 80 | + ) |
| 81 | + |
| 82 | + |
| 83 | +@pytest.mark.unit |
| 84 | +def test_rotor_vel_control_accepts_n_motors_shape(): |
| 85 | + n_worlds, n_drones = 2, 1 |
| 86 | + data = _build_rotor_vel_sim_data(n_worlds, n_drones, N_MOTORS) |
| 87 | + |
| 88 | + controls = np.full((n_worlds, n_drones, N_MOTORS), 1000.0) |
| 89 | + updated = rotor_vel_control(data, controls) |
| 90 | + assert updated.controls.rotor_vel.shape == (n_worlds, n_drones, N_MOTORS) |
| 91 | + |
| 92 | + # A control array shaped for the old hardcoded 4-motor assumption must be rejected. |
| 93 | + wrong_shape_controls = np.full((n_worlds, n_drones, 4), 1000.0) |
| 94 | + with pytest.raises(AssertionError): |
| 95 | + rotor_vel_control(data, wrong_shape_controls) |
| 96 | + |
| 97 | + |
| 98 | +@pytest.mark.unit |
| 99 | +def test_force_torque2rotor_vel_scales_with_mixing_matrix(): |
| 100 | + params = load_params(force_torque2rotor_vel, "cf2x_L250") |
| 101 | + params["mixing_matrix"] = HEXA_MIXING_MATRIX |
| 102 | + # Zero torque: thrust must split evenly across all N_MOTORS motors, not divided by 4. Keep the |
| 103 | + # per-motor share (force / N_MOTORS) within [thrust_min, thrust_max] so it isn't clipped. |
| 104 | + force = np.array([0.3]) |
| 105 | + torque = np.zeros(3) |
| 106 | + rotor_vel = force_torque2rotor_vel(force, torque, **params) |
| 107 | + assert rotor_vel.shape == (N_MOTORS,) |
| 108 | + expected = motor_force2rotor_vel(force / N_MOTORS, params["rpm2thrust"]) |
| 109 | + assert rotor_vel == pytest.approx(np.full(N_MOTORS, expected.item())) |
| 110 | + |
| 111 | + |
| 112 | +@pytest.mark.unit |
| 113 | +def test_state2attitude_scales_with_mixing_matrix(): |
| 114 | + params = load_params(state2attitude, "cf2x_L250") |
| 115 | + params["mixing_matrix"] = HEXA_MIXING_MATRIX |
| 116 | + pos, quat, vel = np.zeros(3), np.array([0.0, 0.0, 0.0, 1.0]), np.zeros(3) |
| 117 | + cmd = np.zeros(13) |
| 118 | + rpyt, pos_err_i = state2attitude(pos, quat, vel, cmd, ctrl_freq=100, **params) |
| 119 | + # Collective thrust command stays 4D ([roll, pitch, yaw, thrust]) regardless of motor count. |
| 120 | + assert rpyt.shape == (4,) |
| 121 | + assert pos_err_i.shape == (3,) |
| 122 | + |
| 123 | + |
| 124 | +@pytest.mark.unit |
| 125 | +def test_rotor_vel_limits_scale_with_n_motors(monkeypatch: pytest.MonkeyPatch): |
| 126 | + import crazyflow.sim.sim as sim_module |
| 127 | + |
| 128 | + real_params = sim_module.load_drone_params("cf2x_L250") |
| 129 | + hexa_params = real_params | {"mixing_matrix": HEXA_MIXING_MATRIX.tolist()} |
| 130 | + monkeypatch.setattr(sim_module, "load_drone_params", lambda drone: hexa_params) |
| 131 | + |
| 132 | + lower, upper = rotor_vel_limits(Dynamics.so_rpy, "cf2x_L250") |
| 133 | + assert lower == pytest.approx(N_MOTORS * real_params["thrust_min"]) |
| 134 | + assert upper == pytest.approx(N_MOTORS * real_params["thrust_max"]) |
| 135 | + |
| 136 | + |
| 137 | +@pytest.mark.unit |
| 138 | +def test_action_space_thrust_bounds_scale_with_n_motors(monkeypatch: pytest.MonkeyPatch): |
| 139 | + import crazyflow.envs.drone_env as drone_env_module |
| 140 | + |
| 141 | + real_params = drone_env_module.load_params("cf2x_L250") |
| 142 | + hexa_params = real_params | {"mixing_matrix": HEXA_MIXING_MATRIX.tolist()} |
| 143 | + monkeypatch.setattr(drone_env_module, "load_params", lambda drone: hexa_params) |
| 144 | + |
| 145 | + space = action_space(Control.attitude, "cf2x_L250") |
| 146 | + assert space.low[-1] == pytest.approx(N_MOTORS * real_params["thrust_min"]) |
| 147 | + assert space.high[-1] == pytest.approx(N_MOTORS * real_params["thrust_max"]) |
0 commit comments