diff options
| -rw-r--r-- | .envrc | 3 | ||||
| -rw-r--r-- | .gitignore | 9 | ||||
| -rw-r--r-- | devenv.lock | 100 | ||||
| -rw-r--r-- | devenv.nix | 79 | ||||
| -rw-r--r-- | devenv.yaml | 15 | ||||
| -rw-r--r-- | main.py | 1 | ||||
| -rw-r--r-- | requirements.txt | 105 | ||||
| -rw-r--r-- | src/interactive_drone.py | 205 |
8 files changed, 517 insertions, 0 deletions
@@ -0,0 +1,3 @@ +source_url "https://raw.githubusercontent.com/cachix/devenv/82c0147677e510b247d8b9165c54f73d32dfd899/direnvrc" "sha256-7u4iDd1nZpxL4tCzmPG0dQgC5V+/44Ba+tHkPob1v2k=" + +use devenv diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..4d058db --- /dev/null +++ b/.gitignore @@ -0,0 +1,9 @@ +# Devenv +.devenv* +devenv.local.nix + +# direnv +.direnv + +# pre-commit +.pre-commit-config.yaml diff --git a/devenv.lock b/devenv.lock new file mode 100644 index 0000000..3a86f3f --- /dev/null +++ b/devenv.lock @@ -0,0 +1,100 @@ +{ + "nodes": { + "devenv": { + "locked": { + "dir": "src/modules", + "lastModified": 1736426010, + "owner": "cachix", + "repo": "devenv", + "rev": "1c384bc4be3ee571511fbbc6fdc94fe47d60f6cf", + "type": "github" + }, + "original": { + "dir": "src/modules", + "owner": "cachix", + "repo": "devenv", + "type": "github" + } + }, + "flake-compat": { + "flake": false, + "locked": { + "lastModified": 1733328505, + "owner": "edolstra", + "repo": "flake-compat", + "rev": "ff81ac966bb2cae68946d5ed5fc4994f96d0ffec", + "type": "github" + }, + "original": { + "owner": "edolstra", + "repo": "flake-compat", + "type": "github" + } + }, + "gitignore": { + "inputs": { + "nixpkgs": [ + "pre-commit-hooks", + "nixpkgs" + ] + }, + "locked": { + "lastModified": 1709087332, + "owner": "hercules-ci", + "repo": "gitignore.nix", + "rev": "637db329424fd7e46cf4185293b9cc8c88c95394", + "type": "github" + }, + "original": { + "owner": "hercules-ci", + "repo": "gitignore.nix", + "type": "github" + } + }, + "nixpkgs": { + "locked": { + "lastModified": 1733477122, + "owner": "cachix", + "repo": "devenv-nixpkgs", + "rev": "7bd9e84d0452f6d2e63b6e6da29fe73fac951857", + "type": "github" + }, + "original": { + "owner": "cachix", + "ref": "rolling", + "repo": "devenv-nixpkgs", + "type": "github" + } + }, + "pre-commit-hooks": { + "inputs": { + "flake-compat": "flake-compat", + "gitignore": "gitignore", + "nixpkgs": [ + "nixpkgs" + ] + }, + "locked": { + "lastModified": 1735882644, + "owner": "cachix", + "repo": "pre-commit-hooks.nix", + "rev": "a5a961387e75ae44cc20f0a57ae463da5e959656", + "type": "github" + }, + "original": { + "owner": "cachix", + "repo": "pre-commit-hooks.nix", + "type": "github" + } + }, + "root": { + "inputs": { + "devenv": "devenv", + "nixpkgs": "nixpkgs", + "pre-commit-hooks": "pre-commit-hooks" + } + } + }, + "root": "root", + "version": 7 +} diff --git a/devenv.nix b/devenv.nix new file mode 100644 index 0000000..8c8819e --- /dev/null +++ b/devenv.nix @@ -0,0 +1,79 @@ +{ pkgs, lib, config, inputs, ... }: + +rec { + # https://devenv.sh/basics/ + env = { + GREET = "devenv"; + + # Fixes failure to launch viewer due to missing OpenGL context + PYOPENGL_PLATFORM = "glx"; + + # Fixes missing linux input headers (supplied by pkgs.linuxHeaders) + C_INCLUDE_PATH = "${pkgs.linuxHeaders}/include"; + }; + # https://devenv.sh/packages/ + packages = with pkgs; [ + git + + # Graphics dependencies + xorg.libX11 + libGL + xorg.libXrender + xorg.libXrandr + libglibutil + glib + glfw-wayland + glew + openblas + zlib + + # Linux kernel headers, needed for input + linuxHeaders + ]; + + # https://devenv.sh/languages/ + # languages.rust.enable = true; + + # https://devenv.sh/processes/ + # processes.cargo-watch.exec = "cargo-watch"; + + # https://devenv.sh/services/ + # services.postgres.enable = true; + + # https://devenv.sh/scripts/ + scripts = { + hello.exec = '' + echo hello from $GREET + ''; + drone-sim.exec = '' + echo "Running demo drone simulation..." + python ./src/interactive_drone.py + ''; + }; + + enterShell = '' + hello + git --version + ''; + + languages.python = { + enable = true; + venv.enable = true; + }; + # https://devenv.sh/tasks/ + # tasks = { + # "myproj:setup".exec = "mytool build"; + # "devenv:enterShell".after = [ "myproj:setup" ]; + # }; + + # https://devenv.sh/tests/ + enterTest = '' + echo "Running tests" + git --version | grep --color=auto "${pkgs.git.version}" + ''; + + # https://devenv.sh/pre-commit-hooks/ + # pre-commit.hooks.shellcheck.enable = true; + + # See full reference at https://devenv.sh/reference/options/ +} diff --git a/devenv.yaml b/devenv.yaml new file mode 100644 index 0000000..116a2ad --- /dev/null +++ b/devenv.yaml @@ -0,0 +1,15 @@ +# yaml-language-server: $schema=https://devenv.sh/devenv.schema.json +inputs: + nixpkgs: + url: github:cachix/devenv-nixpkgs/rolling + +# If you're using non-OSS software, you can set allowUnfree to true. +# allowUnfree: true + +# If you're willing to use a package that's vulnerable +# permittedInsecurePackages: +# - "openssl-1.1.1w" + +# If you have more than one devenv you can merge them +#imports: +# - ./backend @@ -0,0 +1 @@ + diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..20f0a34 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,105 @@ +absl-py==2.1.0 +annotated-types==0.7.0 +black==24.10.0 +certifi==2024.12.14 +charset-normalizer==3.4.1 +click==8.1.8 +coacd==1.0.5 +colorama==0.4.6 +contourpy==1.3.1 +cycler==0.12.1 +dataclasses-json==0.6.7 +decorator==5.1.1 +Deprecated==1.2.15 +dill==0.3.9 +etils==1.11.0 +evdev==1.7.1 +filelock==3.16.1 +fonttools==4.55.3 +freetype-py==2.5.1 +fsspec==2024.12.0 +genesis-world==0.2.1 +glfw==2.8.0 +idna==3.10 +imageio==2.36.1 +imageio-ffmpeg==0.5.1 +importlib_resources==6.5.2 +Jinja2==3.1.5 +kiwisolver==1.4.8 +lazy_loader==0.4 +libigl==2.5.1 +llvmlite==0.43.0 +lxml==5.3.0 +markdown-it-py==3.0.0 +MarkupSafe==3.0.2 +marshmallow==3.25.1 +matplotlib==3.10.0 +mdurl==0.1.2 +moviepy==2.1.2 +mpmath==1.3.0 +mujoco==3.2.5 +mypy-extensions==1.0.0 +networkx==3.4.2 +numba==0.60.0 +numpy==1.26.4 +nvidia-cublas-cu12==12.4.5.8 +nvidia-cuda-cupti-cu12==12.4.127 +nvidia-cuda-nvrtc-cu12==12.4.127 +nvidia-cuda-runtime-cu12==12.4.127 +nvidia-cudnn-cu12==9.1.0.70 +nvidia-cufft-cu12==11.2.1.3 +nvidia-curand-cu12==10.3.5.147 +nvidia-cusolver-cu12==11.6.1.9 +nvidia-cusparse-cu12==12.3.1.170 +nvidia-nccl-cu12==2.21.5 +nvidia-nvjitlink-cu12==12.4.127 +nvidia-nvtx-cu12==12.4.127 +opencv-python==4.10.0.84 +OpenEXR==3.3.2 +packaging==24.2 +pathspec==0.12.1 +pillow==10.4.0 +platformdirs==4.3.6 +plotly==5.24.1 +pooch==1.8.2 +proglog==0.1.10 +psutil==6.1.1 +pycollada==0.8 +pydantic==2.7.1 +pydantic_core==2.18.2 +PyGEL3D==0.5.2 +pyglet==2.1.0 +pygltflib==1.16.0 +Pygments==2.19.1 +pymeshlab==2023.12.post2 +pynput==1.7.7 +PyOpenGL==3.1.7 +pyparsing==3.2.1 +python-dateutil==2.9.0.post0 +python-dotenv==1.0.1 +python-xlib==0.33 +pyvista==0.44.2 +requests==2.32.3 +rich==13.9.4 +scikit-image==0.25.0 +scipy==1.15.1 +scooby==0.10.0 +screeninfo==0.8.1 +setuptools==75.8.0 +six==1.17.0 +sympy==1.13.1 +taichi==1.7.3 +tenacity==9.0.0 +tetgen==0.6.4 +tifffile==2025.1.10 +torch==2.5.1 +torchaudio==2.5.1 +torchvision==0.20.1 +tqdm==4.67.1 +triton==3.1.0 +typing-inspect==0.9.0 +typing_extensions==4.12.2 +urllib3==2.3.0 +vtk==9.3.1 +wrapt==1.17.1 +zipp==3.21.0 diff --git a/src/interactive_drone.py b/src/interactive_drone.py new file mode 100644 index 0000000..ce7ef6f --- /dev/null +++ b/src/interactive_drone.py @@ -0,0 +1,205 @@ +import argparse +import numpy as np +import genesis as gs +import time +import threading +from pynput import keyboard + + +class DroneController: + def __init__(self): + self.thrust = 14468.429183500699 # Base hover RPM - constant hover + self.rotation_delta = 200 # Differential RPM for rotation + self.thrust_delta = 10 # Amount to change thrust by when accelerating/decelerating + self.running = True + self.rpms = [self.thrust] * 4 + self.pressed_keys = set() + + def on_press(self, key): + try: + if key == keyboard.Key.esc: + self.running = False + return False + self.pressed_keys.add(key) + print(f"Key pressed: {key}") + except AttributeError: + pass + + def on_release(self, key): + try: + self.pressed_keys.discard(key) + except KeyError: + pass + + def update_thrust(self): + # Store previous RPMs for debugging + prev_rpms = self.rpms.copy() + + # Reset RPMs to hover thrust + self.rpms = [self.thrust] * 4 + + # Acceleration (Spacebar) - All rotors spin faster + if keyboard.Key.space in self.pressed_keys: + self.thrust += self.thrust_delta + self.rpms = [self.thrust] * 4 + print("Accelerating") + + # Deceleration (Left Shift) - All rotors spin slower + if keyboard.Key.shift in self.pressed_keys: + self.thrust -= self.thrust_delta + self.rpms = [self.thrust] * 4 + print("Decelerating") + + # Forward (North) - Front rotors spin faster + if keyboard.Key.up in self.pressed_keys: + self.rpms[0] += self.rotation_delta # Front left + self.rpms[1] += self.rotation_delta # Front right + self.rpms[2] -= self.rotation_delta # Back left + self.rpms[3] -= self.rotation_delta # Back right + print("Moving Forward") + + # Backward (South) - Back rotors spin faster + if keyboard.Key.down in self.pressed_keys: + self.rpms[0] -= self.rotation_delta # Front left + self.rpms[1] -= self.rotation_delta # Front right + self.rpms[2] += self.rotation_delta # Back left + self.rpms[3] += self.rotation_delta # Back right + print("Moving Backward") + + # Left (West) - Left rotors spin faster + if keyboard.Key.left in self.pressed_keys: + self.rpms[0] -= self.rotation_delta # Front left + self.rpms[2] -= self.rotation_delta # Back left + self.rpms[1] += self.rotation_delta # Front right + self.rpms[3] += self.rotation_delta # Back right + print("Moving Left") + + # Right (East) - Right rotors spin faster + if keyboard.Key.right in self.pressed_keys: + self.rpms[0] += self.rotation_delta # Front left + self.rpms[2] += self.rotation_delta # Back left + self.rpms[1] -= self.rotation_delta # Front right + self.rpms[3] -= self.rotation_delta # Back right + print("Moving Right") + + self.rpms = np.clip(self.rpms, 0, 25000) + + # Debug print if any RPMs changed + if not np.array_equal(prev_rpms, self.rpms): + print(f"RPMs changed from {prev_rpms} to {self.rpms}") + + return self.rpms + + +def update_camera(scene, drone): + """Updates the camera position to follow the drone""" + if not scene.viewer: + return + + drone_pos = drone.get_pos() + + # Camera position relative to drone + offset_x = 0.0 # centered horizontally + offset_y = -4.0 # 4 units behind (in Y axis) + offset_z = 2.0 # 2 units above + + camera_pos = (float(drone_pos[0] + offset_x), float(drone_pos[1] + offset_y), float(drone_pos[2] + offset_z)) + + # Update camera position and look target + scene.viewer.set_camera_pose(pos=camera_pos, lookat=tuple(float(x) for x in drone_pos)) + + +def run_sim(scene, drone, controller): + while controller.running: + try: + # Update drone with current RPMs + rpms = controller.update_thrust() + drone.set_propellels_rpm(rpms) + + # Update physics + scene.step() + + # Update camera position to follow drone + update_camera(scene, drone) + + time.sleep(1 / 60) # Limit simulation rate + except Exception as e: + print(f"Error in simulation loop: {e}") + + if scene.viewer: + scene.viewer.stop() + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("-v", "--vis", action="store_true", default=True, help="Enable visualization (default: True)") + parser.add_argument("-m", "--mac", action="store_true", default=False, help="Running on MacOS (default: False)") + args = parser.parse_args() + + # Initialize Genesis + gs.init(backend=gs.cpu) + + # Create scene with initial camera view + viewer_options = gs.options.ViewerOptions( + camera_pos=(0.0, -4.0, 2.0), # Now behind the drone (negative Y) + camera_lookat=(0.0, 0.0, 0.5), + camera_fov=45, + max_FPS=60, + ) + + scene = gs.Scene( + sim_options=gs.options.SimOptions( + dt=0.01, + gravity=(0, 0, -9.81), + ), + viewer_options=viewer_options, + show_viewer=True, + #show_viewer=False, + ) + + # Add entities + plane = scene.add_entity(gs.morphs.Plane()) + drone = scene.add_entity( + morph=gs.morphs.Drone( + file="urdf/drones/cf2x.urdf", + pos=(0.0, 0, 0.5), # Start a bit higher + ), + ) + + # Build scene + scene.build() + + # Initialize controller + controller = DroneController() + + # Print control instructions + print("\nDrone Controls:") + print("↑ - Move Forward (North)") + print("↓ - Move Backward (South)") + print("← - Move Left (West)") + print("→ - Move Right (East)") + print("ESC - Quit\n") + print("Initial hover RPM:", controller.thrust) + + # Start keyboard listener + listener = keyboard.Listener(on_press=controller.on_press, on_release=controller.on_release) + listener.start() + + if args.mac: + # Run simulation in another thread + sim_thread = threading.Thread(target=run_sim, args=(scene, drone, controller)) + sim_thread.start() + + if args.vis: + scene.viewer.start() + + # Wait for threads to finish + sim_thread.join() + else: + # Run simulation in main thread + run_sim(scene, drone, controller) + listener.stop() + + +if __name__ == "__main__": + main() |