diff --git a/crazyflow/control/core.py b/crazyflow/control/core.py index 6c18d67e..d8cf682c 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]: @@ -83,46 +123,6 @@ def load_params( 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. - - 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 - - @jax.jit def controllable(step: Array, freq: int, control_steps: Array, control_freq: int) -> Array: """Check which worlds can currently update their controllers. diff --git a/crazyflow/control/mellinger/params.toml b/crazyflow/control/mellinger/params.toml index e4270c01..e8b213cf 100644 --- a/crazyflow/control/mellinger/params.toml +++ b/crazyflow/control/mellinger/params.toml @@ -7,7 +7,7 @@ pwm_max = 65535 thrust_min = 0.012817578393224994 # in N per motor thrust_max = 0.12 # in N per motor torque_pwm_max = [32000.0, 32000.0, 32000.0] -L = 0.03253 +L = 0.03253 # Distance from axis to rotor, i.e., arm / sqrt(2) rpm2thrust = [0.0, -5.382196214637237e-7, 2.4582929831265485e-10] rpm2torque = [0.0, 1.410454111996297e-9, 1.4592584373980652e-12] thrust2torque = 0.007350862856566459 @@ -47,7 +47,7 @@ pwm_max = 65535 thrust_min = 0.012817578393224994 # in N per motor thrust_max = 0.12 # in N per motor torque_pwm_max = [32000.0, 32000.0, 32000.0] -L = 0.03253 +L = 0.03253 # Distance from axis to rotor, i.e., arm / sqrt(2) 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 @@ -87,7 +87,7 @@ pwm_max = 65535 thrust_min = 0.01922636758983749 # in N per motor thrust_max = 0.18 # in N per motor torque_pwm_max = [32000.0, 32000.0, 32000.0] -L = 0.03253 +L = 0.03253 # Distance from axis to rotor, i.e., arm / sqrt(2) 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 @@ -122,7 +122,7 @@ int_err_max = [1.0, 1.0, 1500.0] [cf21B_500.core] gravity_vec = [0.0, 0.0, -9.81] mass = 0.0393 # The controller is using the wrong mass by default -L = 0.035355 +L = 0.035355 # Distance from axis to rotor, i.e., arm / sqrt(2) rpm2thrust = [0.0, -3.133427287299859e-7, 4.407354891648379e-10] rpm2torque = [0.0, 1.65886356219615e-9, 2.4693477924534137e-12] thrust2torque = 0.00593893393599368 @@ -157,3 +157,43 @@ kw = [20000.0, 20000.0, 12000.0] ki_m = [0.0, 0.0, 500.0] kd_omega = [200.0, 200.0, 0.0] int_err_max = [1.0, 1.0, 1500.0] + +[hb_x500] # made up numbers, not used anyway +[hb_x500.core] +gravity_vec = [0.0, 0.0, -9.81] +mass = 2.28 +L = 0.17678 # Distance from axis to rotor, i.e., arm / sqrt(2) +rpm2thrust = [0.0, 1e-6, 1e-9] # Unknown +rpm2torque = [0.0, 1e-9, 1e-12] # Unknown +thrust2torque = 0.01 # Unknown +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] +] +pwm_min = 0 +pwm_max = 1 +thrust_min = 1 # in N per motor +thrust_max = 12.13 # in N per motor +mass_thrust = 0.02061006 +torque_pwm_max = [0.4882887, 0.4882887, 0.4882887] + +[hb_x500.state2attitude] +kp = [12.0, 12.0, 35.0] +kd = [6.0, 6.0, 8.0] +ki = [1.0, 1.0, 1.0] +int_err_max = [4.0, 4.0, 2.0] + +[hb_x500.attitude2force_torque] +kR = [1.068132, 1.068132, 0.9155413] +kw = [0.3051804, 0.3051804, 0.1831083] +ki_m = [0.0, 0.0, 0.007629511] +kd_omega = [0.003051804, 0.003051804, 0.0] +int_err_max = [1.0, 1.0, 1500.0] + +[hb_x500.body_rate2force_torque] +kR = [1.068132, 1.068132, 0.9155413] +kw = [0.3051804, 0.3051804, 0.1831083] +ki_m = [0.0, 0.0, 0.007629511] +kd_omega = [0.003051804, 0.003051804, 0.0] +int_err_max = [1.0, 1.0, 1500.0] diff --git a/crazyflow/drones/__init__.py b/crazyflow/drones/__init__.py index 5c686bb0..26b3a44f 100644 --- a/crazyflow/drones/__init__.py +++ b/crazyflow/drones/__init__.py @@ -17,7 +17,8 @@ # * **cf2x_P250** — Crazyflie 2.x with plus propellers # * **cf2x_T350** — Crazyflie 2.x with thrust upgrade kit # * **cf21B_500** — Crazyflie 2.1 Brushless with 500 mAh battery -available_drones: tuple[str, ...] = ("cf2x_L250", "cf2x_P250", "cf2x_T350", "cf21B_500") +# * **hb_x500** — Holybro X500 V2 +available_drones: tuple[str, ...] = ("cf2x_L250", "cf2x_P250", "cf2x_T350", "cf21B_500", "hb_x500") __all__ = ["available_drones", "load_params"] diff --git a/crazyflow/drones/assets/hb_x500/hb_x500_PropHub.stl b/crazyflow/drones/assets/hb_x500/hb_x500_PropHub.stl new file mode 100644 index 00000000..bafa6246 Binary files /dev/null and b/crazyflow/drones/assets/hb_x500/hb_x500_PropHub.stl differ diff --git a/crazyflow/drones/assets/hb_x500/hb_x500_PropL.stl b/crazyflow/drones/assets/hb_x500/hb_x500_PropL.stl new file mode 100644 index 00000000..62b02ce0 Binary files /dev/null and b/crazyflow/drones/assets/hb_x500/hb_x500_PropL.stl differ diff --git a/crazyflow/drones/assets/hb_x500/hb_x500_PropR.stl b/crazyflow/drones/assets/hb_x500/hb_x500_PropR.stl new file mode 100644 index 00000000..375dada8 Binary files /dev/null and b/crazyflow/drones/assets/hb_x500/hb_x500_PropR.stl differ diff --git a/crazyflow/drones/assets/hb_x500/hb_x500_arm_plastics.stl b/crazyflow/drones/assets/hb_x500/hb_x500_arm_plastics.stl new file mode 100644 index 00000000..0336fa76 Binary files /dev/null and b/crazyflow/drones/assets/hb_x500/hb_x500_arm_plastics.stl differ diff --git a/crazyflow/drones/assets/hb_x500/hb_x500_battery_holder.stl b/crazyflow/drones/assets/hb_x500/hb_x500_battery_holder.stl new file mode 100644 index 00000000..d79bdf83 Binary files /dev/null and b/crazyflow/drones/assets/hb_x500/hb_x500_battery_holder.stl differ diff --git a/crazyflow/drones/assets/hb_x500/hb_x500_frame.stl b/crazyflow/drones/assets/hb_x500/hb_x500_frame.stl new file mode 100644 index 00000000..aeebb9de Binary files /dev/null and b/crazyflow/drones/assets/hb_x500/hb_x500_frame.stl differ diff --git a/crazyflow/drones/assets/hb_x500/hb_x500_fused.stl b/crazyflow/drones/assets/hb_x500/hb_x500_fused.stl new file mode 100644 index 00000000..5086ff58 Binary files /dev/null and b/crazyflow/drones/assets/hb_x500/hb_x500_fused.stl differ diff --git a/crazyflow/drones/assets/hb_x500/hb_x500_gps_antenna.stl b/crazyflow/drones/assets/hb_x500/hb_x500_gps_antenna.stl new file mode 100644 index 00000000..c5bf4db6 Binary files /dev/null and b/crazyflow/drones/assets/hb_x500/hb_x500_gps_antenna.stl differ diff --git a/crazyflow/drones/assets/hb_x500/hb_x500_gps_mount.stl b/crazyflow/drones/assets/hb_x500/hb_x500_gps_mount.stl new file mode 100644 index 00000000..dec09fc3 Binary files /dev/null and b/crazyflow/drones/assets/hb_x500/hb_x500_gps_mount.stl differ diff --git a/crazyflow/drones/assets/hb_x500/hb_x500_gps_sma.stl b/crazyflow/drones/assets/hb_x500/hb_x500_gps_sma.stl new file mode 100644 index 00000000..97607709 Binary files /dev/null and b/crazyflow/drones/assets/hb_x500/hb_x500_gps_sma.stl differ diff --git a/crazyflow/drones/assets/hb_x500/hb_x500_gps_tube.stl b/crazyflow/drones/assets/hb_x500/hb_x500_gps_tube.stl new file mode 100644 index 00000000..1e2c1fcd Binary files /dev/null and b/crazyflow/drones/assets/hb_x500/hb_x500_gps_tube.stl differ diff --git a/crazyflow/drones/assets/hb_x500/hb_x500_leg.stl b/crazyflow/drones/assets/hb_x500/hb_x500_leg.stl new file mode 100644 index 00000000..903840f9 Binary files /dev/null and b/crazyflow/drones/assets/hb_x500/hb_x500_leg.stl differ diff --git a/crazyflow/drones/assets/hb_x500/hb_x500_motors.stl b/crazyflow/drones/assets/hb_x500/hb_x500_motors.stl new file mode 100644 index 00000000..11d3f206 Binary files /dev/null and b/crazyflow/drones/assets/hb_x500/hb_x500_motors.stl differ diff --git a/crazyflow/drones/assets/hb_x500/hb_x500_propguard_center.stl b/crazyflow/drones/assets/hb_x500/hb_x500_propguard_center.stl new file mode 100644 index 00000000..85af1454 Binary files /dev/null and b/crazyflow/drones/assets/hb_x500/hb_x500_propguard_center.stl differ diff --git a/crazyflow/drones/assets/hb_x500/hb_x500_propguard_left.stl b/crazyflow/drones/assets/hb_x500/hb_x500_propguard_left.stl new file mode 100644 index 00000000..a2d86297 Binary files /dev/null and b/crazyflow/drones/assets/hb_x500/hb_x500_propguard_left.stl differ diff --git a/crazyflow/drones/assets/hb_x500/hb_x500_propguard_leg.stl b/crazyflow/drones/assets/hb_x500/hb_x500_propguard_leg.stl new file mode 100644 index 00000000..12071615 Binary files /dev/null and b/crazyflow/drones/assets/hb_x500/hb_x500_propguard_leg.stl differ diff --git a/crazyflow/drones/assets/hb_x500/hb_x500_propguard_right.stl b/crazyflow/drones/assets/hb_x500/hb_x500_propguard_right.stl new file mode 100644 index 00000000..a566885f Binary files /dev/null and b/crazyflow/drones/assets/hb_x500/hb_x500_propguard_right.stl differ diff --git a/crazyflow/drones/assets/hb_x500/hb_x500_screws.stl b/crazyflow/drones/assets/hb_x500/hb_x500_screws.stl new file mode 100644 index 00000000..0c5a04ca Binary files /dev/null and b/crazyflow/drones/assets/hb_x500/hb_x500_screws.stl differ diff --git a/crazyflow/drones/assets/hb_x500/hb_x500_tubes.stl b/crazyflow/drones/assets/hb_x500/hb_x500_tubes.stl new file mode 100644 index 00000000..66845609 Binary files /dev/null and b/crazyflow/drones/assets/hb_x500/hb_x500_tubes.stl differ diff --git a/crazyflow/drones/hb_x500.xml b/crazyflow/drones/hb_x500.xml new file mode 100644 index 00000000..55c5d39e --- /dev/null +++ b/crazyflow/drones/hb_x500.xml @@ -0,0 +1,189 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/crazyflow/drones/params.toml b/crazyflow/drones/params.toml index a8119bdd..2359df88 100644 --- a/crazyflow/drones/params.toml +++ b/crazyflow/drones/params.toml @@ -3,7 +3,7 @@ [cf2x_L250] gravity_vec = [0.0, 0.0, -9.81] mass = 0.0319 -L = 0.03253 +L = 0.03253 # Distance from axis to rotor, i.e., arm / sqrt(2) J = [ # TODO [16.8e-6, 0.0, 0.0], [0.0, 16.8e-6, 0.0], @@ -41,7 +41,7 @@ prop_inertia = 34.52e-9 # TODO seems off [cf2x_P250] gravity_vec = [0.0, 0.0, -9.81] mass = 0.0318 -L = 0.03253 +L = 0.03253 # Distance from axis to rotor, i.e., arm / sqrt(2) J = [ # TODO from L250 [16.8e-6, 0.0, 0.0], [0.0, 16.8e-6, 0.0], @@ -79,7 +79,7 @@ prop_inertia = 26.97e-9 [cf2x_T350] gravity_vec = [0.0, 0.0, -9.81] mass = 0.0379 -L = 0.03253 +L = 0.03253 # Distance from axis to rotor, i.e., arm / sqrt(2) J = [ [15.7e-6, 0.0, 0.0], [0.0, 17.1e-6, 0.0], @@ -117,7 +117,7 @@ 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 +L = 0.035355 # Distance from axis to rotor, i.e., arm / sqrt(2) J = [ [25e-6, 0.0, 0.0], [0.0, 28e-6, 0.0], @@ -149,4 +149,38 @@ vmotor2thrust = [-0.014058926705279723, 0.04265273261724981, 0.00183277601440174 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 +prop_inertia = 38.93e-9 + +[hb_x500] +gravity_vec = [0.0, 0.0, -9.81] +mass = 2.28 +L = 0.17678 # Distance from axis to rotor, i.e., arm / sqrt(2) +J = [ # approximated + [24e-3, 0.0, 0.0], + [0.0, 24e-3, 0.0], + [0.0, 0.0, 44e-3] +] +rpm2thrust = [0.0, 1e-6, 1e-9] # Unknown +rpm2torque = [0.0, 1e-9, 1e-12] # Unknown +thrust2torque = 0.01 # Unknown +rotor_dyn_coef = [ 20.0, 0.0, 10.0, 0.0001,] # Unknown +rotor_dyn_coef_simple = 20.0 # Unknown +thrust_dyn_coef = 14.06 # Unknown +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.8031707417931839, 0.0, 0.0,], + [ 0.0, -0.8031707417931839, 0.0,], + [ 0.0, 0.0, -1.013867567938818,], +] +# 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 = 0 +pwm_max = 1 +thrust_min = 1 # in N per motor +thrust_max = 12.13 # in N per motor +prop_radius = 126.0e-3 +prop_inertia = 70.0e-6 # approximated \ No newline at end of file diff --git a/crazyflow/dynamics/__init__.py b/crazyflow/dynamics/__init__.py index 3c450880..71a13da6 100644 --- a/crazyflow/dynamics/__init__.py +++ b/crazyflow/dynamics/__init__.py @@ -14,13 +14,27 @@ from typing import Callable -from crazyflow.dynamics.core import Dynamics, load_params, parametrize +from crazyflow.dynamics.core import ( + Dynamics, + load_params, + parametrize, + supported_drones, + supported_dynamics, +) 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", + "available_dynamics", + "dynamics_features", + "supported_drones", + "supported_dynamics", + "Dynamics", +] available_dynamics: dict[str, Callable] = { diff --git a/crazyflow/dynamics/core.py b/crazyflow/dynamics/core.py index e5999820..09621a69 100644 --- a/crazyflow/dynamics/core.py +++ b/crazyflow/dynamics/core.py @@ -9,6 +9,7 @@ import numpy as np +from crazyflow.drones import available_drones 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 +22,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. @@ -104,8 +115,9 @@ def load_params( dynamics-specific coefficients for ``dynamics``. Raises: - KeyError: If ``drone`` is not found in either TOML file, or if ``dynamics`` does not + KeyError: If ``drone`` is not found in ``drones/params.toml``, or if ``dynamics`` does not correspond to a known sub-package. + NotImplementedError: If ``dynamics`` is not available for ``drone``. """ assert isinstance(fn, Callable), f"Expected a function, got {type(fn)}" dynamics = fn.__module__.split(".")[-2] @@ -114,7 +126,10 @@ def load_params( 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") + raise NotImplementedError( + f"Dynamics `{dynamics}` not available for drone `{drone}`: not found in " + f"{dynamics}/params.toml" + ) params = load_physical_params(drone) | 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. @@ -122,11 +137,22 @@ def load_params( return to_xp(filter_to_signature(params, fn), xp=xp, device=device) -class Dynamics(StrEnum): - """Dynamics mode for the simulation.""" +def _param_sections(dynamics: str) -> set[str]: + """Return the drone sections declared in a dynamics' ``params.toml``.""" + if dynamics not in Dynamics: + raise KeyError(f"Dynamics `{dynamics}` not found. Available dynamics: {tuple(Dynamics)}") + with open(Path(__file__).parent / f"{dynamics}/params.toml", "rb") as f: + return set(tomllib.load(f)) - 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 supported_drones(dynamics: str) -> tuple[str, ...]: + """Return the drones that ``dynamics`` can be parametrized for.""" + sections = _param_sections(dynamics) + return tuple(drone for drone in available_drones if drone in sections) + + +def supported_dynamics(drone: str) -> tuple[str, ...]: + """Return the dynamics that ``drone`` can be simulated with.""" + if drone not in available_drones: + raise KeyError(f"Drone `{drone}` not found. Available drones: {available_drones}") + return tuple(str(d) for d in Dynamics if drone in _param_sections(d)) diff --git a/crazyflow/dynamics/so_rpy/params.toml b/crazyflow/dynamics/so_rpy/params.toml index 2de1657b..39ca9f06 100644 --- a/crazyflow/dynamics/so_rpy/params.toml +++ b/crazyflow/dynamics/so_rpy/params.toml @@ -28,3 +28,11 @@ cmd_f_coef = 0.96836458 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] + + +[hb_x500] +acc_coef = 0.0 +cmd_f_coef = 0.9121643489648786 +rpy_coef = [ -51.42302192586755, -51.42302192586755, -26.929070173696157,] +rpy_rates_coef = [ -8.93635377240008, -8.93635377240008, -8.646484332625702,] +cmd_rpy_coef = [ 47.3447720777806, 47.3447720777806, 25.310886504404653,] diff --git a/crazyflow/dynamics/so_rpy_rotor/params.toml b/crazyflow/dynamics/so_rpy_rotor/params.toml index edb6a40c..2ea7ba8a 100644 --- a/crazyflow/dynamics/so_rpy_rotor/params.toml +++ b/crazyflow/dynamics/so_rpy_rotor/params.toml @@ -31,4 +31,13 @@ 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] + + +[hb_x500] +acc_coef = 0.0 +cmd_f_coef = 0.9122876298664497 +thrust_time_coef = 0.04493428195421334 +rpy_coef = [ -51.42302192586755, -51.42302192586755, -26.929070173696157,] +rpy_rates_coef = [ -8.93635377240008, -8.93635377240008, -8.646484332625702,] +cmd_rpy_coef = [ 47.3447720777806, 47.3447720777806, 25.310886504404653,] diff --git a/crazyflow/dynamics/so_rpy_rotor_drag/params.toml b/crazyflow/dynamics/so_rpy_rotor_drag/params.toml index 3c37ef14..1f87c9d3 100644 --- a/crazyflow/dynamics/so_rpy_rotor_drag/params.toml +++ b/crazyflow/dynamics/so_rpy_rotor_drag/params.toml @@ -52,3 +52,14 @@ drag_matrix = [ 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] + +[hb_x500] +acc_coef = 0.0 +cmd_f_coef = 0.9234799918739313 +thrust_time_coef = 0.07111262651857447 +drag_matrix = [ [ -0.8031707417931839, 0.0, 0.0,], + [ 0.0, -0.8031707417931839, 0.0,], + [ 0.0, 0.0, -1.013867567938818,],] +rpy_coef = [ -51.423019722268464, -51.423019722268464, -26.92907102709692,] +rpy_rates_coef = [ -8.9363626451314, -8.9363626451314, -8.646483972362423,] +cmd_rpy_coef = [ 47.34477305534468, 47.34477305534468, 25.310887633982862,] diff --git a/docs/index.md b/docs/index.md index 522ed396..34f96fa1 100644 --- a/docs/index.md +++ b/docs/index.md @@ -98,7 +98,7 @@ Crazyflow is a research simulator for Crazyflie-style quadrotors that runs milli --> -All drone configurations are bundled with `crazyflow.dynamics`. Available configurations: `cf2x_L250`, `cf2x_P250`, `cf2x_T350`, `cf21B_500`, and any drone returned by `crazyflow.available_drones`. +All drone configurations are bundled with `crazyflow.dynamics`. Available configurations: `cf2x_L250`, `cf2x_P250`, `cf2x_T350`, `cf21B_500`, `hb_x500`, and any drone returned by `crazyflow.available_drones`. --- diff --git a/docs/user-guide/control/parametrize.md b/docs/user-guide/control/parametrize.md index 006eadcd..80016bac 100644 --- a/docs/user-guide/control/parametrize.md +++ b/docs/user-guide/control/parametrize.md @@ -61,6 +61,7 @@ The following configurations ship with pre-fitted parameters: | `"cf2x_P250"` | Crazyflie 2.x, plus propellers | | `"cf2x_T350"` | Crazyflie 2.x, thrust upgrade kit | | `"cf21B_500"` | Crazyflie 2.1 Brushless | +| `"hb_x500"` | Holybro X500 V2 | Pass the drone name as a plain string: diff --git a/docs/user-guide/dynamics/parametrize.md b/docs/user-guide/dynamics/parametrize.md index 1b7a2ecf..f1bd8804 100644 --- a/docs/user-guide/dynamics/parametrize.md +++ b/docs/user-guide/dynamics/parametrize.md @@ -21,7 +21,7 @@ The following configurations ship with pre-fitted parameters. They cover both th ```python from crazyflow.drones import available_drones -available_drones # ('cf2x_L250', 'cf2x_P250', 'cf2x_T350', 'cf21B_500') +available_drones # ('cf2x_L250', 'cf2x_P250', 'cf2x_T350', 'cf21B_500', 'hb_x500') ``` | `drone` | Platform | @@ -30,6 +30,7 @@ available_drones # ('cf2x_L250', 'cf2x_P250', 'cf2x_T350', 'cf21B_500') | `"cf2x_P250"` | Crazyflie 2.x, plus propellers | | `"cf2x_T350"` | Crazyflie 2.x, thrust upgrade kit | | `"cf21B_500"` | Crazyflie 2.1 Brushless | +| `"hb_x500"` | Holybro X500 V2 | If your drone is not listed, you can identify the parameters from flight data using the [system identification pipeline](system-identification.md) and inject them into any dynamics. diff --git a/tests/conftest.py b/tests/conftest.py index 7c88d620..9478e81b 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -7,6 +7,7 @@ import jax import pytest +from _pytest.mark import ParameterSet # The cache dir is per-user. A shared dir like /tmp/jax_cache breaks on multi-user machines, since # jax hard-fails on GPU autotune cache writes when another user owns the directory. @@ -43,3 +44,24 @@ def device() -> str: os.environ.get("DISPLAY") is None, reason="DISPLAY is not set, skipping test in headless environment", ) + + +def drone_dynamics_fns() -> list[ParameterSet]: + from crazyflow.dynamics import available_dynamics, supported_drones + + return [ + pytest.param(name, fn, drone, id=f"{drone}-{name}") + for name, fn in available_dynamics.items() + for drone in supported_drones(name) + ] + + +def drone_dynamics() -> list[ParameterSet]: + from crazyflow.drones import available_drones + from crazyflow.dynamics import supported_dynamics + + return [ + pytest.param(dynamics, drone, id=f"{drone}-{dynamics}") + for drone in available_drones + for dynamics in supported_dynamics(drone) + ] diff --git a/tests/integration/test_models.py b/tests/integration/test_models.py index 53d88966..3e1790da 100644 --- a/tests/integration/test_models.py +++ b/tests/integration/test_models.py @@ -1,13 +1,12 @@ import pytest +from conftest import drone_dynamics -from crazyflow import available_drones from crazyflow.dynamics import Dynamics from crazyflow.sim import Sim @pytest.mark.integration -@pytest.mark.parametrize("dynamics", Dynamics) -@pytest.mark.parametrize("drone", available_drones) +@pytest.mark.parametrize("dynamics, drone", drone_dynamics()) def test_attitude_symbolic(dynamics: Dynamics, drone: "str"): """Tests if xml files contain syntax errors.""" Sim(dynamics=dynamics, drone=drone) diff --git a/tests/unit/dynamics/test_dynamics.py b/tests/unit/dynamics/test_dynamics.py index 8333bae1..0cdcb285 100644 --- a/tests/unit/dynamics/test_dynamics.py +++ b/tests/unit/dynamics/test_dynamics.py @@ -12,8 +12,8 @@ import numpy as np import pytest from array_api_compat import device as xp_device +from conftest import drone_dynamics_fns -from crazyflow.drones import available_drones from crazyflow.dynamics import available_dynamics, dynamics_features from crazyflow.dynamics.core import parametrize @@ -177,15 +177,13 @@ def test_dynamics_features(dynamics_name: str, dynamics: Callable): @pytest.mark.unit -@pytest.mark.parametrize("dynamics_name, dynamics", available_dynamics.items()) -@pytest.mark.parametrize("drone", available_drones) +@pytest.mark.parametrize("dynamics_name, dynamics, drone", drone_dynamics_fns()) def test_dynamics_shapes(dynamics_name: str, dynamics: Callable, drone: str): check_shapes(parametrize(dynamics, drone)) @pytest.mark.unit -@pytest.mark.parametrize("dynamics_name, dynamics", available_dynamics.items()) -@pytest.mark.parametrize("drone", available_drones) +@pytest.mark.parametrize("dynamics_name, dynamics, drone", drone_dynamics_fns()) def test_dynamics_shapes_batched(dynamics_name: str, dynamics: Callable, drone: str): dynamics = parametrize(dynamics, drone, xp=xp) batch = (10, 5) @@ -196,8 +194,7 @@ def test_dynamics_shapes_batched(dynamics_name: str, dynamics: Callable, drone: @pytest.mark.unit -@pytest.mark.parametrize("dynamics_name, dynamics", available_dynamics.items()) -@pytest.mark.parametrize("drone", available_drones) +@pytest.mark.parametrize("dynamics_name, dynamics, drone", drone_dynamics_fns()) @pytest.mark.parametrize("ext_wrench", [False, True]) @pytest.mark.parametrize("per_motor_params", [False, True]) def test_symbolic_dynamics( @@ -229,8 +226,7 @@ def test_symbolic_dynamics( @pytest.mark.unit -@pytest.mark.parametrize("dynamics_name, dynamics", available_dynamics.items()) -@pytest.mark.parametrize("drone", available_drones) +@pytest.mark.parametrize("dynamics_name, dynamics, drone", drone_dynamics_fns()) def test_compare_batched_non_batched(dynamics_name: str, dynamics: Callable, drone: str): """Tests if batching works and if the results are identical to the non-batched version.""" dynamics = parametrize(dynamics, drone) @@ -246,8 +242,7 @@ def test_compare_batched_non_batched(dynamics_name: str, dynamics: Callable, dro @pytest.mark.unit -@pytest.mark.parametrize("dynamics_name, dynamics", available_dynamics.items()) -@pytest.mark.parametrize("drone", available_drones) +@pytest.mark.parametrize("dynamics_name, dynamics, drone", drone_dynamics_fns()) def test_batched_params(dynamics_name: str, dynamics: Callable, drone: str): """Tests if batched parameters give the same results as the shared parameters.""" dynamics = parametrize(dynamics, drone, xp=xp) @@ -274,8 +269,7 @@ def test_batched_params(dynamics_name: str, dynamics: Callable, drone: str): @pytest.mark.unit -@pytest.mark.parametrize("dynamics_name, dynamics", available_dynamics.items()) -@pytest.mark.parametrize("drone", available_drones) +@pytest.mark.parametrize("dynamics_name, dynamics, drone", drone_dynamics_fns()) def test_numeric_jit(dynamics_name: str, dynamics: Callable, drone: str): """Tests if the dynamics are jitable and if the results are identical to the array API ones.""" dynamics = parametrize(dynamics, drone) diff --git a/tests/unit/dynamics/test_parametrization.py b/tests/unit/dynamics/test_parametrization.py index e2f87c71..4b440ff9 100644 --- a/tests/unit/dynamics/test_parametrization.py +++ b/tests/unit/dynamics/test_parametrization.py @@ -5,22 +5,20 @@ from typing import Callable import pytest +from conftest import drone_dynamics_fns -from crazyflow.drones import available_drones -from crazyflow.dynamics import available_dynamics, load_params, parametrize +from crazyflow.dynamics import load_params, parametrize @pytest.mark.unit -@pytest.mark.parametrize("dynamics_name, dynamics", available_dynamics.items()) -@pytest.mark.parametrize("drone", available_drones) +@pytest.mark.parametrize("dynamics_name, dynamics, drone", drone_dynamics_fns()) 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) @pytest.mark.unit -@pytest.mark.parametrize("dynamics_name, dynamics", available_dynamics.items()) -@pytest.mark.parametrize("drone", available_drones) +@pytest.mark.parametrize("dynamics_name, dynamics, drone", drone_dynamics_fns()) def test_dynamics_parametrization(dynamics_name: str, dynamics: Callable, drone: str): """Check that we can parametrize all available dynamics with all drones.""" parametrize(dynamics, drone)