diff --git a/crazyflow/sim/sim.py b/crazyflow/sim/sim.py index 5180f23c..6e79f192 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,20 @@ 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 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]) * 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_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. 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()}"