From 30ecd606868198630ee6f1bc075852139361cbc3 Mon Sep 17 00:00:00 2001 From: radu_dell Date: Tue, 22 Sep 2026 20:32:01 +0200 Subject: [PATCH 1/5] Adds analytical implementation of ground effect from NeuralLander paper as external force --- examples/plugins/ground_effect.py | 99 +++++++++++++++++++++++++++++++ 1 file changed, 99 insertions(+) create mode 100644 examples/plugins/ground_effect.py diff --git a/examples/plugins/ground_effect.py b/examples/plugins/ground_effect.py new file mode 100644 index 00000000..35002881 --- /dev/null +++ b/examples/plugins/ground_effect.py @@ -0,0 +1,99 @@ +"""External-force implementation of the ground effect in Eq. (15) of Shi et al arXiv:1811.08027v.""" +from __future__ import annotations +import os + +os.environ["SCIPY_ARRAY_API"] = "1" + + + +from typing import TYPE_CHECKING + +import jax.numpy as jnp +import numpy as np +from scipy.spatial.transform import Rotation as R + +from crazyflow.sim.pipeline import insert_fn_before + +if TYPE_CHECKING: + from crazyflow.sim import Sim + from crazyflow.sim.data import SimData + + +# Parameters for cf21B_500. Tune MU for the actual airframe/propeller layout. +PROPELLER_DIAMETER = 55e-3 # m +MU = 2.0 +MIN_HEIGHT = 0.02 # m; Eq. (15) is not valid arbitrarily close to the floor +MAX_GAIN = 2.0 # avoid the model's singularity near the floor + + +def ground_effect_fn(data: SimData) -> SimData: + rpm = data.states.rotor_vel + c, b, a = ( + data.params.rpm2thrust[..., 0], + data.params.rpm2thrust[..., 1], + data.params.rpm2thrust[..., 2], + ) + nominal_thrust = jnp.sum(c + b * rpm + a * rpm**2, axis=-1) + + height = jnp.maximum(data.states.pos[..., 2], MIN_HEIGHT) + gain = 1.0 / (1.0 - MU * (PROPELLER_DIAMETER / (8.0 * height)) ** 2) + gain = jnp.minimum(gain, MAX_GAIN) + extra_thrust = nominal_thrust * (gain - 1.0) + + # The force follows the body z thrust axis; ``states.force`` needs world coordinates. + body_force = jnp.zeros_like(data.states.pos).at[..., 2].set(extra_thrust) + ground_force = R.from_quat(data.states.quat).apply(body_force) + return data.replace(states=data.states.replace(force=ground_force)) + + +def install_ground_effect(sim: Sim) -> None: + """Install the force stage immediately before first-principles integration.""" + insert_fn_before(sim.step_pipeline, "integration", ground_effect_fn) + sim.build_step_fn() + + +def main(plot: bool = True) -> None: + from crazyflow.sim import Sim + + sim = Sim(n_drones=1, drone="cf21B_500", control="state") + install_ground_effect(sim) + + upper_pos = np.array([0.0, 0.0, 0.5]) + + sim.data = sim.data.replace( + states=sim.data.states.replace(pos=jnp.array([[upper_pos]])) + ) + sim.build_default_data() + + duration = 5.0 + speed = 1 / duration + + command = np.zeros((1, 1, 16)) + command[..., 9:13] = [0.0, 0.0, 0.0, 1.0] + command[0, 0, :3] = upper_pos + heights, vertical_forces = [], [] + + for step in range(int(duration * sim.control_freq)): + t = step / sim.control_freq + command[0, 0, :3] = [0.0, 0.0, 0.5 - speed * t/2] + command[0, 0, 3:6] = [0, 0.0, -speed] + sim.state_control(command) + sim.step(sim.freq // sim.control_freq) + heights.append(float(sim.data.states.pos[0, 0, 2])) + vertical_forces.append(float(sim.data.states.force[0, 0, 2])) + sim.render() + + sim.close() + if plot: + import matplotlib.pyplot as plt + + plt.plot(heights, vertical_forces) + plt.xlabel("Drone height (m)") + plt.ylabel("Ground-effect force (N)") + plt.gca().invert_xaxis() + plt.show() + + + +if __name__ == "__main__": + main() From f006d89f38fc5f3059a24e35e3dd1d1d60084f0d Mon Sep 17 00:00:00 2001 From: radu_workstation Date: Wed, 23 Sep 2026 14:56:10 +0200 Subject: [PATCH 2/5] Adds descent procedure and scatter plot --- examples/plugins/ground_effect.py | 91 ++++++++++++++++++------------- 1 file changed, 52 insertions(+), 39 deletions(-) diff --git a/examples/plugins/ground_effect.py b/examples/plugins/ground_effect.py index 35002881..4ee8a0e7 100644 --- a/examples/plugins/ground_effect.py +++ b/examples/plugins/ground_effect.py @@ -1,13 +1,12 @@ """External-force implementation of the ground effect in Eq. (15) of Shi et al arXiv:1811.08027v.""" + from __future__ import annotations + import os +from typing import TYPE_CHECKING os.environ["SCIPY_ARRAY_API"] = "1" - - -from typing import TYPE_CHECKING - import jax.numpy as jnp import numpy as np from scipy.spatial.transform import Rotation as R @@ -25,6 +24,11 @@ MIN_HEIGHT = 0.02 # m; Eq. (15) is not valid arbitrarily close to the floor MAX_GAIN = 2.0 # avoid the model's singularity near the floor +# Descend points +HOVER_HEIGHTS = np.linspace(0.50, 0.02, 10) +SETTLE_DURATION = 4.0 # s +SAMPLE_DURATION = 0.2 # s + def ground_effect_fn(data: SimData) -> SimData: rpm = data.states.rotor_vel @@ -46,54 +50,63 @@ def ground_effect_fn(data: SimData) -> SimData: return data.replace(states=data.states.replace(force=ground_force)) -def install_ground_effect(sim: Sim) -> None: - """Install the force stage immediately before first-principles integration.""" - insert_fn_before(sim.step_pipeline, "integration", ground_effect_fn) - sim.build_step_fn() +def measure_hover_points( + sim: Sim, heights: np.ndarray, render: bool = False +) -> tuple[np.ndarray, np.ndarray]: + """Hold at each setpoint and return the mean measured height and thrust command.""" + command = np.zeros((sim.n_worlds, sim.n_drones, 16)) + command[..., 9:13] = [0.0, 0.0, 0.0, 1.0] + settle_steps = int(SETTLE_DURATION * sim.control_freq) + total_steps = settle_steps + int(SAMPLE_DURATION * sim.control_freq) + hover_heights, hover_thrusts = [], [] -def main(plot: bool = True) -> None: + for height in heights: + command[..., 2] = height + height_samples = [] + thrust_samples = [] + for step in range(total_steps): + sim.state_control(command) + sim.step(sim.freq // sim.control_freq) + + if step >= settle_steps: + height_samples.append(float(sim.data.states.pos[0, 0, 2])) + # This is the collective force command passed to the motor mixer. + thrust_samples.append(float(sim.data.controls.force_torque.cmd[0, 0, 0])) + if render: + sim.render() + + hover_heights.append(np.mean(height_samples)) + hover_thrusts.append(np.mean(thrust_samples)) + + return np.asarray(hover_heights), np.asarray(hover_thrusts) + + +def main(plot: bool = True, render: bool = False) -> None: from crazyflow.sim import Sim sim = Sim(n_drones=1, drone="cf21B_500", control="state") - install_ground_effect(sim) - upper_pos = np.array([0.0, 0.0, 0.5]) + insert_fn_before(sim.step_pipeline, "integration", ground_effect_fn) + sim.build_step_fn() sim.data = sim.data.replace( - states=sim.data.states.replace(pos=jnp.array([[upper_pos]])) + states=sim.data.states.replace(pos=jnp.array([[[0.0, 0.0, HOVER_HEIGHTS[0]]]])) ) - sim.build_default_data() - - duration = 5.0 - speed = 1 / duration - - command = np.zeros((1, 1, 16)) - command[..., 9:13] = [0.0, 0.0, 0.0, 1.0] - command[0, 0, :3] = upper_pos - heights, vertical_forces = [], [] - - for step in range(int(duration * sim.control_freq)): - t = step / sim.control_freq - command[0, 0, :3] = [0.0, 0.0, 0.5 - speed * t/2] - command[0, 0, 3:6] = [0, 0.0, -speed] - sim.state_control(command) - sim.step(sim.freq // sim.control_freq) - heights.append(float(sim.data.states.pos[0, 0, 2])) - vertical_forces.append(float(sim.data.states.force[0, 0, 2])) - sim.render() - - sim.close() + try: + hover_heights, hover_thrusts = measure_hover_points(sim, HOVER_HEIGHTS, render=render) + finally: + sim.close() + if plot: import matplotlib.pyplot as plt - plt.plot(heights, vertical_forces) - plt.xlabel("Drone height (m)") - plt.ylabel("Ground-effect force (N)") - plt.gca().invert_xaxis() + plt.scatter(hover_heights, hover_thrusts) + plt.xlabel("Measured hover height (m)") + plt.ylabel("Commanded collective thrust (N)") + plt.grid() plt.show() - if __name__ == "__main__": - main() + main(render=False) From 5dea42eba4252948ea0fba65c5a8ad2689f8abdd Mon Sep 17 00:00:00 2001 From: radu_workstation Date: Wed, 23 Sep 2026 15:08:14 +0200 Subject: [PATCH 3/5] Extends settle duration --- examples/plugins/ground_effect.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/plugins/ground_effect.py b/examples/plugins/ground_effect.py index 4ee8a0e7..beded70e 100644 --- a/examples/plugins/ground_effect.py +++ b/examples/plugins/ground_effect.py @@ -25,8 +25,8 @@ MAX_GAIN = 2.0 # avoid the model's singularity near the floor # Descend points -HOVER_HEIGHTS = np.linspace(0.50, 0.02, 10) -SETTLE_DURATION = 4.0 # s +HOVER_HEIGHTS = np.linspace(0.50, 0.02, 15) +SETTLE_DURATION = 10.0 # s SAMPLE_DURATION = 0.2 # s From efe1d3248197ea6ebf7eecb5ce740be67d7e7ee7 Mon Sep 17 00:00:00 2001 From: radu_workstation Date: Wed, 23 Sep 2026 15:24:11 +0200 Subject: [PATCH 4/5] Clean up --- examples/plugins/ground_effect.py | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/examples/plugins/ground_effect.py b/examples/plugins/ground_effect.py index beded70e..8121ca90 100644 --- a/examples/plugins/ground_effect.py +++ b/examples/plugins/ground_effect.py @@ -9,24 +9,24 @@ import jax.numpy as jnp import numpy as np -from scipy.spatial.transform import Rotation as R +from jax.scipy.spatial.transform import Rotation as R +from crazyflow.sim import Sim from crazyflow.sim.pipeline import insert_fn_before if TYPE_CHECKING: - from crazyflow.sim import Sim from crazyflow.sim.data import SimData -# Parameters for cf21B_500. Tune MU for the actual airframe/propeller layout. +# Parameters for cf21B_500 PROPELLER_DIAMETER = 55e-3 # m MU = 2.0 MIN_HEIGHT = 0.02 # m; Eq. (15) is not valid arbitrarily close to the floor MAX_GAIN = 2.0 # avoid the model's singularity near the floor -# Descend points +# Descent points HOVER_HEIGHTS = np.linspace(0.50, 0.02, 15) -SETTLE_DURATION = 10.0 # s +SETTLE_DURATION = 10.0 # s SAMPLE_DURATION = 0.2 # s @@ -83,7 +83,6 @@ def measure_hover_points( def main(plot: bool = True, render: bool = False) -> None: - from crazyflow.sim import Sim sim = Sim(n_drones=1, drone="cf21B_500", control="state") @@ -93,10 +92,10 @@ def main(plot: bool = True, render: bool = False) -> None: sim.data = sim.data.replace( states=sim.data.states.replace(pos=jnp.array([[[0.0, 0.0, HOVER_HEIGHTS[0]]]])) ) - try: - hover_heights, hover_thrusts = measure_hover_points(sim, HOVER_HEIGHTS, render=render) - finally: - sim.close() + + hover_heights, hover_thrusts = measure_hover_points(sim, HOVER_HEIGHTS, render=render) + + sim.close() if plot: import matplotlib.pyplot as plt From b9d5bf829ab046068f9dcdb3fbf0c4ada3b80c14 Mon Sep 17 00:00:00 2001 From: radu_workstation Date: Thu, 24 Sep 2026 16:32:38 +0200 Subject: [PATCH 5/5] Applied suggestions --- examples/plugins/ground_effect.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/examples/plugins/ground_effect.py b/examples/plugins/ground_effect.py index 8121ca90..e0c830d9 100644 --- a/examples/plugins/ground_effect.py +++ b/examples/plugins/ground_effect.py @@ -55,10 +55,12 @@ def measure_hover_points( ) -> tuple[np.ndarray, np.ndarray]: """Hold at each setpoint and return the mean measured height and thrust command.""" command = np.zeros((sim.n_worlds, sim.n_drones, 16)) - command[..., 9:13] = [0.0, 0.0, 0.0, 1.0] + command[..., 9:13] = R.from_euler("z", 0.0).as_quat() settle_steps = int(SETTLE_DURATION * sim.control_freq) total_steps = settle_steps + int(SAMPLE_DURATION * sim.control_freq) + fps = 60 + hover_heights, hover_thrusts = [], [] for height in heights: @@ -74,7 +76,8 @@ def measure_hover_points( # This is the collective force command passed to the motor mixer. thrust_samples.append(float(sim.data.controls.force_torque.cmd[0, 0, 0])) if render: - sim.render() + if ((step * fps) % sim.control_freq) < fps: + sim.render() hover_heights.append(np.mean(height_samples)) hover_thrusts.append(np.mean(thrust_samples)) @@ -108,4 +111,4 @@ def main(plot: bool = True, render: bool = False) -> None: if __name__ == "__main__": - main(render=False) + main(render=True)