Skip to content

Commit 26bdf2d

Browse files
author
Lagnesh2003
committed
Add regression tests for arbitrary motor count support
1 parent 9900074 commit 26bdf2d

1 file changed

Lines changed: 147 additions & 0 deletions

File tree

Lines changed: 147 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,147 @@
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

Comments
 (0)