diff --git a/.gitconfig b/.gitconfig new file mode 100644 index 00000000..d5e0350a --- /dev/null +++ b/.gitconfig @@ -0,0 +1,3 @@ +# .gitconfig in your project directory +[url "https://github.com/"] + insteadOf = git@github.com: diff --git a/.github/workflows/CI.yml b/.github/workflows/CI.yml index 7efbe3b7..ae18eb37 100644 --- a/.github/workflows/CI.yml +++ b/.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 diff --git a/Cargo.toml b/Cargo.toml index cd9945ac..85070254 100644 --- a/Cargo.toml +++ b/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,7 +30,7 @@ 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" @@ -38,3 +38,9 @@ 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" diff --git a/config/quad.yaml b/config/quad.yaml index 538a45de..504aad9f 100644 --- a/config/quad.yaml +++ b/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 diff --git a/flake.lock b/flake.lock index 3093ae4a..0ee9be4c 100644 --- a/flake.lock +++ b/flake.lock @@ -20,17 +20,17 @@ }, "nixpkgs": { "locked": { - "lastModified": 1730785428, - "narHash": "sha256-Zwl8YgTVJTEum+L+0zVAWvXAGbWAuXHax3KzuejaDyo=", + "lastModified": 1731357700, + "narHash": "sha256-iXHxD3UYvy1lu2QG5yu0ofJZG0Gb2uF2rrkGAtdnTkY=", "owner": "NixOS", "repo": "nixpkgs", - "rev": "4aa36568d413aca0ea84a1684d2d46f55dbabad7", + "rev": "2203866060b897f600fd6407b41b4fe54fe18a3a", "type": "github" }, "original": { - "id": "nixpkgs", - "ref": "nixos-unstable", - "type": "indirect" + "owner": "NixOS", + "repo": "nixpkgs", + "type": "github" } }, "root": { diff --git a/flake.nix b/flake.nix index 92849a9d..745ce314 100644 --- a/flake.nix +++ b/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)" diff --git a/src/config.rs b/src/config.rs index 6f20f481..1c7fea84 100644 --- a/src/config.rs +++ b/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>, } #[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 { + Matrix3::from_row_slice(&self.inertia_matrix) + } + + pub fn inverse_inertia_matrix(&self) -> Result, 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]); + } +} diff --git a/src/lib.rs b/src/lib.rs index a4d09280..2353a245 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -36,16 +36,19 @@ //! //! ## Example //! ``` +//! use peng_quad::config; //! use nalgebra::Vector3; //! use peng_quad::{Quadrotor, SimulationError}; //! let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01); //! let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977]; -//! let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix); +//! let quadrotor = Quadrotor::new(time_step, config::SimulationConfig::default(), mass, gravity, drag_coefficient, inertia_matrix).unwrap(); //! ``` use rand::SeedableRng; use rayon::prelude::*; pub mod config; +pub mod quadrotor; use nalgebra::{Matrix3, Quaternion, Rotation3, SMatrix, UnitQuaternion, Vector3}; +use quadrotor::{QuadrotorInterface, QuadrotorState}; use rand_chacha::ChaCha8Rng; use rand_distr::{Distribution, Normal}; use std::f32::consts::PI; @@ -75,38 +78,17 @@ pub enum SimulationError { OtherError(String), } -pub trait Quadrotor { - fn control( - &mut self, - step_number: usize, - config: &config::Config, - thrust: f32, - torque: &Vector3, - ); - - fn observe( - &self, - ) -> Result< - ( - Vector3, - Vector3, - UnitQuaternion, - Vector3, - ), - SimulationError, - >; -} - /// Represents a quadrotor with its physical properties and state /// # Example /// ``` +/// use peng_quad::config; /// use nalgebra::Vector3; /// use peng_quad::Quadrotor; /// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01); /// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977]; -/// let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix); +/// let quadrotor = Quadrotor::new(time_step, config::SimulationConfig::default(), mass, gravity, drag_coefficient, inertia_matrix).unwrap(); /// ``` -pub struct SimulatedQuadrotor { +pub struct Quadrotor { /// Current position of the quadrotor in 3D space pub position: Vector3, /// Current velocity of the quadrotor @@ -133,59 +115,84 @@ pub struct SimulatedQuadrotor { pub previous_thrust: f32, /// Previous Torque pub previous_torque: Vector3, + /// Config + pub config: config::SimulationConfig, } -impl Quadrotor for SimulatedQuadrotor { +impl QuadrotorInterface for Quadrotor { fn control( &mut self, step_number: usize, - config: &config::Config, thrust: f32, torque: &Vector3, - ) { - if step_number - % (config.simulation.simulation_frequency / config.simulation.control_frequency) - == 0 - { - if config.use_rk4_for_dynamics_control { - self.update_dynamics_with_controls_rk4(thrust, &torque); + ) -> Result<(), SimulationError> { + if step_number % (self.config.simulation_frequency / self.config.control_frequency) == 0 { + if self.config.use_rk4_for_dynamics_control { + self.update_dynamics_with_controls_rk4(thrust, torque); } else { - self.update_dynamics_with_controls_euler(thrust, &torque); + self.update_dynamics_with_controls_euler(thrust, torque); } self.previous_thrust = thrust; self.previous_torque = *torque; } else { let previous_thrust = self.previous_thrust; let previous_torque = self.previous_torque; - if config.use_rk4_for_dynamics_update { + if self.config.use_rk4_for_dynamics_update { self.update_dynamics_with_controls_rk4(previous_thrust, &previous_torque); } else { self.update_dynamics_with_controls_euler(previous_thrust, &previous_torque); } } + Ok(()) } - fn observe( - &self, - ) -> Result< - ( - Vector3, - Vector3, - UnitQuaternion, - Vector3, - ), - SimulationError, - > { - return Ok(( - self.position, - self.velocity, - self.orientation, - self.angular_velocity, - )); + + fn observe(&mut self) -> Result { + Ok(QuadrotorState { + time: 0.0, + position: self.position, + velocity: self.velocity, + acceleration: self.acceleration, + orientation: self.orientation, + angular_velocity: self.angular_velocity, + }) + } + + fn max_thrust(&self) -> f32 { + 2.5 * self.gravity + } + + // TDO: need a method to retrieve quadrotor physical constants + fn max_torque(&self) -> Vector3 { + let motor_thrust = self.max_thrust() / 4.0; + let max_rp_torque = 2.0 * 0.65 * motor_thrust; + let yaw_torque = 2.0 * 0.005 * motor_thrust; + Vector3::new(max_rp_torque, max_rp_torque, yaw_torque) + } + + /// Simulates IMU readings + /// # Returns + /// * A tuple containing the true acceleration and angular velocity of the quadrotor + /// # Errors + /// * Returns a SimulationError if the IMU readings cannot be calculated + /// # Example + /// ``` + /// use peng_quad::config; + /// use nalgebra::Vector3; + /// use peng_quad::Quadrotor; + /// use peng_quad::quadrotor::QuadrotorInterface; + /// + /// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01); + /// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977]; + /// let quadrotor = Quadrotor::new(time_step, config::SimulationConfig::default(), mass, gravity, drag_coefficient, inertia_matrix).unwrap(); + /// let (true_acceleration, true_angular_velocity) = quadrotor.read_imu().unwrap(); + /// ``` + fn read_imu(&self) -> Result<(Vector3, Vector3), SimulationError> { + Ok((self.acceleration, self.angular_velocity)) } } /// Implementation of the Quadrotor struct -impl SimulatedQuadrotor { +impl Quadrotor { /// Creates a new Quadrotor with default parameters /// # Arguments /// * `time_step` - The simulation time step in seconds @@ -199,15 +206,18 @@ impl SimulatedQuadrotor { /// * Returns a SimulationError if the inertia matrix cannot be inverted /// # Example /// ``` + /// use peng_quad::config;; /// use nalgebra::Vector3; /// use peng_quad::Quadrotor; /// /// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01); /// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977]; - /// let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix); + /// let quadrotor = Quadrotor::new(time_step, config::SimulationConfig::default(), mass, gravity, drag_coefficient, inertia_matrix).unwrap(); /// ``` + /// TODO: time step to simulation config, others to quadrotor config pub fn new( time_step: f32, + config: config::SimulationConfig, mass: f32, gravity: f32, drag_coefficient: f32, @@ -221,6 +231,7 @@ impl SimulatedQuadrotor { "Failed to invert inertia matrix".to_string(), ))?; Ok(Self { + config, position: Vector3::zeros(), velocity: Vector3::zeros(), acceleration: Vector3::zeros(), @@ -243,12 +254,13 @@ impl SimulatedQuadrotor { /// * `control_torque` - The 3D torque vector applied to the quadrotor /// # Example /// ``` + /// use peng_quad::config;; /// use nalgebra::Vector3; /// use peng_quad::Quadrotor; /// /// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01); /// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977]; - /// let mut quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap(); + /// let mut quadrotor = Quadrotor::new(time_step, config::SimulationConfig::default(), mass, gravity, drag_coefficient, inertia_matrix).unwrap(); /// let control_thrust = mass * gravity; /// let control_torque = Vector3::new(0.0, 0.0, 0.0); /// quadrotor.update_dynamics_with_controls_euler(control_thrust, &control_torque); @@ -277,11 +289,12 @@ impl SimulatedQuadrotor { /// * `control_torque` - The 3D torque vector applied to the quadrotor /// # Example /// ``` + /// use peng_quad::config;; /// use nalgebra::Vector3; /// use peng_quad::Quadrotor; /// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01); /// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977]; - /// let mut quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap(); + /// let mut quadrotor = Quadrotor::new(time_step, config::SimulationConfig::default(), mass, gravity, drag_coefficient, inertia_matrix).unwrap(); /// let control_thrust = mass * gravity; /// let control_torque = Vector3::new(0.0, 0.0, 0.0); /// quadrotor.update_dynamics_with_controls_rk4(control_thrust, &control_torque); @@ -327,12 +340,13 @@ impl SimulatedQuadrotor { /// * `state` - The state of the quadrotor /// # Example /// ``` + /// use peng_quad::config;; /// use nalgebra::Vector3; /// use peng_quad::Quadrotor; /// use nalgebra::UnitQuaternion; /// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01); /// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977]; - /// let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap(); + /// let quadrotor = Quadrotor::new(time_step, config::SimulationConfig::default(), mass, gravity, drag_coefficient, inertia_matrix).unwrap(); /// let state = quadrotor.get_state(); /// ``` pub fn get_state(&self) -> [f32; 13] { @@ -348,12 +362,13 @@ impl SimulatedQuadrotor { /// * `state` - The state of the quadrotor /// # Example /// ``` + /// use peng_quad::config;; /// use nalgebra::Vector3; /// use peng_quad::Quadrotor; /// use nalgebra::UnitQuaternion; /// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01); /// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977]; - /// let mut quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap(); + /// let mut quadrotor = Quadrotor::new(time_step, config::SimulationConfig::default(), mass, gravity, drag_coefficient, inertia_matrix).unwrap(); /// let state = [ /// 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 /// ]; @@ -376,12 +391,13 @@ impl SimulatedQuadrotor { /// The derivative of the state /// # Example /// ``` + /// use peng_quad::config;; /// use nalgebra::Vector3; /// use peng_quad::Quadrotor; /// use nalgebra::UnitQuaternion; /// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01); /// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977]; - /// let mut quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap(); + /// let mut quadrotor = Quadrotor::new(time_step, config::SimulationConfig::default(), mass, gravity, drag_coefficient, inertia_matrix).unwrap(); /// let state = [ /// 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 /// ]; @@ -421,24 +437,6 @@ impl SimulatedQuadrotor { derivative[10..13].copy_from_slice(angular_acceleration.as_slice()); derivative } - /// Simulates IMU readings - /// # Returns - /// * A tuple containing the true acceleration and angular velocity of the quadrotor - /// # Errors - /// * Returns a SimulationError if the IMU readings cannot be calculated - /// # Example - /// ``` - /// use nalgebra::Vector3; - /// use peng_quad::Quadrotor; - /// - /// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01); - /// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977]; - /// let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap(); - /// let (true_acceleration, true_angular_velocity) = quadrotor.read_imu().unwrap(); - /// ``` - pub fn read_imu(&self) -> Result<(Vector3, Vector3), SimulationError> { - Ok((self.acceleration, self.angular_velocity)) - } } /// Represents an Inertial Measurement Unit (IMU) with bias and noise characteristics /// # Example @@ -750,7 +748,7 @@ impl PIDController { let thrust = self.mass * total_acc_norm; let desired_orientation = if total_acc_norm > 1e-6 { let z_body = total_acceleration / total_acc_norm; - let yaw_rotation = UnitQuaternion::from_euler_angles(0.0, 0.0, desired_yaw); + let yaw_rotation = UnitQuaternion::from_axis_angle(&Vector3::z_axis(), desired_yaw); let x_body_horizontal = yaw_rotation * Vector3::new(1.0, 0.0, 0.0); let y_body = z_body.cross(&x_body_horizontal).normalize(); let x_body = y_body.cross(&z_body); @@ -1775,6 +1773,7 @@ impl Planner for MinimumSnapWaypointPlanner { && (current_position - last_waypoint).norm() < 0.1) } } +#[derive(Debug)] /// Represents a step in the planner schedule. /// # Example /// ``` @@ -1806,7 +1805,8 @@ pub struct PlannerStepConfig { /// * If the planner could not be created /// # Example /// ``` -/// use peng_quad::{PlannerManager, Quadrotor, Obstacle, PlannerStepConfig, update_planner}; +/// use peng_quad::config; +/// use peng_quad::{PlannerManager, Quadrotor, Obstacle, PlannerStepConfig, update_planner, quadrotor::QuadrotorInterface}; /// use nalgebra::Vector3; /// let simulation_frequency = 1000; /// let initial_position = Vector3::new(0.0, 0.0, 0.0); @@ -1816,7 +1816,8 @@ pub struct PlannerStepConfig { /// let time = 0.0; /// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01); /// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977]; -/// let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap(); +/// let mut quadrotor = Quadrotor::new(time_step, config::SimulationConfig::default(), mass, gravity, drag_coefficient, inertia_matrix).unwrap(); +/// let quad_state = quadrotor.observe().unwrap(); /// let obstacles = vec![Obstacle::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(0.0, 0.0, 0.0), 1.0)]; /// let planner_config = vec![PlannerStepConfig { /// step: 0, @@ -1828,23 +1829,20 @@ pub struct PlannerStepConfig { /// duration: 2.0 /// "#).unwrap(), /// }]; -/// update_planner(&mut planner_manager, step, time, simulation_frequency, &quadrotor, &obstacles, &planner_config).unwrap(); +/// update_planner(&mut planner_manager, step, time, simulation_frequency, &quad_state, &obstacles, &planner_config).unwrap(); /// ``` pub fn update_planner( planner_manager: &mut PlannerManager, step: usize, time: f32, - simulation_frequency: usize, - quad: &SimulatedQuadrotor, + _simulation_frequency: usize, + quad_state: &QuadrotorState, obstacles: &[Obstacle], planner_config: &[PlannerStepConfig], ) -> Result<(), SimulationError> { - if let Some(planner_step) = planner_config - .iter() - .find(|s| s.step * simulation_frequency == step * 1000) - { + if let Some(planner_step) = planner_config.iter().find(|s| s.step == step) { log::info!("Time: {:.2} s,\tSwitch {}", time, planner_step.planner_type); - planner_manager.set_planner(create_planner(planner_step, quad, time, obstacles)?); + planner_manager.set_planner(create_planner(planner_step, quad_state, time, obstacles)?); } Ok(()) } @@ -1860,7 +1858,8 @@ pub fn update_planner( /// * If the planner type is not recognized /// # Example /// ``` -/// use peng_quad::{PlannerType, Quadrotor, Obstacle, PlannerStepConfig, create_planner}; +/// use peng_quad::config; +/// use peng_quad::{PlannerType, Quadrotor, Obstacle, PlannerStepConfig, create_planner, quadrotor::QuadrotorInterface}; /// use nalgebra::Vector3; /// let step = PlannerStepConfig { /// step: 0, @@ -1875,9 +1874,10 @@ pub fn update_planner( /// let time = 0.0; /// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01); /// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977]; -/// let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap(); +/// let mut quadrotor = Quadrotor::new(time_step, config::SimulationConfig::default(), mass, gravity, drag_coefficient, inertia_matrix).unwrap(); +/// let quadrotor_state = quadrotor.observe().unwrap(); /// let obstacles = vec![Obstacle::new(Vector3::new(0.0, 0.0, 0.0), Vector3::new(0.0, 0.0, 0.0), 1.0)]; -/// let planner = create_planner(&step, &quadrotor, time, &obstacles).unwrap(); +/// let planner = create_planner(&step, &quadrotor_state, time, &obstacles).unwrap(); /// match planner { /// PlannerType::MinimumJerkLine(_) => log::info!("Created MinimumJerkLine planner"), /// _ => log::info!("Created another planner"), @@ -1885,29 +1885,33 @@ pub fn update_planner( /// ``` pub fn create_planner( step: &PlannerStepConfig, - quad: &SimulatedQuadrotor, + quad_state: &QuadrotorState, time: f32, obstacles: &[Obstacle], ) -> Result { let params = &step.params; match step.planner_type.as_str() { "MinimumJerkLine" => Ok(PlannerType::MinimumJerkLine(MinimumJerkLinePlanner { - start_position: quad.position, + start_position: quad_state.position, end_position: parse_vector3(params, "end_position")?, - start_yaw: quad.orientation.euler_angles().2, + start_yaw: quad_state.orientation.euler_angles().2, end_yaw: parse_f32(params, "end_yaw")?, start_time: time, duration: parse_f32(params, "duration")?, })), + "Hover" => Ok(PlannerType::Hover(HoverPlanner { + target_position: quad_state.position, + target_yaw: parse_f32(params, "target_yaw")?, + })), "Lissajous" => Ok(PlannerType::Lissajous(LissajousPlanner { - start_position: quad.position, + start_position: quad_state.position, center: parse_vector3(params, "center")?, amplitude: parse_vector3(params, "amplitude")?, frequency: parse_vector3(params, "frequency")?, phase: parse_vector3(params, "phase")?, start_time: time, duration: parse_f32(params, "duration")?, - start_yaw: quad.orientation.euler_angles().2, + start_yaw: quad_state.orientation.euler_angles().2, end_yaw: parse_f32(params, "end_yaw")?, ramp_time: parse_f32(params, "ramp_time")?, })), @@ -1915,18 +1919,18 @@ pub fn create_planner( center: parse_vector3(params, "center")?, radius: parse_f32(params, "radius")?, angular_velocity: parse_f32(params, "angular_velocity")?, - start_position: quad.position, + start_position: quad_state.position, start_time: time, duration: parse_f32(params, "duration")?, - start_yaw: quad.orientation.euler_angles().2, - end_yaw: quad.orientation.euler_angles().2, + start_yaw: quad_state.orientation.euler_angles().2, + end_yaw: quad_state.orientation.euler_angles().2, ramp_time: parse_f32(params, "ramp_time")?, })), "ObstacleAvoidance" => Ok(PlannerType::ObstacleAvoidance(ObstacleAvoidancePlanner { target_position: parse_vector3(params, "target_position")?, start_time: time, duration: parse_f32(params, "duration")?, - start_yaw: quad.orientation.euler_angles().2, + start_yaw: quad_state.orientation.euler_angles().2, end_yaw: parse_f32(params, "end_yaw")?, obstacles: obstacles.to_owned(), k_att: parse_f32(params, "k_att")?, @@ -1937,7 +1941,7 @@ pub fn create_planner( max_speed: parse_f32(params, "max_speed")?, })), "MinimumSnapWaypoint" => { - let mut waypoints = vec![quad.position]; + let mut waypoints = vec![quad_state.position]; waypoints.extend( params["waypoints"] .as_sequence() @@ -1956,7 +1960,7 @@ pub fn create_planner( }) .collect::>, SimulationError>>()?, ); - let mut yaws = vec![quad.orientation.euler_angles().2]; + let mut yaws = vec![quad_state.orientation.euler_angles().2]; yaws.extend( params["yaws"] .as_sequence() @@ -1983,10 +1987,10 @@ pub fn create_planner( .map(PlannerType::MinimumSnapWaypoint) } "Landing" => Ok(PlannerType::Landing(LandingPlanner { - start_position: quad.position, + start_position: quad_state.position, start_time: time, duration: parse_f32(params, "duration")?, - start_yaw: quad.orientation.euler_angles().2, + start_yaw: quad_state.orientation.euler_angles().2, })), _ => Err(SimulationError::OtherError(format!( "Unknown planner type: {}", @@ -2444,25 +2448,28 @@ pub fn ray_cast( /// * If the data cannot be logged to the recording stream /// # Example /// ```no_run -/// use peng_quad::{Quadrotor, log_data}; +/// use peng_quad::config;; +/// use peng_quad::{Quadrotor, log_data, quadrotor::QuadrotorInterface}; /// use nalgebra::Vector3; /// let rec = rerun::RecordingStreamBuilder::new("peng").connect().unwrap(); /// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01); /// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977]; -/// let quad = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap(); +/// let mut quadrotor = Quadrotor::new(time_step, config::SimulationConfig::default(), mass, gravity, drag_coefficient, inertia_matrix).unwrap(); +/// let quad_state = quadrotor.observe().unwrap(); /// let desired_position = Vector3::new(0.0, 0.0, 0.0); /// let desired_velocity = Vector3::new(0.0, 0.0, 0.0); /// let measured_accel = Vector3::new(0.0, 0.0, 0.0); /// let measured_gyro = Vector3::new(0.0, 0.0, 0.0); -/// log_data(&rec, &quad, &desired_position, &desired_velocity, &measured_accel, &measured_gyro).unwrap(); +/// log_data(&rec, &quad_state, &desired_position, &desired_velocity, &measured_accel, &measured_gyro, &Vector3::new(0.0, 0.0, 0.0)).unwrap(); /// ``` pub fn log_data( rec: &rerun::RecordingStream, - quad: &SimulatedQuadrotor, + quad_state: &QuadrotorState, desired_position: &Vector3, desired_velocity: &Vector3, measured_accel: &Vector3, measured_gyro: &Vector3, + torque: &Vector3, ) -> Result<(), SimulationError> { rec.log( "world/quad/desired_position", @@ -2473,26 +2480,31 @@ pub fn log_data( rec.log( "world/quad/base_link", &rerun::Transform3D::from_translation_rotation( - rerun::Vec3D::new(quad.position.x, quad.position.y, quad.position.z), + rerun::Vec3D::new( + quad_state.position.x, + quad_state.position.y, + quad_state.position.z, + ), rerun::Quaternion::from_xyzw([ - quad.orientation.i, - quad.orientation.j, - quad.orientation.k, - quad.orientation.w, + quad_state.orientation.i, + quad_state.orientation.j, + quad_state.orientation.k, + quad_state.orientation.w, ]), ) .with_axis_length(0.7), )?; - let (quad_roll, quad_pitch, quad_yaw) = quad.orientation.euler_angles(); + let (quad_roll, quad_pitch, quad_yaw) = quad_state.orientation.euler_angles(); let quad_euler_angles: Vector3 = Vector3::new(quad_roll, quad_pitch, quad_yaw); for (pre, vec) in [ - ("position", &quad.position), - ("velocity", &quad.velocity), + ("position", &quad_state.position), + ("velocity", &quad_state.velocity), ("accel", measured_accel), ("orientation", &quad_euler_angles), ("gyro", measured_gyro), ("desired_position", desired_position), ("desired_velocity", desired_velocity), + ("torque", torque), ] { for (i, a) in ["x", "y", "z"].iter().enumerate() { rec.log(format!("{}/{}", pre, a), &rerun::Scalar::new(vec[i] as f64))?; @@ -2798,7 +2810,6 @@ pub fn log_depth_image( /// let camera = Camera::new((800, 600), 60.0, 0.1, 100.0); /// log_pinhole_depth(&rec, &camera, cam_position, cam_orientation, cam_transform).unwrap(); /// ``` - pub fn log_pinhole_depth( rec: &rerun::RecordingStream, cam: &Camera, @@ -2865,6 +2876,57 @@ pub fn color_map_fn(gray: f32) -> (u8, u8, u8) { (r, g, b) } +/// log joystick positions +/// # Arguments +/// * `rec` - The rerun::RecordingStream instance +/// * `trajectory` - The Trajectory instance +/// # Errors +/// * If the data cannot be logged to the recording stream +/// # Example +/// ```no_run +/// use peng_quad::{Trajectory, log_trajectory}; +/// use nalgebra::Vector3; +/// let rec = rerun::RecordingStreamBuilder::new("log.rerun").connect().unwrap(); +/// let mut trajectory = Trajectory::new(nalgebra::Vector3::new(0.0, 0.0, 0.0)); +/// trajectory.add_point(nalgebra::Vector3::new(1.0, 0.0, 0.0)); +/// log_trajectory(&rec, &trajectory).unwrap(); +/// ``` +pub fn log_joy( + rec: &rerun::RecordingStream, + _thrust: f32, + _torque: &Vector3, +) -> Result<(), SimulationError> { + let num_points = 100; + let radius = 1.0; + let circle_points: Vec<(f32, f32)> = (0..num_points) + .map(|i| { + let theta = i as f32 * 2.5 * PI / num_points as f32; + let x = radius * theta.cos(); + let y = radius * theta.sin(); + (x, y) + }) + .collect(); + rec.log_static( + "world/quad/joy/left", + &rerun::LineStrips2D::new([circle_points.clone()]) + .with_colors([rerun::Color::from_rgb(0, 255, 255)]), + )?; + // rec.log( + // "joy/right", + // &rerun::LineStrips2D::new([circle_points]) + // .with_colors([rerun::Color::from_rgb(0, 255, 255)]), + // )?; + rec.log( + "world/quad/joy/left", + &rerun::Points2D::new([(0.0, 0.5)]).with_colors([rerun::Color::from_rgb(0, 255, 255)]), + )?; + // rec.log( + // "joy/right", + // &rerun::Points2D::new([(0.0, -0.5)]).with_colors([rerun::Color::from_rgb(0, 255, 255)]), + // )?; + Ok(()) +} + /// Fast square root function /// # Arguments /// * `x` - The input value diff --git a/src/liftoff_quad.rs b/src/liftoff_quad.rs deleted file mode 100644 index 84cd3d77..00000000 --- a/src/liftoff_quad.rs +++ /dev/null @@ -1,60 +0,0 @@ -use nalgebra::{Matrix3, Quaternion, Rotation3, SMatrix, UnitQuaternion, Vector3}; -use peng_quad::{Quadrotor, SimulationError}; - -/// Represents an RC quadrotor -/// # Example -/// ``` -/// use nalgebra::Vector3; -/// use peng_quad::Quadrotor; -/// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01); -/// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977]; -/// let quadrotor = RCQuad::new(time_step, mass, inertia_matrix); -/// ``` -pub struct LiftoffQuad { - /// Current position of the quadrotor in 3D space - pub position: Vector3, - /// Current velocity of the quadrotor - pub velocity: Vector3, - /// Current orientation of the quadrotor - pub orientation: UnitQuaternion, - /// Current angular velocity of the quadrotor - pub angular_velocity: Vector3, - /// Simulation time step in seconds - pub time_step: f32, - /// Mass of the quadrotor in kg - pub mass: f32, - /// Inertia matrix of the quadrotor - pub inertia_matrix: Matrix3, - /// Inverse of the inertia matrix - pub inertia_matrix_inv: Matrix3, - /// Previous Thrust - pub previous_thrust: f32, - /// Previous Torque - pub previous_torque: Vector3, -} - -impl Quadrotor for LiftoffQuad { - fn control( - &mut self, - step_number: usize, - config: &peng_quad::config::Config, - thrust: f32, - torque: &Vector3, - ) { - todo!("implement control outputs to CyberRC - gamepad mode") - } - - fn observe( - &self, - ) -> Result< - ( - Vector3, - Vector3, - UnitQuaternion, - Vector3, - ), - SimulationError, - > { - todo!("implement state feedback from Liftoff UDP") - } -} diff --git a/src/main.rs b/src/main.rs index f21f6f6d..0f5f62b6 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,14 +1,14 @@ #![feature(thread_sleep_until)] use nalgebra::Vector3; +use peng_quad::quadrotor::QuadrotorInterface; use peng_quad::*; use std::thread; use std::time::Duration; use std::time::Instant; -mod liftoff_quad; - +#[tokio::main] /// Main function for the simulation -fn main() -> Result<(), SimulationError> { +async fn main() -> Result<(), SimulationError> { let mut config_str = "config/quad.yaml"; let args: Vec = std::env::args().collect(); if args.len() != 2 { @@ -29,22 +29,11 @@ fn main() -> Result<(), SimulationError> { "[\x1b[32mINFO\x1b[0m peng_quad]Use rerun.io: {}", config.use_rerun ); - let mut quad = Quadrotor::new( - 1.0 / config.simulation.simulation_frequency as f32, - config.quadrotor.mass, - config.quadrotor.gravity, - config.quadrotor.drag_coefficient, - config.quadrotor.inertia_matrix, - )?; - let _pos_gains = config.pid_controller.pos_gains; - let _att_gains = config.pid_controller.att_gains; - let mut controller = PIDController::new( - [_pos_gains.kp, _pos_gains.kd, _pos_gains.ki], - [_att_gains.kp, _att_gains.kd, _att_gains.ki], - config.pid_controller.pos_max_int, - config.pid_controller.att_max_int, - config.quadrotor.mass, - config.quadrotor.gravity, + let time_step = 1.0 / config.simulation.simulation_frequency as f32; + // TODO: quadrotor factory + println!( + "[\x1b[32mINFO\x1b[0m peng_quad] Using quadrotor: {:?}", + config.quadrotor ); let mut imu = Imu::new( config.imu.accel_noise_std, @@ -67,7 +56,7 @@ fn main() -> Result<(), SimulationError> { ); let mut planner_manager = PlannerManager::new(Vector3::zeros(), 0.0); let mut trajectory = Trajectory::new(Vector3::new(0.0, 0.0, 0.0)); - let mut depth_buffer: Vec = vec![0.0; camera.resolution.0 * camera.resolution.1]; + let mut _depth_buffer: Vec = vec![0.0; camera.resolution.0 * camera.resolution.1]; let planner_config: Vec = config .planner_schedule .iter() @@ -87,44 +76,78 @@ fn main() -> Result<(), SimulationError> { Some(_rec) } else { env_logger::builder() - .parse_env(env_logger::Env::default().default_filter_or("info")) + .parse_env(env_logger::Env::default().default_filter_or("debug")) .init(); None }; - log::info!("Use rerun.io: {}", config.use_rerun); + // log::info!("Use rerun.io: {}", config.use_rerun); if let Some(rec) = &rec { - rec.log_file_from_path(config.rerun_blueprint, None, false)?; + rec.log_file_from_path(config.rerun_blueprint.clone(), None, false)?; + rec.set_time_seconds("timestamp", 0); log_mesh(rec, config.mesh.division, config.mesh.spacing)?; log_maze_tube(rec, &maze)?; log_maze_obstacles(rec, &maze)?; } + let (mut quad, mass): (Box, f32) = match config.quadrotor { + config::QuadrotorConfigurations::Peng(quad_config) => ( + Box::new(Quadrotor::new( + 1.0 / config.simulation.simulation_frequency as f32, + config.simulation.clone(), + quad_config.mass, + config.simulation.gravity, + quad_config.drag_coefficient, + quad_config.inertia_matrix, + )?), + quad_config.mass, + ), + _ => { + return Err(SimulationError::OtherError( + "Unsupported quadrotor type".to_string(), + )) + } + }; + + println!( + "[\x1b[32mINFO\x1b[0m peng_quad] Quadrotor: {:?} {:?}", + mass, config.simulation.gravity + ); + let _pos_gains = config.pid_controller.pos_gains; + let _att_gains = config.pid_controller.att_gains; + let mut controller = PIDController::new( + [_pos_gains.kp, _pos_gains.kd, _pos_gains.ki], + [_att_gains.kp, _att_gains.kd, _att_gains.ki], + config.pid_controller.pos_max_int, + config.pid_controller.att_max_int, + mass, + config.simulation.gravity, + ); log::info!("Starting simulation..."); let mut i = 0; - let frame_time = Duration::from_secs_f32(1.0 / config.simulation.simulation_frequency as f32); + let frame_time = Duration::from_secs_f32(time_step); let mut next_frame = Instant::now(); - println!("frame_time: {:?}", frame_time); + let mut quad_state = quad.observe()?; loop { // If real-time mode is enabled, sleep until the next frame simulation frame if config.real_time { thread::sleep_until(next_frame); next_frame += frame_time; } - let time = quad.time_step * i as f32; - maze.update_obstacles(quad.time_step); + let time = time_step * i as f32; + maze.update_obstacles(time_step); update_planner( &mut planner_manager, i, time, config.simulation.simulation_frequency, - &quad, + &quad_state, &maze.obstacles, &planner_config, )?; let (desired_position, desired_velocity, desired_yaw) = planner_manager.update( - quad.position, - quad.orientation, - quad.velocity, + quad_state.position, + quad_state.orientation, + quad_state.velocity, time, &maze.obstacles, )?; @@ -132,49 +155,63 @@ fn main() -> Result<(), SimulationError> { &desired_position, &desired_velocity, desired_yaw, - &quad.position, - &quad.velocity, - quad.time_step, + &quad_state.position, + &quad_state.velocity, + time_step, ); + let torque = controller.compute_attitude_control( &calculated_desired_orientation, - &quad.orientation, - &quad.angular_velocity, - quad.time_step, + &quad_state.orientation, + &(quad_state.angular_velocity / (2.0 * std::f32::consts::PI)), + time_step, ); - quad.control(i, &config, thrust, &torque); - imu.update(quad.time_step)?; + let first_planner = planner_config.first().unwrap(); + if i >= first_planner.step { + let _ = quad.control(i, thrust, &torque); + } + quad_state = quad.observe()?; + imu.update(time_step)?; let (true_accel, true_gyro) = quad.read_imu()?; - let (measured_accel, measured_gyro) = imu.read(true_accel, true_gyro)?; + let (measured_accel, _measured_gyro) = imu.read(true_accel, true_gyro)?; if i % (config.simulation.simulation_frequency / config.simulation.log_frequency) == 0 { if config.render_depth { camera.render_depth( - &quad.position, - &quad.orientation, + &quad_state.position, + &quad_state.orientation, &maze, config.use_multithreading_depth_rendering, )?; } + if let Some(rec) = &rec { rec.set_time_seconds("timestamp", time); - if trajectory.add_point(quad.position) { + let rerun_quad_state = quad_state.clone(); + if trajectory.add_point(rerun_quad_state.position) { log_trajectory(rec, &trajectory)?; } + // log_joy(rec, thrust, &torque)?; log_data( rec, - &quad, + &rerun_quad_state, &desired_position, &desired_velocity, &measured_accel, - &measured_gyro, + &quad_state.angular_velocity, + &torque, )?; + let rotation = nalgebra::UnitQuaternion::from_axis_angle( + &Vector3::z_axis(), + // std::f32::consts::FRAC_PI_2, + 0.0, + ); if config.render_depth { log_depth_image(rec, &camera, config.use_multithreading_depth_rendering)?; log_pinhole_depth( rec, &camera, - quad.position, - quad.orientation, + rerun_quad_state.position, + rotation * quad_state.orientation, config.camera.rotation_transform, )?; } diff --git a/src/quadrotor.rs b/src/quadrotor.rs new file mode 100644 index 00000000..bc834af4 --- /dev/null +++ b/src/quadrotor.rs @@ -0,0 +1,39 @@ +use crate::SimulationError; +use nalgebra::{UnitQuaternion, Vector3}; + +#[derive(Clone, Debug, Default)] +/// Represents the state of a quadrotor +pub struct QuadrotorState { + /// Time stampe of the state + pub time: f32, + /// Current position of the quadrotor in NED + pub position: Vector3, + /// Current velocity of the quadrotor + pub velocity: Vector3, + /// Current Acceleration of the quadrotor + pub acceleration: Vector3, + /// Current orientation of the quadrotor + pub orientation: UnitQuaternion, + /// Current angular velocity of the quadrotor + pub angular_velocity: Vector3, +} + +/// Types implementing this trait can be used as a quadrotor in the simulation. +/// The underlying model can be internal or external to the simulator. +pub trait QuadrotorInterface { + // TODO remove config from control + fn control( + &mut self, + step_number: usize, + thrust: f32, + torque: &Vector3, + ) -> Result<(), SimulationError>; + + fn observe(&mut self) -> Result; + + fn max_thrust(&self) -> f32; + + fn max_torque(&self) -> Vector3; + + fn read_imu(&self) -> Result<(Vector3, Vector3), SimulationError>; +} diff --git a/tests/testdata/test_config_base.yaml b/tests/testdata/test_config_base.yaml new file mode 100644 index 00000000..20300738 --- /dev/null +++ b/tests/testdata/test_config_base.yaml @@ -0,0 +1,130 @@ +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) +real_time: true # Enable real time mode. If not enabled, sim will run in fast time. + +rerun_blueprint: "config/peng_default_blueprint.rbl" + +simulation: + control_frequency: 200 # Frequency of control loop execution (Hz) + 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) + gravity: 9.81 # Gravity in m/s^2 + 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 + # Inertia matrix [Ixx, Ixy, Ixz, Iyx, Iyy, Iyz, Izx, Izy, Izz] (kg*m^2) + inertia_matrix: [3.04e-3, 0.0, 0.0, 0.0, 4.55e-3, 0.0, 0.0, 0.0, 2.82e-3] + +pid_controller: + pos_gains: # PID gains for position control + kp: [7.1, 7.1, 11.9] # Proportional gains [x, y, z] + kd: [2.4, 2.4, 6.7] # Derivative gains [x, y, z] + ki: [0.0, 0.0, 0.0] # Integral gains [x, y, z] + att_gains: # PID gains for attitude control + kp: [1.5, 1.5, 1.0] # Proportional gains [roll, pitch, yaw] + kd: [0.13, 0.13, 0.1] # Derivative gains [roll, pitch, yaw] + ki: [0.0, 0.0, 0.0] # Integral gains [roll, pitch, yaw] + pos_max_int: [10.0, 10.0, 10.0] # Maximum integral error for position control [x, y, z] + att_max_int: [0.5, 0.5, 0.5] # Maximum integral error for attitude control [roll, pitch, yaw] + +imu: + accel_noise_std: 0.02 # Standard deviation of accelerometer noise (m/s^2) + gyro_noise_std: 0.01 # Standard deviation of gyroscope noise (rad/s) + accel_bias_std: 0.0001 # Standard deviation of accelerometer bias instability (m/s^2) + gyro_bias_std: 0.0001 # Standard deviation of gyroscope bias instability (rad/s) + +maze: + lower_bounds: [-4.0, -2.0, 0.0] # Lower bounds of the maze [x, y, z] (m) + upper_bounds: [4.0, 2.0, 2.0] # Upper bounds of the maze [x, y, z] (m) + num_obstacles: 20 # Number of obstacles in the maze + obstacles_velocity_bounds: [0.2, 0.2, 0.1] # Maximum velocity of obstacles [x, y, z] (m/s) + obstacles_radius_bounds: [0.05, 0.1] # Range of obstacle radii [min, max] (m) + +camera: + resolution: [128, 96] # Camera resolution [width, height] (pixels) + fov_vertical: 90 # Vertical Field of View (degrees) + near: 0.1 # Near clipping plane (m) + far: 5.0 # Far clipping plane (m) + rotation_transform: [0.0, 0.0, 1.0, -1.0, 0.0, 0.0, 0.0, -1.0, 0.0] # Rotates camera to positive x-axis + +mesh: + division: 7 # Number of divisions in the mesh grid + spacing: 0.5 # Spacing between mesh lines (m) + +planner_schedule: + # Minimum Jerk Line trajectory + - step: 1000 # Simulation step in ms to start this planner + planner_type: MinimumJerkLine + params: + end_position: [0.0, 0.0, 1.0] # Target end position [x, y, z] (m) + end_yaw: 0.0 # Target end yaw angle (rad) + duration: 2.5 # Duration of the trajectory (s) + + # Lissajous trajectory + - step: 5000 + planner_type: Lissajous + params: + center: [0.5, 0.5, 1.0] # Center of the Lissajous curve [x, y, z] (m) + amplitude: [0.5, 0.5, 0.2] # Amplitudes of the curve [x, y, z] (m) + frequency: [1.0, 2.0, 3.0] # Frequencies of the curve [x, y, z] (Hz) + phase: [0.0, 1.5707963267948966, 0.0] # Phase offsets [x, y, z] (rad) + duration: 20.0 # Duration of the trajectory (s) + end_yaw: 6.283185307179586 # Target end yaw angle (2*PI rad) + ramp_time: 5.0 # Time for smooth transition (s) + + # Circular trajectory + - step: 27000 + planner_type: Circle + params: + center: [0.5, 0.5, 1.0] # Center of the circle [x, y, z] (m) + radius: 0.5 # Radius of the circle (m) + angular_velocity: 1.0 # Angular velocity (rad/s) + duration: 5.0 # Duration of the trajectory (s) + ramp_time: 2.0 # Time for smooth transition (s) + + # Another Minimum Jerk Line trajectory + - step: 32000 + planner_type: MinimumJerkLine + params: + end_position: [-2.5, 0.0, 1.0] # Target end position [x, y, z] (m) + end_yaw: 0.0 # Target end yaw angle (rad) + duration: 3.0 # Duration of the trajectory (s) + + # Obstacle Avoidance trajectory + - step: 35000 + planner_type: ObstacleAvoidance + params: + target_position: [2.5, 1.0, 0.5] # Target position [x, y, z] (m) + duration: 10.0 # Duration of the trajectory (s) + end_yaw: 0.0 # Target end yaw angle (rad) + k_att: 0.03 # Attractive force gain + k_rep: 0.01 # Repulsive force gain + k_vortex: 0.005 # Vortex force gain + d0: 0.5 # Obstacle influence distance (m) + d_target: 0.5 # Target influence distance (m) + max_speed: 0.1 # Maximum speed (m/s) + + # Minimum Snap Waypoint trajectory + - step: 45000 + planner_type: MinimumSnapWaypoint + params: + waypoints: # List of waypoints [x, y, z] (m) + - [1.0, 1.0, 1.5] + - [-1.0, 1.0, 1.75] + - [0.0, -1.0, 1.0] + - [0.0, 0.0, 0.5] + yaws: [1.5707963267948966, 3.141592653589793, -1.5707963267948966, 0.0] # Yaw angles at waypoints (rad) + segment_times: [5.0, 5.0, 5.0, 5.0] # Time to reach each waypoint (s) + + # Landing trajectory + - step: 65000 + planner_type: Landing + params: + duration: 5.0 # Duration of the landing maneuver (s) diff --git a/tests/testdata/test_liftoff_base.yaml b/tests/testdata/test_liftoff_base.yaml new file mode 100644 index 00000000..9bd19dac --- /dev/null +++ b/tests/testdata/test_liftoff_base.yaml @@ -0,0 +1,133 @@ +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" + +simulation: + control_frequency: 200 # Frequency of control loop execution (Hz) + 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) + +quadrotor: + type: Liftoff + mass: 1.3 # Mass of the quadrotor (kg) + gravity: 9.81 # Gravitational acceleration (m/s^2) + drag_coefficient: 0.000 # Aerodynamic drag coefficient + # Inertia matrix [Ixx, Ixy, Ixz, Iyx, Iyy, Iyz, Izx, Izy, Izz] (kg*m^2) + inertia_matrix: [3.04e-3, 0.0, 0.0, 0.0, 4.55e-3, 0.0, 0.0, 0.0, 2.82e-3] + +vehicle_configuration: + type: LiftoffConfiguration + ip_address: 0.0.0.0:9001 + +pid_controller: + pos_gains: # PID gains for position control + kp: [7.1, 7.1, 11.9] # Proportional gains [x, y, z] + kd: [2.4, 2.4, 6.7] # Derivative gains [x, y, z] + ki: [0.0, 0.0, 0.0] # Integral gains [x, y, z] + att_gains: # PID gains for attitude control + kp: [1.5, 1.5, 1.0] # Proportional gains [roll, pitch, yaw] + kd: [0.13, 0.13, 0.1] # Derivative gains [roll, pitch, yaw] + ki: [0.0, 0.0, 0.0] # Integral gains [roll, pitch, yaw] + pos_max_int: [10.0, 10.0, 10.0] # Maximum integral error for position control [x, y, z] + att_max_int: [0.5, 0.5, 0.5] # Maximum integral error for attitude control [roll, pitch, yaw] + +imu: + accel_noise_std: 0.02 # Standard deviation of accelerometer noise (m/s^2) + gyro_noise_std: 0.01 # Standard deviation of gyroscope noise (rad/s) + accel_bias_std: 0.0001 # Standard deviation of accelerometer bias instability (m/s^2) + gyro_bias_std: 0.0001 # Standard deviation of gyroscope bias instability (rad/s) + +maze: + lower_bounds: [-4.0, -2.0, 0.0] # Lower bounds of the maze [x, y, z] (m) + upper_bounds: [4.0, 2.0, 2.0] # Upper bounds of the maze [x, y, z] (m) + num_obstacles: 20 # Number of obstacles in the maze + obstacles_velocity_bounds: [0.2, 0.2, 0.1] # Maximum velocity of obstacles [x, y, z] (m/s) + obstacles_radius_bounds: [0.05, 0.1] # Range of obstacle radii [min, max] (m) + +camera: + resolution: [128, 96] # Camera resolution [width, height] (pixels) + fov_vertical: 90 # Vertical Field of View (degrees) + near: 0.1 # Near clipping plane (m) + far: 5.0 # Far clipping plane (m) + rotation_transform: [0.0, 0.0, 1.0, -1.0, 0.0, 0.0, 0.0, -1.0, 0.0] # Rotates camera to positive x-axis + +mesh: + division: 7 # Number of divisions in the mesh grid + spacing: 0.5 # Spacing between mesh lines (m) + +planner_schedule: + # Minimum Jerk Line trajectory + - step: 1000 # Simulation step in ms to start this planner + planner_type: MinimumJerkLine + params: + end_position: [0.0, 0.0, 1.0] # Target end position [x, y, z] (m) + end_yaw: 0.0 # Target end yaw angle (rad) + duration: 2.5 # Duration of the trajectory (s) + + # Lissajous trajectory + - step: 5000 + planner_type: Lissajous + params: + center: [0.5, 0.5, 1.0] # Center of the Lissajous curve [x, y, z] (m) + amplitude: [0.5, 0.5, 0.2] # Amplitudes of the curve [x, y, z] (m) + frequency: [1.0, 2.0, 3.0] # Frequencies of the curve [x, y, z] (Hz) + phase: [0.0, 1.5707963267948966, 0.0] # Phase offsets [x, y, z] (rad) + duration: 20.0 # Duration of the trajectory (s) + end_yaw: 6.283185307179586 # Target end yaw angle (2*PI rad) + ramp_time: 5.0 # Time for smooth transition (s) + + # Circular trajectory + - step: 27000 + planner_type: Circle + params: + center: [0.5, 0.5, 1.0] # Center of the circle [x, y, z] (m) + radius: 0.5 # Radius of the circle (m) + angular_velocity: 1.0 # Angular velocity (rad/s) + duration: 5.0 # Duration of the trajectory (s) + ramp_time: 2.0 # Time for smooth transition (s) + + # Another Minimum Jerk Line trajectory + - step: 32000 + planner_type: MinimumJerkLine + params: + end_position: [-2.5, 0.0, 1.0] # Target end position [x, y, z] (m) + end_yaw: 0.0 # Target end yaw angle (rad) + duration: 3.0 # Duration of the trajectory (s) + + # Obstacle Avoidance trajectory + - step: 35000 + planner_type: ObstacleAvoidance + params: + target_position: [2.5, 1.0, 0.5] # Target position [x, y, z] (m) + duration: 10.0 # Duration of the trajectory (s) + end_yaw: 0.0 # Target end yaw angle (rad) + k_att: 0.03 # Attractive force gain + k_rep: 0.01 # Repulsive force gain + k_vortex: 0.005 # Vortex force gain + d0: 0.5 # Obstacle influence distance (m) + d_target: 0.5 # Target influence distance (m) + max_speed: 0.1 # Maximum speed (m/s) + + # Minimum Snap Waypoint trajectory + - step: 45000 + planner_type: MinimumSnapWaypoint + params: + waypoints: # List of waypoints [x, y, z] (m) + - [1.0, 1.0, 1.5] + - [-1.0, 1.0, 1.75] + - [0.0, -1.0, 1.0] + - [0.0, 0.0, 0.5] + yaws: [1.5707963267948966, 3.141592653589793, -1.5707963267948966, 0.0] # Yaw angles at waypoints (rad) + segment_times: [5.0, 5.0, 5.0, 5.0] # Time to reach each waypoint (s) + + # Landing trajectory + - step: 65000 + planner_type: Landing + params: + duration: 5.0 # Duration of the landing maneuver (s)