use rerun::external::ndarray; add render_depth option; change unit of…

… planner schedule to ms; use usize for frequencies config

Cargo.toml

@@ -30,7 +30,6 @@ nalgebra = "0.33.0"
 rand = "0.8.5"
 rand_distr = "0.4.3"
 rerun = "0.18.2"
-ndarray = "0.15.6"
 thiserror = "1.0.63"
 serde = { version = "1.0.209", features = ["derive"] }
 serde_yaml = "0.9.34"

config/quad.yaml

@@ -1,10 +1,10 @@
-# Enable visualization using rerun.io
-use_rerun: true
+use_rerun: true # Enable visualization using rerun.io
+render_depth: true # Enable render depth if use_rerun is true
 
 simulation:
-  control_frequency: 200.0 # Frequency of control loop execution (Hz)
-  simulation_frequency: 1000.0 # Frequency of physics simulation updates (Hz)
-  log_frequency: 20.0 # Frequency of data logging (Hz)
+  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:
@@ -14,7 +14,7 @@ quadrotor:
   # 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]
 
-controller:
+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]
@@ -51,7 +51,7 @@ mesh:
 
 planner_schedule:
   # Minimum Jerk Line trajectory
-  - step: 1000 # Simulation step to start this planner
+  - 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)

src/config.rs

@@ -5,8 +5,8 @@ pub struct Config {
     pub simulation: SimulationConfig,
     /// Quadrotor configuration
     pub quadrotor: QuadrotorConfig,
-    /// Controller configuration
-    pub controller: ControllerConfig,
+    /// PID Controller configuration
+    pub pid_controller: PIDControllerConfig,
     /// IMU configuration
     pub imu: ImuConfig,
     /// Maze configuration
@@ -19,12 +19,14 @@ pub struct Config {
     pub planner_schedule: Vec<PlannerStep>,
     /// Use rerun.io for recording
     pub use_rerun: bool,
+    /// Render depth
+    pub render_depth: bool,
 }
 
 #[derive(serde::Deserialize)]
 /// Configuration for a planner step
 pub struct PlannerStep {
-    /// Step number that the planner should be executed
+    /// Step number that the planner should be executed (Unit: ms)
     pub step: usize,
     /// Type of planner to use
     pub planner_type: String,
@@ -36,11 +38,11 @@ pub struct PlannerStep {
 /// Configuration for the simulation
 pub struct SimulationConfig {
     /// Control frequency in Hz
-    pub control_frequency: f32,
+    pub control_frequency: usize,
     /// Simulation frequency in Hz
-    pub simulation_frequency: f32,
+    pub simulation_frequency: usize,
     /// Log frequency in Hz
-    pub log_frequency: f32,
+    pub log_frequency: usize,
     /// Duration of the simulation in seconds
     pub duration: f32,
 }
@@ -59,8 +61,8 @@ pub struct QuadrotorConfig {
 }
 
 #[derive(serde::Deserialize)]
-/// Configuration for the controller
-pub struct ControllerConfig {
+/// Configuration for the PID controller
+pub struct PIDControllerConfig {
     /// Position gains
     pub pos_gains: PIDGains,
     /// Attitude gains

src/lib.rs

@@ -16,6 +16,7 @@
 //! 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);
 //! ```
+pub mod config;
 use nalgebra::{Matrix3, Rotation3, SMatrix, UnitQuaternion, Vector3};
 use rand_distr::{Distribution, Normal};
 use std::f32::consts::PI;
@@ -1491,7 +1492,7 @@ impl Planner for MinimumSnapWaypointPlanner {
 /// };
 /// ```
 pub struct PlannerStepConfig {
-    /// The simulation step at which this planner should be activated.
+    /// The simulation step at which this planner should be activated (in ms unit).
     pub step: usize,
     /// The type of planner to use for this step.
     pub planner_type: String,
@@ -1501,7 +1502,7 @@ pub struct PlannerStepConfig {
 /// Updates the planner based on the current simulation step and configuration
 /// # Arguments
 /// * `planner_manager` - The PlannerManager instance to update
-/// * `step` - The current simulation step
+/// * `step` - The current simulation step in ms unit
 /// * `time` - The current simulation time
 /// * `quad` - The Quadrotor instance
 /// * `obstacles` - The current obstacles in the simulation
@@ -1537,19 +1538,23 @@ pub fn update_planner(
     planner_manager: &mut PlannerManager,
     step: usize,
     time: f32,
+    simulation_frequency: usize,
     quad: &Quadrotor,
     obstacles: &Vec<Obstacle>,
     planner_config: &Vec<PlannerStepConfig>,
 ) -> Result<(), SimulationError> {
-    if let Some(planner_step) = planner_config.iter().find(|s| s.step == step) {
+    if let Some(planner_step) = planner_config
+        .iter()
+        .find(|s| s.step * simulation_frequency == step * 1000)
+    {
         log::info!("Time: {:.2} s,\tSwitch {}", time, planner_step.planner_type);
         planner_manager.set_planner(create_planner(planner_step, quad, time, obstacles)?);
     }
     Ok(())
 }
 /// Creates a planner based on the configuration
 /// # Arguments
-/// * `step` - The configuration for the planner step
+/// * `step` - The configuration for the planner step in ms unit
 /// * `quad` - The Quadrotor instance
 /// * `time` - The current simulation time
 /// * `obstacles` - The current obstacles in the simulation
@@ -2357,13 +2362,16 @@ pub fn log_depth_image(
     min_depth: f32,
     max_depth: f32,
 ) -> Result<(), SimulationError> {
-    let mut image = ndarray::Array::zeros((height, width, 3));
+    let mut image = rerun::external::ndarray::Array::zeros((height, width, 3));
     let depth_range = max_depth - min_depth;
     image
-        .axis_iter_mut(ndarray::Axis(0))
+        .axis_iter_mut(rerun::external::ndarray::Axis(0))
         .enumerate()
         .for_each(|(y, mut row)| {
-            for (x, mut pixel) in row.axis_iter_mut(ndarray::Axis(0)).enumerate() {
+            for (x, mut pixel) in row
+                .axis_iter_mut(rerun::external::ndarray::Axis(0))
+                .enumerate()
+            {
                 let depth = depth_image[y * width + x];
                 let color = if depth.is_finite() {
                     let normalized_depth = ((depth - min_depth) / depth_range).clamp(0.0, 1.0);

src/main.rs

@@ -1,36 +1,33 @@
 use nalgebra::Vector3;
-use std::env;
-mod config;
-use config::Config;
 use peng_quad::*;
 fn main() -> Result<(), SimulationError> {
     env_logger::builder()
         .parse_env(env_logger::Env::default().default_filter_or("info"))
         .init();
     let mut config_str = "config/quad.yaml";
-    let args: Vec<String> = env::args().collect();
+    let args: Vec<String> = std::env::args().collect();
     if args.len() != 2 {
         log::warn!("Usage: {} <config.yaml>.", args[0]);
         log::warn!("Loading default configuration: config/quad.yaml");
     } else {
         log::info!("Loading configuration: {}", args[1]);
         config_str = &args[1];
     }
-    let config = Config::from_yaml(config_str).expect("Failed to load configuration");
+    let config = config::Config::from_yaml(config_str).expect("Failed to load configuration");
     let mut quad = Quadrotor::new(
-        1.0 / config.simulation.simulation_frequency,
+        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.controller.pos_gains;
-    let _att_gains = config.controller.att_gains;
+    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.controller.pos_max_int,
-        config.controller.att_max_int,
+        config.pid_controller.pos_max_int,
+        config.pid_controller.att_max_int,
     );
     let mut imu = Imu::new(
         config.imu.accel_noise_std,
@@ -87,6 +84,7 @@ fn main() -> Result<(), SimulationError> {
             &mut planner_manager,
             i,
             time,
+            config.simulation.simulation_frequency,
             &quad,
             &maze.obstacles,
             &planner_config,
@@ -114,8 +112,7 @@ fn main() -> Result<(), SimulationError> {
             &quad.angular_velocity,
             quad.time_step,
         );
-        if i % (config.simulation.simulation_frequency as usize
-            / config.simulation.control_frequency as usize)
+        if i % (config.simulation.simulation_frequency / config.simulation.control_frequency)
             == 0
         {
             quad.update_dynamics_with_controls(thrust, &torque);
@@ -127,8 +124,7 @@ fn main() -> Result<(), SimulationError> {
         imu.update(quad.time_step)?;
         let (true_accel, true_gyro) = quad.read_imu()?;
         let (_measured_accel, _measured_gyro) = imu.read(true_accel, true_gyro)?;
-        if i % (config.simulation.simulation_frequency as usize
-            / config.simulation.log_frequency as usize)
+        if i % (config.simulation.simulation_frequency / config.simulation.log_frequency)
             == 0
         {
             if let Some(rec) = &rec {
@@ -144,15 +140,22 @@ fn main() -> Result<(), SimulationError> {
                     &_measured_accel,
                     &_measured_gyro,
                 )?;
-                camera.render_depth(&quad.position, &quad.orientation, &maze, &mut depth_buffer)?;
-                log_depth_image(
-                    &rec,
-                    &depth_buffer,
-                    camera.resolution.0,
-                    camera.resolution.1,
-                    camera.near,
-                    camera.far,
-                )?;
+                if config.render_depth {
+                    camera.render_depth(
+                        &quad.position,
+                        &quad.orientation,
+                        &maze,
+                        &mut depth_buffer,
+                    )?;
+                    log_depth_image(
+                        &rec,
+                        &depth_buffer,
+                        camera.resolution.0,
+                        camera.resolution.1,
+                        camera.near,
+                        camera.far,
+                    )?;
+                }
                 log_maze_obstacles(&rec, &maze)?;
             }
         }