diff --git a/SKILL.md b/SKILL.md index ab4eaf95..0a28e084 100644 --- a/SKILL.md +++ b/SKILL.md @@ -16,7 +16,7 @@ Settings are in `pyproject.toml`, layout in the tree, API reference in `docs/`. `array_namespace(...)`, do all math through it, coerce bound parameters with `to_xp`, and never import `numpy` into a computation path. - `crazyflow/control/mellinger/params.toml` deliberately holds values that differ from the true - physical constants in `crazyflow/drones/params.toml`, reproducing the real firmware. Do not + physical constants in `crazyflow/dynamics/*/params.toml`, reproducing the real firmware. Do not unify them. ## Testing @@ -36,8 +36,8 @@ Use pixi, not uv. `pixi run ` resolves task names only, so arbitrary comman checked goes in a `pycon` fence with `>>>` prompts, which doctest picks up. - `tests/integration/test_examples.py` runs every script under `examples/`, so a new example is a new test. -- `tests/conftest.py` forces JAX's persistent cache on at `/tmp/jax_cache`, shared across branches. - Delete it when failures make no sense. +- `tests/conftest.py` forces JAX's persistent cache on at `/tmp/jax_cache-`, shared across + branches. Delete it when failures make no sense. - Request the `device` fixture rather than a GPU marker. It falls back to CPU silently, so `gpu-tests` asserts nothing about placement on a machine without CUDA. @@ -46,10 +46,12 @@ Use pixi, not uv. `pixi run ` resolves task names only, so arbitrary comman Grepping the name of an existing model or drone finds every registration site, except when matching against all models in the simulation's `build_control_fns`. -Define the function in `dynamics.py` and never in the package `__init__.py`, because `load_params` -derives the model name from `fn.__module__.split(".")[-2]`. `parametrize` binds exactly the -keyword-only parameters after the bare `*`, so anything before it is never bound. Every drone in -`available_drones` needs a section in every `crazyflow/dynamics/*/params.toml`, even an empty one. +Define the function in `dynamics.py` and never in the package `__init__.py`, because +`load_fn_params` derives the model name from `fn.__module__.split(".")[-2]`. `parametrize` +binds exactly the keyword-only parameters after the bare `*`, so anything before it is never bound. +Every drone in `available_drones` needs a complete section in every model's +`crazyflow/dynamics/*/params.toml`. The commented example at the top of each file lists the keys. +Only `gravity_vec` is global, in `crazyflow/dynamics/params.toml`. Registration alone produces roughly 40 parametrized tests. These do not include derivatives tests. @@ -60,9 +62,9 @@ Pure, batched, array-API functions with no dependency on `Sim`. - Import crazyflow before scipy. `crazyflow/__init__.py` sets `SCIPY_ARRAY_API=1` and imports scipy immediately, and scipy cannot be reconfigured once loaded. Transitive imports through acados or sklearn trigger this too. -- Three different `load_params` exist. The two in `.core` filter to the target signature and - silently drop the rest, so hardware constants like `thrust_max` need - `crazyflow.drones.load_params`. +- `dynamics` and `control` each have `load_params(name, drone)`, returning everything defined for + the drone, and `load_fn_params(fn, drone)`, which filters to the signature of `fn` and + drops the rest. Additional platform data is stored as a comment in the drone MJCF. - `parametrize` returns a `functools.partial` whose `keywords` dict is shared by every reference to it. Call `parametrize` again for an independent copy. - Leading batch dimensions, trailing feature axis. `quat` is scalar-last xyzw, `force` is `(..., 1)` diff --git a/crazyflow/control/__init__.py b/crazyflow/control/__init__.py index b1c3e84e..ebbe4080 100644 --- a/crazyflow/control/__init__.py +++ b/crazyflow/control/__init__.py @@ -14,7 +14,7 @@ __all__ = [] -from crazyflow.control.core import Control, load_params, parametrize +from crazyflow.control.core import Control, load_fn_params, load_params, parametrize from crazyflow.control.mellinger import attitude2force_torque as mellinger_attitude2force_torque from crazyflow.control.mellinger import body_rate2force_torque as mellinger_body_rate2force_torque from crazyflow.control.mellinger import state2attitude as mellinger_state2attitude @@ -25,4 +25,4 @@ "mellinger_body_rate2force_torque": mellinger_body_rate2force_torque, } -__all__ = ["Control", "load_params", "parametrize"] +__all__ = ["Control", "load_params", "load_fn_params", "parametrize"] diff --git a/crazyflow/control/core.py b/crazyflow/control/core.py index 6c18d67e..91247c14 100644 --- a/crazyflow/control/core.py +++ b/crazyflow/control/core.py @@ -22,6 +22,46 @@ R = TypeVar("R") +class Control(StrEnum): + """Control type of the simulated onboard controller.""" + + state = "state" + """State control takes [x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, wx, wy, wz]. + + Note: + Recommended frequency is >=20 Hz. + + Warning: + Only the yaw of the attitude quaternion is used, as in the firmware. The so_rpy family + ignores the body rate setpoint. + """ + attitude = "attitude" + """Attitude control takes [roll, pitch, yaw, collective thrust]. + + Note: + Recommended frequency is >=100 Hz. + """ + body_rate = "body_rate" + """Body rate control takes [wx, wy, wz, collective thrust]. + + Note: + Recommended frequency is >=200 Hz. + """ + force_torque = "force_torque" + """Force and torque control takes [fc, tx, ty, tz]. + + Note: + Recommended frequency is >=500 Hz. + """ + rotor_vel = "rotor_vel" + """Rotor velocity control takes [w1, w2, w3, w4] in RPMs. + + Note: + Recommended frequency is >=500 Hz. + """ + default = attitude + + def parametrize( fn: Callable[P, R], drone: str, xp: ModuleType | None = None, device: str | None = None ) -> Callable[P, R]: @@ -50,77 +90,57 @@ def parametrize( Returns: The parametrized controller function with all keyword argument only parameters filled in. """ - return _parametrize(fn, drone, load_params, xp=xp, device=device) + return _parametrize(fn, drone, load_fn_params, xp=xp, device=device) def load_params( - fn: Callable, drone: str, xp: ModuleType | None = None, device: str | None = None -) -> dict[str, Array]: - """Load the parameters a specific controller function accepts. + controller: str, drone: str, xp: ModuleType | None = None, device: str | None = None +) -> dict[str, dict[str, Array]]: + """Load all parameters of a drone for a controller. - Merges the ``"core"`` section with the function's ``[drone.]`` section (function values - take precedence), then keeps only the parameters in ``fn``'s signature. + Returns the drone's table of ``crazyflow/control//params.toml``: the ``core`` + section shared by all functions of the controller, and one section per controller function. Args: - fn: The controller function for which to load parameters. + controller: Name of the controller package, e.g. ``"mellinger"``. drone: Name of the drone configuration, e.g. ``"cf2x_L250"``. xp: The array API module to use. If not provided, numpy is used. device: The device to use. If None, the device is inferred from the xp module. - Returns: - A flat dict mapping parameter names to arrays in the requested array namespace. + Raises: + KeyError: If ``controller`` has no ``params.toml`` or ``drone`` has no section in it. """ - assert isinstance(fn, Callable), f"Expected a function, got {type(fn)}" - controller = fn.__module__.split(".")[-2] params_path = Path(__file__).parent / f"{controller}/params.toml" if not params_path.exists(): - raise KeyError(f"`{controller}` not found. Available controllers: {tuple(Control)}") + raise KeyError(f"Controller `{controller}` not found") with open(params_path, "rb") as f: params = tomllib.load(f) if drone not in params: raise KeyError(f"Drone `{drone}` not found in {controller}/params.toml") - merged = params[drone].get("core", {}) | params[drone].get(fn.__name__, {}) - return to_xp(filter_to_signature(merged, fn), xp=xp, device=device) - - -class Control(StrEnum): - """Control type of the simulated onboard controller.""" - - state = "state" - """State control takes [x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, wx, wy, wz]. - - Note: - Recommended frequency is >=20 Hz. + return to_xp(params[drone], xp=xp, device=device) - Warning: - Only the yaw of the attitude quaternion is used, as in the firmware. The so_rpy family - ignores the body rate setpoint. - """ - attitude = "attitude" - """Attitude control takes [roll, pitch, yaw, collective thrust]. - Note: - Recommended frequency is >=100 Hz. - """ - body_rate = "body_rate" - """Body rate control takes [wx, wy, wz, collective thrust]. +def load_fn_params( + fn: Callable, drone: str, xp: ModuleType | None = None, device: str | None = None +) -> dict[str, Array]: + """Load the parameters a controller function accepts. - Note: - Recommended frequency is >=200 Hz. - """ - force_torque = "force_torque" - """Force and torque control takes [fc, tx, ty, tz]. + The controller is derived from the function's package. Merges the ``core`` section with the + function's section (function values take precedence), then keeps only the parameters in + ``fn``'s signature. - Note: - Recommended frequency is >=500 Hz. - """ - rotor_vel = "rotor_vel" - """Rotor velocity control takes [w1, w2, w3, w4] in RPMs. + Args: + fn: The controller function for which to load parameters. + drone: Name of the drone configuration, e.g. ``"cf2x_L250"``. + xp: The array API module to use. If not provided, numpy is used. + device: The device to use. If None, the device is inferred from the xp module. - Note: - Recommended frequency is >=500 Hz. + Returns: + A flat dict mapping parameter names to arrays in the requested array namespace. """ - default = attitude + assert callable(fn), f"Expected a function, got {type(fn)}" + params = load_params(fn.__module__.split(".")[-2], drone, xp=xp, device=device) + return filter_to_signature(params.get("core", {}) | params.get(fn.__name__, {}), fn) @jax.jit diff --git a/crazyflow/control/mellinger/control.py b/crazyflow/control/mellinger/control.py index 6e05d7b4..c92a90ad 100644 --- a/crazyflow/control/mellinger/control.py +++ b/crazyflow/control/mellinger/control.py @@ -20,7 +20,7 @@ from flax.struct import dataclass, field from scipy.spatial.transform import Rotation as R -from crazyflow.control.core import controllable, load_params +from crazyflow.control.core import controllable, load_fn_params from crazyflow.control.transform import force2pwm, motor_force2rotor_vel, pwm2force from crazyflow.utils import CORE_NDIM_KEY, leaf_replace @@ -513,7 +513,7 @@ def create( zeros_3d = jnp.zeros((n_worlds, n_drones, 3), device=device) cmd = jnp.zeros((n_worlds, n_drones, 16), device=device).at[..., 12].set(1.0) steps = -jnp.ones((n_worlds, 1), dtype=jnp.int32, device=device) - params = load_params(state2attitude, drone, xp=jnp, device=device) + params = load_fn_params(state2attitude, drone, xp=jnp, device=device) return MellingerStateData( cmd=cmd, staged_cmd=cmd.copy(), @@ -553,7 +553,7 @@ def create( zeros_3d = jnp.zeros((n_worlds, n_drones, 3), device=device) zeros_4d = jnp.zeros((n_worlds, n_drones, 4), device=device) steps = -jnp.ones((n_worlds, 1), dtype=jnp.int32, device=device) - params = load_params(attitude2force_torque, drone, xp=jnp, device=device) + params = load_fn_params(attitude2force_torque, drone, xp=jnp, device=device) return MellingerAttitudeData( cmd=zeros_4d.copy(), staged_cmd=zeros_4d.copy(), @@ -595,7 +595,7 @@ def create( zeros_3d = jnp.zeros((n_worlds, n_drones, 3), device=device) zeros_4d = jnp.zeros((n_worlds, n_drones, 4), device=device) steps = -jnp.ones((n_worlds, 1), dtype=jnp.int32, device=device) - params = load_params(body_rate2force_torque, drone, xp=jnp, device=device) + params = load_fn_params(body_rate2force_torque, drone, xp=jnp, device=device) return MellingerBodyRateData( cmd=zeros_4d.copy(), staged_cmd=zeros_4d.copy(), @@ -629,7 +629,7 @@ def create( ) -> MellingerForceTorqueData: zeros_4d = jnp.zeros((n_worlds, n_drones, 4), device=device) steps = -jnp.ones((n_worlds, 1), dtype=jnp.int32, device=device) - params = load_params(force_torque2rotor_vel, drone, xp=jnp, device=device) + params = load_fn_params(force_torque2rotor_vel, drone, xp=jnp, device=device) return MellingerForceTorqueData( cmd=zeros_4d.copy(), staged_cmd=zeros_4d.copy(), steps=steps, freq=freq, params=params ) diff --git a/crazyflow/drones/__init__.py b/crazyflow/drones/__init__.py index 5c686bb0..ace00398 100644 --- a/crazyflow/drones/__init__.py +++ b/crazyflow/drones/__init__.py @@ -1,17 +1,11 @@ """Hardware descriptions for the supported drone platforms. -This package bundles the physical assets that define each drone configuration: the MuJoCo MJCF scene -files, their referenced meshes (``assets/``), and the physical parameters shared across all dynamics -(``params.toml`` with mass, inertia, thrust and torque curves, …). These describe the *hardware* and -are independent of the dynamics formulation used to simulate it (see [crazyflow.dynamics][]). +This package bundles the MuJoCo MJCF scene files that define each drone configuration and their +referenced meshes (``assets/``). For the physical params, see [crazyflow.dynamics.load_params][]. -Use ``available_drones`` to enumerate the supported configurations, and ``load_params`` to read all -physical parameters of a drone. +Use ``available_drones`` to enumerate the supported configurations. """ -import tomllib -from pathlib import Path - # Currently supported platforms: # * **cf2x_L250** — Crazyflie 2.x # * **cf2x_P250** — Crazyflie 2.x with plus propellers @@ -19,19 +13,4 @@ # * **cf21B_500** — Crazyflie 2.1 Brushless with 500 mAh battery available_drones: tuple[str, ...] = ("cf2x_L250", "cf2x_P250", "cf2x_T350", "cf21B_500") -__all__ = ["available_drones", "load_params"] - - -def load_params(drone: str) -> dict: - """Load all physical parameters of a drone from ``params.toml``. - - Returns the raw values (lists/scalars) for the whole drone. - - Args: - drone: Name of the drone configuration, e.g. ``"cf2x_L250"``. - """ - with open(Path(__file__).parent / "params.toml", "rb") as f: - params = tomllib.load(f) - if drone not in params or drone not in available_drones: - raise KeyError(f"Drone `{drone}` not found in drones/params.toml") - return params[drone] +__all__ = ["available_drones"] diff --git a/crazyflow/drones/cf21B_500.xml b/crazyflow/drones/cf21B_500.xml index c20932fe..ae02aab2 100644 --- a/crazyflow/drones/cf21B_500.xml +++ b/crazyflow/drones/cf21B_500.xml @@ -1,4 +1,13 @@ + diff --git a/crazyflow/drones/cf2x_L250.xml b/crazyflow/drones/cf2x_L250.xml index 47396b04..104c2186 100644 --- a/crazyflow/drones/cf2x_L250.xml +++ b/crazyflow/drones/cf2x_L250.xml @@ -1,4 +1,13 @@ + diff --git a/crazyflow/drones/cf2x_P250.xml b/crazyflow/drones/cf2x_P250.xml index 6a7fa833..295c1f10 100644 --- a/crazyflow/drones/cf2x_P250.xml +++ b/crazyflow/drones/cf2x_P250.xml @@ -1,4 +1,13 @@ + diff --git a/crazyflow/drones/cf2x_T350.xml b/crazyflow/drones/cf2x_T350.xml index 13785057..0fd0ae4f 100644 --- a/crazyflow/drones/cf2x_T350.xml +++ b/crazyflow/drones/cf2x_T350.xml @@ -1,4 +1,13 @@ + diff --git a/crazyflow/drones/params.toml b/crazyflow/drones/params.toml deleted file mode 100644 index a8119bdd..00000000 --- a/crazyflow/drones/params.toml +++ /dev/null @@ -1,152 +0,0 @@ -# This file contains all the **physical** parameters for all drones. - -[cf2x_L250] -gravity_vec = [0.0, 0.0, -9.81] -mass = 0.0319 -L = 0.03253 -J = [ # TODO - [16.8e-6, 0.0, 0.0], - [0.0, 16.8e-6, 0.0], - [0.0, 0.0, 29.8e-6] -] -rpm2thrust = [0.0, -5.382196214637237e-7, 2.4582929831265485e-10] -rpm2torque = [0.0, 1.410454111996297e-9, 1.4592584373980652e-12] -thrust2torque = 0.007350862856566459 -rotor_dyn_coef = [ 7.355623702172756, 0.0, 0.0, 0.00024443862952110715,] # [ 7.355623702172756, 0.0, 0.0, 0.00024443862952110715,] -rotor_dyn_coef_simple = 6.886705423469015 -thrust_dyn_coef = 6.8932095506763345 -mixing_matrix = [ - [-1.0, -1.0, 1.0, 1.0], - [-1.0, 1.0, 1.0, -1.0], - [-1.0, 1.0, -1.0, 1.0] -] -drag_matrix = [ # This term is from the so_rpy_rotor_drag dynamics - [-0.01471782, 0.0, 0.0 ], - [0.0, -0.01471782, 0.0 ], - [0.0, 0.0, -0.01277641 ] -] -# The following parameters are for the platform, but are maybe not actually used by the dynamics. However, -# we still keep them here in one place, since some other things (sim, estimator, firmware) might need them. -pwm_min = 7000 -pwm_max = 65535 -thrust_min = 0.012817578393224994 # in N per motor -thrust_max = 0.12 # in N per motor -vmotor2thrust = [-0.014830744918356092, 0.04724465241828281, -0.01847364358025878, 0.005960923942142] # Index is order -vmotor2torque = [-3.3261514624778425e-6, 8.109636684075977e-5, 5.751459172588052e-5, -1.898633582060136e-7] # Index is order -vmotor2rpm = [2968.1791506049194, 6647.948592402306] # Index is order -prop_radius = 23.55e-3 # TODO check -prop_inertia = 34.52e-9 # TODO seems off - - -[cf2x_P250] -gravity_vec = [0.0, 0.0, -9.81] -mass = 0.0318 -L = 0.03253 -J = [ # TODO from L250 - [16.8e-6, 0.0, 0.0], - [0.0, 16.8e-6, 0.0], - [0.0, 0.0, 29.8e-6] -] -rpm2thrust = [0.0, -3.6200226530383495e-7, 1.6060924304100328e-10] # Index is order -rpm2torque = [0.0, -2.2665265829562245e-9, 1.1149485566919186e-12] # Index is order -thrust2torque = 0.0069928948992470565 -rotor_dyn_coef = [ 5.172596691828673, 8.14774234381285e-5, 0.0, 0.0002095253491455741,] -rotor_dyn_coef_simple = 7.709730027690284 -thrust_dyn_coef = 7.9435775497736785 -mixing_matrix = [ - [-1.0, -1.0, 1.0, 1.0], - [-1.0, 1.0, 1.0, -1.0], - [-1.0, 1.0, -1.0, 1.0] -] -drag_matrix = [ # This term is from the so_rpy_rotor_drag dynamics - [-0.01351483, 0.0, 0.0 ], - [0.0, -0.01351483, 0.0 ], - [0.0, 0.0, -0.01677452 ] -] -# The following parameters are for the platform, but are maybe not actually used by the dynamics. However, -# we still keep them here in one place, since some other things (sim, estimator, firmware) might need them. -pwm_min = 7000 -pwm_max = 65535 -thrust_min = 0.012817578393224994 # in N per motor -thrust_max = 0.12 # in N per motor -vmotor2thrust = [-0.02476537915958403, 0.06523793527519485, -0.026792504967750107, 0.006776789303971145] # Index is order -vmotor2torque = [-2.8633106919309745e-5, 0.00011679386117520097, 5.105754520419129e-5, 0.0] # Index is order -vmotor2rpm = [4657.542534331524, 7536.161830990926] # Index is order -prop_radius = 23.4e-3 # TODO check -prop_inertia = 26.97e-9 - - -[cf2x_T350] -gravity_vec = [0.0, 0.0, -9.81] -mass = 0.0379 -L = 0.03253 -J = [ - [15.7e-6, 0.0, 0.0], - [0.0, 17.1e-6, 0.0], - [0.0, 0.0, 30.0e-6] -] -rpm2thrust = [0.0, -7.167227176573658e-7, 2.9401303690194613e-10] # Index is order -rpm2torque = [0.0, 5.815894847811497e-10, 1.331813874166509e-12] # Index is order -thrust2torque = 0.005355990836477486 -rotor_dyn_coef = [ 11.374753209400291, 0.0, 0.0, 0.00037867688499079635,] -rotor_dyn_coef_simple = 11.352970450445243 -thrust_dyn_coef = 11.12424272978587 -mixing_matrix = [ - [-1.0, -1.0, 1.0, 1.0], - [-1.0, 1.0, 1.0, -1.0], - [-1.0, 1.0, -1.0, 1.0] -] -drag_matrix = [ # This term is from the so_rpy_rotor_drag dynamics - [-0.01556697, 0.0, 0.0 ], - [0.0, -0.01556697, 0.0 ], - [0.0, 0.0, -0.02191672 ] -] -# The following parameters are for the platform, but are maybe not actually used by the dynamics. However, -# we still keep them here in one place, since some other things (sim, estimator, firmware) might need them. -pwm_min = 7000 -pwm_max = 65535 -thrust_min = 0.01922636758983749 # in N per motor -thrust_max = 0.18 # in N per motor -vmotor2thrust = [0.006728127583707208, 0.01011557616217668, 0.010263198062061085, 0.0028358638322392503] # Index is order -vmotor2torque = [-1.2906047901738756e-5, 0.0001436101487030899, 2.794753913624656e-5, 1.3104535533494383e-5] # Index is order -vmotor2rpm = [2977.884883031915, 8101.0293594093055] # Index is order -prop_radius = 25.4e-3 # TODO check -prop_inertia = 38.93e-9 # TODO value from B500, currenty unknown - - -[cf21B_500] -gravity_vec = [0.0, 0.0, -9.81] -mass = 0.04338 -L = 0.035355 -J = [ - [25e-6, 0.0, 0.0], - [0.0, 28e-6, 0.0], - [0.0, 0.0, 49e-6] -] -rpm2thrust = [0.0, -3.133427287299859e-7, 4.407354891648379e-10] # TODO , Index is order -rpm2torque = [0.0, 1.65886356219615e-9, 2.4693477924534137e-12] # TODO , Index is order -thrust2torque = 0.00593893393599368 -rotor_dyn_coef = [ 13.996001897562685, 0.00011093207920685363, 5.933168530682111, 0.00031951312393561264,] -rotor_dyn_coef_simple = 15.416891997523813 -thrust_dyn_coef = 15.09965949800411 -mixing_matrix = [ - [-1.0, -1.0, 1.0, 1.0], - [-1.0, 1.0, 1.0, -1.0], - [-1.0, 1.0, -1.0, 1.0] -] -drag_matrix = [ # This term is from the so_rpy_rotor_drag dynamics - [-0.02149163, 0.0, 0.0 ], - [0.0, -0.02149163, 0.0 ], - [0.0, 0.0, -0.02359736 ] -] -# The following parameters are for the platform, but are maybe not actually used by the dynamics. However, -# we still keep them here in one place, since some other things (sim, estimator, firmware) might need them. -pwm_min = 7000 -pwm_max = 65535 -thrust_min = 0.02136263065537499 # in N per motor -thrust_max = 0.2 # in N per motor -vmotor2thrust = [-0.014058926705279723, 0.04265273261724981, 0.0018327760144017432, 0.0020576974784587178] # Index is order -vmotor2torque = [-0.00016088354909542246, 0.0003960426420309137, -4.6274122414327404e-5, 1.8490661674309596e-5] # TODO, Index is order -vmotor2rpm = [2938.3995608848436, 6001.834195381014] # Index is order -prop_radius = 27.5e-3 # TODO check -prop_inertia = 38.93e-9 \ No newline at end of file diff --git a/crazyflow/dynamics/__init__.py b/crazyflow/dynamics/__init__.py index 3c450880..0e2ad1b9 100644 --- a/crazyflow/dynamics/__init__.py +++ b/crazyflow/dynamics/__init__.py @@ -14,13 +14,20 @@ from typing import Callable -from crazyflow.dynamics.core import Dynamics, load_params, parametrize +from crazyflow.dynamics.core import Dynamics, load_fn_params, load_params, parametrize from crazyflow.dynamics.first_principles import dynamics as _first_principles_dynamics from crazyflow.dynamics.so_rpy import dynamics as _so_rpy_dynamics from crazyflow.dynamics.so_rpy_rotor import dynamics as _so_rpy_rotor_dynamics from crazyflow.dynamics.so_rpy_rotor_drag import dynamics as _so_rpy_rotor_drag_dynamics -__all__ = ["parametrize", "load_params", "available_dynamics", "dynamics_features", "Dynamics"] +__all__ = [ + "parametrize", + "load_params", + "load_fn_params", + "available_dynamics", + "dynamics_features", + "Dynamics", +] available_dynamics: dict[str, Callable] = { diff --git a/crazyflow/dynamics/core.py b/crazyflow/dynamics/core.py index e5999820..b93cc608 100644 --- a/crazyflow/dynamics/core.py +++ b/crazyflow/dynamics/core.py @@ -9,7 +9,6 @@ import numpy as np -from crazyflow.drones import load_params as load_physical_params from crazyflow.utils import filter_to_signature, to_xp from crazyflow.utils import parametrize as _parametrize @@ -21,6 +20,16 @@ R = TypeVar("R") +class Dynamics(StrEnum): + """Dynamics mode for the simulation.""" + + first_principles = "first_principles" + so_rpy = "so_rpy" + so_rpy_rotor = "so_rpy_rotor" + so_rpy_rotor_drag = "so_rpy_rotor_drag" + default = first_principles + + def supports(rotor_dynamics: bool = True) -> Callable[[F], F]: """Decorator that declares which optional inputs a dynamics function supports. @@ -72,61 +81,63 @@ def parametrize( Returns: The parametrized dynamics function with all keyword argument only parameters filled in. """ - return _parametrize(fn, drone, load_params, xp=xp, device=device) + return _parametrize(fn, drone, load_fn_params, xp=xp, device=device) def load_params( - fn: Callable, drone: str, xp: ModuleType | None = None, device: str | None = None + dynamics: Dynamics | str, drone: str, xp: ModuleType | None = None, device: str | None = None ) -> dict: - """Load and merge physical and dynamics-specific parameters for a drone configuration. - - Reads parameters from two TOML files: - - * ``crazyflow/drones/params.toml`` — physical parameters shared across all dynamics (mass, - inertia, thrust curves, …). - * ``crazyflow/dynamics//params.toml`` — dynamics-specific coefficients (e.g. fitted - RPY coefficients for ``so_rpy``). + """Load all parameters of a drone for a dynamics model. - The two dicts are merged (dynamics-specific values take precedence), and ``J_inv`` is computed - from ``J`` and added to the result. + Merges the global parameters in ``crazyflow/dynamics/params.toml`` with the drone's section in + ``crazyflow/dynamics//params.toml`` and adds ``J_inv``. Args: - fn: The dynamics function for which to load parameters. - drone: Name of the drone configuration, e.g. ``"cf2x_L250"``. Must exist as a section in - both TOML files. + dynamics: The dynamics model, e.g. ``Dynamics.so_rpy`` or ``"so_rpy"``. + drone: Name of the drone configuration, e.g. ``"cf2x_L250"``. xp: Array API module used to convert parameter values. If ``None``, NumPy is used. device: The device to use for the arrays. If ``None``, the device is inferred from the xp module. Returns: - A flat dict mapping parameter names to arrays (or scalars) in the requested array namespace. - Always contains at least ``mass``, ``J``, ``J_inv``, ``gravity_vec``, and the - dynamics-specific coefficients for ``dynamics``. + A flat dict mapping parameter names to arrays in the requested array namespace. Raises: - KeyError: If ``drone`` is not found in either TOML file, or if ``dynamics`` does not - correspond to a known sub-package. + ValueError: If ``dynamics`` is not a known model. + KeyError: If ``drone`` has no section for ``dynamics``. """ - assert isinstance(fn, Callable), f"Expected a function, got {type(fn)}" - dynamics = fn.__module__.split(".")[-2] - if dynamics not in Dynamics: - raise KeyError(f"Dynamics `{dynamics}` not found. Available dynamics: {tuple(Dynamics)}") + dynamics = Dynamics(dynamics) + with open(Path(__file__).parent / "params.toml", "rb") as f: + global_params = tomllib.load(f) with open(Path(__file__).parent / f"{dynamics}/params.toml", "rb") as f: dynamics_params = tomllib.load(f) if drone not in dynamics_params: raise KeyError(f"Drone `{drone}` not found in {dynamics}/params.toml") - params = load_physical_params(drone) | dynamics_params[drone] + params = global_params | dynamics_params[drone] # Make sure J_inv does not have a dtype fixed before conversion to xp arrays to avoid fixing it # to np.float64 when other frameworks might prefer a different dtype. params["J_inv"] = np.linalg.inv(params["J"]).tolist() - return to_xp(filter_to_signature(params, fn), xp=xp, device=device) + return to_xp(params, xp=xp, device=device) -class Dynamics(StrEnum): - """Dynamics mode for the simulation.""" +def load_fn_params( + fn: Callable, drone: str, xp: ModuleType | None = None, device: str | None = None +) -> dict: + """Load the parameters a dynamics function accepts. - first_principles = "first_principles" - so_rpy = "so_rpy" - so_rpy_rotor = "so_rpy_rotor" - so_rpy_rotor_drag = "so_rpy_rotor_drag" - default = first_principles + The dynamics model is derived from the function's package, so ``fn`` must be defined in + ``crazyflow/dynamics//``. Only the keyword-only parameters of ``fn`` are kept. + + Args: + fn: The dynamics function for which to load parameters. + drone: Name of the drone configuration, e.g. ``"cf2x_L250"``. + xp: Array API module used to convert parameter values. If ``None``, NumPy is used. + device: The device to use for the arrays. If ``None``, the device is inferred from the xp + module. + + Returns: + A flat dict mapping parameter names to arrays in the requested array namespace. + """ + assert callable(fn), f"Expected a function, got {type(fn)}" + dynamics = fn.__module__.split(".")[-2] + return filter_to_signature(load_params(dynamics, drone, xp=xp, device=device), fn) diff --git a/crazyflow/dynamics/first_principles/dynamics.py b/crazyflow/dynamics/first_principles/dynamics.py index fac11c7e..38b85f22 100644 --- a/crazyflow/dynamics/first_principles/dynamics.py +++ b/crazyflow/dynamics/first_principles/dynamics.py @@ -25,7 +25,7 @@ from scipy.spatial.transform import Rotation as R import crazyflow.dynamics.symbols as symbols -from crazyflow.dynamics.core import load_params, supports +from crazyflow.dynamics.core import load_fn_params, supports from crazyflow.dynamics.utils import rotation from crazyflow.utils import CORE_NDIM_KEY, to_xp @@ -342,7 +342,7 @@ class Params: @staticmethod def create(drone: str, device: Device) -> Params: """Create the default parameters for the simulation.""" - p = load_params(dynamics, drone) + p = load_fn_params(dynamics, drone) J = jnp.asarray(p["J"], device=device) return Params( mass=jnp.asarray([p["mass"]], device=device), diff --git a/crazyflow/dynamics/first_principles/params.toml b/crazyflow/dynamics/first_principles/params.toml index f699bade..aa8c66ee 100644 --- a/crazyflow/dynamics/first_principles/params.toml +++ b/crazyflow/dynamics/first_principles/params.toml @@ -1,13 +1,131 @@ -# Since the first principles dynamics only rely on physical parameters, -# which are already defined in data/params.toml, this file is intentionally left empty. +# Parameters of the first principles dynamics. +# +# [my_drone] +# mass = 0.0 # kg +# J = [ # kg m^2, inertia matrix +# [1.0e-5, 0.0, 0.0], +# [0.0, 1.0e-5, 0.0], +# [0.0, 0.0, 2.0e-5] +# ] +# thrust_min = 0.0 # N per motor +# thrust_max = 0.0 # N per motor +# L = 0.0 # m, CoM to motor distance +# prop_inertia = 0.0 # kg m^2 +# rpm2thrust = [0.0, 0.0, 0.0] # N, polynomial in RPM by index +# rpm2torque = [0.0, 0.0, 0.0] # Nm, polynomial in RPM by index +# rotor_dyn_coef = [0.0, 0.0, 0.0, 0.0] # The coefficients are [a,b,c,d], where a & b are the + # linear & quadratic rise constants. c & d equally for fall. +# mixing_matrix = [ # motor locations relative to the CoM and turn directions +# [-1.0, -1.0, 1.0, 1.0], +# [-1.0, 1.0, 1.0, -1.0], +# [-1.0, 1.0, -1.0, 1.0] +# ] +# drag_matrix = [ # 1/s +# [0.0, 0.0, 0.0], +# [0.0, 0.0, 0.0], +# [0.0, 0.0, 0.0] +# ] +# [cf2x_L250] +mass = 0.0319 +J = [ # TODO + [16.8e-6, 0.0, 0.0], + [0.0, 16.8e-6, 0.0], + [0.0, 0.0, 29.8e-6] +] +thrust_min = 0.012817578393224994 # in N per motor +thrust_max = 0.12 # in N per motor +L = 0.03253 +prop_inertia = 34.52e-9 # TODO seems off +rpm2thrust = [0.0, -5.382196214637237e-7, 2.4582929831265485e-10] +rpm2torque = [0.0, 1.410454111996297e-9, 1.4592584373980652e-12] +rotor_dyn_coef = [ 7.355623702172756, 0.0, 0.0, 0.00024443862952110715,] # [ 7.355623702172756, 0.0, 0.0, 0.00024443862952110715,] +mixing_matrix = [ + [-1.0, -1.0, 1.0, 1.0], + [-1.0, 1.0, 1.0, -1.0], + [-1.0, 1.0, -1.0, 1.0] +] +drag_matrix = [ # This term is from the so_rpy_rotor_drag dynamics + [-0.01471782, 0.0, 0.0 ], + [0.0, -0.01471782, 0.0 ], + [0.0, 0.0, -0.01277641 ] +] [cf2x_P250] +mass = 0.0318 +J = [ # TODO from L250 + [16.8e-6, 0.0, 0.0], + [0.0, 16.8e-6, 0.0], + [0.0, 0.0, 29.8e-6] +] +thrust_min = 0.012817578393224994 # in N per motor +thrust_max = 0.12 # in N per motor +L = 0.03253 +prop_inertia = 26.97e-9 +rpm2thrust = [0.0, -3.6200226530383495e-7, 1.6060924304100328e-10] # Index is order +rpm2torque = [0.0, -2.2665265829562245e-9, 1.1149485566919186e-12] # Index is order +rotor_dyn_coef = [ 5.172596691828673, 8.14774234381285e-5, 0.0, 0.0002095253491455741,] +mixing_matrix = [ + [-1.0, -1.0, 1.0, 1.0], + [-1.0, 1.0, 1.0, -1.0], + [-1.0, 1.0, -1.0, 1.0] +] +drag_matrix = [ # This term is from the so_rpy_rotor_drag dynamics + [-0.01351483, 0.0, 0.0 ], + [0.0, -0.01351483, 0.0 ], + [0.0, 0.0, -0.01677452 ] +] [cf2x_T350] +mass = 0.0379 +J = [ + [15.7e-6, 0.0, 0.0], + [0.0, 17.1e-6, 0.0], + [0.0, 0.0, 30.0e-6] +] +thrust_min = 0.01922636758983749 # in N per motor +thrust_max = 0.18 # in N per motor +L = 0.03253 +prop_inertia = 38.93e-9 # TODO value from B500, currenty unknown +rpm2thrust = [0.0, -7.167227176573658e-7, 2.9401303690194613e-10] # Index is order +rpm2torque = [0.0, 5.815894847811497e-10, 1.331813874166509e-12] # Index is order +rotor_dyn_coef = [ 11.374753209400291, 0.0, 0.0, 0.00037867688499079635,] +mixing_matrix = [ + [-1.0, -1.0, 1.0, 1.0], + [-1.0, 1.0, 1.0, -1.0], + [-1.0, 1.0, -1.0, 1.0] +] +drag_matrix = [ # This term is from the so_rpy_rotor_drag dynamics + [-0.01556697, 0.0, 0.0 ], + [0.0, -0.01556697, 0.0 ], + [0.0, 0.0, -0.02191672 ] +] -[cf21B_500] \ No newline at end of file +[cf21B_500] +mass = 0.04338 +J = [ + [25e-6, 0.0, 0.0], + [0.0, 28e-6, 0.0], + [0.0, 0.0, 49e-6] +] +thrust_min = 0.02136263065537499 # in N per motor +thrust_max = 0.2 # in N per motor +L = 0.035355 +prop_inertia = 38.93e-9 +rpm2thrust = [0.0, -3.133427287299859e-7, 4.407354891648379e-10] # TODO , Index is order +rpm2torque = [0.0, 1.65886356219615e-9, 2.4693477924534137e-12] # TODO , Index is order +rotor_dyn_coef = [ 13.996001897562685, 0.00011093207920685363, 5.933168530682111, 0.00031951312393561264,] +mixing_matrix = [ + [-1.0, -1.0, 1.0, 1.0], + [-1.0, 1.0, 1.0, -1.0], + [-1.0, 1.0, -1.0, 1.0] +] +drag_matrix = [ # This term is from the so_rpy_rotor_drag dynamics + [-0.02149163, 0.0, 0.0 ], + [0.0, -0.02149163, 0.0 ], + [0.0, 0.0, -0.02359736 ] +] diff --git a/crazyflow/dynamics/params.toml b/crazyflow/dynamics/params.toml new file mode 100644 index 00000000..a185d242 --- /dev/null +++ b/crazyflow/dynamics/params.toml @@ -0,0 +1,3 @@ +# Global environment parameters. + +gravity_vec = [0.0, 0.0, -9.81] # m/s^2 diff --git a/crazyflow/dynamics/so_rpy/dynamics.py b/crazyflow/dynamics/so_rpy/dynamics.py index dfa8d70f..b031909f 100644 --- a/crazyflow/dynamics/so_rpy/dynamics.py +++ b/crazyflow/dynamics/so_rpy/dynamics.py @@ -24,7 +24,7 @@ from scipy.spatial.transform import Rotation as R import crazyflow.dynamics.symbols as symbols -from crazyflow.dynamics.core import load_params, supports +from crazyflow.dynamics.core import load_fn_params, supports from crazyflow.dynamics.utils import rotation from crazyflow.utils import CORE_NDIM_KEY, to_xp @@ -345,7 +345,7 @@ class Params: @staticmethod def create(drone: str, device: Device) -> Params: """Create the default parameters for the simulation.""" - p = load_params(dynamics, drone) + p = load_fn_params(dynamics, drone) J = jnp.asarray(p["J"], device=device) return Params( mass=jnp.asarray([p["mass"]], device=device), diff --git a/crazyflow/dynamics/so_rpy/params.toml b/crazyflow/dynamics/so_rpy/params.toml index 2de1657b..1dee51b7 100644 --- a/crazyflow/dynamics/so_rpy/params.toml +++ b/crazyflow/dynamics/so_rpy/params.toml @@ -1,4 +1,33 @@ +# Parameters of the fitted so_rpy dynamics. +# +# [my_drone] +# mass = 0.0 # kg +# J = [ # kg m^2. Only used to apply external torques, so an +# [1.0e-5, 0.0, 0.0], # estimate works, but the drone then reacts wrongly to +# [0.0, 1.0e-5, 0.0], # disturbance torques. Must be invertible. +# [0.0, 0.0, 2.0e-5] +# ] +# thrust_min = 0.0 # N per motor +# thrust_max = 0.0 # N per motor +# acc_coef = 0.0 +# cmd_f_coef = 0.0 +# rpy_coef = [0.0, 0.0, 0.0] +# rpy_rates_coef = [0.0, 0.0, 0.0] +# cmd_rpy_coef = [0.0, 0.0, 0.0] +# +# Identify the coefficients from flight data with the system identification pipeline (see +# docs/user-guide/dynamics/system-identification.md). gravity_vec is global and lives in +# crazyflow/dynamics/params.toml. + [cf2x_L250] +mass = 0.0319 +J = [ + [16.8e-6, 0.0, 0.0], + [0.0, 16.8e-6, 0.0], + [0.0, 0.0, 29.8e-6] +] +thrust_min = 0.012817578393224994 # in N per motor +thrust_max = 0.12 # in N per motor acc_coef = 0.0 cmd_f_coef = 0.97605781 rpy_coef = [-245.67, -245.67, -227.78] @@ -7,6 +36,14 @@ cmd_rpy_coef = [196.18, 196.18, 390.27] [cf2x_P250] +mass = 0.0318 +J = [ + [16.8e-6, 0.0, 0.0], + [0.0, 16.8e-6, 0.0], + [0.0, 0.0, 29.8e-6] +] +thrust_min = 0.012817578393224994 # in N per motor +thrust_max = 0.12 # in N per motor acc_coef = 0.0 cmd_f_coef = 0.98275823 rpy_coef = [-319.14, -319.14, -284.28] @@ -15,6 +52,14 @@ cmd_rpy_coef = [263.30, 263.30, 502.58] [cf2x_T350] +mass = 0.0379 +J = [ + [15.7e-6, 0.0, 0.0], + [0.0, 17.1e-6, 0.0], + [0.0, 0.0, 30.0e-6] +] +thrust_min = 0.01922636758983749 # in N per motor +thrust_max = 0.18 # in N per motor acc_coef = 0.0 cmd_f_coef = 1.0089779349974615 rpy_coef = [-371.41695523, -371.41695523, -261.99549945] @@ -23,6 +68,14 @@ cmd_rpy_coef = [347.94260321, 347.94260321, 241.06977014] [cf21B_500] +mass = 0.04338 +J = [ + [25e-6, 0.0, 0.0], + [0.0, 28e-6, 0.0], + [0.0, 0.0, 49e-6] +] +thrust_min = 0.02136263065537499 # in N per motor +thrust_max = 0.2 # in N per motor acc_coef = 0.0 cmd_f_coef = 0.96836458 rpy_coef = [-188.9910, -188.9910, -138.3109] diff --git a/crazyflow/dynamics/so_rpy_rotor/dynamics.py b/crazyflow/dynamics/so_rpy_rotor/dynamics.py index 80fb23da..4bb3ad68 100644 --- a/crazyflow/dynamics/so_rpy_rotor/dynamics.py +++ b/crazyflow/dynamics/so_rpy_rotor/dynamics.py @@ -26,7 +26,7 @@ from scipy.spatial.transform import Rotation as R import crazyflow.dynamics.symbols as symbols -from crazyflow.dynamics.core import load_params, supports +from crazyflow.dynamics.core import load_fn_params, supports from crazyflow.dynamics.utils import rotation from crazyflow.utils import CORE_NDIM_KEY, to_xp @@ -398,7 +398,7 @@ class Params: @staticmethod def create(drone: str, device: Device) -> Params: """Create the default parameters for the simulation.""" - p = load_params(dynamics, drone) + p = load_fn_params(dynamics, drone) J = jnp.asarray(p["J"], device=device) return Params( mass=jnp.asarray([p["mass"]], device=device), diff --git a/crazyflow/dynamics/so_rpy_rotor/params.toml b/crazyflow/dynamics/so_rpy_rotor/params.toml index edb6a40c..5a4dabad 100644 --- a/crazyflow/dynamics/so_rpy_rotor/params.toml +++ b/crazyflow/dynamics/so_rpy_rotor/params.toml @@ -1,4 +1,34 @@ +# Parameters of the fitted so_rpy_rotor dynamics. +# +# [my_drone] +# mass = 0.0 # kg +# J = [ # kg m^2. Only used to apply external torques, so an +# [1.0e-5, 0.0, 0.0], # estimate works, but the drone then reacts wrongly to +# [0.0, 1.0e-5, 0.0], # disturbance torques. Must be invertible. +# [0.0, 0.0, 2.0e-5] +# ] +# thrust_min = 0.0 # N per motor +# thrust_max = 0.0 # N per motor +# acc_coef = 0.0 +# cmd_f_coef = 0.0 +# rpy_coef = [0.0, 0.0, 0.0] +# rpy_rates_coef = [0.0, 0.0, 0.0] +# cmd_rpy_coef = [0.0, 0.0, 0.0] +# thrust_time_coef = 0.0 # s +# +# Identify the coefficients from flight data with the system identification pipeline (see +# docs/user-guide/dynamics/system-identification.md). gravity_vec is global and lives in +# crazyflow/dynamics/params.toml. + [cf2x_L250] +mass = 0.0319 +J = [ + [16.8e-6, 0.0, 0.0], + [0.0, 16.8e-6, 0.0], + [0.0, 0.0, 29.8e-6] +] +thrust_min = 0.012817578393224994 # in N per motor +thrust_max = 0.12 # in N per motor acc_coef = 0.0 cmd_f_coef = 0.97732585 thrust_time_coef = 0.0858607 @@ -8,6 +38,14 @@ cmd_rpy_coef = [196.18, 196.18, 390.27] [cf2x_P250] +mass = 0.0318 +J = [ + [16.8e-6, 0.0, 0.0], + [0.0, 16.8e-6, 0.0], + [0.0, 0.0, 29.8e-6] +] +thrust_min = 0.012817578393224994 # in N per motor +thrust_max = 0.12 # in N per motor acc_coef = 0.0 cmd_f_coef = 0.98323006 thrust_time_coef = 0.0578952 @@ -17,6 +55,14 @@ cmd_rpy_coef = [263.30, 263.30, 502.58] [cf2x_T350] +mass = 0.0379 +J = [ + [15.7e-6, 0.0, 0.0], + [0.0, 17.1e-6, 0.0], + [0.0, 0.0, 30.0e-6] +] +thrust_min = 0.01922636758983749 # in N per motor +thrust_max = 0.18 # in N per motor acc_coef = 0.0 cmd_f_coef = 1.022561164673754 thrust_time_coef = 0.5712805549388994 # High value, maybe not correct? @@ -26,9 +72,17 @@ cmd_rpy_coef = [347.94260321, 347.94260321, 241.06977014] [cf21B_500] +mass = 0.04338 +J = [ + [25e-6, 0.0, 0.0], + [0.0, 28e-6, 0.0], + [0.0, 0.0, 49e-6] +] +thrust_min = 0.02136263065537499 # in N per motor +thrust_max = 0.2 # in N per motor acc_coef = 0.0 cmd_f_coef = 0.96841816 thrust_time_coef = 0.02055366 rpy_coef = [-188.9910, -188.9910, -138.3109] rpy_rates_coef = [-12.7803, -12.7803, -16.8485] -cmd_rpy_coef = [138.0834, 138.0834, 198.5161] \ No newline at end of file +cmd_rpy_coef = [138.0834, 138.0834, 198.5161] diff --git a/crazyflow/dynamics/so_rpy_rotor_drag/dynamics.py b/crazyflow/dynamics/so_rpy_rotor_drag/dynamics.py index 6f02b5a5..103ba270 100644 --- a/crazyflow/dynamics/so_rpy_rotor_drag/dynamics.py +++ b/crazyflow/dynamics/so_rpy_rotor_drag/dynamics.py @@ -28,7 +28,7 @@ from scipy.spatial.transform import Rotation as R import crazyflow.dynamics.symbols as symbols -from crazyflow.dynamics.core import load_params, supports +from crazyflow.dynamics.core import load_fn_params, supports from crazyflow.dynamics.utils import rotation from crazyflow.utils import CORE_NDIM_KEY, to_xp @@ -434,7 +434,7 @@ class Params: @staticmethod def create(drone: str, device: Device) -> Params: """Create the default parameters for the simulation.""" - p = load_params(dynamics, drone) + p = load_fn_params(dynamics, drone) J = jnp.asarray(p["J"], device=device) return Params( mass=jnp.asarray([p["mass"]], device=device), diff --git a/crazyflow/dynamics/so_rpy_rotor_drag/params.toml b/crazyflow/dynamics/so_rpy_rotor_drag/params.toml index 3c37ef14..23fb8182 100644 --- a/crazyflow/dynamics/so_rpy_rotor_drag/params.toml +++ b/crazyflow/dynamics/so_rpy_rotor_drag/params.toml @@ -1,4 +1,39 @@ +# Parameters of the fitted so_rpy_rotor_drag dynamics. +# +# [my_drone] +# mass = 0.0 # kg +# J = [ # kg m^2. Only used to apply external torques, so an +# [1.0e-5, 0.0, 0.0], # estimate works, but the drone then reacts wrongly to +# [0.0, 1.0e-5, 0.0], # disturbance torques. Must be invertible. +# [0.0, 0.0, 2.0e-5] +# ] +# thrust_min = 0.0 # N per motor +# thrust_max = 0.0 # N per motor +# acc_coef = 0.0 +# cmd_f_coef = 0.0 +# rpy_coef = [0.0, 0.0, 0.0] +# rpy_rates_coef = [0.0, 0.0, 0.0] +# cmd_rpy_coef = [0.0, 0.0, 0.0] +# thrust_time_coef = 0.0 # s +# drag_matrix = [ # 1/s +# [0.0, 0.0, 0.0], +# [0.0, 0.0, 0.0], +# [0.0, 0.0, 0.0] +# ] +# +# Identify the coefficients from flight data with the system identification pipeline (see +# docs/user-guide/dynamics/system-identification.md). gravity_vec is global and lives in +# crazyflow/dynamics/params.toml. + [cf2x_L250] +mass = 0.0319 +J = [ + [16.8e-6, 0.0, 0.0], + [0.0, 16.8e-6, 0.0], + [0.0, 0.0, 29.8e-6] +] +thrust_min = 0.012817578393224994 # in N per motor +thrust_max = 0.12 # in N per motor acc_coef = 0.0 cmd_f_coef = 0.98325003 thrust_time_coef = 0.12116392 @@ -13,6 +48,14 @@ cmd_rpy_coef = [196.18, 196.18, 390.27] [cf2x_P250] +mass = 0.0318 +J = [ + [16.8e-6, 0.0, 0.0], + [0.0, 16.8e-6, 0.0], + [0.0, 0.0, 29.8e-6] +] +thrust_min = 0.012817578393224994 # in N per motor +thrust_max = 0.12 # in N per motor acc_coef = 0.0 cmd_f_coef = 0.99085062 thrust_time_coef = 0.11554009 @@ -27,6 +70,14 @@ cmd_rpy_coef = [263.30, 263.30, 502.58] [cf2x_T350] +mass = 0.0379 +J = [ + [15.7e-6, 0.0, 0.0], + [0.0, 17.1e-6, 0.0], + [0.0, 0.0, 30.0e-6] +] +thrust_min = 0.01922636758983749 # in N per motor +thrust_max = 0.18 # in N per motor acc_coef = 0.0 cmd_f_coef = 1.0226418398769022 thrust_time_coef = 0.16711124468068936 @@ -41,6 +92,14 @@ cmd_rpy_coef = [347.94260321, 347.94260321, 241.06977014] [cf21B_500] +mass = 0.04338 +J = [ + [25e-6, 0.0, 0.0], + [0.0, 28e-6, 0.0], + [0.0, 0.0, 49e-6] +] +thrust_min = 0.02136263065537499 # in N per motor +thrust_max = 0.2 # in N per motor acc_coef = 0.0 cmd_f_coef = 0.98023254 thrust_time_coef = 0.07993871 diff --git a/crazyflow/envs/drone_env.py b/crazyflow/envs/drone_env.py index 12930a52..bf5754cf 100644 --- a/crazyflow/envs/drone_env.py +++ b/crazyflow/envs/drone_env.py @@ -12,19 +12,19 @@ from numpy.typing import NDArray from crazyflow.control import Control -from crazyflow.drones import load_params -from crazyflow.dynamics import Dynamics +from crazyflow.dynamics import Dynamics, load_params from crazyflow.sim import Sim from crazyflow.sim.data import SimData from crazyflow.sim.pipeline import append_fn from crazyflow.utils import leaf_replace -def action_space(control_type: Control, drone: str) -> spaces.Box: +def action_space(control_type: Control, dynamics: Dynamics, drone: str) -> spaces.Box: """Select the appropriate action space for a given control type. Args: control_type: The desired control mode. + dynamics: Dynamics of the environment. drone: Drone of the environment. Returns: @@ -32,7 +32,7 @@ def action_space(control_type: Control, drone: str) -> spaces.Box: """ match control_type: case Control.attitude: - params = load_params(drone) + params = load_params(dynamics, drone) thrust_min, thrust_max = params["thrust_min"] * 4, params["thrust_max"] * 4 return spaces.Box( np.array([-np.pi / 2, -np.pi / 2, -np.pi / 2, thrust_min], dtype=np.float32), @@ -106,7 +106,7 @@ def __init__( self._marked_for_reset = jnp.zeros((self.sim.n_worlds), dtype=jnp.bool_, device=self.device) # Define action and observation spaces - self.single_action_space = action_space(self.sim.control, self.sim.drone) + self.single_action_space = action_space(self.sim.control, self.sim.dynamics, self.sim.drone) self.action_space = batch_space(self.single_action_space, self.sim.n_worlds) self.single_observation_space = spaces.Dict( { diff --git a/crazyflow/sim/sim.py b/crazyflow/sim/sim.py index 0baa5d86..35103c61 100644 --- a/crazyflow/sim/sim.py +++ b/crazyflow/sim/sim.py @@ -25,8 +25,8 @@ control_state2attitude, ) from crazyflow.control.transform import motor_force2rotor_vel -from crazyflow.drones import load_params as load_drone_params from crazyflow.dynamics import Dynamics +from crazyflow.dynamics import load_params as load_dynamics_params from crazyflow.dynamics.first_principles import sim_dynamics as first_principles_dynamics from crazyflow.dynamics.so_rpy import sim_dynamics as so_rpy_dynamics from crazyflow.dynamics.so_rpy_rotor import sim_dynamics as so_rpy_rotor_dynamics @@ -710,8 +710,8 @@ def clip_floor_pos(data: SimData) -> SimData: def rotor_vel_limits(dynamics: Dynamics, drone: str) -> tuple[float, float]: """Limits of ``rotor_vel`` in RPM (first principles) or collective thrust in N (others).""" - params = load_drone_params(drone) - thrust_min, thrust_max = params["thrust_min"], params["thrust_max"] + 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]) diff --git a/docs/user-guide/control/mellinger.md b/docs/user-guide/control/mellinger.md index 0c61126b..0f31187d 100644 --- a/docs/user-guide/control/mellinger.md +++ b/docs/user-guide/control/mellinger.md @@ -121,10 +121,10 @@ torque.shape # (3,) ```python import numpy as np -from crazyflow.control import load_params +from crazyflow.control import load_fn_params from crazyflow.control.mellinger import body_rate2force_torque -params = load_params(body_rate2force_torque, "cf2x_L250") +params = load_fn_params(body_rate2force_torque, "cf2x_L250") params["kR"], params["ki_m"] = np.zeros(3), np.zeros(3) # pure body rate tracking quat = np.array([0.0, 0.0, 0.0, 1.0]) diff --git a/docs/user-guide/control/parametrize.md b/docs/user-guide/control/parametrize.md index 006eadcd..2c9c6da2 100644 --- a/docs/user-guide/control/parametrize.md +++ b/docs/user-guide/control/parametrize.md @@ -81,13 +81,13 @@ rpyt, _ = ctrl(pos, quat, vel, cmd) ## Loading raw parameters -Use [`load_params`][crazyflow.control.load_params] to inspect or override the values that `parametrize` would bind for a specific controller function: +Use [`load_fn_params`][crazyflow.control.load_fn_params] to inspect or override the values that `parametrize` would bind for a specific controller function, or [`load_params`][crazyflow.control.load_params] for all sections of a controller: ```python -from crazyflow.control import load_params +from crazyflow.control import load_fn_params from crazyflow.control.mellinger import state2attitude -params = load_params(state2attitude, "cf2x_L250") +params = load_fn_params(state2attitude, "cf2x_L250") float(params["mass"]) # 0.029 ``` diff --git a/docs/user-guide/dynamics/parametrize.md b/docs/user-guide/dynamics/parametrize.md index 1b7a2ecf..2222e600 100644 --- a/docs/user-guide/dynamics/parametrize.md +++ b/docs/user-guide/dynamics/parametrize.md @@ -114,14 +114,17 @@ parametrized_dynamics = parametrize(dynamics, drone="cf2x_T350") ## Loading raw parameters -If you need the parameter values directly, for example, to pass them to [`symbolic_dynamics`](symbolic.md), use [`load_params`][crazyflow.dynamics.load_params]: +If you need the parameter values directly, for example, to pass them to [`symbolic_dynamics`](symbolic.md), use [`load_fn_params`][crazyflow.dynamics.load_fn_params] for exactly what a dynamics function accepts, or [`load_params`][crazyflow.dynamics.load_params] for everything a model defines for a drone: ```python { .python continuation } -from crazyflow.dynamics import load_params +from crazyflow.dynamics import Dynamics, load_fn_params, load_params -params = load_params(dynamics, "cf2x_L250") +params = load_fn_params(dynamics, "cf2x_L250") params["mass"] # 0.0319 params["J_inv"] # array([...]) + +params = load_params(Dynamics.first_principles, "cf2x_L250") +params["thrust_max"] # 0.12, used by the simulator but not by the dynamics function ``` --- diff --git a/examples/control/sampling.py b/examples/control/sampling.py index f0a83d88..ec06f066 100644 --- a/examples/control/sampling.py +++ b/examples/control/sampling.py @@ -18,7 +18,7 @@ from crazyflow.control import Control from crazyflow.control.transform import motor_force2rotor_vel -from crazyflow.drones import load_params +from crazyflow.dynamics import load_params from crazyflow.sim import Dynamics, Sim from crazyflow.sim.data import SimData from crazyflow.sim.visualize import draw_capsule, draw_line @@ -226,7 +226,7 @@ def main() -> None: sim.max_visual_geom = 100_000 # To be able to show all rollouts sim.reset() start_pos = lissajous_reference(0.0)["pos"] - drone_params = load_params(DRONE) + drone_params = load_params(Dynamics.first_principles, DRONE) hover_thrust_value = np.asarray(drone_params["mass"] * 9.81, dtype=np.float32) hover_rotor_vel = motor_force2rotor_vel( np.full(4, hover_thrust_value / 4.0, dtype=np.float32), drone_params["rpm2thrust"] @@ -253,6 +253,7 @@ def main() -> None: rollout_simulator.reset() thrust_estimate = hover_thrust_value # Initial thrust estimate + thrust_time_coef = float(rollout_simulator.data.params.thrust_time_coef[0]) hover_cmd = jax.device_put( jnp.array([0.0, 0.0, 0.0, hover_thrust_value], dtype=jnp.float32), controller_device ) @@ -304,9 +305,7 @@ def main() -> None: action, key, mean_controls, best_positions, sampled_positions = control( t, obs, key, mean_controls, controller_fn, controller_device ) - thrust_estimate += ( - drone_params["thrust_dyn_coef"] * (action[3] - thrust_estimate) / CTRL_FREQ - ) + thrust_estimate += (action[3] - thrust_estimate) / thrust_time_coef / CTRL_FREQ sim.attitude_control(action[None, None]) sim.step(sim.freq // CTRL_FREQ) position_history.append(np.asarray(sim.data.states.pos[0, 0])) diff --git a/pyproject.toml b/pyproject.toml index 1d25db8b..ffce46c7 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -79,8 +79,8 @@ version = { attr = "crazyflow.__version__" } [tool.setuptools.package-data] crazyflow = ["scene.xml"] -"crazyflow.drones" = ["*.xml", "params.toml", "assets/*/*.stl"] -"crazyflow.dynamics" = ["*/params.toml"] +"crazyflow.drones" = ["*.xml", "assets/*/*.stl"] +"crazyflow.dynamics" = ["params.toml", "*/params.toml"] "crazyflow.control" = ["*/params.toml"] [tool.pytest.ini_options] diff --git a/tests/integration/test_interfaces.py b/tests/integration/test_interfaces.py index 7f63d508..71cfaadc 100644 --- a/tests/integration/test_interfaces.py +++ b/tests/integration/test_interfaces.py @@ -4,7 +4,7 @@ import pytest from scipy.spatial.transform import Rotation as R -from crazyflow.control import Control, load_params, parametrize +from crazyflow.control import Control, load_fn_params, parametrize from crazyflow.control.mellinger import force_torque2rotor_vel, state2attitude from crazyflow.control.transform import motor_force2rotor_vel from crazyflow.sim import Dynamics, Sim @@ -112,8 +112,8 @@ def test_body_rate_interface(): @pytest.mark.integration def test_rotor_vel_interface(): sim = Sim(dynamics=Dynamics.first_principles, control=Control.rotor_vel) - thrust_max = load_params(state2attitude, sim.drone)["thrust_max"] - rpm2thrust = load_params(force_torque2rotor_vel, sim.drone)["rpm2thrust"] + thrust_max = load_fn_params(state2attitude, sim.drone)["thrust_max"] + rpm2thrust = load_fn_params(force_torque2rotor_vel, sim.drone)["rpm2thrust"] max_rpm = motor_force2rotor_vel(np.array([thrust_max]), rpm2thrust)[0] sim.data = sim.data.replace( diff --git a/tests/unit/control/test_core.py b/tests/unit/control/test_core.py index 4b2f2dbd..180767cd 100644 --- a/tests/unit/control/test_core.py +++ b/tests/unit/control/test_core.py @@ -6,7 +6,7 @@ import array_api_strict import pytest -from crazyflow.control import load_params, parametrize +from crazyflow.control import load_fn_params, load_params, parametrize from crazyflow.control.mellinger import ( attitude2force_torque, body_rate2force_torque, @@ -26,23 +26,27 @@ @pytest.mark.unit @pytest.mark.parametrize("fn", _MELLINGER_FNS, ids=lambda fn: fn.__name__) @pytest.mark.parametrize("drone", available_drones) -def test_load_params_keys(fn: Callable[..., Any], drone: str) -> None: - params = load_params(fn, drone) +def test_load_fn_params_keys(fn: Callable[..., Any], drone: str) -> None: + params = load_fn_params(fn, drone) fn_params = inspect.signature(fn).parameters fn_kwargs = {k for k, v in fn_params.items() if v.kind == inspect.Parameter.KEYWORD_ONLY} assert fn_kwargs <= set(params.keys()), f"Missing keys: {fn_kwargs - set(params.keys())}" @pytest.mark.unit -def test_load_params_unknown_drone() -> None: +def test_unknown_drone() -> None: with pytest.raises(KeyError, match="nonexistent_drone"): - load_params(state2attitude, "nonexistent_drone") + load_params("mellinger", "nonexistent_drone") + with pytest.raises(KeyError, match="nonexistent_drone"): + load_fn_params(state2attitude, "nonexistent_drone") + with pytest.raises(KeyError, match="nonexistent_drone"): + parametrize(state2attitude, "nonexistent_drone") @pytest.mark.unit -def test_parametrize_unknown_drone() -> None: - with pytest.raises(KeyError): - parametrize(state2attitude, "nonexistent_drone") +def test_unknown_controller() -> None: + with pytest.raises(KeyError, match="nonexistent_controller"): + load_params("nonexistent_controller", "cf2x_L250") @pytest.mark.unit diff --git a/tests/unit/control/test_mellinger.py b/tests/unit/control/test_mellinger.py index 8d19a5ef..2c8833e0 100644 --- a/tests/unit/control/test_mellinger.py +++ b/tests/unit/control/test_mellinger.py @@ -6,7 +6,7 @@ import pytest from scipy.spatial.transform import Rotation as R -from crazyflow.control import load_params, parametrize +from crazyflow.control import load_fn_params, parametrize from crazyflow.control.mellinger import ( attitude2force_torque, body_rate2force_torque, @@ -123,7 +123,7 @@ def test_state2attitude_integral_error_accumulation(drone: str) -> None: # A constant position error must cause the integral error to accumulate # linearly until it would exceed int_err_max (clipped by the controller). controller = parametrize(state2attitude, drone) - params = load_params(state2attitude, drone) + params = load_fn_params(state2attitude, drone) pos = np.zeros(3) quat = np.array([0.0, 0.0, 0.0, 1.0]) vel = np.zeros(3) @@ -243,7 +243,7 @@ def test_body_rate2force_torque_matches_attitude(drone: str): def test_body_rate2force_torque_leveling(drone: str): # The firmware levels a tilted drone even at the rate setpoint. Zero attitude gains disable it. controller = parametrize(body_rate2force_torque, drone) - params = load_params(body_rate2force_torque, drone) + params = load_fn_params(body_rate2force_torque, drone) quat = R.from_euler("xyz", [0.2, 0.0, 0.0]).as_quat() # Rolled by 0.2 rad ang_vel = np.zeros(3) cmd = np.array([0.0, 0.0, 0.0, 0.5]) diff --git a/tests/unit/control/test_transform.py b/tests/unit/control/test_transform.py index 6df2d5fe..6ea6b9d0 100644 --- a/tests/unit/control/test_transform.py +++ b/tests/unit/control/test_transform.py @@ -5,13 +5,13 @@ import numpy as np import pytest +from crazyflow.control import load_params from crazyflow.control.transform import force2pwm, motor_force2rotor_vel, pwm2force -from crazyflow.drones import load_params @pytest.fixture(scope="module") def core_params() -> dict[str, Any]: - return {k: np.asarray(v) for k, v in load_params("cf2x_L250").items()} + return load_params("mellinger", "cf2x_L250")["core"] @pytest.mark.unit diff --git a/tests/unit/dynamics/test_parametrization.py b/tests/unit/dynamics/test_parametrization.py index e2f87c71..c4c436b9 100644 --- a/tests/unit/dynamics/test_parametrization.py +++ b/tests/unit/dynamics/test_parametrization.py @@ -7,7 +7,14 @@ import pytest from crazyflow.drones import available_drones -from crazyflow.dynamics import available_dynamics, load_params, parametrize +from crazyflow.dynamics import ( + Dynamics, + available_dynamics, + load_fn_params, + load_params, + parametrize, +) +from crazyflow.dynamics.so_rpy import dynamics as so_rpy @pytest.mark.unit @@ -15,7 +22,32 @@ @pytest.mark.parametrize("drone", available_drones) def test_dynamics_parameter_loading(dynamics_name: str, dynamics: Callable, drone: str) -> None: """Check that parameters can be loaded for all available dynamics and drones.""" - load_params(dynamics, drone) + load_fn_params(dynamics, drone) + + +@pytest.mark.unit +@pytest.mark.parametrize("dynamics_name, dynamics", available_dynamics.items()) +@pytest.mark.parametrize("drone", available_drones) +def test_model_parameter_loading(dynamics_name: str, dynamics: Callable, drone: str) -> None: + """Check that all parameters of a model can be loaded for all drones.""" + params = load_params(dynamics_name, drone) + assert "mass" in params and "gravity_vec" in params + + +@pytest.mark.unit +def test_unknown_drone() -> None: + with pytest.raises(KeyError, match="nonexistent_drone"): + load_params(Dynamics.so_rpy, "nonexistent_drone") + with pytest.raises(KeyError, match="nonexistent_drone"): + load_fn_params(so_rpy, "nonexistent_drone") + with pytest.raises(KeyError, match="nonexistent_drone"): + parametrize(so_rpy, "nonexistent_drone") + + +@pytest.mark.unit +def test_unknown_dynamics() -> None: + with pytest.raises(ValueError, match="nonexistent_dynamics"): + load_params("nonexistent_dynamics", "cf2x_L250") @pytest.mark.unit