Merge pull request #4 from HSL-UCSC/quad_interface

Quad interface implementation

.gitconfig

@@ -0,0 +1,3 @@
+# .gitconfig in your project directory
+[url "https://github.com/"]
+    insteadOf = [email protected]:

.github/workflows/CI.yml

@@ -33,6 +33,41 @@ jobs:
           restore-keys: |
             ${{ runner.os }}-cargo-build-
 
+      - name: Install rerun requirements on Ubuntu
+        if: matrix.os == 'ubuntu-latest'
+        run: |
+          sudo apt-get -y install \
+            libclang-dev \
+            libatk-bridge2.0 \
+            libfontconfig1-dev \
+            libfreetype6-dev \
+            libglib2.0-dev \
+            libgtk-3-dev \
+            libssl-dev \
+            libxcb-render0-dev \
+            libxcb-shape0-dev \
+            libxcb-xfixes0-dev \
+            libxkbcommon-dev \
+            patchelf \
+            libudev-dev \
+            cmake \
+            protobuf-compiler
+
+      - name: Install Protobuf and CMake on macOS
+        if: matrix.os == 'macos-latest'
+        run: |
+          brew install cmake protobuf
+          protoc --version
+          cmake --version
+
+      - name: Install Protobuf on Windows
+        if: matrix.os == 'windows-latest'
+        run: |
+          choco install cmake --installargs '"ADD_CMAKE_TO_PATH=System"'
+          choco install protoc
+          protoc --version
+          cmake --version
+
       - name: Build
         run: cargo build --verbose
       - name: Run tests

Cargo.toml

@@ -11,10 +11,10 @@ documentation = "https://docs.rs/peng_quad/latest/peng_quad"
 homepage = "https://github.com/makeecat/Peng"
 repository = "https://github.com/makeecat/Peng"
 categories = [
-    "science::robotics",
-    "aerospace::simulation",
-    "aerospace::unmanned-aerial-vehicles",
-    "algorithms",
+  "science::robotics",
+  "aerospace::simulation",
+  "aerospace::unmanned-aerial-vehicles",
+  "algorithms",
 ]
 keywords = ["quadrotor", "quadcopter", "robotics", "drone", "simulation"]
 readme = "README.md"
@@ -30,11 +30,17 @@ lto = "thin"      # Do a second optimization pass over the entire program, inclu
 nalgebra = "0.33.2"
 rand = { version = "0.8.5", features = ["rand_chacha"] }
 rand_distr = "0.4.3"
-rerun = "0.19.1"
+rerun = "0.20.0"
 thiserror = "2.0.1"
 serde = { version = "1.0.214", features = ["derive"] }
 serde_yaml = "0.9.34"
 env_logger = "0.11.5"
 log = "0.4.22"
 rayon = "1.10.0"
 rand_chacha = "0.3.1"
+tokio = { version = "1.41.0", features = ["full"] }
+binrw = "0.14.1"
+cyber_rc = { git = "https://github.com/friend0/CyberRC.git" }
+serialport = "4.6.0"
+tempfile = "3.14.0"
+prost = "0.13.3"

config/quad.yaml

@@ -1,8 +1,6 @@
 use_rerun: true # Enable visualization using rerun.io
 render_depth: true # Enable rendering depth
 use_multithreading_depth_rendering: true # Enable multithreading for depth rendering for large resolution (above 32x24)
-use_rk4_for_dynamics_update: false # Enable Runge-Kutta 4th order integration for dynamics, otherwise Euler integration is used
-use_rk4_for_dynamics_control: false # Enable Runge-Kutta 4th order integration for dynamics, otherwise Euler integration is used
 real_time: true # Enable real time mode. If not enabled, sim will run in fast time.
 
 rerun_blueprint: "config/peng_default_blueprint.rbl"
@@ -12,8 +10,11 @@ simulation:
   simulation_frequency: 1000 # Frequency of physics simulation updates (Hz)
   log_frequency: 20 # Frequency of data logging (Hz)
   duration: 70.0 # Total duration of the simulation (seconds)
+  use_rk4_for_dynamics_update: false # Enable Runge-Kutta 4th order integration for dynamics, otherwise Euler integration is used
+  use_rk4_for_dynamics_control: false # Enable Runge-Kutta 4th order integration for dynamics, otherwise Euler integration is used
 
 quadrotor:
+  type: "Peng"
   mass: 1.3 # Mass of the quadrotor (kg)
   gravity: 9.81 # Gravitational acceleration (m/s^2)
   drag_coefficient: 0.000 # Aerodynamic drag coefficient

flake.nix

@@ -3,7 +3,7 @@
 
   # Specify the inputs, such as nixpkgs
   inputs = {
-    nixpkgs.url = "nixpkgs/nixos-unstable";
+    nixpkgs.url = "github:NixOS/nixpkgs";
     flake-utils.url = "github:numtide/flake-utils";
   };
 
@@ -15,6 +15,8 @@
     devShell = pkgs.mkShell {
       # Add Rust and Cargo to the environment
       buildInputs = [
+        pkgs.protobuf
+        pkgs.cmake
         pkgs.rustup
         pkgs.zsh
       ];
@@ -25,6 +27,9 @@
 
       # Optional shellHook to fetch dependencies when entering the shell
       shellHook = ''
+        export GIT_CONFIG=$PWD/.gitconfig
+        export CARGO_NET_GIT_FETCH_WITH_CLI=true
+        export GIT_SSH_COMMAND="ssh -F ~/.ssh/config"  # Ensure it uses your SSH config
         # Start Zsh if not already the active shell
         if [ "$SHELL" != "$(command -v zsh)" ]; then
           export SHELL="$(command -v zsh)"

src/config.rs

@@ -3,13 +3,18 @@
 //! This module contains the configuration for the simulation, quadrotor, PID controller, IMU, maze, camera, mesh, and planner schedule.
 //! The configuration is loaded from a YAML file using the serde library.
 //! The configuration is then used to initialize the simulation, quadrotor, PID controller, IMU, maze, camera, mesh, and planner schedule.
+
+use nalgebra::Matrix3;
+
+use crate::SimulationError;
+
 #[derive(serde::Deserialize)]
 /// Configuration for the simulation
 pub struct Config {
     /// Simulation configuration
     pub simulation: SimulationConfig,
     /// Quadrotor configuration
-    pub quadrotor: QuadrotorConfig,
+    pub quadrotor: QuadrotorConfigurations,
     /// PID Controller configuration
     pub pid_controller: PIDControllerConfig,
     /// IMU configuration
@@ -30,12 +35,10 @@ pub struct Config {
     pub render_depth: bool,
     /// MultiThreading depth rendering
     pub use_multithreading_depth_rendering: bool,
-    /// Use RK4 for updating quadrotor dynamics_with_controls
-    pub use_rk4_for_dynamics_control: bool,
-    /// Use RK4 for updating quadrotor dynamics without controls
-    pub use_rk4_for_dynamics_update: bool,
-    // Run the simulation in real time mode
+    /// Run the simulation in real time mode
     pub real_time: bool,
+    /// Angle limits
+    pub angle_limits: Option<Vec<f32>>,
 }
 
 #[derive(serde::Deserialize)]
@@ -49,9 +52,11 @@ pub struct PlannerStep {
     pub params: serde_yaml::Value,
 }
 
-#[derive(serde::Deserialize)]
+#[derive(Clone, serde::Deserialize)]
 /// Configuration for the simulation
 pub struct SimulationConfig {
+    /// Gravity in m/s^2
+    pub gravity: f32,
     /// Control frequency in Hz
     pub control_frequency: usize,
     /// Simulation frequency in Hz
@@ -60,19 +65,77 @@ pub struct SimulationConfig {
     pub log_frequency: usize,
     /// Duration of the simulation in seconds
     pub duration: f32,
+    /// Use RK4 for updating quadrotor dynamics_with_controls
+    pub use_rk4_for_dynamics_control: bool,
+    /// Use RK4 for updating quadrotor dynamics without controls
+    pub use_rk4_for_dynamics_update: bool,
 }
 
-#[derive(serde::Deserialize)]
+impl Default for SimulationConfig {
+    fn default() -> Self {
+        SimulationConfig {
+            gravity: 9.81,
+            control_frequency: 200,
+            simulation_frequency: 1000,
+            log_frequency: 70,
+            duration: 70.0,
+            use_rk4_for_dynamics_control: false,
+            use_rk4_for_dynamics_update: false,
+        }
+    }
+}
+
+#[derive(Clone, Debug, serde::Deserialize)]
+#[serde(tag = "type")]
+/// Vehicle Specifig configuration
+pub enum QuadrotorConfigurations {
+    Peng(QuadrotorConfig),
+    Liftoff(QuadrotorConfig),
+}
+
+#[derive(Copy, Clone, Debug, serde::Deserialize)]
+#[serde(default)]
 /// Configuration for the quadrotor
 pub struct QuadrotorConfig {
     /// Mass of the quadrotor in kg
     pub mass: f32,
-    /// Gravity in m/s^2
-    pub gravity: f32,
     /// Drag coefficient in Ns^2/m^2
     pub drag_coefficient: f32,
     /// Inertia matrix in kg*m^2
     pub inertia_matrix: [f32; 9],
+    /// Maximum thrust in kilograms
+    pub max_thrust_kg: f32,
+    /// Arm length in meters
+    pub arm_length_m: f32,
+    /// Yaw torque constant
+    pub yaw_torque_constant: f32,
+}
+
+impl Default for QuadrotorConfig {
+    fn default() -> Self {
+        QuadrotorConfig {
+            mass: 1.3,
+            drag_coefficient: 0.000,
+            inertia_matrix: [3.04e-3, 0.0, 0.0, 0.0, 4.55e-3, 0.0, 0.0, 0.0, 2.82e-3],
+            max_thrust_kg: 1.3 * 2.5,
+            arm_length_m: 0.150,
+            yaw_torque_constant: 0.05,
+        }
+    }
+}
+
+impl QuadrotorConfig {
+    pub fn inertia_matrix(&self) -> Matrix3<f32> {
+        Matrix3::from_row_slice(&self.inertia_matrix)
+    }
+
+    pub fn inverse_inertia_matrix(&self) -> Result<Matrix3<f32>, SimulationError> {
+        self.inertia_matrix()
+            .try_inverse()
+            .ok_or(SimulationError::NalgebraError(
+                "Failed to invert inertia matrix".to_string(),
+            ))
+    }
 }
 
 #[derive(serde::Deserialize)]
@@ -164,3 +227,28 @@ impl Config {
         Ok(serde_yaml::from_str(&contents)?)
     }
 }
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    #[test]
+    fn test_base_config() {
+        let config = Config::from_yaml("tests/testdata/test_config_base.yaml").unwrap();
+        let mut quadrotor_config: QuadrotorConfig = match config.quadrotor {
+            QuadrotorConfigurations::Peng(quadrotor_config) => quadrotor_config,
+            _ => panic!("Failed to load Peng configuration"),
+        };
+
+        assert_eq!(config.simulation.control_frequency, 200);
+        assert_eq!(config.simulation.simulation_frequency, 1000);
+        assert_eq!(config.simulation.log_frequency, 20);
+        assert_eq!(config.simulation.duration, 70.0);
+        assert_eq!(quadrotor_config.mass, 1.3);
+        assert_eq!(quadrotor_config.drag_coefficient, 0.0);
+        assert_eq!(config.pid_controller.pos_gains.kp, [7.1, 7.1, 11.9]);
+        assert_eq!(config.pid_controller.att_gains.kd, [0.13, 0.13, 0.1]);
+        assert_eq!(config.pid_controller.pos_max_int, [10.0, 10.0, 10.0]);
+        assert_eq!(config.imu.accel_noise_std, 0.02);
+        assert_eq!(config.maze.upper_bounds, [4.0, 2.0, 2.0]);
+    }
+}