init: first working demo of genesisHEADmaster

8 files changed, 517 insertions, 0 deletions

diff --git a/.envrc b/.envrc
new file mode 100644
index 0000000..894571b
--- /dev/null
+++ b/.envrc
@@ -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
diff --git a/main.py b/main.py
new file mode 100644
index 0000000..8b13789
--- /dev/null
+++ b/main.py
@@ -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()