From 46a69a43a4a7c9c05de1ed41cb9dec98528ca81e Mon Sep 17 00:00:00 2001 From: ratheron Date: Wed, 18 Nov 2026 20:02:18 +0100 Subject: [PATCH 1/2] Add spinning props. Fixes #73 --- crazyflow/sim/sim.py | 15 +++++++++++++++ tests/unit/test_render.py | 28 +++++++++++++++++++++++++++- 2 files changed, 42 insertions(+), 1 deletion(-) diff --git a/crazyflow/sim/sim.py b/crazyflow/sim/sim.py index e0c61af9..d5f0a60e 100644 --- a/crazyflow/sim/sim.py +++ b/crazyflow/sim/sim.py @@ -15,6 +15,7 @@ from gymnasium.envs.mujoco.mujoco_rendering import MujocoRenderer from jax import Array, Device from jax.sharding import NamedSharding, PartitionSpec +from scipy.spatial.transform import Rotation as R import crazyflow.sim.functional as F from crazyflow.control import Control @@ -297,6 +298,9 @@ def render( self.mj_data.qpos[:] = self.mjx_data.qpos[world, :] self.mj_data.mocap_pos[:] = self.mjx_data.mocap_pos[world, :] self.mj_data.mocap_quat[:] = self.mjx_data.mocap_quat[world, :] + # Only first principles dynamics track rotor RPM. The fused mesh has no separate propellers + if self.dynamics == Dynamics.first_principles and not self.fused_mjx_model: + self._spin_props(world) # mj_forward raises on contacts between two static bodies (e.g. drones welded to the world). # We only need poses, cameras and lights for rendering mujoco.mj_kinematics(self.mj_model, self.mj_data) @@ -304,6 +308,17 @@ def render( mujoco.mj_camlight(self.mj_model, self.mj_data) return self.viewer.render(mode) + def _spin_props(self, world: int): + """Turn the propeller bodies to their rotor angle at the current sim time.""" + # Rotors spin opposite to the yaw torque they exert on the body + rpm = -self.data.states.rotor_vel * self.data.params.mixing_matrix[..., 2, :] + angle = np.asarray(rpm[world]) * float(self.time[world, 0]) * 2 * np.pi / 60 + names = [f"{i}:prop{j}" for i in range(self.n_drones) for j in range(4)] + ids = [self.mj_model.body(name).id for name in names] + rot_rest = R.from_quat(np.asarray(self.mjx_model.body_quat)[ids], scalar_first=True) + rot_prop = rot_rest * R.from_euler("z", angle.reshape(-1, 1)) + self.mj_model.body_quat[ids] = rot_prop.as_quat(scalar_first=True) + def seed(self, seed: int): """Set the JAX rng key for the simulation. diff --git a/tests/unit/test_render.py b/tests/unit/test_render.py index 96e401b2..66ec681e 100644 --- a/tests/unit/test_render.py +++ b/tests/unit/test_render.py @@ -2,8 +2,9 @@ import numpy as np import pytest from conftest import skip_if_headless +from scipy.spatial.transform import Rotation as R -from crazyflow import Sim +from crazyflow.sim import Dynamics, Sim @pytest.mark.unit @@ -64,3 +65,28 @@ def test_render_free_camera(): viewer_cam = sim.viewer.viewer.cam assert viewer_cam.type == mujoco.mjtCamera.mjCAMERA_FREE, "Camera type was not set to FREE" sim.close() + + +@pytest.mark.unit +@pytest.mark.parametrize("dynamics", Dynamics) +@pytest.mark.render +@skip_if_headless +def test_render_spins_props(dynamics: Dynamics): + sim = Sim(dynamics=dynamics) + ids = [sim.mj_model.body(f"0:prop{i}").id for i in range(4)] + sim.render(mode="rgb_array") + quat_before = sim.mj_data.xquat[ids].copy() + # 7.5 RPM for one second turns each prop by 45 degrees + states = sim.data.states.replace(rotor_vel=sim.data.states.rotor_vel + 7.5) + core = sim.data.core.replace(steps=sim.data.core.steps + sim.freq) + sim.data = sim.data.replace(states=states, core=core) + sim.render(mode="rgb_array") + quat_after = sim.mj_data.xquat[ids].copy() + sim.close() + rot_before = R.from_quat(quat_before, scalar_first=True) + rot = rot_before.inv() * R.from_quat(quat_after, scalar_first=True) + expected = np.zeros((4, 3)) + if dynamics == Dynamics.first_principles: # Only first principles dynamics track rotor RPM + # Rotors spin opposite to the yaw torque they exert on the body + expected[:, 2] = -np.asarray(sim.data.params.mixing_matrix[2]) * np.pi / 4 + assert np.allclose(rot.as_rotvec(), expected, atol=1e-6), f"Prop rotations {rot.as_rotvec()}" From 3fb95308bfeaca80fc28b578005b7f3477a2468b Mon Sep 17 00:00:00 2001 From: ratheron Date: Wed, 18 Nov 2026 20:14:37 +0100 Subject: [PATCH 2/2] Rotate by correct angles --- crazyflow/sim/sim.py | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/crazyflow/sim/sim.py b/crazyflow/sim/sim.py index d5f0a60e..9fac2dae 100644 --- a/crazyflow/sim/sim.py +++ b/crazyflow/sim/sim.py @@ -309,15 +309,18 @@ def render( return self.viewer.render(mode) def _spin_props(self, world: int): - """Turn the propeller bodies to their rotor angle at the current sim time.""" + """Turn the propeller bodies by the angle their rotors travelled since the last render.""" + time = float(self.time[world, 0]) + dt = max(time - self.mj_data.time, 0.0) # Time jumps back on reset + self.mj_data.time = time # Rotors spin opposite to the yaw torque they exert on the body rpm = -self.data.states.rotor_vel * self.data.params.mixing_matrix[..., 2, :] - angle = np.asarray(rpm[world]) * float(self.time[world, 0]) * 2 * np.pi / 60 + angle = np.asarray(rpm[world]) * 2 * np.pi / 60 * dt names = [f"{i}:prop{j}" for i in range(self.n_drones) for j in range(4)] ids = [self.mj_model.body(name).id for name in names] - rot_rest = R.from_quat(np.asarray(self.mjx_model.body_quat)[ids], scalar_first=True) - rot_prop = rot_rest * R.from_euler("z", angle.reshape(-1, 1)) - self.mj_model.body_quat[ids] = rot_prop.as_quat(scalar_first=True) + rot_prop = R.from_quat(self.mj_model.body_quat[ids], scalar_first=True) + rot_delta = R.from_euler("z", angle.reshape(-1, 1)) + self.mj_model.body_quat[ids] = (rot_prop * rot_delta).as_quat(scalar_first=True) def seed(self, seed: int): """Set the JAX rng key for the simulation.