Contact examples

Querying contacts between drones and the scene, and choosing the collision geometry of the drone. See MuJoCo Integration for how the scene is built.

---

Contact queries

Query contacts between drones and the scene with box collision geometry.

The default collision geometry is a sphere around the drone frame. use_box_collision replaces it with a tighter oriented box, useful for narrow-gap flight and accurate contact debugging.

examples/contacts/contacts.py

"""Query contacts between drones and the scene with box collision geometry.

The default collision geometry is a sphere around the drone frame. `use_box_collision` replaces it
with a tighter oriented box, useful for narrow-gap flight and accurate contact debugging.
"""

import numpy as np

from crazyflow.sim import Dynamics, Sim
from crazyflow.sim.sim import use_box_collision


def main():
    """Spawn multiple drones in multiple worlds and check for contacts."""
    n_worlds, n_drones = 2, 3
    sim = Sim(n_worlds=n_worlds, n_drones=n_drones, dynamics=Dynamics.so_rpy, device="cpu")
    use_box_collision(sim, enable=True)  # Enable box collision for all drones
    fps = 60

    cmd = np.zeros((sim.n_worlds, sim.n_drones, 4))
    cmd[..., 3] = sim.data.params.mass[0] * 9.81 * 1.04
    for i in range(int(2 * sim.control_freq)):
        sim.attitude_control(cmd)
        sim.step(sim.freq // sim.control_freq)
        if ((i * fps) % sim.control_freq) < fps:
            sim.render()
            print(f"Contacts: {sim.contacts().any()}")
    sim.close()


if __name__ == "__main__":
    main()

---

Crash detection

Detect a crash with the floor through contact queries.

The drone hovers for three seconds and is then commanded below the floor.

examples/contacts/crash.py

"""Detect a crash with the floor through contact queries.

The drone hovers for three seconds and is then commanded below the floor.
"""

import os

os.environ["SCIPY_ARRAY_API"] = "1"

import numpy as np
from scipy.spatial.transform import Rotation as R

from crazyflow.control import Control
from crazyflow.sim import Sim
from crazyflow.sim.sim import use_box_collision


def main():
    """Example showing a drone crash."""
    sim = Sim(control=Control.state, freq=500, attitude_freq=500, state_freq=100)
    sim.reset()
    fps = 60
    use_box_collision(sim, enable=False)

    print("Phase 1: Hovering at [0, 0, 0.5] for 3 seconds")
    hover_duration = 3.0
    hover_cmd = np.zeros((sim.n_worlds, sim.n_drones, 16))
    hover_cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
    hover_cmd[..., :3] = [0.0, 0.0, 0.5]  # x, y, z position
    sim.state_control(hover_cmd)

    for i in range(int(hover_duration * sim.control_freq)):
        sim.step(sim.freq // sim.control_freq)
        if ((i * fps) % sim.control_freq) < fps:
            sim.render()
            print(f"Crash detected: {sim.contacts().any()}")

    print("Phase 2: Dropping to [-5, 0, -0.5] for 3 seconds")
    drop_duration = 3.0
    drop_cmd = np.zeros((sim.n_worlds, sim.n_drones, 16))
    drop_cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat()
    drop_cmd[..., :3] = [-5.0, 0.0, -0.5]  # x, y, z position
    sim.state_control(drop_cmd)

    for i in range(int(drop_duration * sim.control_freq)):
        sim.step(sim.freq // sim.control_freq)
        if ((i * fps) % sim.control_freq) < fps:
            sim.render()
            print(f"Crash detected: {sim.contacts().any()}")
    sim.close()


if __name__ == "__main__":
    main()

---

Collision pairs

Find out which drone collides with which body.

examples/contacts/collision_pairs.py

"""Find out which drone collides with which body."""

import jax.numpy as jnp
import mujoco
import numpy as np

from crazyflow.sim import Sim

OBSTACLE_XML = """
<mujoco>
  <worldbody>
    <body name="obstacle">
      <geom type="box" size="0.1 0.1 0.1" rgba="0.8 0.2 0.2 1"/>
    </body>
  </worldbody>
</mujoco>
"""


def main():
    """Place drones into obstacles, the floor and each other, and print each collision."""
    sim = Sim(n_drones=5)
    obstacle_spec = mujoco.MjSpec.from_string(OBSTACLE_XML)
    frame = sim.spec.worldbody.add_frame()
    for i, pos in enumerate([[1.0, 0.0, 0.5], [2.0, 0.0, 0.5]]):
        frame.attach_body(obstacle_spec.body("obstacle"), "", f":{i}").pos = pos
    sim.build_mjx()
    sim.reset()

    pos = jnp.array(
        [[0.85, 0.0, 0.5], [2.15, 0.0, 0.5], [1.5, 0.5, 0.5], [1.5, 0.5, 0.5], [1.5, -0.5, 0.0]]
    )
    sim.data = sim.data.replace(
        states=sim.data.states.replace(pos=pos[None]), core=sim.data.core.replace(mjx_synced=False)
    )

    body1, body2, touching = sim.contact_bodies()
    for i in np.flatnonzero(touching[0]):
        name1 = sim.mj_model.body(int(body1[0, i])).name
        name2 = sim.mj_model.body(int(body2[0, i])).name
        print(f"{name1} collides with {name2}")

    for _ in range(120):
        sim.render()
    sim.close()


if __name__ == "__main__":
    main()