diff --git a/crazyflow/drones/cf21B_500.xml b/crazyflow/drones/cf21B_500.xml index ae02aab2..455da94b 100644 --- a/crazyflow/drones/cf21B_500.xml +++ b/crazyflow/drones/cf21B_500.xml @@ -49,7 +49,7 @@ - + @@ -92,7 +92,7 @@ - + diff --git a/crazyflow/drones/cf2x_L250.xml b/crazyflow/drones/cf2x_L250.xml index 104c2186..9082d9f8 100644 --- a/crazyflow/drones/cf2x_L250.xml +++ b/crazyflow/drones/cf2x_L250.xml @@ -49,7 +49,7 @@ - + @@ -93,7 +93,7 @@ - + diff --git a/crazyflow/drones/cf2x_P250.xml b/crazyflow/drones/cf2x_P250.xml index 295c1f10..40b14bda 100644 --- a/crazyflow/drones/cf2x_P250.xml +++ b/crazyflow/drones/cf2x_P250.xml @@ -49,7 +49,7 @@ - + @@ -92,7 +92,7 @@ - + diff --git a/crazyflow/drones/cf2x_T350.xml b/crazyflow/drones/cf2x_T350.xml index 0fd0ae4f..2b24b91e 100644 --- a/crazyflow/drones/cf2x_T350.xml +++ b/crazyflow/drones/cf2x_T350.xml @@ -50,7 +50,7 @@ - + @@ -93,7 +93,7 @@ - + diff --git a/crazyflow/dynamics/first_principles/params.toml b/crazyflow/dynamics/first_principles/params.toml index d5e2e415..0d2fda1b 100644 --- a/crazyflow/dynamics/first_principles/params.toml +++ b/crazyflow/dynamics/first_principles/params.toml @@ -32,7 +32,7 @@ # rotor model, no drag and no propeller gyroscopic torque are reasonable defaults for a new drone. [cf2x_L250] -mass = 0.0319 +mass = 0.0328 J = [ # TODO [16.8e-6, 0.0, 0.0], [0.0, 16.8e-6, 0.0], @@ -51,9 +51,9 @@ mixing_matrix = [ [-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 ] + [-0.014953790232821078, 0.0, 0.0], + [0.0, -0.014953790232821078, 0.0], + [0.0, 0.0, -0.013900090000325344] ] @@ -103,14 +103,14 @@ mixing_matrix = [ [-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 ] + [-0.015203241038199845, 0.0, 0.0], + [0.0, -0.015203241038199845, 0.0], + [0.0, 0.0, -0.02204579179094129] ] [cf21B_500] -mass = 0.04338 +mass = 0.0434 J = [ [25e-6, 0.0, 0.0], [0.0, 28e-6, 0.0], @@ -129,7 +129,7 @@ mixing_matrix = [ [-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 ] + [-0.021643637770852733, 0.0, 0.0], + [0.0, -0.021643637770852733, 0.0], + [0.0, 0.0, -0.02477138502714662] ] diff --git a/crazyflow/dynamics/so_rpy/params.toml b/crazyflow/dynamics/so_rpy/params.toml index 44f40317..435beb27 100644 --- a/crazyflow/dynamics/so_rpy/params.toml +++ b/crazyflow/dynamics/so_rpy/params.toml @@ -19,7 +19,7 @@ # docs/user-guide/dynamics/system-identification.md). [cf2x_L250] -mass = 0.0319 +mass = 0.0328 J = [ [16.8e-6, 0.0, 0.0], [0.0, 16.8e-6, 0.0], @@ -28,10 +28,10 @@ J = [ 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] -rpy_rates_coef = [-17.32, -17.32, -25.63] -cmd_rpy_coef = [196.18, 196.18, 390.27] +cmd_f_coef = 1.023224599403443 +rpy_coef = [-485.8620950386863, -485.8620950386863, -333.2438787006866] +rpy_rates_coef = [-33.17729279340664, -33.17729279340664, -39.535506757052424] +cmd_rpy_coef = [448.0183990789311, 448.0183990789311, 306.31685518928396] [cf2x_P250] @@ -60,14 +60,14 @@ J = [ 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] -rpy_rates_coef = [-29.26311118, -29.26311118, -29.74357219] -cmd_rpy_coef = [347.94260321, 347.94260321, 241.06977014] +cmd_f_coef = 1.0140372607776442 +rpy_coef = [-373.81009672301474, -373.81009672301474, -262.01237938054936] +rpy_rates_coef = [-29.447753470026274, -29.447753470026274, -29.745699818936384] +cmd_rpy_coef = [350.209624193645, 350.209624193645, 241.085024111866] [cf21B_500] -mass = 0.04338 +mass = 0.0434 J = [ [25e-6, 0.0, 0.0], [0.0, 28e-6, 0.0], @@ -76,10 +76,10 @@ J = [ 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] -rpy_rates_coef = [-12.7803, -12.7803, -16.8485] -cmd_rpy_coef = [138.0834, 138.0834, 198.5161] +cmd_f_coef = 0.9459441379738676 +rpy_coef = [-156.34812197843243, -156.34812197843243, -144.7222372741053] +rpy_rates_coef = [-16.300330418164553, -16.300330418164553, -17.368699336318954] +cmd_rpy_coef = [139.7494294272397, 139.7494294272397, 127.01037564242333] [hb_x500] mass = 2.28 diff --git a/crazyflow/dynamics/so_rpy_rotor/params.toml b/crazyflow/dynamics/so_rpy_rotor/params.toml index 2cdab943..ae42deea 100644 --- a/crazyflow/dynamics/so_rpy_rotor/params.toml +++ b/crazyflow/dynamics/so_rpy_rotor/params.toml @@ -20,7 +20,7 @@ # docs/user-guide/dynamics/system-identification.md). [cf2x_L250] -mass = 0.0319 +mass = 0.0328 J = [ [16.8e-6, 0.0, 0.0], [0.0, 16.8e-6, 0.0], @@ -29,11 +29,11 @@ J = [ 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 -rpy_coef = [-245.67, -245.67, -227.78] -rpy_rates_coef = [-17.32, -17.32, -25.63] -cmd_rpy_coef = [196.18, 196.18, 390.27] +cmd_f_coef = 1.0242686698819605 +thrust_time_coef = 0.08671854102279604 +rpy_coef = [-485.8620950386863, -485.8620950386863, -333.2438787006866] +rpy_rates_coef = [-33.17729279340664, -33.17729279340664, -39.535506757052424] +cmd_rpy_coef = [448.0183990789311, 448.0183990789311, 306.31685518928396] [cf2x_P250] @@ -63,15 +63,15 @@ J = [ 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? -rpy_coef = [-371.41695523, -371.41695523, -261.99549945] -rpy_rates_coef = [-29.26311118, -29.26311118, -29.74357219] -cmd_rpy_coef = [347.94260321, 347.94260321, 241.06977014] +cmd_f_coef = 1.0145454356801935 +thrust_time_coef = 0.05941543454583637 +rpy_coef = [-373.81009672301474, -373.81009672301474, -262.01237938054936] +rpy_rates_coef = [-29.447753470026274, -29.447753470026274, -29.745699818936384] +cmd_rpy_coef = [350.209624193645, 350.209624193645, 241.085024111866] [cf21B_500] -mass = 0.04338 +mass = 0.0434 J = [ [25e-6, 0.0, 0.0], [0.0, 28e-6, 0.0], @@ -80,11 +80,11 @@ J = [ 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] +cmd_f_coef = 0.9472350463278153 +thrust_time_coef = 0.05180073413161973 +rpy_coef = [-156.34812197843243, -156.34812197843243, -144.7222372741053] +rpy_rates_coef = [-16.300330418164553, -16.300330418164553, -17.368699336318954] +cmd_rpy_coef = [139.7494294272397, 139.7494294272397, 127.01037564242333] [hb_x500] mass = 2.28 diff --git a/crazyflow/dynamics/so_rpy_rotor_drag/params.toml b/crazyflow/dynamics/so_rpy_rotor_drag/params.toml index 7ed93725..749b1ac5 100644 --- a/crazyflow/dynamics/so_rpy_rotor_drag/params.toml +++ b/crazyflow/dynamics/so_rpy_rotor_drag/params.toml @@ -25,7 +25,7 @@ # docs/user-guide/dynamics/system-identification.md). [cf2x_L250] -mass = 0.0319 +mass = 0.0328 J = [ [16.8e-6, 0.0, 0.0], [0.0, 16.8e-6, 0.0], @@ -34,16 +34,16 @@ J = [ 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 +cmd_f_coef = 1.0322435843278281 +thrust_time_coef = 0.14876008226610188 drag_matrix = [ - [-0.01471782, 0.0, 0.0 ], - [0.0, -0.01471782, 0.0 ], - [0.0, 0.0, -0.01277641 ] + [-0.014953790232821078, 0.0, 0.0], + [0.0, -0.014953790232821078, 0.0], + [0.0, 0.0, -0.013900090000325344] ] -rpy_coef = [-245.67, -245.67, -227.78] -rpy_rates_coef = [-17.32, -17.32, -25.63] -cmd_rpy_coef = [196.18, 196.18, 390.27] +rpy_coef = [-485.8620950386863, -485.8620950386863, -333.2438787006866] +rpy_rates_coef = [-33.17729279340664, -33.17729279340664, -39.535506757052424] +cmd_rpy_coef = [448.0183990789311, 448.0183990789311, 306.31685518928396] [cf2x_P250] @@ -78,20 +78,20 @@ J = [ 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 +cmd_f_coef = 1.0229179982077607 +thrust_time_coef = 0.17585713583659168 drag_matrix = [ - [-0.01521728, 0.0, 0.0 ], - [0.0, -0.01521728, 0.0 ], - [0.0, 0.0, -0.02144565 ] + [-0.015203241038199845, 0.0, 0.0], + [0.0, -0.015203241038199845, 0.0], + [0.0, 0.0, -0.02204579179094129] ] -rpy_coef = [-371.41695523, -371.41695523, -261.99549945] -rpy_rates_coef = [-29.26311118, -29.26311118, -29.74357219] -cmd_rpy_coef = [347.94260321, 347.94260321, 241.06977014] +rpy_coef = [-373.81009672301474, -373.81009672301474, -262.01237938054936] +rpy_rates_coef = [-29.447753470026274, -29.447753470026274, -29.745699818936384] +cmd_rpy_coef = [350.209624193645, 350.209624193645, 241.085024111866] [cf21B_500] -mass = 0.04338 +mass = 0.0434 J = [ [25e-6, 0.0, 0.0], [0.0, 28e-6, 0.0], @@ -100,16 +100,16 @@ J = [ 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 +cmd_f_coef = 0.959471532998666 +thrust_time_coef = 0.08824147411162254 drag_matrix = [ - [-0.02149163, 0.0, 0.0 ], - [0.0, -0.02149163, 0.0 ], - [0.0, 0.0, -0.02359736 ] + [-0.021643637770852733, 0.0, 0.0], + [0.0, -0.021643637770852733, 0.0], + [0.0, 0.0, -0.02477138502714662] ] -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] +rpy_coef = [-156.34812197843243, -156.34812197843243, -144.7222372741053] +rpy_rates_coef = [-16.300330418164553, -16.300330418164553, -17.368699336318954] +cmd_rpy_coef = [139.7494294272397, 139.7494294272397, 127.01037564242333] [hb_x500] mass = 2.28 diff --git a/crazyflow/dynamics/utils/identification.py b/crazyflow/dynamics/utils/identification.py index d18ac6d9..664d1309 100644 --- a/crazyflow/dynamics/utils/identification.py +++ b/crazyflow/dynamics/utils/identification.py @@ -170,16 +170,23 @@ def _residual_fun_trans_jac( constants: dict[str, Array], acc_observed: Array, ) -> Callable: + # The residuals ignore the parameters the dynamics do not have, so their Jacobian columns + # must be zero as well. Otherwise the optimizer steps along gradients of the full model + # that change nothing in the residuals and settles far from the minimum. match dynamics: # Dummy values for other params case "so_rpy": params_jnp = jnp.array([params[0], 0.0, 0.0, 0.0]) + mask = jnp.array([1.0, 0.0, 0.0, 0.0]) case "so_rpy_rotor": params_jnp = jnp.array([params[0], params[1], 0.0, 0.0]) + mask = jnp.array([1.0, 1.0, 0.0, 0.0]) case "so_rpy_rotor_drag": params_jnp = jnp.array([params[0], params[1], params[2], params[3]]) + mask = jnp.array([1.0, 1.0, 1.0, 1.0]) case _: raise ValueError(f"Unknown dynamics type: {dynamics}") - return jax.device_get(jac_fun(params_jnp, quat, vel, cmd_f, t, constants, acc_observed)) + jac = jac_fun(params_jnp, quat, vel, cmd_f, t, constants, acc_observed) + return jax.device_get(jac * mask) return _residual_fun_trans, _residual_fun_trans_jac diff --git a/docs/user-guide/dynamics/dynamics-functions.md b/docs/user-guide/dynamics/dynamics-functions.md index 4287ddb1..5d49a1bf 100644 --- a/docs/user-guide/dynamics/dynamics-functions.md +++ b/docs/user-guide/dynamics/dynamics-functions.md @@ -60,8 +60,8 @@ from crazyflow.dynamics.so_rpy_rotor_drag import dynamics dynamics = parametrize(dynamics, drone="cf2x_L250") # Reuses pos, quat, vel, ang_vel from above; the command interface is what differs -cmd = np.array([0.0, 0.0, 0.0, 0.31]) # [roll_rad, pitch_rad, yaw_rad, thrust_N] -rotor_vel = np.full(4, 0.31) # shape (4,) — thrust state [N]; None to skip thrust dynamics +cmd = np.array([0.0, 0.0, 0.0, 0.32]) # [roll_rad, pitch_rad, yaw_rad, thrust_N] +rotor_vel = np.full(4, 0.32) # shape (4,) — thrust state [N]; None to skip thrust dynamics pos_dot, quat_dot, vel_dot, ang_vel_dot, rotor_vel_dot = dynamics( pos, quat, vel, ang_vel, cmd, rotor_vel diff --git a/docs/user-guide/dynamics/parametrize.md b/docs/user-guide/dynamics/parametrize.md index da461e68..8f975926 100644 --- a/docs/user-guide/dynamics/parametrize.md +++ b/docs/user-guide/dynamics/parametrize.md @@ -88,7 +88,7 @@ rotor_vel = np.zeros(4) cmd = np.zeros(4) # Simulate with a 10 g payload for this call only — dynamics.keywords is not modified. -pos_dot, *_ = dynamics(pos, quat, vel, ang_vel, cmd, rotor_vel, mass=0.0419) +pos_dot, *_ = dynamics(pos, quat, vel, ang_vel, cmd, rotor_vel, mass=0.0428) ``` This becomes particularly useful for domain randomization: instead of baking randomized parameters into the partial, you can pass a batch of them as call-time arguments and keep the step function JIT-compiled across parameter changes. See [Batching & domain randomization](batching.md) for the full pattern. @@ -130,7 +130,7 @@ If you need the parameter values directly, for example, to pass them to [`symbol from crazyflow.dynamics import Dynamics, load_fn_params, load_params params = load_fn_params(dynamics, "cf2x_L250") -params["mass"] # 0.0319 +params["mass"] # 0.0328 params["J_inv"] # array([...]) params = load_params(Dynamics.first_principles, "cf2x_L250") diff --git a/docs/user-guide/dynamics/system-identification.md b/docs/user-guide/dynamics/system-identification.md index 31ed9562..72aeefef 100644 --- a/docs/user-guide/dynamics/system-identification.md +++ b/docs/user-guide/dynamics/system-identification.md @@ -43,7 +43,7 @@ data = derivatives_svf(data) # Step 4 — fit translational parameters trans_params = sys_id_translation( dynamics="so_rpy_rotor_drag", - mass=0.0319, # drone mass in kg — measure this directly + mass=0.0328, # drone mass in kg — measure this directly data=data, verbose=0, # 0 = silent, 1 = progress, 2 = full optimizer output plot=True, # show fit vs. measured plots @@ -71,7 +71,7 @@ data_valid = derivatives_svf(data_valid) trans_params = sys_id_translation( dynamics="so_rpy_rotor_drag", - mass=0.0319, + mass=0.0328, data=data, data_validation=data_valid, plot=True, @@ -84,14 +84,14 @@ Once you have the identified coefficients, add them to the relevant `params.toml ```toml [my_drone] -cmd_f_coef = 0.983 # from trans_params["cmd_f_coef"] -thrust_time_coef = 0.121 # from trans_params["thrust_time_coef"] -drag_matrix = [[-0.0147, 0.0, 0.0], - [0.0, -0.0147, 0.0], - [0.0, 0.0, -0.0128]] # diag([drag_xy, drag_xy, drag_z]) -rpy_coef = [-245.67, -245.67, -227.78] # from rot_params["rpy_coef"] -rpy_rates_coef = [-17.32, -17.32, -25.63] # from rot_params["rpy_rates_coef"] -cmd_rpy_coef = [196.18, 196.18, 390.27] # from rot_params["cmd_rpy_coef"] +cmd_f_coef = 1.032 # from trans_params["cmd_f_coef"] +thrust_time_coef = 0.149 # from trans_params["thrust_time_coef"] +drag_matrix = [[-0.0150, 0.0, 0.0], + [0.0, -0.0150, 0.0], + [0.0, 0.0, -0.0139]] # diag([drag_xy, drag_xy, drag_z]) +rpy_coef = [-485.86, -485.86, -333.24] # from rot_params["rpy_coef"] +rpy_rates_coef = [-33.18, -33.18, -39.54] # from rot_params["rpy_rates_coef"] +cmd_rpy_coef = [448.02, 448.02, 306.32] # from rot_params["cmd_rpy_coef"] ``` !!! note diff --git a/tests/unit/dynamics/test_identification.py b/tests/unit/dynamics/test_identification.py new file mode 100644 index 00000000..273b10aa --- /dev/null +++ b/tests/unit/dynamics/test_identification.py @@ -0,0 +1,65 @@ +"""Tests for the system identification residual functions.""" + +from __future__ import annotations + +import jax +import jax.numpy as jnp +import numpy as np +import pytest + +from crazyflow.dynamics.utils.identification import _build_residuals_fun_translation + + +@pytest.fixture(autouse=True) +def _enable_x64(): + prev = jax.config.jax_enable_x64 + jax.config.update("jax_enable_x64", True) + try: + yield + finally: + jax.config.update("jax_enable_x64", prev) + + +@pytest.mark.unit +@pytest.mark.parametrize( + "dynamics, active", + [ + ("so_rpy", [True, False, False, False]), + ("so_rpy_rotor", [True, True, False, False]), + ("so_rpy_rotor_drag", [True, True, True, True]), + ], +) +def test_translation_jacobian(dynamics: str, active: list[bool]): + n, mass = 50, 0.03 + t = np.linspace(0.0, 1.0, n) + phi = 2 * np.pi * t + quat = np.tile([0.0, 0.0, 0.0, 1.0], (n, 1)) + vel = np.stack((np.cos(phi), np.sin(phi), 0.5 * np.cos(2 * phi)), axis=-1) + acc = np.stack((-np.sin(phi), np.cos(phi), -np.sin(2 * phi)), axis=-1) + cmd_f = (1.0 + 0.3 * np.cos(phi)) * mass * 9.81 + constants = {"mass": mass, "gravity_vec": np.array([0, 0, -9.81])} + args = ( + jnp.array(quat), + jnp.array(vel), + jnp.array(cmd_f), + jnp.array(t), + constants, + jnp.array(acc), + ) + + residuals, jacobian = _build_residuals_fun_translation(dynamics) + params = np.array([1.05, 0.3, -0.02, -0.03]) + jac = jacobian(params, *args) + assert jac.shape == (n, 4) + + eps = 1e-6 + fd = np.zeros_like(jac) + for i in range(4): + step = np.zeros(4) + step[i] = eps + fd[:, i] = (residuals(params + step, *args) - residuals(params - step, *args)) / (2 * eps) + + active = np.array(active) + assert np.all(jac[:, ~active] == 0.0) + assert np.all(fd[:, ~active] == 0.0) + np.testing.assert_allclose(jac[:, active], fd[:, active], rtol=1e-6, atol=1e-8)