Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 18 additions & 0 deletions crazyflow/sim/sim.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -297,13 +298,30 @@ 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)
mujoco.mj_comPos(self.mj_model, self.mj_data)
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.

Expand Down
28 changes: 27 additions & 1 deletion tests/unit/test_render.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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()}"