diff --git a/config/liftoff_quad.yaml b/config/liftoff_quad.yaml deleted file mode 100644 index d396fdf4..00000000 --- a/config/liftoff_quad.yaml +++ /dev/null @@ -1,87 +0,0 @@ -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: 120 # Frequency of control loop execution (Hz) - simulation_frequency: 240 # 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: "Liftoff" - mass: .131 # Mass of the quadrotor (kg) - gravity: 9.81 # Gravitational acceleration (m/s^2) - drag_coefficient: 0.800 # Aerodynamic drag coefficient - # Inertia matrix [Ixx, Ixy, Ixz, Iyx, Iyy, Iyz, Izx, Izy, Izz] (kg*m^2) - inertia_matrix: [2.5e-5, 0.0, 0.0, 0.0, 2.5e-5, 0.0, 0.0, 0.0, 4.5e-5] - max_thrust_kg: .5 - arm_length_m: .065 - yaw_torque_constant: .0052 - serial_port: "/dev/tty.usbserial-B001JE6N" - baud_rate: 460800 - -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] - -angle_limits: [20, 20, 360] - -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: 200 # 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: 5.0 # Duration of the trajectory (s) - - # Hover - - step: 1200 # Simulation step in ms to start this planner - planner_type: Hover - params: - target_position: [0.0, 0.0, 1.0] # Target end position [x, y, z] (m) - target_yaw: 0.0 - - # Landing trajectory - - step: 13000 - planner_type: Landing - params: - duration: 10.0 # Duration of the landing maneuver (s) diff --git a/src/config.rs b/src/config.rs index 702cc4b9..c1904a42 100644 --- a/src/config.rs +++ b/src/config.rs @@ -4,7 +4,7 @@ //! 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, Vector3}; +use nalgebra::Matrix3; use crate::SimulationError; @@ -46,7 +46,6 @@ pub struct Config { /// Vehicle Specifig configuration pub enum QuadrotorConfigurations { Peng(QuadrotorConfig), - Liftoff(LiftoffQuadrotorConfig), } #[derive(serde::Deserialize)] @@ -60,9 +59,11 @@ pub struct PlannerStep { pub params: serde_yaml::Value, } -#[derive(Clone, Default, 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 @@ -77,14 +78,26 @@ pub struct SimulationConfig { pub use_rk4_for_dynamics_update: bool, } +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(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 @@ -101,7 +114,6 @@ impl Default for QuadrotorConfig { fn default() -> Self { QuadrotorConfig { mass: 1.3, - gravity: 9.81, 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, @@ -126,59 +138,6 @@ impl QuadrotorConfig { } } -#[derive(Clone, Debug, serde::Deserialize)] -#[serde(default)] -/// Configuration for the quadrotor -pub struct LiftoffQuadrotorConfig { - /// Mass of the quadrotor in kg - pub mass: f32, - /// Gravity in m/s^2 - pub gravity: 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, - /// The IP address where Liftoff is publishing state data - pub ip_address: String, - pub connection_timeout: tokio::time::Duration, - pub max_retry_delay: tokio::time::Duration, - pub serial_port: Option, - pub baud_rate: u32, -} - -impl Default for LiftoffQuadrotorConfig { - fn default() -> Self { - LiftoffQuadrotorConfig { - mass: 1.3, - gravity: 9.81, - 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, - ip_address: String::from("0.0.0.0:9001"), - connection_timeout: tokio::time::Duration::from_secs(5 * 60), - max_retry_delay: tokio::time::Duration::from_secs(30), - serial_port: None, - baud_rate: 460800, - } - } -} - -impl LiftoffQuadrotorConfig { - /// Calculate all maximum torques and return them as a tuple - pub fn max_torques(&self) -> (f32, f32, f32) { - let motor_thrust = self.max_thrust_kg / 4.0; - // The maximum roll and pitch torques - let max_rp_torque = 2.0 * self.arm_length_m * motor_thrust; - let yaw_torque = 2.0 * self.yaw_torque_constant * motor_thrust; - (max_rp_torque, max_rp_torque, yaw_torque) - } -} - #[derive(serde::Deserialize)] /// Configuration for the PID controller pub struct PIDControllerConfig { @@ -285,7 +244,6 @@ mod tests { 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.gravity, 9.81); 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]); @@ -293,14 +251,4 @@ mod tests { assert_eq!(config.imu.accel_noise_std, 0.02); assert_eq!(config.maze.upper_bounds, [4.0, 2.0, 2.0]); } - - #[test] - fn test_liftoff_config() { - let config = Config::from_yaml("tests/testdata/test_liftoff_base.yaml").unwrap(); - let liftoff_config = match config.quadrotor { - QuadrotorConfigurations::Liftoff(liftoff_config) => liftoff_config, - _ => panic!("Failed to load Liftoff configuration"), - }; - assert_eq!(liftoff_config.ip_address, "0.0.0.0:9001"); - } } diff --git a/src/main.rs b/src/main.rs index de36c229..262a7b1e 100644 --- a/src/main.rs +++ b/src/main.rs @@ -91,34 +91,28 @@ async fn main() -> Result<(), SimulationError> { log_maze_tube(rec, &maze)?; log_maze_obstacles(rec, &maze)?; } - let (mut quad, mass, gravity): (Box, f32, f32) = match config.quadrotor - { + 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, - quad_config.gravity, + config.simulation.gravity, quad_config.drag_coefficient, quad_config.inertia_matrix, )?), quad_config.mass, - quad_config.gravity, - ), - config::QuadrotorConfigurations::Liftoff(ref liftoff_quad_config) => ( - Box::new(LiftoffQuad::new( - 1.0 / config.simulation.control_frequency as f32, - config.simulation.clone(), - liftoff_quad_config.clone(), - )?), - liftoff_quad_config.mass, - liftoff_quad_config.gravity, ), + _ => { + return Err(SimulationError::OtherError( + "Unsupported quadrotor type".to_string(), + )) + } }; println!( "[\x1b[32mINFO\x1b[0m peng_quad] Quadrotor: {:?} {:?}", - mass, gravity + mass, config.simulation.gravity ); let _pos_gains = config.pid_controller.pos_gains; let _att_gains = config.pid_controller.att_gains; @@ -128,7 +122,7 @@ async fn main() -> Result<(), SimulationError> { config.pid_controller.pos_max_int, config.pid_controller.att_max_int, mass, - gravity, + config.simulation.gravity, ); log::info!("Starting simulation..."); let mut i = 0;