Repository navigation
Add hull collision type #139
+164
−55
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,4 +1,17 @@ | ||
| from enum import StrEnum | ||
|
|
||
| from crazyflow.dynamics import Dynamics | ||
|
|
||
|
|
||
| class CollisionType(StrEnum): | ||
| """Collision geometry of the drones, selected with ``select_collision_type``.""" | ||
|
|
||
| sphere = "sphere" | ||
| box = "box" | ||
| hull = "hull" | ||
| default = sphere | ||
|
|
||
|
|
||
| from crazyflow.sim.sim import Sim # noqa: E402 (sim.py imports CollisionType from this package) | ||
|
|
||
| __all__ = ["Sim", "Dynamics", "CollisionType"] | ||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -216,7 +216,7 @@ python examples/rendering/led_deck.py | |
|
|
||
| ## Contact queries | ||
|
|
||
| The default collision geometry is a sphere around the drone frame. `select_collision_type` switches to a tighter oriented box or to the convex hull of the drone mesh, useful for narrow-gap flight and accurate contact debugging. Hull contacts are cheap on GPU but slow down significantly on CPU for swarms with many drones. | ||
|
|
||
| <div class="example-media-grid example-media-grid--contacts"> | ||
| <figure> | ||
|
|
||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -168,4 +168,4 @@ in_contact = check_contacts(sim.data, sim.mjx_data.mocap_pos) | |
|
|
||
| - [Pipelines](pipelines.md) — inserting custom stages into the step and reset pipelines | ||
| - [Examples](../examples/index.md#cameras-and-rgbd) — FPV camera and RGBD rendering | ||
| - [Examples](../examples/index.md#contact-queries) — contact detection with sphere, box or hull collision geometry | ||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change | ||||||||||||||||||||||||||
|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|
| @@ -1,24 +1,82 @@ | ||||||||||||||||||||||||||||
| import mujoco | ||||||||||||||||||||||||||||
| import numpy as np | ||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||
| from crazyflow.control import Control | ||||||||||||||||||||||||||||
| from crazyflow.sim import CollisionType, Dynamics, Sim | ||||||||||||||||||||||||||||
| from crazyflow.sim.sim import select_collision_type | ||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||
| RED, GREEN = (1.0, 0.0, 0.0), (0.0, 1.0, 0.0) | ||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||
| def add_contact_indicators(sim: Sim) -> list[int]: | ||||||||||||||||||||||||||||
| """Add a disc above each drone that turns red on contact and return the geom ids.""" | ||||||||||||||||||||||||||||
| for i in range(sim.n_drones): | ||||||||||||||||||||||||||||
| geom = sim.spec.body(f"drone:{i}").add_geom() | ||||||||||||||||||||||||||||
| geom.name = f"indicator:{i}" | ||||||||||||||||||||||||||||
| geom.type = mujoco.mjtGeom.mjGEOM_CYLINDER | ||||||||||||||||||||||||||||
| geom.size[:] = [0.05, 0.003, 0.0] | ||||||||||||||||||||||||||||
| geom.pos[:] = [0.0, 0.0, 0.1] | ||||||||||||||||||||||||||||
| geom.contype = 0 | ||||||||||||||||||||||||||||
| geom.conaffinity = 0 | ||||||||||||||||||||||||||||
| geom.rgba[:] = [*GREEN, 1.0] | ||||||||||||||||||||||||||||
| sim.build_mjx() | ||||||||||||||||||||||||||||
| return [sim.mj_model.geom(f"indicator:{i}").id for i in range(sim.n_drones)] | ||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||
| def crossing_trajectory(t: float, amplitude: float, period: float) -> tuple[np.ndarray, np.ndarray]: | ||||||||||||||||||||||||||||
| """Drones 0 and 2 oscillate along x in opposite directions through the hovering drone 1. | ||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||
| Returns: | ||||||||||||||||||||||||||||
| Position and velocity setpoints at time ``t`` with shape (3, 3) each. | ||||||||||||||||||||||||||||
| """ | ||||||||||||||||||||||||||||
| direction = np.array([1.0, 0.0, -1.0]) | ||||||||||||||||||||||||||||
| phase = 2 * np.pi * t / period | ||||||||||||||||||||||||||||
| pos, vel = np.zeros((3, 3)), np.zeros((3, 3)) | ||||||||||||||||||||||||||||
| pos[:, 0] = amplitude * np.sin(phase) * direction | ||||||||||||||||||||||||||||
| pos[:, 2] = 1.0 | ||||||||||||||||||||||||||||
| vel[:, 0] = amplitude * 2 * np.pi / period * np.cos(phase) * direction | ||||||||||||||||||||||||||||
| return pos, vel | ||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||
| def main(): | ||||||||||||||||||||||||||||
| """Fly the outer drones back and forth through a hovering drone and check for contacts. | ||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||
| The crossing is repeated with every collision type. Each type reports contacts for a different | ||||||||||||||||||||||||||||
| fraction of the crossing, from the loose sphere to the tight convex hull. A disc above each | ||||||||||||||||||||||||||||
| drone turns red while the drone is in contact. Press 3 in the viewer to show the collision | ||||||||||||||||||||||||||||
| geometry. | ||||||||||||||||||||||||||||
| """ | ||||||||||||||||||||||||||||
|
Comment on lines
+42
to
+48
Collaborator
There was a problem hiding this comment.
Suggested change
|
||||||||||||||||||||||||||||
| n_worlds, n_drones = 2, 3 | ||||||||||||||||||||||||||||
| sim = Sim( | ||||||||||||||||||||||||||||
| n_worlds=n_worlds, | ||||||||||||||||||||||||||||
| n_drones=n_drones, | ||||||||||||||||||||||||||||
| control=Control.state, | ||||||||||||||||||||||||||||
| dynamics=Dynamics.so_rpy, | ||||||||||||||||||||||||||||
| device="cpu", | ||||||||||||||||||||||||||||
|
Comment on lines
+54
to
+55
Collaborator
There was a problem hiding this comment.
Suggested change
We initially added different options per example to show how these things can be changed on the side. Now that we have a dedicated example for almost all of them, we should remove them to focus on the stuff the example is actually about. We can probably clean these up as we edit examples (like this one here) |
||||||||||||||||||||||||||||
| ) | ||||||||||||||||||||||||||||
| indicators = add_contact_indicators(sim) | ||||||||||||||||||||||||||||
| fps = 60 | ||||||||||||||||||||||||||||
| amplitude, period = 0.3, 2.0 | ||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||
| pos, _ = crossing_trajectory(0.0, amplitude, period) # Start on the trajectory | ||||||||||||||||||||||||||||
| states = sim.data.states | ||||||||||||||||||||||||||||
| sim.data = sim.data.replace(states=states.replace(pos=states.pos.at[:].set(pos))) | ||||||||||||||||||||||||||||
| cmd = np.zeros((sim.n_worlds, sim.n_drones, 16)) | ||||||||||||||||||||||||||||
| cmd[..., 12] = 1.0 # Identity quaternion | ||||||||||||||||||||||||||||
| for collision_type in CollisionType: | ||||||||||||||||||||||||||||
| print(f"Collision type: {collision_type}") | ||||||||||||||||||||||||||||
| select_collision_type(sim, collision_type) # Switches mid-flight, the sim keeps its state | ||||||||||||||||||||||||||||
| for i in range(int(2 * period * sim.control_freq)): | ||||||||||||||||||||||||||||
| t = i / sim.control_freq | ||||||||||||||||||||||||||||
| cmd[..., :3], cmd[..., 3:6] = crossing_trajectory(t, amplitude, period) | ||||||||||||||||||||||||||||
| sim.state_control(cmd) | ||||||||||||||||||||||||||||
| sim.step(sim.freq // sim.control_freq) | ||||||||||||||||||||||||||||
| if ((i * fps) % sim.control_freq) < fps: | ||||||||||||||||||||||||||||
| for drone, gid in enumerate(indicators): | ||||||||||||||||||||||||||||
| hit = sim.contacts(f"drone:{drone}")[0].any() | ||||||||||||||||||||||||||||
| sim.mj_model.geom_rgba[gid, :3] = RED if hit else GREEN | ||||||||||||||||||||||||||||
| sim.render() | ||||||||||||||||||||||||||||
| print(f"Contacts: {sim.contacts().any()}") | ||||||||||||||||||||||||||||
| sim.close() | ||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||
|
|
||||||||||||||||||||||||||||
Oops, something went wrong.
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Should this rather be in
crazyflow/sim/collision.pythen?