From 38a8d1bd5b0f48994f95b57047a7a09921ab5d37 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sun, 10 Nov 2024 22:04:02 -0800 Subject: [PATCH 01/54] add vehicle specific configs --- src/config.rs | 61 +++++++++++- tests/testdata/test_config_base.yaml | 126 +++++++++++++++++++++++++ tests/testdata/test_liftoff_base.yaml | 130 ++++++++++++++++++++++++++ 3 files changed, 316 insertions(+), 1 deletion(-) create mode 100644 tests/testdata/test_config_base.yaml create mode 100644 tests/testdata/test_liftoff_base.yaml diff --git a/src/config.rs b/src/config.rs index 1d1b5dfeb..e1f4c305e 100644 --- a/src/config.rs +++ b/src/config.rs @@ -32,8 +32,36 @@ pub struct Config { 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, + /// Vehicle specific configuration + pub vehicle_configuration: Option, +} + +#[derive(serde::Deserialize)] +#[serde(tag = "type")] +/// Vehicle Specifig configuration +pub enum VehicleConfigurations { + LiftoffConfiguration(LiftoffConfiguration), +} + +#[derive(serde::Deserialize)] +#[serde(default)] +/// Configurations for controlling drones in Liftoff +pub struct LiftoffConfiguration { + pub ip_address: String, + pub connection_timeout: tokio::time::Duration, + pub max_retry_delay: tokio::time::Duration, +} + +impl Default for LiftoffConfiguration { + fn default() -> Self { + LiftoffConfiguration { + 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), + } + } } #[derive(serde::Deserialize)] @@ -162,3 +190,34 @@ 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(); + 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!(config.quadrotor.mass, 1.3); + assert_eq!(config.quadrotor.gravity, 9.81); + assert_eq!(config.quadrotor.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]); + } + + #[test] + fn test_liftoff_config() { + let config = Config::from_yaml("tests/testdata/test_liftoff_base.yaml").unwrap(); + let liftoff_config = match config.vehicle_configuration { + Some(VehicleConfigurations::LiftoffConfiguration(liftoff_config)) => liftoff_config, + _ => panic!("Failed to load Liftoff configuration"), + }; + assert_eq!(liftoff_config.ip_address, "0.0.0.0:9001"); + } +} diff --git a/tests/testdata/test_config_base.yaml b/tests/testdata/test_config_base.yaml new file mode 100644 index 000000000..ae9e1e2dc --- /dev/null +++ b/tests/testdata/test_config_base.yaml @@ -0,0 +1,126 @@ +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. + +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: + 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 000000000..0f6d80fd3 --- /dev/null +++ b/tests/testdata/test_liftoff_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) +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. + +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: + 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) From 3a06b8a457906f3d804110984ffab48d268d9a84 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sun, 10 Nov 2024 22:04:53 -0800 Subject: [PATCH 02/54] add quadrotor file, with trait and state object --- src/lib.rs | 60 ++++++++++++------------------------------------ src/quadrotor.rs | 22 ++++++++++++++++++ 2 files changed, 37 insertions(+), 45 deletions(-) create mode 100644 src/quadrotor.rs diff --git a/src/lib.rs b/src/lib.rs index 8df38648e..e1e1cc727 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -19,7 +19,9 @@ 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; @@ -49,28 +51,6 @@ 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 /// ``` @@ -80,7 +60,7 @@ pub trait Quadrotor { /// 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 struct SimulatedQuadrotor { +pub struct Quadrotor { /// Current position of the quadrotor in 3D space pub position: Vector3, /// Current velocity of the quadrotor @@ -107,7 +87,7 @@ pub struct SimulatedQuadrotor { pub previous_torque: Vector3, } -impl Quadrotor for SimulatedQuadrotor { +impl QuadrotorInterface for Quadrotor { fn control( &mut self, step_number: usize, @@ -136,28 +116,18 @@ impl Quadrotor for SimulatedQuadrotor { } } } - 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 { + return Ok(QuadrotorState { + position: self.position, + velocity: self.velocity, + orientation: self.orientation, + angular_velocity: 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 @@ -1804,7 +1774,7 @@ pub fn update_planner( step: usize, time: f32, simulation_frequency: usize, - quad: &SimulatedQuadrotor, + quad: &Quadrotor, obstacles: &[Obstacle], planner_config: &[PlannerStepConfig], ) -> Result<(), SimulationError> { @@ -1854,7 +1824,7 @@ pub fn update_planner( /// ``` pub fn create_planner( step: &PlannerStepConfig, - quad: &SimulatedQuadrotor, + quad: &Quadrotor, time: f32, obstacles: &[Obstacle], ) -> Result { @@ -2407,7 +2377,7 @@ impl Camera { /// ``` pub fn log_data( rec: &rerun::RecordingStream, - quad: &SimulatedQuadrotor, + quad: &Quadrotor, desired_position: &Vector3, desired_velocity: &Vector3, measured_accel: &Vector3, diff --git a/src/quadrotor.rs b/src/quadrotor.rs new file mode 100644 index 000000000..cc521f178 --- /dev/null +++ b/src/quadrotor.rs @@ -0,0 +1,22 @@ +use crate::config::Config; +use crate::SimulationError; +use nalgebra::{UnitQuaternion, Vector3}; + +/// Represents the state of a quadrotor +pub struct QuadrotorState { + 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, +} + +/// 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 { + fn control(&mut self, step_number: usize, config: &Config, thrust: f32, torque: &Vector3); + + fn observe(&mut self) -> Result; +} From badf5548cda6176479fd97f6698a8c7e151be28c Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sun, 10 Nov 2024 22:04:53 -0800 Subject: [PATCH 03/54] add quadrotor file, with trait and state object --- src/main.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main.rs b/src/main.rs index 99eb5890e..98f3e5571 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,12 +1,12 @@ #![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; -mod rc_quad; /// Main function for the simulation fn main() -> Result<(), SimulationError> { @@ -23,7 +23,7 @@ fn main() -> Result<(), SimulationError> { config_str = &args[1]; } let config = config::Config::from_yaml(config_str).expect("Failed to load configuration"); - let mut quad = SimulatedQuadrotor::new( + let mut quad = Quadrotor::new( 1.0 / config.simulation.simulation_frequency as f32, config.quadrotor.mass, config.quadrotor.gravity, From b997d546ba512a6b2c319198082cc2fc11f67220 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sun, 10 Nov 2024 22:54:24 -0800 Subject: [PATCH 04/54] add a liftoff specific config --- config/liftoff_quad.yaml | 129 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 129 insertions(+) create mode 100644 config/liftoff_quad.yaml diff --git a/config/liftoff_quad.yaml b/config/liftoff_quad.yaml new file mode 100644 index 000000000..e7ba6d8fe --- /dev/null +++ b/config/liftoff_quad.yaml @@ -0,0 +1,129 @@ +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. + +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: + 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: + 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) From f51f5ec62506f8352b2207030036497c1bdf882a Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sun, 10 Nov 2024 22:56:24 -0800 Subject: [PATCH 05/54] add initial liftoff quad configuration --- src/liftoff_quad.rs | 194 +++++++++++++++++++++++++++++++++++++++----- 1 file changed, 172 insertions(+), 22 deletions(-) diff --git a/src/liftoff_quad.rs b/src/liftoff_quad.rs index 84cd3d77f..7dba0923c 100644 --- a/src/liftoff_quad.rs +++ b/src/liftoff_quad.rs @@ -1,5 +1,13 @@ -use nalgebra::{Matrix3, Quaternion, Rotation3, SMatrix, UnitQuaternion, Vector3}; -use peng_quad::{Quadrotor, SimulationError}; +use binrw::{binrw, BinRead}; +use nalgebra::{zero, Matrix3, Quaternion, Rotation3, SMatrix, Unit, UnitQuaternion, Vector3}; +use peng_quad::config::{Config, LiftoffConfiguration, QuadrotorConfig}; +use peng_quad::quadrotor::{QuadrotorInterface, QuadrotorState}; +use peng_quad::SimulationError; +use rand; +use std::net::UdpSocket; +use std::sync::Arc; +use tokio::sync::Mutex; +use tokio::time::{sleep, Duration}; /// Represents an RC quadrotor /// # Example @@ -13,10 +21,12 @@ use peng_quad::{Quadrotor, SimulationError}; pub struct LiftoffQuad { /// Current position of the quadrotor in 3D space pub position: Vector3, + pub last_position: Vector3, /// Current velocity of the quadrotor pub velocity: Vector3, /// Current orientation of the quadrotor pub orientation: UnitQuaternion, + pub last_orientation: UnitQuaternion, /// Current angular velocity of the quadrotor pub angular_velocity: Vector3, /// Simulation time step in seconds @@ -31,30 +41,170 @@ pub struct LiftoffQuad { pub previous_thrust: f32, /// Previous Torque pub previous_torque: Vector3, + /// Quadrotor sample mutex + pub shared_data: Arc>>, } -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") +impl QuadrotorInterface for LiftoffQuad { + fn control(&mut self, step_number: usize, config: &Config, thrust: f32, torque: &Vector3) { + // TODO: implement control outputs to CyberRC - gamepad mode + // todo!("implement control outputs to CyberRC - gamepad mode") } - fn observe( - &self, - ) -> Result< - ( - Vector3, - Vector3, - UnitQuaternion, - Vector3, - ), - SimulationError, - > { + /// Observe the current state of the quadrotor + /// # Returns + /// A tuple containing the position, velocity, orientation, and angular velocity of the quadrotor + fn observe(&mut self) -> Result { + let mut data_lock = tokio::runtime::Handle::current().block_on(self.shared_data.lock()); + while let Some(sample) = data_lock.take() { + self.position = Vector3::from_row_slice(&sample.position); + self.velocity = (self.position - self.last_position) / self.time_step; + self.orientation = sample.attitude_quaternion(); + let delta_q = + UnitQuaternion::new(self.last_orientation * self.last_position.conjugate()); + let angle = 2.0 * delta_q.scalar().acos(); + let angular_velocity = angle / self.time_step; + let axis = delta_q + .axis() + .unwrap_or(Unit::new_normalize(Vector3::::zeros())); + self.angular_velocity = axis.scale(angular_velocity); + // Update last values for future velocity and angular velocity calculations + self.last_position = self.position; + self.last_orientation = self.orientation; + return Ok(QuadrotorState { + position: self.position, + velocity: self.velocity, + orientation: self.orientation, + angular_velocity: self.angular_velocity, + }); + } todo!("implement state feedback from Liftoff UDP") } } + +impl LiftoffQuad { + fn new( + time_step: f32, + vehicle_config: QuadrotorConfig, + liftoff_config: LiftoffConfiguration, + ) -> Result { + let inertia_matrix = Matrix3::from_row_slice(&vehicle_config.inertia_matrix); + let inertia_matrix_inv = + inertia_matrix + .try_inverse() + .ok_or(SimulationError::NalgebraError( + "Failed to invert inertia matrix".to_string(), + ))?; + let shared_data = Arc::new(Mutex::new(None)); + let shared_data_clone = Arc::clone(&shared_data); + tokio::spawn(async move { + let _ = feedback_loop( + &liftoff_config.ip_address.to_string(), + liftoff_config.connection_timeout, + liftoff_config.max_retry_delay, + shared_data_clone, + ) + .await; + }); + Ok(Self { + position: Vector3::zeros(), + last_position: Vector3::zeros(), + velocity: Vector3::zeros(), + orientation: UnitQuaternion::identity(), + last_orientation: UnitQuaternion::identity(), + angular_velocity: Vector3::zeros(), + time_step, + mass: vehicle_config.mass, + inertia_matrix, + inertia_matrix_inv, + previous_thrust: 0.0, + previous_torque: Vector3::zeros(), + shared_data: Arc::new(Mutex::new(None)), + }) + } +} + +// TODO: configure packet based on the content of the Liftoff config file +#[binrw] +#[br(little)] +#[derive(Debug)] +struct LiftoffPacket { + timestamp: f32, + position: [f32; 3], + // TODO: binrw read to quaternion + attitude: [f32; 4], + motor_num: u8, + #[br(count = motor_num)] + motor_rpm: Vec, +} + +impl LiftoffPacket { + pub fn attitude_quaternion(&self) -> UnitQuaternion { + UnitQuaternion::from_quaternion(Quaternion::new( + self.attitude[0], + self.attitude[1], + self.attitude[2], + self.attitude[3], + )) + } +} + +async fn feedback_loop( + address: &str, + timeout: Duration, + max_retry_delay: Duration, + data_lock: Arc>>, +) -> Result<(), SimulationError> { + let mut buf = [0; 256]; + let mut current_wait = Duration::from_secs(0); + let mut delay = Duration::from_secs(2); + + loop { + match UdpSocket::bind(address) { + // Bind successful + Ok(socket) => { + socket + .set_read_timeout(Some(Duration::from_secs(15))) + .map_err(|e| SimulationError::OtherError(e.to_string()))?; + current_wait = Duration::from_secs(0); + delay = Duration::from_secs(1); + // Read loop + loop { + match socket.recv_from(&mut buf) { + Ok((len, _)) => { + let mut cursor = std::io::Cursor::new(&buf[..len]); + // TODO: more robust handling of packet parsing errors during resets + let sample = LiftoffPacket::read_be(&mut cursor) + .expect("Failed to read LiftoffPacket"); + let mut data_lock = data_lock.lock().await; + *data_lock = Some(sample); + } + Err(_) => { + if current_wait >= timeout { + return Err(SimulationError::OtherError( + "Bind loop exceeded max wait time".into(), + )); + } + current_wait += delay; + sleep( + delay + + Duration::from_millis( + (rand::random::() * 1000.0) as u64, + ), + ) + .await; + delay = (delay * 2).min(max_retry_delay); + // break; + } + } + } + } + Err(e) => { + return Err(SimulationError::OtherError( + "Bind loop exceeded max wait time".into(), + )); + } + } + } + Ok(()) +} From 72137d1143eb0f8e354f871664c199ecb5e53de3 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sun, 10 Nov 2024 23:11:34 -0800 Subject: [PATCH 06/54] add binrw and tokio for liftoff quad --- Cargo.toml | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 3cfe80bf1..00f780c5e 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" @@ -38,3 +38,5 @@ 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" From 7ab4564aa9ac895bf2c6a5757a43e1f8ee2aa1d3 Mon Sep 17 00:00:00 2001 From: Ryan A Rodriguez Date: Sun, 10 Nov 2024 23:59:41 -0800 Subject: [PATCH 07/54] cyber rc controller wip --- Cargo.toml | 1 + src/lib.rs | 69 ++++++++++++++++++++++++++++++++++++++++++++- src/liftoff_quad.rs | 29 +++++++++++++++++++ src/main.rs | 5 +++- 4 files changed, 102 insertions(+), 2 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 00f780c5e..ce98f1dcc 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -40,3 +40,4 @@ 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", branch = "cyberrc-crate" } diff --git a/src/lib.rs b/src/lib.rs index e1e1cc727..aaa60bb8a 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -94,7 +94,7 @@ impl QuadrotorInterface for Quadrotor { config: &config::Config, thrust: f32, torque: &Vector3, - ) { + ) -> Result<(), SimulationError> { if step_number % (config.simulation.simulation_frequency / config.simulation.control_frequency) == 0 @@ -115,6 +115,7 @@ impl QuadrotorInterface for Quadrotor { self.update_dynamics_with_controls_euler(previous_thrust, &previous_torque); } } + Ok(()) } fn observe(&mut self) -> Result { return Ok(QuadrotorState { @@ -2382,6 +2383,8 @@ pub fn log_data( desired_velocity: &Vector3, measured_accel: &Vector3, measured_gyro: &Vector3, + thrust: f32, + torque: &Vector3, ) -> Result<(), SimulationError> { rec.log( "world/quad/desired_position", @@ -2389,6 +2392,18 @@ pub fn log_data( .with_radii([0.1]) .with_colors([rerun::Color::from_rgb(255, 255, 255)]), )?; + rec.log( + "world/quad/desired_torque", + &rerun::Points3D::new([(torque.x, torque.y, torque.z)]) + .with_radii([0.1]) + .with_colors([rerun::Color::from_rgb(255, 255, 255)]), + )?; + rec.log( + "world/quad/thrust", + &rerun::Points3D::new([(torque.x, torque.y, torque.z)]) + .with_radii([0.2]) + .with_colors([rerun::Color::from_rgb(255, 255, 255)]), + )?; rec.log( "world/quad/base_link", &rerun::Transform3D::from_translation_rotation( @@ -2412,6 +2427,7 @@ pub fn log_data( ("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))?; @@ -2757,6 +2773,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 index 7dba0923c..70c031663 100644 --- a/src/liftoff_quad.rs +++ b/src/liftoff_quad.rs @@ -8,6 +8,17 @@ use std::net::UdpSocket; use std::sync::Arc; use tokio::sync::Mutex; use tokio::time::{sleep, Duration}; +use cyber_rc::{cyberrc, Writer}; +use nalgebra::{Matrix3, Matrix4, Quaternion, Rotation3, SMatrix, UnitQuaternion, Vector3}; +use peng_quad::{quadrotor::MultirotorInterface, SimulationError}; + +#[rustfmt::skip] +const MOTOR_MIXING_MATRIX: Matrix4 = Matrix4::new( + 1.0, 1.0, 1.0, 1.0, + -1.0, 1.0, -1.0, 1.0, + -1.0, -1.0, 1.0, 1.0, + 1.0, -1.0, -1.0, 1.0, +); /// Represents an RC quadrotor /// # Example @@ -19,6 +30,8 @@ use tokio::time::{sleep, Duration}; /// let quadrotor = RCQuad::new(time_step, mass, inertia_matrix); /// ``` pub struct LiftoffQuad { + /// The serial writer to communicate with the quadrotor + pub writer: Writer, /// Current position of the quadrotor in 3D space pub position: Vector3, pub last_position: Vector3, @@ -49,6 +62,22 @@ impl QuadrotorInterface for LiftoffQuad { fn control(&mut self, step_number: usize, config: &Config, thrust: f32, torque: &Vector3) { // TODO: implement control outputs to CyberRC - gamepad mode // todo!("implement control outputs to CyberRC - gamepad mode") +impl MultirotorInterface for LiftoffQuad { + fn control( + &mut self, + step_number: usize, + config: &peng_quad::config::Config, + thrust: f32, + torque: &Vector3, + ) -> Result<(), SimulationError> { + // Given thrust and torque, calculate the control inputs + + let rc_data = control_inputs_to_rc_commands(thrust, torque); + + self.writer + .write(rc_data) + .map_err(|e| SimulationError::OtherError(e.to_string()))?; + Ok(()) } /// Observe the current state of the quadrotor diff --git a/src/main.rs b/src/main.rs index 98f3e5571..e63314a7f 100644 --- a/src/main.rs +++ b/src/main.rs @@ -127,7 +127,7 @@ fn main() -> Result<(), SimulationError> { &quad.angular_velocity, quad.time_step, ); - quad.control(i, &config, thrust, &torque); + let _ = quad.control(i, &config, thrust, &torque); imu.update(quad.time_step)?; let (true_accel, true_gyro) = quad.read_imu()?; let (_measured_accel, _measured_gyro) = imu.read(true_accel, true_gyro)?; @@ -146,6 +146,7 @@ fn main() -> Result<(), SimulationError> { if trajectory.add_point(quad.position) { log_trajectory(rec, &trajectory)?; } + log_joy(rec, thrust, &torque)?; log_data( rec, &quad, @@ -153,6 +154,8 @@ fn main() -> Result<(), SimulationError> { &desired_velocity, &_measured_accel, &_measured_gyro, + thrust, + &torque, )?; if config.render_depth { log_depth_image( From 65ccf859e25d592f3b882ea7054050388257eb6d Mon Sep 17 00:00:00 2001 From: Ryan A Rodriguez Date: Mon, 11 Nov 2024 01:46:40 -0800 Subject: [PATCH 08/54] rc transformers --- src/liftoff_quad.rs | 30 +++++++++++++++++++++++++++++- 1 file changed, 29 insertions(+), 1 deletion(-) diff --git a/src/liftoff_quad.rs b/src/liftoff_quad.rs index 70c031663..d0aecfd4f 100644 --- a/src/liftoff_quad.rs +++ b/src/liftoff_quad.rs @@ -234,6 +234,34 @@ async fn feedback_loop( )); } } +fn scale_to_rc_command(value: f32, min_output: f32, max_output: f32, center: f32) -> i32 { + let scaled_value = value * (max_output - center) + center; + scaled_value.clamp(min_output, max_output) as i32 +} + +fn control_inputs_to_rc_commands(thrust: f32, torque: &Vector3) -> cyberrc::RcData { + let roll_torque = torque[0]; + let pitch_torque = torque[1]; + let yaw_torque = torque[2]; + + // Normalize inputs + let normalized_thrust = normalize(thrust, MIN_THRUST_KG, MAX_THRUST_KG); + let normalized_roll = normalize_torque(roll_torque, TAU_PHI_MAX); + let normalized_pitch = normalize_torque(pitch_torque, TAU_THETA_MAX); + let normalized_yaw = normalize_torque(yaw_torque, TAU_PSI_MAX); + + // Scale to RC commands + let throttle_command = scale_throttle(normalized_thrust); + let aileron_command = scale_control(normalized_roll); + let elevator_command = scale_control(normalized_pitch); + let rudder_command = scale_control(normalized_yaw); + + cyberrc::RcData { + throttle: throttle_command, + aileron: aileron_command, + elevator: elevator_command, + rudder: rudder_command, + arm: 0, + mode: 0, } - Ok(()) } From 91bf6ef240a4c3847b18317e00cc1760c58d9038 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Mon, 11 Nov 2024 01:21:01 -0800 Subject: [PATCH 09/54] add inertia matrix functions onto the quadrotor config --- src/config.rs | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/src/config.rs b/src/config.rs index e1f4c305e..8849081d1 100644 --- a/src/config.rs +++ b/src/config.rs @@ -3,6 +3,11 @@ //! 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 { @@ -88,7 +93,7 @@ pub struct SimulationConfig { pub duration: f32, } -#[derive(serde::Deserialize)] +#[derive(Clone, serde::Deserialize)] /// Configuration for the quadrotor pub struct QuadrotorConfig { /// Mass of the quadrotor in kg @@ -101,6 +106,21 @@ pub struct QuadrotorConfig { pub inertia_matrix: [f32; 9], } +impl QuadrotorConfig { + pub fn inertia_matrix(&self) -> Matrix3 { + Matrix3::from_row_slice(&self.inertia_matrix) + } + + pub fn inverse_inertia_matrix(&self) -> Result, SimulationError> { + Ok(self + .inertia_matrix() + .try_inverse() + .ok_or(SimulationError::NalgebraError( + "Failed to invert inertia matrix".to_string(), + ))?) + } +} + #[derive(serde::Deserialize)] /// Configuration for the PID controller pub struct PIDControllerConfig { From f1d6f360d581513a2229118eec5c6ed32f272dcf Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Mon, 11 Nov 2024 01:23:16 -0800 Subject: [PATCH 10/54] quadrotor state is a state struct --- src/liftoff_quad.rs | 75 ++++++++++++++++++--------------------------- 1 file changed, 29 insertions(+), 46 deletions(-) diff --git a/src/liftoff_quad.rs b/src/liftoff_quad.rs index d0aecfd4f..4dbcc4a99 100644 --- a/src/liftoff_quad.rs +++ b/src/liftoff_quad.rs @@ -20,30 +20,22 @@ const MOTOR_MIXING_MATRIX: Matrix4 = Matrix4::new( 1.0, -1.0, -1.0, 1.0, ); -/// Represents an RC quadrotor +/// Represents a quadrotor in the game Liftoff /// # 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); +/// let quad = LiftoffQuad{ } /// ``` pub struct LiftoffQuad { /// The serial writer to communicate with the quadrotor pub writer: Writer, - /// Current position of the quadrotor in 3D space - pub position: Vector3, - pub last_position: Vector3, - /// Current velocity of the quadrotor - pub velocity: Vector3, - /// Current orientation of the quadrotor - pub orientation: UnitQuaternion, - pub last_orientation: UnitQuaternion, - /// Current angular velocity of the quadrotor - pub angular_velocity: Vector3, + /// The current state of the quadrotor + pub state: QuadrotorState, + /// The last state of the quadrotor + pub last_state: QuadrotorState, /// Simulation time step in seconds pub time_step: f32, + /// Configured physical parameters + pub config: QuadrotorConfig, /// Mass of the quadrotor in kg pub mass: f32, /// Inertia matrix of the quadrotor @@ -86,25 +78,29 @@ impl MultirotorInterface for LiftoffQuad { fn observe(&mut self) -> Result { let mut data_lock = tokio::runtime::Handle::current().block_on(self.shared_data.lock()); while let Some(sample) = data_lock.take() { - self.position = Vector3::from_row_slice(&sample.position); - self.velocity = (self.position - self.last_position) / self.time_step; - self.orientation = sample.attitude_quaternion(); - let delta_q = - UnitQuaternion::new(self.last_orientation * self.last_position.conjugate()); + // update the last state value + self.last_state = self.state.clone(); + // store the sample values into templ variables + let position = Vector3::from_row_slice(&sample.position); + let velocity = (position - self.last_state.position) / self.time_step; + let orientation = sample.attitude_quaternion(); + let delta_q = self.last_state.orientation * orientation.conjugate(); let angle = 2.0 * delta_q.scalar().acos(); - let angular_velocity = angle / self.time_step; let axis = delta_q .axis() .unwrap_or(Unit::new_normalize(Vector3::::zeros())); - self.angular_velocity = axis.scale(angular_velocity); - // Update last values for future velocity and angular velocity calculations - self.last_position = self.position; - self.last_orientation = self.orientation; + let angular_velocity = axis.scale(angle / self.time_step); + self.state = QuadrotorState { + position, + velocity, + orientation, + angular_velocity, + }; return Ok(QuadrotorState { - position: self.position, - velocity: self.velocity, - orientation: self.orientation, - angular_velocity: self.angular_velocity, + position, + velocity, + orientation, + angular_velocity, }); } todo!("implement state feedback from Liftoff UDP") @@ -117,13 +113,6 @@ impl LiftoffQuad { vehicle_config: QuadrotorConfig, liftoff_config: LiftoffConfiguration, ) -> Result { - let inertia_matrix = Matrix3::from_row_slice(&vehicle_config.inertia_matrix); - let inertia_matrix_inv = - inertia_matrix - .try_inverse() - .ok_or(SimulationError::NalgebraError( - "Failed to invert inertia matrix".to_string(), - ))?; let shared_data = Arc::new(Mutex::new(None)); let shared_data_clone = Arc::clone(&shared_data); tokio::spawn(async move { @@ -136,16 +125,10 @@ impl LiftoffQuad { .await; }); Ok(Self { - position: Vector3::zeros(), - last_position: Vector3::zeros(), - velocity: Vector3::zeros(), - orientation: UnitQuaternion::identity(), - last_orientation: UnitQuaternion::identity(), - angular_velocity: Vector3::zeros(), + state: QuadrotorState::default(), + last_state: QuadrotorState::default(), + config: vehicle_config.clone(), time_step, - mass: vehicle_config.mass, - inertia_matrix, - inertia_matrix_inv, previous_thrust: 0.0, previous_torque: Vector3::zeros(), shared_data: Arc::new(Mutex::new(None)), From 8a5bde0e170698889216b425ea3bdec0a9d7345e Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Mon, 11 Nov 2024 01:23:32 -0800 Subject: [PATCH 11/54] quadrotor state derives --- src/quadrotor.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/src/quadrotor.rs b/src/quadrotor.rs index cc521f178..6bcc4c0d2 100644 --- a/src/quadrotor.rs +++ b/src/quadrotor.rs @@ -2,6 +2,7 @@ use crate::config::Config; use crate::SimulationError; use nalgebra::{UnitQuaternion, Vector3}; +#[derive(Clone, Debug, Default)] /// Represents the state of a quadrotor pub struct QuadrotorState { pub position: Vector3, From 2a2d21d3a1615f34177dc07ca8ec192939760aa2 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Mon, 11 Nov 2024 01:31:23 -0800 Subject: [PATCH 12/54] removing unused fields --- src/liftoff_quad.rs | 16 +++------------- 1 file changed, 3 insertions(+), 13 deletions(-) diff --git a/src/liftoff_quad.rs b/src/liftoff_quad.rs index 4dbcc4a99..15b907a96 100644 --- a/src/liftoff_quad.rs +++ b/src/liftoff_quad.rs @@ -1,16 +1,16 @@ use binrw::{binrw, BinRead}; +use cyber_rc::{cyberrc, Writer}; use nalgebra::{zero, Matrix3, Quaternion, Rotation3, SMatrix, Unit, UnitQuaternion, Vector3}; +use nalgebra::{Matrix3, Matrix4, Quaternion, Rotation3, SMatrix, UnitQuaternion, Vector3}; use peng_quad::config::{Config, LiftoffConfiguration, QuadrotorConfig}; use peng_quad::quadrotor::{QuadrotorInterface, QuadrotorState}; use peng_quad::SimulationError; +use peng_quad::{quadrotor::MultirotorInterface, SimulationError}; use rand; use std::net::UdpSocket; use std::sync::Arc; use tokio::sync::Mutex; use tokio::time::{sleep, Duration}; -use cyber_rc::{cyberrc, Writer}; -use nalgebra::{Matrix3, Matrix4, Quaternion, Rotation3, SMatrix, UnitQuaternion, Vector3}; -use peng_quad::{quadrotor::MultirotorInterface, SimulationError}; #[rustfmt::skip] const MOTOR_MIXING_MATRIX: Matrix4 = Matrix4::new( @@ -36,12 +36,6 @@ pub struct LiftoffQuad { pub time_step: f32, /// Configured physical parameters pub config: QuadrotorConfig, - /// 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 @@ -51,10 +45,6 @@ pub struct LiftoffQuad { } impl QuadrotorInterface for LiftoffQuad { - fn control(&mut self, step_number: usize, config: &Config, thrust: f32, torque: &Vector3) { - // TODO: implement control outputs to CyberRC - gamepad mode - // todo!("implement control outputs to CyberRC - gamepad mode") -impl MultirotorInterface for LiftoffQuad { fn control( &mut self, step_number: usize, From eb3be02ff35e0b2e5c410705091f8bc5248051f8 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Mon, 11 Nov 2024 12:54:48 -0800 Subject: [PATCH 13/54] get cyberrc lib pull working --- .gitconfig | 3 +++ Cargo.toml | 3 ++- flake.lock | 12 ++++++------ flake.nix | 7 ++++++- src/liftoff_quad.rs | 4 +++- src/quadrotor.rs | 2 +- 6 files changed, 21 insertions(+), 10 deletions(-) create mode 100644 .gitconfig diff --git a/.gitconfig b/.gitconfig new file mode 100644 index 000000000..d5e0350a5 --- /dev/null +++ b/.gitconfig @@ -0,0 +1,3 @@ +# .gitconfig in your project directory +[url "https://github.com/"] + insteadOf = git@github.com: diff --git a/Cargo.toml b/Cargo.toml index ce98f1dcc..2a9569b78 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -40,4 +40,5 @@ 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", branch = "cyberrc-crate" } +cyber_rc = { git = "https://github.com/friend0/CyberRC.git" } +serialport = "4.6.0" diff --git a/flake.lock b/flake.lock index 3093ae4a0..0ee9be4c1 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 92849a9d9..745ce314c 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/liftoff_quad.rs b/src/liftoff_quad.rs index 15b907a96..ed818a7e1 100644 --- a/src/liftoff_quad.rs +++ b/src/liftoff_quad.rs @@ -1,12 +1,12 @@ use binrw::{binrw, BinRead}; use cyber_rc::{cyberrc, Writer}; -use nalgebra::{zero, Matrix3, Quaternion, Rotation3, SMatrix, Unit, UnitQuaternion, Vector3}; use nalgebra::{Matrix3, Matrix4, Quaternion, Rotation3, SMatrix, UnitQuaternion, Vector3}; use peng_quad::config::{Config, LiftoffConfiguration, QuadrotorConfig}; use peng_quad::quadrotor::{QuadrotorInterface, QuadrotorState}; use peng_quad::SimulationError; use peng_quad::{quadrotor::MultirotorInterface, SimulationError}; use rand; +use serialport::{available_ports, SerialPort, SerialPortBuilder, SerialPortType}; use std::net::UdpSocket; use std::sync::Arc; use tokio::sync::Mutex; @@ -114,7 +114,9 @@ impl LiftoffQuad { ) .await; }); + let writer = Writer("COM3".to_string(), 115200); Ok(Self { + writer: writer, state: QuadrotorState::default(), last_state: QuadrotorState::default(), config: vehicle_config.clone(), diff --git a/src/quadrotor.rs b/src/quadrotor.rs index 6bcc4c0d2..cbc8d3cc2 100644 --- a/src/quadrotor.rs +++ b/src/quadrotor.rs @@ -17,7 +17,7 @@ pub struct QuadrotorState { /// 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 { - fn control(&mut self, step_number: usize, config: &Config, thrust: f32, torque: &Vector3); + fn control(&mut self, step_number: usize, config: &Config, thrust: f32, torque: &Vector3) -> Result<(), SimulationError>; fn observe(&mut self) -> Result; } From f8a42ee96ce9b6a45384366b7a551ee8e3731a3c Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Mon, 11 Nov 2024 15:25:14 -0800 Subject: [PATCH 14/54] add thrust and arm length values to config --- src/config.rs | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/src/config.rs b/src/config.rs index 8849081d1..8082e74d6 100644 --- a/src/config.rs +++ b/src/config.rs @@ -104,6 +104,26 @@ pub struct QuadrotorConfig { 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, + 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, + arm_length_m: 0.150, + yaw_torque_constant: 0.05, + } + } } impl QuadrotorConfig { @@ -119,6 +139,14 @@ impl QuadrotorConfig { "Failed to invert inertia matrix".to_string(), ))?) } + + /// 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; + 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)] From dcef6a04dbb2e87d20e987ab79a9b0be4fb03265 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Mon, 11 Nov 2024 15:25:30 -0800 Subject: [PATCH 15/54] close the loop on writer --- src/liftoff_quad.rs | 49 +++++++++++++++++++++++++++++++++------------ 1 file changed, 36 insertions(+), 13 deletions(-) diff --git a/src/liftoff_quad.rs b/src/liftoff_quad.rs index ed818a7e1..945f291ec 100644 --- a/src/liftoff_quad.rs +++ b/src/liftoff_quad.rs @@ -1,10 +1,9 @@ use binrw::{binrw, BinRead}; use cyber_rc::{cyberrc, Writer}; -use nalgebra::{Matrix3, Matrix4, Quaternion, Rotation3, SMatrix, UnitQuaternion, Vector3}; -use peng_quad::config::{Config, LiftoffConfiguration, QuadrotorConfig}; +use nalgebra::{Matrix4, Quaternion, Unit, UnitQuaternion, Vector3}; +use peng_quad::config::{LiftoffConfiguration, QuadrotorConfig}; use peng_quad::quadrotor::{QuadrotorInterface, QuadrotorState}; use peng_quad::SimulationError; -use peng_quad::{quadrotor::MultirotorInterface, SimulationError}; use rand; use serialport::{available_ports, SerialPort, SerialPortBuilder, SerialPortType}; use std::net::UdpSocket; @@ -53,11 +52,12 @@ impl QuadrotorInterface for LiftoffQuad { torque: &Vector3, ) -> Result<(), SimulationError> { // Given thrust and torque, calculate the control inputs - - let rc_data = control_inputs_to_rc_commands(thrust, torque); - self.writer - .write(rc_data) + .write(control_inputs_to_rc_commands( + config.quadrotor.clone(), + thrust, + torque, + )) .map_err(|e| SimulationError::OtherError(e.to_string()))?; Ok(()) } @@ -114,7 +114,8 @@ impl LiftoffQuad { ) .await; }); - let writer = Writer("COM3".to_string(), 115200); + let writer = Writer::new("COM3".to_string(), 115200) + .map_err(|e| SimulationError::OtherError(e.to_string()))?; Ok(Self { writer: writer, state: QuadrotorState::default(), @@ -209,21 +210,43 @@ async fn feedback_loop( )); } } + } +} + +fn normalize(value: f32, min: f32, max: f32) -> f32 { + (value - min) / (max - min) +} + fn scale_to_rc_command(value: f32, min_output: f32, max_output: f32, center: f32) -> i32 { let scaled_value = value * (max_output - center) + center; scaled_value.clamp(min_output, max_output) as i32 } -fn control_inputs_to_rc_commands(thrust: f32, torque: &Vector3) -> cyberrc::RcData { +fn scale_throttle(thrust: f32) -> i32 { + scale_to_rc_command(thrust, 1000.0, 2000.0, 1500.0) +} + +fn scale_control(value: f32) -> i32 { + scale_to_rc_command(value, 1000.0, 2000.0, 1500.0) +} + +fn control_inputs_to_rc_commands( + quadrotor_config: QuadrotorConfig, + thrust: f32, + torque: &Vector3, +) -> cyberrc::RcData { let roll_torque = torque[0]; let pitch_torque = torque[1]; let yaw_torque = torque[2]; + // TODO: what cases will we have this be non zero + let max_torques = quadrotor_config.max_torques(); + // Normalize inputs - let normalized_thrust = normalize(thrust, MIN_THRUST_KG, MAX_THRUST_KG); - let normalized_roll = normalize_torque(roll_torque, TAU_PHI_MAX); - let normalized_pitch = normalize_torque(pitch_torque, TAU_THETA_MAX); - let normalized_yaw = normalize_torque(yaw_torque, TAU_PSI_MAX); + let normalized_thrust = normalize(thrust, 0.0, quadrotor_config.max_thrust_kg); + let normalized_roll = normalize(roll_torque, -max_torques.0, max_torques.0); + let normalized_pitch = normalize(pitch_torque, -max_torques.1, max_torques.1); + let normalized_yaw = normalize(yaw_torque, -max_torques.2, max_torques.2); // Scale to RC commands let throttle_command = scale_throttle(normalized_thrust); From 11214dcb7cd27ac850437da8f8642fdc98d4d780 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Tue, 12 Nov 2024 02:46:29 -0800 Subject: [PATCH 16/54] latest from upstream --- .gitignore | 2 + CONTRIBUTING.md | 66 ++++-- Cargo.toml | 12 +- README.md | 127 +++++----- config/peng_default_blueprint.rbl | Bin 0 -> 150688 bytes config/quad.yaml | 2 + src/config.rs | 3 + src/lib.rs | 380 ++++++++++++++++++------------ src/main.rs | 70 +++--- 9 files changed, 395 insertions(+), 267 deletions(-) create mode 100644 config/peng_default_blueprint.rbl diff --git a/.gitignore b/.gitignore index 4be8e1d2d..0d9c4ed9c 100644 --- a/.gitignore +++ b/.gitignore @@ -13,6 +13,8 @@ Cargo.lock # MSVC Windows builds of rustc generate these, which store debugging information *.pdb +# Rerun +*.rrd # Added by cargo diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 2fb7d3ff4..9c95be8a3 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -1,41 +1,59 @@ -# Contributing to Peng Project +# 🤝 Contributing -Thank you for your interest in contributing! Please follow these guidelines. +We welcome contributions of all kinds! Here's how you can help: + +## Ways to Contribute + +- 🐛 Bug Reports: Open detailed issues on GitHub +- 💡 Feature Requests: Share ideas for improvements +- 🛠️ Code Contributions: Submit pull requests +- 📚 Documentation: Improve docs and examples +- 🧪 Testing: Add test cases and improve coverage ## Getting Started 1. **Fork the repository**: Click the "Fork" button at the top right of the repository page. -2. **Clone your fork**: - ```sh - git clone https://github.com/makeecat/Peng.git - ``` +2. **Clone your fork**: + ```sh + git clone https://github.com/makeecat/Peng.git + ``` 3. **Navigate to the project directory**: - ```sh - cd Peng - ``` -4. **Build and run the project**: - ```sh - cargo run - ``` + ```sh + cd Peng + ``` +4. **Build and run the project**: + ```sh + cargo run + ``` ## How to Contribute 1. **Create a new branch**: - ```sh - git checkout -b feature/your-feature-name - ``` -2. **Make your changes**. + ```sh + git checkout -b feature/your-feature-name + ``` +2. **Make your changes**. Add tests for your changes in the inline documentation. Document public APIs 3. **Test your changes**. + ```sh + cargo test + cargo fmt + cargo clippy -- -D warnings + ``` 4. **Commit your changes**: - ```sh - git commit -m "Brief description of your changes" - ``` + ```sh + git commit -m "Brief description of your changes" + ``` 5. **Push to your fork**: - ```sh - git push origin feature/your-feature-name - ``` + ```sh + git push origin feature/your-feature-name + ``` 6. **Create a pull request**: Go to the repository on GitHub and click "New Pull Request". ## License -By contributing, you agree that your contributions will be licensed under the [GPL-3.0 License](LICENSE). +By contributing, you agree that your contributions will be licensed under: + +- MIT License (LICENSE-MIT or http://opensource.org/licenses/MIT) +- Apache License, Version 2.0 (LICENSE-APACHE or http://www.apache.org/licenses/LICENSE-2.0) + +Unless you explicitly state otherwise, any contribution intentionally submitted for inclusion in this project shall be dual licensed as above, without any additional terms or conditions. diff --git a/Cargo.toml b/Cargo.toml index 2a9569b78..e00f8ad39 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,9 +1,9 @@ [package] exclude = ["assets/", "CONTRIBUTING.md", "CODE_OF_CONDUCT.md", "SECURITY.md"] name = "peng_quad" -version = "0.5.2" +version = "0.5.3" edition = "2021" -rust-version = "1.76" +rust-version = "1.79" authors = ["Yang Zhou "] description = "Peng is a minimal quadrotor pipeline including quadrotor dynamics, IMU simulation, various trajectory planners, PID controller and depth map rendering." license = "MIT OR Apache-2.0" @@ -27,12 +27,12 @@ opt-level = 3 # Enable a large amount of optimization in the dev profile for dep codegen-units = 1 # Compile the entire crate as one unit. lto = "thin" # Do a second optimization pass over the entire program, including dependencies. [dependencies] -nalgebra = "0.33.0" +nalgebra = "0.33.2" rand = { version = "0.8.5", features = ["rand_chacha"] } rand_distr = "0.4.3" -rerun = "0.19.0" -thiserror = "1.0.63" -serde = { version = "1.0.209", features = ["derive"] } +rerun = "0.19.1" +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" diff --git a/README.md b/README.md index bbbeae71d..bd978351c 100644 --- a/README.md +++ b/README.md @@ -8,17 +8,50 @@ [![dependency status](https://deps.rs/repo/github/makeecat/peng/status.svg)](https://deps.rs/repo/github/makeecat/peng) [![Gitter](https://img.shields.io/gitter/room/peng/peng)](https://app.gitter.im/#/room/#peng:gitter.im) -## What is Peng - -Peng is a minimal quadrotor autonomy framework in Rust. It includes a simulator, controller, and planner, providing a basic framework for simulating quadrotor dynamics and control. -![demo](https://raw.githubusercontent.com/makeecat/Peng/main/assets/Peng_demo.gif) - -## Getting Started +## 🔍 Overview + +Peng is a minimal quadrotor autonomy framework written in Rust that provides real-time dynamics simulation, trajectory planning, and control with modern visualization capabilities. + +[![rerun demo](https://raw.githubusercontent.com/makeecat/Peng/main/assets/Peng_demo.gif)](https://rerun.io/viewer?url=https%3A%2F%2Fyangrobotics.com%2Ffiles%2Fpeng_v0.5.3_demo.rrd) + +## 🎯 Key Features + +- 🚁 **Real-time Simulation** + - High-fidelity quadrotor dynamics with configurable parameters + - IMU and depth sensor simulation + - Optional RK4 integration for accurate dynamics +- 🎮 **Advanced Control** + - PID control for position and attitude with tunable gains + - Integral windup prevention + - Support for different control frequencies +- 📍 **Rich Trajectory Planning** + - Minimum jerk line trajectory planner + - Lissajous curve planner + - Circular trajectory planner + - Obstacle avoidance planner + - Waypoint navigation planner + - Landing planner +- 📊 **Visualization & Debug** + - Real-time 3D visualization via rerun.io + - Depth map rendering + - State telemetry logging + - Configurable logging frequencies +- ⚡ **Performance** + - Memory-safe and Efficient Rust implementation + - Multi-threaded depth rendering + +## 🚀 Getting Started + +### Prerequisites + +- [Rust](https://www.rust-lang.org/tools/install) +- [rerun-cli](https://rerun.io/docs/getting-started/installing-viewer) ### Installation from Crates.io ```bash -cargo install rerun-cli # ensure you installed rerun-cli>=0.19.0 by checking rerun --version +# Install rerun-cli (ensure version >= 0.19.0) +cargo install rerun-cli cargo install peng_quad peng_quad config/quad.yaml ``` @@ -26,63 +59,45 @@ peng_quad config/quad.yaml ### Installation from Source ```bash +# Install rerun-cli (ensure version >= 0.19.0) cargo install rerun-cli -git clone https://github.com/makeecat/Peng.git && cd Peng +git clone https://github.com/makeecat/Peng.git +cd Peng cargo run --release config/quad.yaml ``` -You can configure the simulation through config file, see [quad.yaml](config/quad.yaml) for example. - -Please follow [rerun troubleshooting](https://rerun.io/docs/getting-started/troubleshooting) if you are using Linux or WSL2. - -## Overview - -### Quadrotor Simulator - -Simulates realistic quadrotor dynamics with properties like position, velocity, orientation, angular velocity, mass, and inertia. Includes methods for updating dynamics with control inputs and simulating IMU readings and Depth map rendering. - -### PID Controller +## ⚙️ Configuration -Controls position and attitude with configurable gains for proportional, integral, and derivative terms. Handles both position and attitude control. +- You can configure the simulation through config file, see [quad.yaml](config/quad.yaml) for example. +- Configure simulation parameters such as mass, inertia, and control gains. +- Configure control parameters such as PID gains. +- Configure trajectory planner parameters such as waypoints, obstacles, and trajectory type. +- Configure visualization parameters such as camera intrinsics and depth rendering. -### Trajectory Planners +## 🔧 Troubleshooting -Includes multiple planners: +If you encountered any issue with the rerun: -- Hover Planner -- Minimum Jerk Line Planner -- Lissajous Curve Planner -- Circular Trajectory Planner -- Landing Planner -- Obstacle Avoidance Planner -- Waypoint Planner +1. Verify rerun-cli version matches rerun version in [Cargo.toml](https://github.com/makeecat/Peng/blob/main/Cargo.toml): -### Obstacle Simulation - -Simulates moving obstacles in the environment, with collision detection and avoidance capabilities based on potential field. - -### Data Logging and Visualization +```bash +rerun --version +``` -Logs comprehensive simulation data including quadrotor state, desired positions, IMU readings, and depth map rendering. Visualizes the simulation using the rerun library. +2. For Linux/WSL2 users, consult the [rerun troubleshooting](https://rerun.io/docs/getting-started/troubleshooting). -## Features +## 🗺️ Roadmap -- Realistic quadrotor dynamics simulation -- IMU sensor simulation with configurable noise parameters -- Multiple trajectory planners for diverse flight patterns -- PID controller for position and attitude control -- Obstacle generation and avoidance -- Depth map rendering based on primitives -- Integration with rerun for real-time visualization +- [ ] Wind field and environmental effects +- [ ] Motor dynamics simulation +- [ ] Multi-quadrotor simulation +- [ ] Model Predictive Control (MPC) -## TODO +## 🤝 Contributing -- [ ] Environment Effect simulation such as wind field -- [ ] Add motor speed simulation -- [ ] Multi-quadrotor simulation -- [ ] MPC controller +We welcome contributions of all kinds! Please check out the [Contributing Guidelines](CONTRIBUTING.md) for more details. -## License +## 📄 License Peng is free, open source and permissively licensed! Except where noted (below and/or in individual files), all code in this repository is dual-licensed under either: @@ -93,21 +108,17 @@ Except where noted (below and/or in individual files), all code in this reposito This means you can select the license you prefer! -## Why call it Peng? - -Peng (traditional Chinese: 鵬; simplified Chinese: 鹏; pinyin: péng; Wade–Giles: p'eng) or Dapeng (大鵬) is a giant bird that transforms from a Kun (鯤; 鲲; kūn; k'un) giant fish in Chinese mythology. - -The pipeline is designed to be minimal and for educational purpose. -We chose the name Peng because our pipeline is flexible and can transform to suit different needs, just like the mythical bird. +## 🐦 Why call it Peng? -Reference: https://en.wikipedia.org/wiki/Peng_(mythology) +Peng (鵬/鹏, péng), or Dapeng (大鵬), represents a mythical Chinese bird that transforms from a giant Kun (鯤/鲲) fish. This name reflects our framework's adaptability and transformative capabilities. -## Blog posts +## 📝 Blog Posts - [Peng #1: Minimal quadrotor pipeline in Rust](https://yangrobotics.com/peng-1-minimal-quadrotor-pipeline-in-rust) - [Peng #2: Error Handling, Configuration System and Obstacle Avoidance Planner](https://yangrobotics.com/peng-2-error-handling-configuration-system-and-obstacle-avoidance-planner) +- [Peng #3: Optimization of Depth Rendering and RK4-based Dynamics Update](https://yangrobotics.com/peng-3-optimization-of-depth-rendering-and-rk4-based-dynamics-update) -## Citation +## 📚 Citation If you use this project in your research or work, please cite it as follows: diff --git a/config/peng_default_blueprint.rbl b/config/peng_default_blueprint.rbl new file mode 100644 index 0000000000000000000000000000000000000000..7d7445f40d52d7912965f4de335198d759f75b30 GIT binary patch literal 150688 zcmeFa2V4}_`u~4scY$rl!hoWn;EG*jfu$+xs({#0QL&4#D2jl9y$;a2bldr+x;~VfzR9r&5 zIl4nkQbPA&CVLp@7v=94WA^iNH~ahfyBmFs(e9Dvm?(G803Wkoq^G}^r@`=TY;uQ$ zn3&k8*vPmJ=01_#6XVRgmM5n~CZ(98k2%V#*V%sgtRa0h%eP2Q=$RB{9*$CA9ZEgbOzXyMl}0vc4^~M7{`PDV4h1iDWXz3uDw(z%W4?^Z*aL+PR!A}@ z^^C8KGnSk{r^mopAYV;D2Bu%}pTQn8ZEvNdCEnJG5K++Ad**C@7Q4&KqsqMYoJOXaA9?un?6@hQob zTkyKl)QqC;=6Jjzs(ol=au-HBy8`btQ6a+>Xd+|SKrxwUKay3|F`28(CLdR7zl=>~ z+I(h{h4zOFB4ajTGTL>9oqWt}1vi`^)}|Z(+Z2C z65>;m65_TonNztO!9k1@o&16EDxt#@Ikxxm$8jZmMXV6j14CAt~)|t z5ej`5;kpdhQ)gPqgu*hsT@yLeGI5=Z=@m_G;xt&6VBU-!cVjXcYof*qiXfSt#+KRO zq=<$xsLC>>j45;(J^;N zeM2UfvGpw){k*+Sb-Cl&)#cyFRWddu z_wDtWgLAKIZz`F1SIP8+&^l=AqLY%Tsxiw~Ojw}6TPfs%{Fxvq=x>%L!+u-P1wtc9 zm$O6*m!fx>0)@i60PWD%SL`ZZyl%EsbdYs;m{uvS+M$>ADm1#W?$LmbPxZE=Pt5M{ z`tI4r>K$|AGKy9{F?3e<7;97WK93ykIX|ple`kDti6ffl+BM~vg*J?7{h1<_*_c>q z#TKTW!4w&EinJY>Li#(6*|-S;-dUB3)BFzCndKrx_*(xpQLE1OpC7j_5VhW3`R%AB z?A5NLebS+R{(jV=8vPeXEx%ep!66|=pPC;9qK&qghOFR$N-T27%GS$=6;cIJ_s z7#U^mK#|MCKguh{80{6{?&%ke&mzxG-tGa>oub{nJiVf#qGJpOf3yE^=3v`}J+l8O zwlPa=DHLZX4Z(5_s@67~iX7wnNJqhA4hRRPk0MfwyF zzuB0Y`^eb}Ig4Pdg2 z2BNDJZ4FFlD5uo}Xm!z~W6W7jtE3iK#VNy;LbGxdV@+jBtil7Xk0O*Ql(Zp4-)bmJ zT>Y3w-o_LN^@&%d$ZA8uNw3YKP+!s}h`A${xobQqf)?}e3i4pZ@R!UcgSjgl<+RQf zkKPGManT+TJ*N)2d*cyY_o= zPWhbTTo;Rf7w5wMmN*w;@bmWx_$1DGhkdSh=Hu!ATX8M|ajq=t;-8Ll9?cBw38UCh z8+{V$A)!y#hkaWh?o|?|Ohl|Zj?bpGhw$%+_{%P49c zl(8+c0#VBoU-T--nX}?wqE;n2MXjldkE2$ig?@{WA!mJ9$qb#56^`O-wWt4|I)!j+$!{^;#RDpVBA7WmrWPYX-q}%`4Fs7h<>G}EVJp%EG!Ut z49rIvipbL^pZa`fDS+rxu1k^U6@{D<%5fy6dF0v7%H*O(S9Ent;2&Ea71~(PD=51B z5>`$ito(J|rw4JYU|9L-_rl6QtNRq<>08@i_#~|OeKxH48-6pae75`K)hRm0E5OI- z?&}xrgYJ`el)HbFzo)y8r;oQWGAhdKWr+FGefrXU`hTGNq^R@RzLUblzpwso_bEK? z0R^%5-KV@i(0%eQ*nL93s=SNGWb~zE%*iJ8@9r->nBA^mV7v8uf$bmGU-As7$cVEeQEr9TqbzVw&A^q1&{*x%V-8a11Tn)m&s+y7C2sU5!dJpW5>Hz_WaSyAn?+%m6KGbF@2 z$j3hzU7P^pX9G)s!SGvO#6H_&!p)^W|*dq)|4 zeg57aQ>%}@hyAG=PA&fXH=J7kyBkjQW$pdlrV;<^yG?!n;=4_{jL+O|djF-(>$h$; zF(bZ!vERPmBx@kU9eG>9lF2izTp0_Z)#XK4DVC~YvOFt%bNl5slf^z6`%w@oC>DwLs9W1a)R@e;PhkE~=FN`_S7eM&b_dO{3_NQZI*51a0IpbwA7Q`?JfWL}kFbU%R zOF>CRIer6cNH~T_o0*eh*Q>GI6$4G5td`AJOYop?HgQ#0Nepq&t;;2Hn~rkYHES1( zlFS-^Xn*-q`q$p;df3BUu>pro_c8i-A#KGc(!G;kOec&S`gV%(j*9XM@bmn8dv0Hb3O|3x{r~4f zg@M%B`OHuu6J+S`@u9*X-dRg6*jdZqowa{ERJdv`4`T1{bv^u@dtHhZbVIA;FP$n+ z>QuQE7%zM$^N2Pl$0i{+EHNQDma=bxnd{#_VEAy)^@7pv(eFjOe_AK1wlUcFNwoX# z3>f~AX!oTP^`#T_cXy%+?Jd}edi+oBMBT1XFswZJy|D66>qG_D^!g;yn+CEx0QpKtY-cA^H} zF4&2B`d{iq^)kmLL?Qe7f6|G1RA_#|X!q>*qTN5N6Xk6P4hZ?A6ZQG@h(CKL>W@Ub zFP*3_ov6RN6Lp}_qXJo1&;O%N6cTO!vQflRm7GFG@~|R}A~t(jFsyt!ig@gwG>Yi$ zRokp@s1adbB;NehJ8~B!-z+o zy{lO2*I~q$QsC1*e?N@)qS>dzh{G_9cAL++%KVoEFzHu2|F8rwnWYSwW51pM z)qwx{=MUz8ZRsm@+}IB_GAg6vR`XtnygCYV>&6-iL_xA4prQTaW%>e&TZoK_>Qo#O5 zkp0qo`_g;+yL)eUXZ_lHD;52Y&tUO?{Iz55Uz!2-=*Q*-qRKnr_oB)_Z1B>%wx`d> ziKQWi-^c*-`mHH$K0jH6r`ZtTZSXbXhMO6)9r;FkV&WqF+R<$EL24K#j0gz$n{)TS zWPp9i07DA@r;|nefx*jso+S2v$pBmNBadP4Gr(k05c}+85&t#=Y`({b#II`nj@aLy z`}OF`go1HS{(EumpEhh6?CJmUy{{0%e<%0rkHonzy{9j|r@y=R^!=4zdr$fl#Xq?F z^jPs-!LVZSdtv1t)_pP>jJ5ne`cf8R_>J7JKiPfq?iA^VDYXLJBO|?|-HqPH0QXM* zoiJ$W8|7=j?;fKKK7aF_uP@!FFWslVyZdxS@wmWb5tjd1_vu5**Z->f^w@1)!8m92 zdvWd`)_pR183R6<3DxlVJN$ok=G!rjK z1OJUFmW@FnwQKqK`S|&U1bcgW*YXRh6@=NAJq_OeH9zV=u>qL(a{$v>G++iy9}&$Q z98&*vp5;J>S=*FppPy$rpB0#t5Bo1Su}B;pCeXBhKLhD=lw!6wJ_D)lqgna3@LBm{ z1cg!1{yHgNwlvjp`Cq46#uR;QIG<`+x1_+de4;P|KlW!P%%RJ!GyLT$!#}pR>oB_} zCM=C(CYp$O0i&tvM$*~k6EWLidP*i!$O<`2Gcs3Z4&~CBMK4*ty5NkZO%oDQX!60x z*m!diCQq#-G^6PSCx*&obC@S)VV=cA`&rCZw~nzr0-MNe@-S&1X6Lh(<4CobIoiNz z;?b*25iNT^f8W#(lQQc~EKXo*8OxMm`e=(YOuh}7M$ZLtLEdNQV`kxkavS?FdEf8N z$Sk~{z0W07E5py|1Vz5U^7s_Y4P?cXCYtH>{VdD|J`3|6nuWQ*^b0f_v$!Hqz@N%QD#&O$ zMX`hCWbVAleW#LT)IWK4e_4#j`8Ph3sbnTg&kF}2-Ct%Ogji@V9D=+_KYbXIvEk$q zNZZ(~qma^;Cyqf@1s^#MSGms9Ju2=Wc9hN7a)UH{*VJvJ=uH_vVPa5OOV#_n=V6Sl{e->4liGK z1){II@hW7Gffxn4k`RN{RU*m#&paiZK~$5>?erceOVsl{=0>@Aj>x`zYTG5 zS#$?d?a2bnH*LBbGV30s#&&)D(PW|d9 zztgb{7x@5I4yQl4Xrh49syVAMkE`js?=zKG;j9R9Waqqd#JG|%{%tfz-dp! zY9ZsE^tXl#o|;?;QglO{4aDJEcg&h?>Yvfo7NTAfV+YxJ$t*(Z7wcdTIXa@91H`sY zo5GN@^I8^xIKGZ>glzt*RZ++ZzwlxZ%emp0e%rLEVKZk)ncVutA&VmGm4N8qhPpuN zUkP=EG*1gI2}zq7q=%HZtW^pU;8ep6a;;4D(vaaZtCWG%Dd$@j^2B4bJEUv}>&iIF z{`<$kyqZ+m14mi0v9|%vv(X$sNY^Gl{#awKcLq#yH{SrPF=be(DsZmrovT7lCAkDb zjvXpe4dUCYXmyC{pjZR4CrLCx{7$KBLS|OCs|DG1LR%Y>-$@w+X);(F3>kf22!ZUa zC)9zA+Aa@;EY(`ph1@7D)Wbd;-uwoQMqMSXjyJAa>WDjYVMiT0IK>ECa?FC6Sk)N! z6zwG6`bvd1yM0h>oA9cYKXmBs?}`LK>)$Wj56dmaZS^o#g|Dn{5*i4-IOF;hY|SpY zueloRv-^`{u(bvIdUuAj4tTkNHkx5hJWvBm7dQGg%LJ`htn1!dP-Lv^!)k@|hVO@r zEj@r*bI`~>)SAn_N~YF4c33>syfH13sWpe^bjMzrOp315n$Pa-Os%SYks)4DYa%<Whh%nY zM6J2esQT2JmlkPo5z=#XD7EGmT^dqrZmtcc)@)a-))mO5pxV@$?_a7(t$Fv$s??fq zO!1-Ce5^%@n~*B|oPUA@FDgN;c}j0@YR#-+F>1}5t~pU_4qNU;t$DD2k$aGwb`JL; z_0%F7wQ2L(BKZ)n3WcdT_qt|B&3VBNTWZd)v^I|+<9^gqtDfFaO|5$M2JJIQgLEx5 zZXs6v0#dY!?@P!7(=uw~o3B(roiH8WGWxBOtzWsN9;=P(VcPx;3UhZ00-AjF9avYVP4S)0oB^tg-)nBHDV1Ll$J&!Bj z!EBlKOpA?l$@`v~y1r}`G3dhL~oGD8AVP653{?0b+G}h9{)>_H|y6wOQ-DAzs0& zjgYZ>R{21#bo#~@vV8X<@iKpggrtOB{Ze?e8q^1Q(m@!k#{Pz|zl z{m|-=CP@owKz5a$ND*&A?ZFiBv>j(t#9MS?Dn&e>wDA=2T&$;3#PcaRDFl*wdUPGg znPS63A+INmuL~)cI;!@;EuE8N@KJcR1whq@?DM1*W(bkp86;T0+*O$45Z6hbFXwJUiK~HKdCW*#=T> zN4>U?;7WDdK?WYE*B(+u6WalD{83a#h^|cANXVNWk)0rcrWR3)pMD@})7Sd;!u`8t04xet2TmHUr5HUZjJLK@qdhw7c3%n8_g>TnM zgpA9s*Mkc01SUa#o=`0r(yC0U6iA)6WqU&QH1_WWDH>J2H{^5~zdn%p!vgw3mh|=S z2Qgl9>JKqpEIa^Wf6HMYWN#CXL6B=390xe9zT%cyz^_#BH+_bPuC81xRnpsZ|eLUfG2EFd;i($@g@byaRCYOd< z%u;$`LNmvv(_8E-3vE$!T$CBQspQYOHs_48^p5n6Vo9Mv%2 zy2zVi_f}n2INnqk<{yAQiQ84^5cJH=I%q4H(wn(WQA^+OwS|>z%JEUsOljxvfRwA? z4tNZ3MfH|x1|}l3ijDw>E>-}DHX=fXh;}cc@ru~0*b$&Dh^WvaYNd#(C8B;#$M1UL__fxF-dc#Ai+8j~kIAEqb@^q?Xzf@&ZH zAU25+AQIpli)gPR8l8xiBcl3?sJ-F>fO;fu06%~|;0QPaE`vPq0KCK-%E#g;wJ;8# z1Sks(AOO??^+7mj2Vww%hu8-U0b>A~l8E*oqEd@%z;^(ZNjv~ff(zg}xJRdST()!# zol!tXRvk0}v|+kdXsmogbZw3SdLdnlLgN#eSPF*DJ-vVqoer4}l@5^(4Tq%HeibGi z0_{5OGVLnuBJCRO5^eDvRxCCFEBe5Mfo@f_K#b5_G2OeWZo2l4Z1(;ya5zzUY4%P#z)2PdO*|bop?$UW{2B?mX21@|d zMXH0SdwSh;Y>lo+2%x$(0DKKL0IEB8u#)B1p;FTeL_tfC1jd6EU^ln~=yKW5z}7rq z>H#xI1+&3sZ~~yp>#b&nN>!Yyb_7TURE1Z9J%FkzRZ)jo*cw&I`haQ*RlPajJ8%+E z)l$w5m1+o8sa7BbOaQCEUXTl3Va398ur*JZ2B0$-3g&_>;1sxzl~i*>ndk;n1GEM` z0aea#z&>yVyvB+}=3#4IFbzQ$FbvEC--9ghGgea14`pI$7!zm%dVxtG9qb2J!5ggT zxBy%8hG_(1!Ei7iYz3!5K338!43%P0O%${Ry}@L#8f1cN;4M}xx(Hh{!ZZe5!3eMb z`~c2?2UtnFIP?!Tm?hYn4_*}px`B~kA=n1af`{0e_0rHk&|sEfB`3VX7c>EJU=;Y5 z2D2P*`EMJ{iqPL}FjJhHO23!$!#gwuJpf(GO#qisuWaToo%Hr77z?N(+`t0oaD#MR zsrKRu=x>`{nSL7889L2$iaopFabD3%_DI%5yFY)8V{zMx{yLA-7rx_dXsVLg47PR? z2Do6VE8NRtHF0xHW62y?gtMZev#yW>lXonEWio#kS&%EOWGAg8S~A&j+=^s62dSX2 zCEYVDV!`BX=>@u?QgJa$X3@>rl{s7UZ5QWjm5`RXU>nc*JC~FS^n7V4$+&S-S~6w0 zDJz+B+?1D01#T)zrV=;qlBsNo8+h!3v(6yBw<Zu@cXF@S4^af z6PPS5Na3cZWO{MaTQYsP=_{Fj-1L{s0B#0KW)L@nC6mexew|DQHj8Qk$?W20w`BHkvsW_v zxY>`>oLPph%K@qQATK^7nZw*1k<3wUj!EV?Hzy=>lABYK$>Qd;WX^DNRx;V#oRiFX zZZ1eBhntI%xx~$7>{vd(GFPPHtGxJ{WUg~_LozqH`AIT)+}x7PZEo&hQ-}F6-jj;& z^WvW+lh4gV$voobv1Fcb^Heg=ESW1~<;v2jdLb3Pb>1TA(%v0>K~z)B&NOE~p3Ug9d;)P}J?A&I)xgsAE7M*UdmU zXbxI{mLLL9AFnlN1E?3&4$vof2hb5jf=(a`;1im6Lt~)y@!bW)g07$&pbkiP5DyYS zBA{+U5=aIqpeN`BdV@ZoFX#vQg8^V57z73b>M#reL%}dG9E<=X!6=;3w&m!Ij>Z`s zgOage92gJ20u#VQFbPZsQvkXW{Hwxr=+|Hdm}_Jd4t02~B| zz+rF%90w=BNst8|fW#MCe&nAXH67{uqr_mIJDb259=JTUf~Nw*hVHC!d0VS>6)~8q z+ErP$z*^MKux3-OMVoQfEX|q?w`Qrd7OrJsTGl|z zLbWVNi@T95P|N(a%&27st=OfUmbqz}tCl%ynWL83YsCbumMOJj64Qz)&o%6^hUIJ6 zU5(iHx`yRySdK;P25ScHa! zYgm|uHPEn74GYpRlZFLqn7@V@HO!!4l{DgdHw|;uFlP;O)G&Ju(`lF%Pl47jg$6xX z_FB!JtJz~U%U83zYL=%Kcjc;Cj+$kwS(cg|SBsgMYPLs>Q6;ub&9SIY^s_~RI_nvmZoOI)ht!b2B=vdHA_*8m*dppm1wnit*x3xs9Ct0 zg{fHsH49aXw@qrM^kn{O@xDRLDyel3620W*)XYuIT-D53%^dLzU^Ua>Db-jJkFHkZ zG1j`by2>m-VJGCP*j*LNQ?ctRy@imYV%aK|rP8Z}!zz}kVtZ72Yhjy;ZBem}D!r|c zu3{@xY>7(mAk0y*87eka#U`rQI2B7%vEeFu9>D+=>!V^RDwe2XaVpkD#iCWLql&dv zu?Q6lSFtb^YoKDGDi)-|y;>HiV*V;-R562!RZ_8XD(0qQt}5oNVvZ_iuVOltzKWn! zF@*|EioI5{=Snr3smHGt1sAL8utEAK?3vNp0s$|YeeQ&{D$#hD6KS7Cs zdjvV9evt6oiaoYs`BwU&LY@`7ZpCt~SdJCTwqjXU?6?&>Y{fFI*d8nWcww6r+hWBw zTCsIjEZvH&uwqNB*a9my$BNCcVpFZyL@PGVilte-b(teDHW$nT^T7hJ5G(?V!4j|( zECb7hP!FYyy%U;wulcF*_2Ahv4@|t(Wc8RQI!4cO`TT#qe|a!MT^Gz7g(Q-{CWEMn z&DMe$M00;XgUHCAK9h)ui~oNogUE!#{^W7TnRr1O_-|$qc^X1$)%-YvC?F`Tz+*rQ zWDt$;@(jSE;MwO>h@64~*^@FJSnfg38Ruy=9`tCNNHaZsCdQ24viSsbat{cI3UCiF zm;-z~BcuHzV|uNsyAW5rQ`7mj{2oMvf6& z!(SgbgXS)@;$$ov}r>o$JCw?xF9^-xz-*UF$6*B2j#dD;zArn2uy@ssDho`tF;(?zX|Lv2C z&;9Zg_i&l^E`N&qO@ST$@Dz83A|w*w-#P#KEcZ%0i)hX#Swwy4aqc-Xde~=b<$`HM zD`c#e!bE9Af&3A{$3My<`tUS&y~(;Pg%_m#P!^GrAvvfR%?l;v6zMa{I=)|~Ut(#; z6j|gjOp7zdDl$b5SxO*6FGk3>sB^p0PD&iwQt33$7-}`@bSA=FLehD3m`tl3dB)KA z(&y1_s+o5$3qjU!^#uA(ra51oItuAu@AxrD>(PggLuRZvg8q~#wB8|fsZ3ur-+v0y z>zn;qkO2$!o`z&r;t4~i#_c)_SNPHn1aQ;fSKH4)F7Mxd9^zDzCk$mh-ir8cT2q=Q z3~jaG2}4UZZoG`bA_?E-LfT(ldj)c<-?vvG<2s~YgS>dT`Z}am2u~OqA>#={Z=;s} zgu)3!m*zpbRObmp?H(+<4HwmY;T=eOBTpEzn=O3=q%ngrRRbB?u_J@H`#Y)l_x;eteRc67~eCFs!%v#&(#MwG$~} zC_ApN1}@}T7cC^;i6;zMuTCli*ZWX68;G?vPZ(O67i$YwHq&eeX)ogmL(`kK$7hvk z^^a}wwaRpEaO=X5CTCg{fq2#C2}8*nBZ|Tu>)EUrEz4=<1PL70)EV;5lP3&iE~|@g zT&D0)o-j1A7*7})aw@1K3hSM!t%nq9T(cB}b*k4P~{RFmB^~C}HSEy1okB z)oV^wAw9k+5eO;!xNtQ{$YMu)9W(8GW>*99^*TFzc$9q2;5k zg5cV$*5Esu$;rSIhTcqJb>QMGdBRYm2#dOKb$YRS*oSS3H%gZA=G;i0FjU6v_FULN z!vUT!R8P+nhGr z??YrW?S6*r>&O#^dUti8=B(~*`w+6zT}REi=NzJ}ux)qweiTUw#p`lZ)(BM65rP4M+0(iR6 zA}gLQbkHS0hec_{5UD&jm81A(URRvbg^HgW1ao3u5eEbu-^2?rTP#jdy3h@)d(_tR zjvRG_)OO4<+DEZ06}go0~r8=|U|>y|zH< znqH2y-`~Vgnv-YD@pmvcU%yu2b=^ZPX}4p(qVqQQ`OlOtfEli>@9PAw3QpqhM8TV;%*oD;qCSu&PpsXA5<^oWrw)DlWN5 z(dvW^&lb{5=h;HTEP1w29oezsSZ4inKOMs1YCK!$TA59*a6fkD*+TE0ZlY)vF>WrBHWpBJcrlE z#ffBt8sOq&3~z|Clrh5^LDskrYz!H)cVHN#x11*njUJ!W6i#T{qZ!1{u~#_c`K2Ds zAuH^=wSXKb5#JJ$*McVttv?dq3a&vFo-E|HqkS7Vc@$3;I)QuT>CTgddN=az2j}9>lZBKs9R|RycH+rGI>wWQ zs)iLAjKX6NY*Ha-Jb1ED*b&`OII9SrEL3Ptq2X|^Uu#D|TEDU$2?=bc83l14sY-)% z4dcl|=ek*rf%|%saxCP*LY^$tucu->nxrv;nyEhQ^;c+;ss67KJWMvrpHGCW9L1A` z%Cvej8O|?)CkyqAdovYod;(7vy7=|e>2N{2pL`A3uX-^9vg*;JnULT$FJ?g=7Jf1t z^3#Bab09rp?#_iwT$?u!@}kbo`H(hKc(PFck~~=`swhtuTKeSTVl3Mea$yPN<;dKn zkj5>}FM}xGKkq-aGXTWK|4L7TUBgnkNf2Se!`hJpJc( z)GP5Dt)q(h#CIa?Oj=draeSDTo;_?NPZn}oS%@bK4PTJVlZEOGyT+4+I+u}>g^CWG z&Xa|5R-ekC*PZAxgeMDa_^A+27P>IsiYE*8R)_Osp~{2D@MIyqt%@fLRX!vo3(Xn1 zkS7Zz|KQ1!g|=Q5P!02~HARc^WT9rAEAwQbXr~`}vXK9`A!sX)WOnZpg$}<@62b@Y zzvk>vl8^=q>_Kr*1|R_awK+SZ^Zw?XoiTr3&W`vLXAf;fuN;e`#HSM zGZ1-yNkBm351A_pVh$zJ0jx z^p^Pi;R7=CmUQ8tV?~jYg!C}Mpe;xSv%q?g34X##mh(fUkYz+ceGm^u06K3}&FQ>7 z!HP;s67qlv0aSZa!33}woB(v*tQLk!Ej0iI5r8iI7_bm*0hhsjtY}S1LUgHW0J`vf z0bO`%F#7;qc;({IUz4loyrHMS2euYL_kt`y=gp3igxp~ofDWJ^muLmLlomVNThGGF zNn@WWD42zUGiD?Sjpz)w2hb@lO-Vwb^bzLsK1oP?pCm-bg#*{)UKF#_STXq+XC!G@ zOQXt#kSVjJIPP1?;#^T`E0x;uv1ZYN3AIY#amPXy&!s6WEiA%EtsOsP2;sgZJLF{R zRESbPoTXJhO%c*Frgg#MlG0*5-+U>_xN%cjGG(|aYeCy$IkaE8^3swDd`U&gRN}^6 zGL^aUkc@#FPsw<3<1HB@H$Kv)b9o+=9|rNm|74m_P3)M7Z>pxW-CEq#mbP1|B<*sr zv?PQtsUw+CZt6;=9yj$R(}0_Xl4-B>zv$;5HfT{7|9BuFNa zn;w!$;wD)#DctmwOfPPFOQsJueI?V6oBom+z|BC(4B}?6WPT%8D2rd5(b76&_zT8L zW*j%;CG!YbCReoAr{} zz|FUk*~rZ%$!zB4JIQR}=6lI(<>m*;Y~yCTWOi_~Q!;-#Rp@zXYBL9GWnt0l2*FSUvWn=ce%MInfu)QESY?6{z$G6V&Ze@H81#UUP|T_ zH?Jl0hMTvjC5co^>{!t<%nDDqr)Sp7@IfF43Sa>&ffY~!6;J~W&;n~v2-pA}umyHN z1oprI6b40rBPa@r0Vm)Lih~ls1-OEefI26ofEy?c=tGx23d;fN22}tRK_x&R@09^{ zL8#;43A})}lCc!ZyZSS!LM)rVqNeUkvd~bR!(jj&f`Z9Hyhk}2Wn;itFb<3dUjgd& zO$5~6p^niMFcnM#(*bpFW`LOhos<7Ts*p6m{_QUV>PAf1(DjY%FB(v9S~9%gfcm_G z1M1}$r}Rx+(ahYVr#U{#d_*>IA2N}1Y5$I+=Kk)GdJkzx9aA{iYTcNv(#R%dqO*|( zFrxmz68Qnx3?_JBTA;+g3a0dB53pUvY(SDZsb_rU*yIk$2}vpDXajTdH?XJ0T(QX0 z-|%5h4lgfHALAz@>cLorDZ^N)Xg-|YfSty>G0y;Rqo>bbGM>J^z;L=&c!UGe;vE|x zY{lUmrOFwbA!BR{M%Z%#g$KIZIPewNqd6cP&`A)qcC0W?fjx7mCt!vM-8?}NCusB8 zP&&)ektva@1jRVkyjyI1H1lUu?_wL6l!LiOv)y`FsG#QuEU>gOS_q9( zV!NB6HpeZCE7C0WCYES{n5$A)1tU9%Rj{yQGlWi5Lydy7MF-lB^)}|ZoyPkqodN^? z`4(-cf|kp&d_U&@uuyX4@aA=caEXvr-?VAtmQ~DsaJb2Mx^v7V&kJstc+;2MDXblpFG3T!9Ld#lZMp9`K>!b z=z;fq=P$=(COzm9XXMLfFkAaf)Q$ftjieo(yBF?j(0+r%s|(E(1bl$T&_*I}*70vQVi=CVJP*(+u2 z0i8wuUkMvGcb=|Lthbb|Am4X7lO{TOviAG|(%{JKgQSPz&mSV~Qt9+z(tf2+9wE&= zbLuE*c-rw}q^kXgkCS$|b>swTlTn9Gk{+43{}kz_oBOj!`(ECAnzT{Zj5DOm_v|`L z>f*FBo3y>>j&r2L*pBn07u#&TK)RyR4>_a>oi|@3EnIukCDK~=H(n;Km;P-oX(88j zS4eMu|Ls-Mmow9^k*0dCxlUTHfBFs57gf`5lD?X~^e57&txNMr_x4|Si*&VN`EAmj zix=J@y`8w=F6rh&Gw+dJtLOAH>5X<1xK?tRmQQ}|lhg;Ksm7rXNq23@eMDNnYvU)R zGfMvWl=S#C&*!A6ZA!f)z1VC2E7DGhZ=u`W8Uspl3Lmat4MRac5r<=AyG{}>bt%g(pObtwWJL?_O~W&`8cT% z=}3n-8`8|i-F2kfD|EFb9j1-3Bdr!<7D=5awzntU^}4MCX~T@xg-OHhTNWWb(?7zI z^x=z$qNIbqZB~r*qGPxd>AKxbok{z4tzVqf^J?7^q#;8?T}Z!c73xa5&pNmy>3zE( zJ*m&Anx#l5&#mr8>bt&LY0}y5Rmzao+2B)_w0(~>chbbI+RCH{J;rbid*SXuUOeVy zAl+-8?MFIyywRU@eA-T~r;hsskiVC&uR=PnjB`~|hdU(#NzeEfsYW{Zrek%|ZN6d+ z(u4QxOr)1usB4n$SZ!O2)Tx=aHtD?MRzaiFE(taj29m@qLbzEHSM7 zW}Yl@cvj(lG|NiWqLq2F#EG6#vP7$`SEtY{D>v6k*%EW+Opc*xR&o=1cP8DP{9*&? z1X0SDh;Ou>=Sw^(+lA*#D30~tM-#2AzdCF`>7k?jGfA75?{k3k+J@wVq+_SY9U@)4 zKj|>(^%HSNNE>g9JxVHD7jul%Wlhv^(sz}kPmo^mZhw;Wahu3fq$yL|W|3+#+MXu8 zl-T+VsmrvcXGufWHpwQfGOyt|(#Re4&y$vLYjlD1ap`(Fq=lN+zeqZ$W#}c+#61lz zlUmIQ$|arLvgQ@iQmWclNt;E~yha*3uIhEt3Rk>ukj{8p{3hw~^3Fez)(a?+NBYFq z`xfc*yG3u4YQ8Uaht#Ew=Uvio>GKB>6G?q|}M$wl%>8}xB_K)Q0J?L$&k zyzUXH|1ixn(l$Lb&q-DLl`lv)uJUVdSg~?BJzE_0w;JKrYq_@f+DoVPf>>?-9z;TYwq)rh#x%POn zqd58L^?xWq+AZS;7t*I)rHqXG*I!%E>?ydPYehP8h*C+K{?j|I^$NdHk?+{SQbSsN z#dEGtwE0@{RchU}Caux!Ng>jh8`o_}4;RkTk!IaIW=mRa;sHC-oevL-q;<|7vnNf> z%yS?ef8bJK(r-%S79l;8FXd?*`u1W`@+Dj@6eFFo^1Ku2g<_|jN#jG06(?S{B23nr52m@q(f${E=4*ZcC{Pnv!3atNnNtnlpzg!cCajItp^9o zkXm!$nJM(LhUb-;BMCx&JP)$(i|TsoXQZC8>T; zd<1E`3-PT;TZVLNO}e5|hc=`gC)aCB+U0O)JJM?F>b57Hc)m*q(w}slI+9Mi*Cvwm zYW2uYq=Dhhqex|aTSSxIei3FS?X@x>hEzP|-uSY1xla`Eq$92WX*I$u8oA7!9spIwM6G;zGe>sWND(=l> zzP$5`DWr40c{7!C-jHY0Nbj6~I-T?&d-^r$<3=xLkk$%(Jd^ZP#*0~`?aDowO}cC1 z!#SjBz3I7RmLDkd`#{jQ#s8lB4glNY3J}ST48)9)edG3{<2^)6meJ zR=Sf)cS`9_DBbC-0&0T>pgEu+QyT0`1T=I?ck$>h9o>bS3zmVkfW};Z1P1}WKJui6 z8_@gU8NOIDnr((|_1XctRqF=mR;w?l0Wg5be-qgn8U^A&PcR6K0(2XYZu2by-++x^ zJJ<(~foyOE+y;-pYrLt&m^|rTauF2Jyjh{;I|wucEkFm*1@r*@0Ntmg`;c^> zaUNI>)`9QAE^r8Bfs5cK_!&IM8wz7_lxi3e(9LMN*-ST+{eTJ71x-O45DmJ6USKdt z1Czikuo$F+O<)Ju4{#fg&yI2xdWTLaC6Ul6rE5q>76|Hqwt$X=?x4;FbgK4(OMtG0 zmS&qN4nyajE;hY@4tYADL!?8aLwbvqOHv|kx^; zK41#i3J!ogtfZM3`f;*GZ!DmFTnV;;%it+ibf82MBTRkJ9#F-b3cdv=0llSSa;Q|1 z=pb4FdP}M(R6(d>WP?{&u^1(ixWiDD?hNPxF9ch_S%4a&x0o9GuVeP~mQ=T?icwvD zjuk~pB&h&X7jy)4<>mo;%fo=KOrhza(zt^^3aD-+0jf(&0M(rjTXUjB5-$`q2Z?~L z0v!M~r5u2otXIzn{WV#mB?@~1+8R|5s`{vbdW)H%(r}133R(cFS*c(-*a1!eegKq6 zQU)d%P!&uD(*aerJ>Vy7O*T7J8dx!)pfQL6;{jdx?*Lu+Cs?o-h+APA>=u*`L9YKFU7oHkSCZG$iUL5*sLKU4iYA_?f60jef0d(H%DUrkjrV)q) z1HgQ+0Z@avie@6p4ML5}dS$H`q(Dy>Y928_zBFU)EaQgtA;g49IlY*F3t21e|`U1!l3FY|--2LYf8s0soBwS(%Q1~37-zO_JY5Co`c zhJZSNDsNqY^P(75LTsR8@~9G}SR-tn&Po_)0-8#T!%6_$>;xHrTHY?O8|(pl z0X2yIAQKz_2f-n57#smd!7*?goB$`mDUbzDgEQbP$Oh-Yd2j*bfQ#S~xD0Z^6>t?? z1J}U~a1;Cl^1v-{8{7eR!98#v{0#EJL+}VZ22a3K@C-Z$FThLi3cLnyz+3PRFvrrO z0F?nbPyh>H39Nt;sDK)1fEHMTLcj*-fGw~CBCrPzpfD%`96?b~3^)O2P#ly1F2EI( z1bR>kxPj833@8iAf%2dNs0bK?~3lM1WSHHE09cf_9)i z=m0u`NYDvHfoNa`F`zT(0%Ac|&<(_a?jRl{fJD#(B!Og*0(yd8pf~6P`htF-KNtW8 zfotI z(IH>!>BLxe1+nA;tys2?R&)>3ir(e4VgRPisFAG^gLRd}`e_=mNk@&?(qALC(`m#g z%smmCp>CFdEhPjWP|e{ z2V4TV;3~Kd@RM+s2W|uW7M$G&_`y1R2p)r{;5m2+UW2!Swc+7ABt8-Qv7=RZr0$<<{{y+Ba1GCX3WPzEXGnS$4ab*1?#W@o3I7jumihb#a`^kK^(?W9LGu6a0cga z0he$E*Kh-N+{Rtp$3y&zCwK-2Uf>np;2l2T6TU#H0Mu}XE8LI=?(l#H`B4Dg@P$7L zBM@4IpeTwX6s1rGWuZd^q7VbFLlEDA!39J`B%m^?qB;^$3$;-f_0bTG@DrH%MErtg zXn`c8APpI4jkaiyjxeAzx}rOJqBr`YKa3cJAsB`c7=N7= zh{afj6p3RP77VR@FG`HqwFj)?->TkF@b$F zB6IE2!SbA0ODLa(+uqmR?+%D+kt1VClzt3+11aV_6zkf2n-GMLCul>|P-! z4`ei@%%}^ip6MmgEcHw>vK+ioPV{u7*PD|IXPqfN_WvBCDU@M8( z#dxsoQsL}+CetxQwo1o`T;<}6*FFix&@Erq3vXGY6)e_<4^ zlmR72$X`}2b;>MZ93rpI?#@f3dJUALulHlMy$k!wvsVpdY1t1w<&swhv-I#6ZjT*5*ru5e57%IYk4+oWT&u+ZII_$<~bThb+^yjvIf zW}osx{~Zf<%BJ`0BG;{I2r3)f+@gU!QJrMGlRW{0+Ow zFMo0u9hZ*jCNGWWo{ERM$;}J%rq8Il%To3bE5m|`l-A|D%ZG3IetpF+-Mh>E7&|j% z;@a+Vxev? za7B?;c)I;LzR_5>EpDC*-TFy_Uzw;*C8*rI^~!~KE*ftsmnqRJ_sN>-v?6-8gUHvU zNQkF+Qd9EMd523>vX!$6a~VHZ4oJ;zPA+AERpkrB2tkgZ>iY35UGSC zL|>BiO5F{S%JlS>%AO06bjL%ah!Ng;<%cNICT}U$5+cPf4axeR+vQPVT8Jc@LZnI- zZ+#VRSKZ<*Rqq-iCAJTdY902LYBvj!>NX0|*XOYYi6N5S?k)Wo7a}#*g-Aa;y!B1F zzu7x)sYQVheG<#bZXxN_h~ue2Non1Ztg5(i;$l8-cWTCg;H!w=F(gO4=Y7%YwL8GM$} z?}P~xFmZ&BG^tszG^J5+)--NU$Be4M(ya2q`Z+Amb<~vRS$y;hSYEirN57c3#OkA8 zMqGZ_M_Tzwo29fy*T8bgM_PARD{Z)@&DzB6&De5WD{b4am3F-I(eL8+ZsDunL)`1; zEA5}9l@3nQN{4-YrK3Z%((%4p>13#{-p1pnb$s`iq?OJ$(MlH+e5K2YTIp(nR=Qr_ zSGuXwO1DC_(w!vVtb43`9}hHI>5;)#dMvd1r`&$_DoF1jK6eC3FCPX;uT8$v+e<-N z@44>-KAL@{&(jN-4sPqN<`nA;ndN zgQQSjzmQTY_n_NlxH}AGQBL(PPzu-ial%l8Mz=jsicRnfiC5VIX?-6Klq%Hs3z1b@ z0%?X@0;MWRe)?*Zs@~c!BvCafFr^kZeo&1Fq-|~tl(Hfhl_KZlp>JO!anEqh0R)Wz<1yBqg(M-TK=oeP-Xn}t5;tJ)u+@6W=3Pkz!s;V%t#_Rkv1 z?O_=1?$3~s{?h0I{?b@qe`$PSe`$i&U&<=#FHH{hm!_8Sm!|9drI}Iw((E{YX>NIc zXfCVUWH^8qaj!p zu{%Pfn&y!CO<7Ju2X)Wl(tz6hWR@d+L4goy5sWqT@J^8+ZT*-a}2* znHL2RfZ`~JI3yqu_0bqDkb#cqhCUdG5g3mtn2iNkjKzk7$Nev_&R*VgQC= zEciW_mI6W0^>0Sv#}5>unt>c#UY%;d0fLC{EFvzhiteqW@kPyj;APu5R?Q1 zeu`LBL^UuXsL`BJ5j(ICM{ydLa1;0O6tD0RYDPZI z4TeG$J_tlHltCoELnSbzs;Gyb&>U%KhtB8)BZgxfCWEnC#R4qHT5Q2?9K;En!&ThI zBRKFD*>I)J##szQ%yRrgi(*jPwB-;9&Z)%r;3Pp*K@HFz7xkdD`5J@sGttsrY?*gC`UI+5!>bY@Hg$zEtmUFt&QYz6TqLc|vL2X!N8x&_@F|=9+?1TPdCHnwz15M z(hQX`bKugajX%CND@@Up@VB|i*k%b>TEhAr=Nr8J)~2_|szB^+Q>VP)iD z#wMmT?9g~n8U~nQ^H`NcX@0|#1QV=qK&zoNjIh8CjTfb1fEhNgc()hrbWAIo1QV=q zKuhDgK8&!y4h=uXNPq!m*dSyQK!tZ+a}le|8Ru)q$D59MKi88)9=VZ1s&2uXqo zRyd&bqcn`Lzz&T+rD1>>HV8Uobx48{Hb}W zQ5Ysz;efU}g<*sRc4%r)7zUVOt1-|0Whq@ErD1{<4rpsq8b(-Pho%;#VSpL7f`zC! zexNW+u)+atZ3@E(3+&L;p)d?E!v;~8&y6IQV1)zPdX$F|7TBSwPk9)?6)D12zj;31 zdIL(s1S=fSHl#F+u)q$Dp3*SD4BOg5FWc%GQ5Ysz;ehr>3d0Bs?9lu~VHjYB4Wcn` z9Z4|33J0`JC=VknutW1R7TBRlp)?FI!v>Mc=SC7t zu)+a;lqu@N2n+1cq*ERSm|=TVnEfS#!Z5)K2ehpy3?nSCL(`hVFu)8OL>pclNie|* z2efS|4$!vHgE5Pc|*B$!}@1KPfnhY=Ro zq3K6?7+{7CqCe%41QV=qKs$i)Fv0>mG)Bt905fb*zOU6G2_{(KfOZh&VT1*CXa-Xr z2AE-k7(#g@!2~ND&<>?MjIh8C%`nQt05fb5!zqs>m|%qi+7Xn85f<2?8A*8r96^gf)x&E$59?eSYU@{Jmq148Mg7S^K$5$ zC=C;=a6mhO(lEjTJ2Vq14Fk-uL1gjjNP-DgIG~+Gc^F}V9h%9MhXH2TN@}U{rcf9r zSmA(nDurQ$1$JnrQ5Xi8VQa4C$PLp?r#MWo!U63Jio*yC?9j}lI1Dhu1~H5GjwG01 zg#+5zl!p-(*rAz2c^F`Z4Pq|kkpvU0a6oIOJdCiw4$VBu!vHgE5c4UIB$!}@1KI_Y zhY=Rop;<_I7+{7CViDz$1QV=qK)aaoFv0>mG)pKC1I(~NETue>V1gA6XqQnQMp$5n zW;x|yfEhN36_iI3Ot8WM?MlkS2n+1ctfD*&FvAA1n(|133063uT|;>oVSybQ3*}*e z88#?C@z)^9q^m|> zHYh#pIwZjaD;&^np*)PRzz)q;%EJINY|}&7^S4nLCRpKsb~}Y(gavkJc2F1wm|;5+ z!XNvc6ov^_IH27{VHjb79h%(~h5=^Sc0c!gxm;(ZG)%C<0qq`2!w3uP(Cno&3^2n6 zv5(JmG>0h<1I)0^F7mbe z_TT2r;v?u$LZ6+IPAhR1fHN}ivC!Q8_0K( zeo}P6Y@EPngq@=Q24k=tk5R}*-wFC)1+E~^Y5IQ97SnMA?@{s$eTf)>EqDl@v-H29 z2bSOhT+eZqgbYl^0lY@h^Yr~;2sYpjqzm-npfl#<3{)5CH$*Z_u)=|$OZ4$z09NA$ zJTB93h7OpG6ZnKOSLlbu7;ML56uL^ED0*W#t|0d{&dAUP({LE?5PF?vGlpX`?!)^A zeVFKmMK}-VoAf6l4U@1BFHyu!|1<_-Ep9<`i`@nW%*9E3LD}2n1!J)jPf+L%c|jj6 z#}(wc%X>v@Ov7QkMTvX7R}8~O+(W_p^pm427UC?_59qT-3MOC=ok0b+GB5=P@EXOQvJYS|*5eNHKjW+t zoiPtKh~GFI(Guga8_y8npbr`SuoBnc{+#|mw8ab@#e0-|!S@3gfz5aTpO@qi-LV+w z;rxmmA`MyChnEO>P5(ItVlC{@ydj6^ggH2gF9>@}4lx!x@EC>Okwf&xa$G^~_wK9WOp#R8mx`V%=sGA3XT90>YM4lw|$aRVM-s0YvQ)nhQ$<2Ld;skrt?B{E@#4T5ju z%AqC3V;7zxz(pnMpf6V98uGfT#CK?m890LXD4B~{VlW(=@c`a@4_h7Gu?XkjoLfb| zzDlGb3;XZ_!Fg1o0S3YXJ2ZJ!LPkf-!AX2Zn7c|e#~5tKV-(7#61C79%W)ZQ9x4%s z)|iSzc#GnmD)A$RVk7RNfJP;%q6_BZ4AgvK9FAm6fEB-iubo9b^v7!4Kt4$&%A-AI z;TS%mbODw41tYN)kKkL79HJ+d-~wE|$sy7)83*tRMSRF124fv=BfqaoRDuC!*pQ8K ze&i72uoF)az*pO~(HARm6?yoo`#ZG7bR5BZgceqb#u$#xxDW3Da)@qNh;wk}EAmLB zA`5%*9Kk{45Jp&Fho@F0DxxFi-~>J+ELbI)VKlbmG5kZwA$nsOF2St`IYcW=#UZ>w zv7+Pu5=F|8Lkz+?D3=d-g{c@co*crAQ^-cSvMSL6 zW3dxYQMepAL|?4H739&8L$twk9Kk!32q%Xaj!n1^?+BHshOStMb8w0zhe*Xl?7?#c zN2%yuC5N!!COo3aAv$66pJT^ z7=jJB3+X%RCv?VqoQCRqa)>01$8P+F!1Ck}{jmy+3MSkukVCY?OdP`pl&nY&F%nzw z5WccX)IfJE#s#<}kVB+n687OGid0gGh8To(xCKpRatH(F;uNw`whHwV#$qR)pm0@k zh(1`3E67t#CF0Qr({UJYQKC9I#4v2aeH5%g{e-Sqh_i4?B!@`B1nj|cXls%~7_kO7 z;8BYlq622*1U{kk59AP|u?@e%zqU%$L{BWmCFH6@4$%rzZ~(7StgcEl!Vql09ptY^ z4$&F&a2l%mDxpJ5jK^*~Ltq1Ph<;d!YjAI<65pd8X5uK`qokf3Vg$C}A$%H9KcPDo z<2+n`B!@^t7WUyKLVhBL7=*R3L(`ZXq7&xg6uuy=2|2`A?7$Ng{#hk{KyNI^73BVf z9HI@T;Sk=UL{pXc3B#}n_fVi2IYd`1z*#soSBVHDV*>WT0c{I%hyhrQ8}MkU5*5$^ zvvC|BQ96knVidOFSNJ7UKcOd<;v#aTkV9l(G7jK1il&l748eNbM*cK%h)m4GX^3>{ zC$z+P?7}kyX3$PRU#!G6X-_)=oiGn|@TyJuSaoB}t2!Rd2yDiEc=soV=!Qi&59a~YPe?@;_TmNjWr=72BP_7P zb09fHC(OYKd`8$H+6fqg9e51?!Q>FVu?&}ydkF0Ww8B&z!W$GHN)9m;8*mo|hS5$y z7c9UTsD_h6B*O$N90(dg{e=EljqAuak{qHvX5tt=qSPpIh*8*zhwvRu4$%Wka1pL! zsGpFI$=HuqC_0vQ0tR6nZo_LFIYcJRIE8GK8&5j{ z#}T|k=tOde;n;-x@Xn&0fNofXb8wnO4v~t9*ozkko=p7&BP_TH&ne^(9Wfgx@EK*M zl0%HacKizeY2*;Sund>sHl6wjtuPe_@dm|b&`!V*Y`|SeGsz*kU_MSmHH#b~2_{(a z8vU}XtierquArTO4w#J-_=GYm$stB#8-9iVD%uI? zg=M&eT&u|;T44$f;th(ep?<;;Y``5z7IKKrn1|C~mQ*@r$stVGjb{j4M-I^st8g9e z>#3j64l{8SA5dxo?F5X#7CeN{MskQASd8;<*+dSJhDq3umk8NR4lxL8aSL8sXeXc( z=He8-pzKz1h_Tp#Cn&s)b^`if1+F0Xc5;X|n1;i6hY~xepD+xYa1RA{l0$UG0-S}@ zF4_r5!9?tV1KQo>5CgCVH{oHWenJP##&LW?nLV@0hySG(+~&AA(Aj2yYLKwhiE6DA6DTS@*XCKXond%iVrAx zg!&01umum`bCevSI~L(QT#nIBKpG}tA6_8jI61^Xti>&8PEbFg6XxP1zM$+$+6fqg z9e9F5r^q4tU^y-$w~ZX4HKyS(-lF(va)@Erh|NJAF(;sru((N4fXtc4w(x5*(oVGd5>3&QSDKVb}Z;4uo_C5PyZWw?yo z_h=`eHKySZ-k|t>a)_bWh#b%9HKpD z;W$2`)MIjpQP_${@Owf#0X?t;7vcJp93lggu^+Ec^cn30490rghSzW85ScK;2IYsa z<&TZ)I|1!63&-#QrG#4if|1yY zNAOjtMGf@85?p|*TFq~O)gm2}uph5bgwdGw7>sqe1uti{s00Jdupt{|UDTol#$hL( zBEVHGYNHQU;41RuQj71<7SnMU?-1&y7L73+n{ginbE`!)bi+cNgL59Wh(rn|VlSQ} zIImhXfDvnO6Q1sBQ4t+62Pg0eW%8*-GmOS|JchrATGT`@EW;(Z@q>^!w89h|#2XaT zsKt*MiVe5}$xAJ&pfl#<3{?5mLWd-nup7T2NK%Ws=!aFf4)+3TQ6B9v6G!m@r3$LW z&lriVcnBYFa)=&Sj0%nyGBbAV&o7* zupW0H6=#lhbjEy~hA2S}k%aNsjo%O$sup$7536tu?j^|~+F=Hc;sZ*SB8M1>EqDN* z(&P}`u^1QNQbsMJkcLUvhnFZ4rWOq`5NmM@nzCw<00ZXYB)*_*IkjkxvDk?xD5N8Y z=!50Bf;{1B5s%iGhQoM^5)tGO!>|$eP%u(0s-i0v;w;or+=Y$Q4ihgbYl<0lY@B@6@6Z24g+$ApiH| z5S=j(HYkG{=+F}5unW%+P=WdheX$Z(k+-5+e2=!6jw5)FP+2XSU<5YfKD-muqB^=^ z5zfQ867>^OF%f(50>PEpw=n=~a1)+Y*e>XRSvZbQC|#9$8zZq558+dd*FtwJ#(6kb z=e3ZEiP!@Nv^B^9`ePNY!#$DDg|?W1BY1}rHOV)IVI%IMKrQMrbjCcKhWJ4(%Ap0u zVJDuTP;K@t^u{t=f?FMqIb>im4&W7v)MY7m<>d5fL996&4el z79Ga)9;sn*sd3?9(c#gNQOT*P=@Gh&Z`V&tNlS}NkBANnk57*ci%N-)4U3P7kK&(* zwDjck$keEq=x^88C8wmN$HnWyqNAcC!lI(1lfzPCGg89BV`EY>B2!Z%;$y?VT|Xux zH8n0prwh}?X7K!JsXTRfRAg96L}Xk{MtEFIbbQ=j)(?+Mj)+Z(3ezRWWQ0Yfr={{% zQlrB%(xPI+)8pbI<5DBOT|YTGHi{RB4U3D=@%-_;v$*h>aNbmOcwAZp?_8i%3n4kN?Z(jZ8_7P0P^5hb1RRriDdCM#YDv z#HH|lVp3ytY@9S*^k1Hzx0ex>77@<|NsD9qr$mOuvyH{2t*i>D3aug58#h*Gn(sS!~Wc?iQXmQ~{pTPj^-p`}4GGJ&&ui%X`*UmGErsa@sRc zm0jKQ#`yrx_UdeR?&Wo_c)Gc;{OVeuXDJVx=U%tMuI0r0f-%w2apB3~abYQu>Ae5& z)QGV7bat(bw2U}iIC+iIrFSmqS?B+p^!=~6s}+7%?;pEVsxBoyB04H1EHX|Zvp#wT06J5{^JR2?>4M-iK+wDwG~0#JB&Vq@XhI8`#)2lVpgAjOrV5&$ zN>fwNTom=t0DAn0#`qac(Ht$2j8vqf722R3I-nCW(FNVm1HI4({V)InF&INJ93wFr zV=*2RkcG*Zis_h%*_exYSb#-Xf@N5NRak?ySdWd^jIG#?o!E^%*oOl+gd;eH6F7y_ zIE(YRh|9Q&>$r(qxPyCmfJbplf6hZ)k5R4)yh7u@=(g;I2gd-Bsh($cUM+L~Jges_p8mNgMPzUwU0DAlgWt@ti z(G<MjCy`YS5(GLSK5Q8xk!!Z)0F&5)70a=)gshEzL zn2ouZhXq)KC0K?PScNrMi}l!u&De_V*oocPgMB!FL*T#ju5RU&k;T6L$&#`kNUsLV zSA2TPr|%7wTSRr0cfA`f5A`s}m&=Tj>n?65AC$+)-Hfed&E#>i{d|f%GVcWW*~8{? z_rNT<;J%;bD9vPf`pqBZNxP@Wmy8YNR~@IxJ|XqwMu9WrpZ5JA=hw`VuiUI5XNSy@ zRokk_I}4cQp%-NNxB2trkZtAVeb460y+gi}8=TO|BTFokpIO3W-*?Mpn`bF`$>Ejqx{k%=^lhu|$YgaMN}2$HYQ%Mf*B=sO&G#3Ry27-R37BtFm5h&j6sgE`CT`FMGuJ@ha$+=O>0<$axJU{X1|dYzZ6U%EPvmkQX*xA#Y18$;VnQr`Su&a|SMz4}2;s&nmD)K71iuZd7Km+;~Z(eE9Gp z+0ZdcZaZv|T>52{+>Th%O=z-X%cn1rhet=t`Km6G@AZh5*QgfB=1s108r@0uMiU)i`o zt~Du6-cQl)6m{pRAMn&~A1#u-%0$Vx#xIsD*dyc)FBi*2{UYR}(M#lZlfvZ>KP{2- zHVl_b_FN*b@(hrCnxIUyPuZGv93B<3S25*-d9ep zBrlcw4J;?m<@4|5^MBsARL<42to(_Xn^^kZQh7x*+xPx2uFK>)FT-+nir*{@w{>t! z;xc(c$1vG@^fLLvqcU=r6U*d)^fK}pwrLXEw0W21a!tR|@{Wzm~?PC z@B=x_sIy9z(~HUb$FGuy*o(>`XI9BaniiGolj{!TdUf<_dEKNU@{ONX%MBYAk==W) zmfv_5k-wk5TAp?yL~hIO`yIRQ^sB4oYl$K9;ZLjO{;nbNA)?WZYP7_gfHiW+{$RP7 zyhh$XFj#Jsv_{U5gXO^k*T`<4weq_8YvkqIw6cBQ8o6#yt?YSkjqG46m2gfJ8EbiP zDZ==goPqdTFJ#wytTJJlvvW>tuIYvlrw!< zVmYI?KAiZ?NN4(3ZpTgY)_+I*-t4WfK&-fwCrx0X5-Ot#81n3;x;hIrptP@Rg0au^ zwTX3XJXnu~`cN9&4Z$F2`X7luJ@nQ$AuFJ{%M;veT(N7>UY8rz>RGq})WGD^(sh9?(;Xea2F$=RX z2XkS@Jj}-eEQE4muoz3Q6w9z2%89~Ctb%gFum%<=Cl2ed9?A*CMr?v|BC!Qqp`1`` z#||hb7Q3(;R_wuE?8AN>fO5id2#0Y5M{x|w3CIbYgmNNc!)YidBxi9B%8AJZT!eCh zav4{koTyyGbtoq+H(|#u+{PW;#Xa1|13bhd{EEkTf~RE{a*T)XQ2X=(=!0_AkA8W=WCpGd4};8lKLp*HHEF6u!!k86O2 zBmDG@h(DVA^o@y4rcvM*7MemixoeIVXo(~wTm1B?M23=knJ0sVR%nejXp44ef7tJ3 zL`N1n!GKJ3Mi+ENH*`l2^h7W8Mj!M=KlFdXlNebTh(Q>PAsC8b7>*GbiBTAhF&K++ z7!Q-i|E2##7P2r2lQ9KTF%8o(12Zuz)c>V=4hwT(#yrfA^8etpkcCB9j3rpATIuS} zw}{z%bDgW2Fe>dtW#?<_%$}Hwy|%@`rIk8&?$o1BmsS%8EmWqFUdC+7>4wg2I(D7k zChc<3G@ULoS{IibmX;Dnr*dp~Y#80-$zky+(J_%JF*;pzOnla$CCbdo%v*fQ*ZD}X zb7eA)=gb{m*L}#Bo%@C#{dVr~Vhg^VJ3P;UTs@WdG>!Me|AOBzXBQwk3*NlQBk~G2 z{$)I+SpMtJrW zDy4xhqGIAi!M+^(t<#eYExWYoo!&B~SJ(6|aqKz5Z3t&kxml~6^38>TsmfXQN)V^N znG#(2vy%Caj}^zIJSvAung3g>tyAZxG?kHZaHj8kopkc~(O;Dx?&rii6_y*6Tut-oQ4=*bUhOHDX2DQWE+bP5K%9k`h z@k#3>_iWRFO|g-yZB+WDyp}0Zt>#&oje8C=C(IFEnhjJgd}&D)tMZIesS=c3XQgnD zq6o*eQUg1u@g*8nqwt(W_2|pC79k1B@#(>rk9_B(cDW*)c8IJ5HXg-<`H3n>nW%d= zd-NX@b?*@BLZMo9L8-OMDTYC+{>?1i*-pw|lLS>zqDHNyB6fboHB)Mt8M*7L{oN9B z{uX?lmYXjvHwg89Czs=*^4E#EdCOeeAdP6|q;$#g7S-gFKgvqBq<4IW=InKU1YZFw zoq_hELd}&2Ig@fH@|B$0lkZEF(Y4vY`?<10IBSHnR(Kl=DP1jJZ+Ka~e}9C(klf2j zqZ`|W6`k2Dxm{c3{Dh}c?g|q{)ja5CP;NUk4yRw{KCABJTj2eL{c8^~58>iFb=DE9 zj3{}o*b(PePP}}cm+BJJdn7H$_6QLV52tA&uJv zq{g{OKc@+1Qn{>+_EI|KHR(TK28~WSvvB zdX-A?@o|+ZM~26SM^=ulTDfX;MD=i8WL#u*vio1_oS3M{|3RHoD3rtM30Fv!>yq9+ zJ+&*d&$l!rcT8`eCJK5b{!_ivTIiSZ(H^?}Uh7!c3JcK)1^!a&?8vEg+Nzx6sC9-Y z-QGF1PS3BkPFPN@lQScIlyW30m5zGpH*y>t9wH?v1E3lK$H|gT5J?pw5sI8>!u&aE&bW z7FWcnBn{06HU@W5k?`-VoQfnt6jxf5Uu%&vIkiaNf2u{8wW77^8doz2Rc^J)pK4C& z(4rPusum&64eqnY=FO=_Y=5ans4zJGyrd?@awaER2Vsr zNIEZ3$Vx4z5pF9*Ay0iE&f%rMLfqf}C&3(Zt&X(y%vV4zp7q~QF- zSs6n%)Y;vk)ZN9@PCU1#vUcE~RaYvt#I*lRW9deRcfQ(BIaOE1ZP^u?JWBuaZ>lcU zivzhd|BF;z)nckvj|=C@gz8lyt5vR4y?SKT=(uX}G11kl{mZH=IwJPpS6v0m#jBG3 zx2dzLRvDRqnV*GemYUN7Yk%dZUY%yI0j`fnRQ?VV`+ zc&S{K#*h=08QlR~Q1q{=uadv07K~M@FLU#Zv7yYrJ2QTJW;LRd!Ks5U)s*kelIv56 zdw;s7GdtbE0;-%@%N(rr!YyYq9w}$;7YkQ3s)P7Jt}`E96|B@Wp0xCo?eJ$+ zbD1-XasQdB=FonQY}T1TpXY8`O~`A6%>G28tGqK zM~DAx9aa4=t)rg*FIq>1LT#$Q(>&tqtpCa8QP+QJ9@YPMRgJ%MeWj{V1>|%D|Fy1R zIv@_Qg3iU&f1{qEt)v>NG@P6c2q#W<=sIyNA#%CUM$-M>Mq;il23*W{a^8kR&cl_b z4jG^4_bQ`;2nnXgNx3u3{oU6lhr4pC$DQjf?*4X;A!MPJ`Ib3l;85 z_ZWlQ#zn?xMZto`|JwgjUKFZYUc4?|PC3l~akbUox!Os1O%gBN{?bJwsF3MJ<76rH zU(^Rte>qvYmUFU158r;_d5w`5l>V7(oGD#XE>TeWXZ|=-YV`F?>F~ceQ=(H^wA16!Nf1E7E{8=58S9@Bi0r=uSr#etN zi|J_DE1bEqIW%X#ZvS7Z1@j-(LVl(1-l5j#^tAkb(v+a|w8(0?bEoz>9W92xb<)IN zxBtmnVb(v@3O)a<6@rbyK0-y`+1IWY+8>pjQRdUeOPc-k2qy`rb2+C^ca^{9RbBb$*Oc2zmkS5C%Hh(h%;VXS zG%-9=j%Gy>m*<(wgpdh8^nK0Tj*fg~n*Fjbn!EQd{P1%eF|ze$pLpWK%LSOT(&4hL zWx1&DxjiwyUSfITf%$i`+=Mm3w^w=vZVtL`;tVNa#)m={(H)t`AmM>q z7p9*~s5G<#?=oSoXPc9RE>Y>H2*X^`Y=nGo(oPfJbZc>j&~<6@S;EMR&6%Pyp`zz6 z=LyB0H@!gUzFU8hkl3&OCBm7yb(tX{;q{T)R|x&*)ZvUSVd~pj*9eW?*St>HIk?6R z!s6K4HwjT^tMY|)!h-Gzw+Pc`R=!QpolUr-+*$7XyUMNl(f0_kr2_90IyEc&fKVYV z;31(-%ZNt=k15I*_6dDU6?#ngcp>}=p|;xRDd9=J0!-A9(Dq#Z-v~b)@TP&5Fkwr9 z=Y(4+`Dv;p1i#jBGMVthDUVl#$My2QCe$|NenYtbH1Au&sGjccl*bZtzbBjtjs8GL z^Ii0jP-k%|nuQ6~x{dteCe|+BnJB7|52r`V-9-4d>}(#gZ~m;J3&s5QlbTz}@%fy2 zsI9C#RgFM*TC)jeDGz&OLfTjtH-h1wYi`1bDO)GVK$BM3WIlZBc|)F0M?f-ZV%+v=l+CGwgf< z>&)b9zRFv%?Y^dbgmvYv_!In3UsgUz+ezp6BncA^o(dp5H6KzwLz5Bf_zVdL0yk+1 z4bH9$CS1wd5JEUve034Ri&ra)5{iFWUX0-QaZPbTy9!525DFw64JCAmKf?7I36p1R zD@9m1Z&ztT!4B3kgqzM=!U!$qY%5FXb$Mes!jY-#bcBiXr_n2wuq)9LLCAe}O(fw{ z^2#Ve<;TnTvOhu7eR&MQC(|5D81rgw9HGVK>G6biS;M{~NYUo+39VNRC{O5GxNil* zuuB6g5)#XrWx~N5;}Qs}P5mkn9xt0wnb3SiRu#gyVPmQiQrcuyBfOAJ)d|;@ji^DG z;yE~xP_p*enuPA^fwc%5`wjVl(E8qh+JqN#2h|}Mjf3hE@?PpwkMK)Y-};0vtNS(} zYzgSxkg)VdCp{tbLia`lujtM{6875L{X`gbp<`phmq8txxQR+d4tMz3P3R)p{=$93 z)JaVVZ|yak5gvX@Y)&}oQL6=^NWa!C38fyVBoWrPYnn_b)IKSNFk#7$se~`7Kc*3q zhtx|aoZJ_eL8y8(wiRK)rl{70d~>4P5YiGN+Y*}Ojc!NqSQ_1)F#Be`4uoBkYjq?v zJr>@H&~Z#kgf!e5+%A3ZZ3%FPMC8fveWqyN0fap9U_$LD9zzI;bMg%(%$S{T7$NW5yu%60isu>4 z^?EWPx!c<*1oP-OQwfzQassTYQxOAYej#NM(qQ_H=Ro3;oh+@9m5?3N*&CMa*l zXH%4ozox~S#1EMkz`A2cdUgpU)^~LK97No6P(MHDCYZ~W8262#j#N2e;C45`t%Mh_vT{MzP*@+ZH>JagCa z+*a=@SJy;*8|lPe=*YKXg8$Ry^p$@1rtDm$?{xCJJs!u1JziJmNXg1O=4+>fFP#fT zKqZkgb$1IQyIRiF)@;X|Y2yzO?RZMBs_WR+U*{?>!yO#(IdgBbH|ES;y`Fd!_gK+w zT>Y=lT#-9CQgbHfX3xo)xc&g~8a}dOp7E?mHI_4bI7e#E?AvV7oSErQ5pT1S%9QwZ zI&^BIoaw|laB`;GCc8OP&)*`xW5v7^c(zj9P~KY-I5s5q5@{9A;Q_v|V(>&(jN^v# zl5DY@iM!cJayn(M5MQ&BCM)slOzG53IWv)Sh~>=Oy^eSSPms-ufs=T)cy3T_6A6Ut6a^lxH*Q;;`)kw}<<7T3=-A>{;D>+X|{JPz~|e1K_*U#Ehv&I8Gy-pP5k)x;xs2;~FhpU$&| zaica`fjUA`b^vzHoDcAfm0V^detk(L3sk!~lc$d%u7UCa9w{GSX5!b`>Xi>r6Us{t zz+&uyl7-(`G0!YsvKTkYgMBh*8g`C($pR(u_wkt(wX<0bZ$R^5cdM_0Sn(K@A{n#~D0jMR#VskV*Mn{JPzg zL-CKqk7rrgZaZ-eZ&}e}Aum~y8Cd75EQ^@^v ztqAF+?54^pym*LLYrgGird+dF^Y^cN3jO|7&(Ewe0>cVQ((vDxmA5_$N`0if17#Ob z_7yg_xa3X0jD~6Vl%Cj$uFACpSzm8V%DFN5>y0TnH>UnS?41W-RMon+*Q6&TnIV&q z06{_soj?-OsF_eh4^4_7A%vo|(5o0JQlx}lL<~(j1nGz-QWO<50xC!gy<_M_g#TIJ z4hZV;>bb}3J@-GJ^RTk^>@vHpJ^OvXZ!M!TTPkym%3P_uZB*t-^83pWgTqWAb}4d8s2oK7I(+e39kG2P>qq(x|MG3c~{G@4H4SYmExS z^0CZ1qw>B~)*F=#QrT!!Hc4f(QQ0Dutwv>=R2XScuX?*wJ~S$f^v9|*j7p|dJ~k>l zq_Wef?2^iEqwbBbBp8 z<(yQ`8VpezJK zIVg{cGSAN%YB3rmni3UWkgLTS!4%yx>h>%eDuRB(RE8>06{OxYfE zL#ms3B4d0dP5)R3;IVue;5D*K{tehVF>63dliznJ^1x!yK3kZ$l=0 z3_D;a?1J6!DeQsI;B)u__QF2Mg0Epe9E9uG@5!}Z(Du9D^pHrXB1AO&fBHqjDWOz5Fia@Fx=e4TmF9O9l+ z$8$s-pKo&LPAJiar=dg^laE^t;krKyFDzUd4i5W)P`|aqId~T^j1EB zZI$E8b~hZJ6~$kCrSB_=-8&sr?N{+7(hjP4zmS6@pdD29?>=A+bVQ|`4k?GiMGvVw zlL_N%*0n>QPOD5^3OgQB_U2V4*TSU_(t(t!+p}Z${z;us*n9TlIBYPrPiKXTH<;r6 z!Z(--+Z|Q*5;XbHPHXn3?98clrotu95VU$!nLe@4Ti?yzj>x3$_KBuAM~Z@lBabR? z$LdZ~>-J1~rDKPFz4}~G<`nxmg^OQN9)#WY=sD8fag@C^b(E9Z>tIAY&2_SRf9{|o zp3EQK~~OfzkFQopC(^GDl1xirwyM_|sT>wa_KO zQ1V|;c3O^I)fGQ-pA3Vlm85* z-!XuGb_x+3B;f^RO_t1BF2*MgHdP`sw2*SPFrMlRSH8I=$`S9Deq!ViZX4&CJ7c^m zVH+efS{>q5k(Aeb^_Fxz7Qc>D9PEq`Ec?6KS5#-*9I~Id2!v>xb++)c;xybFXgCe!ml#gNjtYj;aPWxX zG&H_1PQ$?3;xwez7N_BdP;nXp(!^=Vsv}OrqX2OlHoq-SLzAN7G<01ePD3$&aT=<- zcH*gG`(mUx4VOL=r=iq&aT@M75vSn?)%IIv3EnSGL$96UG~7-Sr{U-kaT*@17pI}S zWBsGdSuC;MF^ZMb#cA+wBu>M$6XG<~*(6Sb{c3Rk;MQB5 z22ZEFH<>f+q&N-ZMu^i;f4(>kU4Im(Vctk_8V-?;;G9mTX{QM^>)0}^@L)$ynNSub~LHC*VO^7%REsKlOFzTQ<4TA%n zUHPnwugQ&~UJY>?9?#e7_-Ktd4PiaRX*hIMoQ90P{MHD&KKDGT*Cx5;8O#98!S?&)+*8x08)#b~(tg%}O# zo5g6T)LD#%(iOyLn6X5RhL4+y(Qy8(7!8&PF&dV{iP7NKKid~UaoAA9XejP@EI*rg zaex>NYfg#L&~Bv|4VMmx(Ga*=jD{~~iP6w4Sd50^C&g&k@VOWb0TE&}yyCDrh?Omf zTv?1_eP?Yn#3rO^qakE~_zaig#AnE}LVSjY8^vdsGE{tqhu-2dY&b1GL+4B4Gh93? zK10p5;xo*8U3`YoC&gzd+*^ExP7lOqIPj+U3_)4qGxX}3#&2}njZ5M)44W@L!-cxy zGfeqFe1@+IN48phIy04XQ&*O!moQ z&rmx=e1^s?lAG~a%z5z{rdJoA!RpnW-~P6dSHx#{bEftgY#ZEqwx;OPSB!>3zG5^S zz9L3LQJ1>y_^fO}F&bKp6Qg0(V=)>^jS!<@`EW5BtlPzCnA%^AhWMYvX!zu|7!9lM ziqWuaXE<*Rwj*DO(a^7_7!4L*F&g@;7o%a+buk*gUnNGvhC^aBEL<)|!`ehK8qOz( z(cr#BjE1Aj#ApZz6Qd#jCNUZ=9TKCVYF9BD>LiHKP~#mj8kTMnqhay}Z8X?g4A4e{ z&CVuwYq#p+Gz2ver=j>|aT@%FX{RCf;QQKX2)QRl!-dsiH0)d{MneM^ciu*9g(D54 zAzh4yC|4(5OKh+86{8{V9Wfeg*DnnXwIgO^gPcx;c+p+&5x0 zYzr2n!Tkd<8d{zZqrq#S7!594#b_whS&W9pK4LWNz9U9Mn`|)}_Pi=a!}seC<(NLaW zhen{AHX8JrZNL`gom#)kE{0EmZ!a>k`($RUK&2jVZ_3-`yOf)NpA6PBtj#_(&JZ$U znGg^9HI&OyL`wZ8dm0`vqlY#c%2LtaEfKgItZA?o_;q2uXj6eozdsj+N}xB{9rXM5 zd!YB09~YwOdQD?(5mfXhwSb^s^MWjiQ|Kg&YNJ7)g$B?I^u@UVGT}S8!Au_W;|%{Gh6(z_>rJMDJ`10~NoI0*H_p(; z!kEwm`oUya0Uv@EJbqwCPi-`mqM|qX%M<_fxGjh?R5yL~>f_cQ^l^I^^oc(Pw^*C+ zLe^HAN<(N1uftT(r%G>9-!M*#;*8_QBl_uypTghzxMjd`xX+9hZ8VgjQX4vf-lRTm z+h8B)Q)Ry-&QJm?GC^-rA2)r&EC+quPJljcKH6vqr&0r+y;j-;tI}b^+i3P@% ze|8?OXwCFj&I2!ts;qqSOY1>@k|a9*zm@fHAwciuAGaR#naq;Y^RI9pyi`ru{=eRP z$ShzuM1Pm}ppVmICycGz0or{C^8L4UAKE_eJ}4cg{iocAB)x=#Lw;*@S_<9R=_aZc z=w`GI=trb}mezy%pqqZ(VC$B>G3X{sKQo$welRzO7VskIA2_W*w~cK;H#)i%(?2d~ zmHuNsa13^VepcwFeh+*GpTifh7xbem3-qIZKj;VJb!|T64XD9o+?#4&=m-5_0O;qz zFnCS!|Fpn=TQzf3W=B<}n7Ub1oevB}&~`7T-Yl-V1gTrata%*D8r|H?E{>U{CX~JD zL=Lb+vgff|k$SMrT6j$NJn?+DW-Yu$9gLB*}@^>Yu3{{{i zRD+nKMRO>`T&zy1kHfs+o26?})`mI|2X&zy)Q1Mp5E?;ah=(T76q>~c=PYT#A1^{n zKwws_fz(~~610VO@G`WA1n2;X&=HcL6Lf|yfKvZ+|2X{J9eO}d=mp8p8~Omrndrat zxd8)UAPj=RFa(CeFnA4y!|RX&Z@~JX%#jqMU^L9OsOc8fue^2KSc~WQG=LZ zv8HynsIC_4v;>Q4Yf-H%s<}nQTT}y!inB0as&!r!i;A|WaEmpqtVNZySQiCYl)pv! zS(G>WuSL0Alna8cMW;kOG^=}Nb<3=B%+_@m%nZD$vd!wKSsgN~gJzXwR(s8Ak6G<9 zt4y=nZdO~&YJ*v&o7F0_b;n|}N;9i@W;NTarkmBE5H-Q9#+ubAvq~|mVN5ZrerA)|-Fs>$r-RTVR14_Hrxn5`#Enyse;%*x-a{LISR ztSn~bZnj=>Fzc+=ho0)5r@G}CuP>VG%|q?4c&ZDY>Wrt#_Ebkb8OT5#^i)}%YOkl- z1EpBsnR{wDo?e{GtkkL=Beg+s@a}_uBKE^HNjJj^$hedrFdG*NoBoE z{XA8&r|Ry>Ahm%$rUXyb)>F0eRLwnAyr*j5sp33UO;6$^RTWRhvQ^=pD#TNj^;9K2 zRgk9&@KpYu%Fk1Idn${ka`$A+Tjk(MuQ%$Uhr>M&ol0X?j`~KvTG*aZrwW<7;(1ib zV|576qeAkyl+B|`=1~E8lwTg@oyY3uP6s07d*v}dbkhrk28Noh_^Ati>WrVt_ESgw z)FD50(2pT!)m}ff$4~9@Q<;8hyPw+Pr#ARGr2FYj>Qln;;OoSDsOj$NP#!hiPx<-A zr<;_@F6-VDIp1ct-6uYcKe8%1=6KlS4nP8=LMG%ufCKkiW|f;Bl7+*lq`ARV&7 z!ojgYGNeHk=)P`4AOTV#6LKKHgXJLw(jgl-N%~~!bFR`Lt5#mVJQo|+s!D}S$N}ym zeV?fmNQZ2&G9d>7u*c#d1=1lKEIco4kPK;%Ro9D+zrezf3Ym}t0fku@QXn0&!4km2 zkPK;%1*!-ehXhE4Ovr(NK$eFTNQZ2&6lHlxhBU}p=XLW^NDvD{Dr7 zNQZ3D-PPD28PXsN^rJKc5+D^aAqN7=u{@+eI%I>TJj+8eq(Rmj)|;h6LRc75Aro>S zAe4n61=1lKEMY7R$&dzF8}-5!SQt_v6LKIRoP{9;(jglx5iAVJkOo;7t(@~n7KT*F zgd7NnVqr*ubjSuvGz&vAq(K(wuBAgD0a76oav-1*%R>sJLpE3{vpgh28e}!`E+0~* znq!H3MV{?t{ErNR5JLCde`K%$tjh%TxTiUtPV7TeL$k%M&^@0&GKxSl`D*lmpQ?_T zb6Ad*R5NGUu@ZY-(JRxf`JevpocYx%47nC-PN}Vzy)0d)7Bt;$VIR%_K(w2!ICRSF zqD)0osyWC{CwF>cQ)Z)s)$TkC%9vG4z3f%prIwdD!O{6?G|Pp2#6ob*{{?+&o%o_< zgF3yRjb@qk`_U{VLiCsX-;&xHUL!mzD)jkimWomFzm8^+)J`%%|6FS4AL%Jx{U^0k z1={HKFO;Z|f49_5+doL{3^oPuO887TNSxi%)XuKgO*LpBQzV{3hhUJ|{UpPazbz?G zQ#`lyHqkcIfh6G>{JK}and0g5a>rZybZTeJS4M=zyIwy^E@#;XnkiJ6)(A*&B~mahDB7i)j-MRoN!MfEOu|!VbHewhws0yvp)7F_mW)BZj+1%i}tZb zgvFIQLy@L!7rq|BOR??s%|m!Ew#6Rr&#SSmXhJe?$F}6}d+>s6>+Rl~cVwGS8HuoH zpVH+^YF$0Md`0ncdx@~Pxke%^rf+P=+q3Nz--H7cJH|?cMOw3$zR|T>lFQl1t2wf{ z&HYi6Llh3nB*Nm{M2WB%SWO}+%5f;bShM%Pt=3nR>#iEe~&r|Gv^@R(1mK_pdQQssH7S*~- zgvE|xVORL9#@>8aDaw@dxkll)JMRw^^~>eUp=fa1=Q>5>#S&o=!7ov2(@RT)#lou+ zVNvFwL|6 za?W-5o=U9yL7mH)S@Mc6wTb;C#Nt-n&+}9J(7dRip1+EpKWlot7L&mCfQ8CVb^hF@MK- zABxU1kLIHYIB=Xj$h^1cd&%WAKa>!QE$fdI;KL&&zSnzlde8y(#P(9NtU?sMHhiM@ zq4$|}?1Rn!@Wud&Yu5EeD9+8<7)a6P#Ok6H``1W_#p-nuVsYUI$>n^b?iVHa?DXT$ zN>W_R-cyQV(uZ41Q=G~8una|&k`iNay6om)YE3@;pd7`X3KC=S)BEc~sIBiVF&5{p ztqr60^|ca7++yzBR+G; zd95);&7QBtQ$)-f+JqwPYI0MGJG*-~qu8D_usOv%|77${+sXZ}AaUB>9xO2y>X^h> z%o;8+7AHSSajMZF&4hp zn{{O3_)^W2C>m^Q(urc$k&2xu9*>CbLg6r_LRX5UJ&~_ajLaYYDn;Y^5@Rv9i^Nzo z361Z;#BUaM@Y0y`zZ=7 z(Kho+iGdVnkA@GTc;p&2m}2KCiLp35zQ9mw=Xy$vMZ_wJu^8qOJe-Ny{UpYs-5U~P zF{O{!8%+EvAnyo@Yxnbvr0_lAF^XbfW%tn(#b1;lixNxR#!#~)OOQpAE6(Gny;oVX zIp3+}@)osEDmqM{nB^it7BhqNjXJfN1X(mX|B&Xxc6^rvS?v1q{uFB2AN@3yqRQ>N z(&z4`4s(opM96&j;{n+?At0q7T$3ZWZ~+6Y!RPT>izvuy!^Sc$)Ci)-H&F!SGzgb1g;JQ0evN;!h+DG5$7hAX0H+q>L zCCH+?+xuD-Ib!7PI#*kZjJ2$BiJ1 zTH$tFhB+RKJ~x6aif=HoIp;?hK^DbM)N1LX^k72{)53BiEgGUzCju9sLviQ7%YKBcvjL^$r}r-p1Qf3F38Ef_}jBxa;Nl?VpC=49Z4c+SYai!bV>qbo}B zK?C4klL!XR`Lh845{BWI%8V8GfO}m60Q4p~Jz`{kLz%_}q8Y)XCH_QP3ba+w)t1OQZLLTlh&m*cjE z@v41A1;DTpwGe;W^|sQ5v0Q$K7#M&&&;*8_ghzadLAGc}1ahv9z z8(pFAVSRP?fU)on=wqty-!jdJuFwnX)2DanGriX4C5`i{zaMJ~Jl=`uig8`2rGvgS z^Xuq}20FT;VQzGVZ*Fu&?zb?%?&?rfjmvLb+4{KZqoMa<5^zhX%>R-6P2^wV2!0hs zVf*d;&3^y4@;8yG|5Ol#ee(a({7o;dy0a@fd-LeuoxSNN*_#V=_GVs_GoG&P#6NB2n0e=2!diz z97;e*CF#GOXOZ+x`AHGVH=<0!G3p7!7ZNegx~*b{vd{-)I8=zU6`z`^i8$h!R~?siJW$hbnp0EX`FjZe`y-$N2dRM zX`Ft#N5-?Hf8D| zy|;?qZ^^9al$+dH)}(5=xES3#+UV-h*_iGf?Pv0;R7eFj?nxk7$`r?E5;<36$}5vS zPsebb%em=EE~leS+5ak)(=l6*jH)|?{H;_@OT*DR%k=qFPM_56hNCd$jNvE@DQ-9l zdo4!J=5G66at$XjzW=_#8K!`Xym8N?Uf~t!uq%Y{Lr_x$q+}O$Te{k;(CaqP|YTeLgWwP zD9j2JN5NyUI0`kt@5~FJ?ZzH)6pFqkj)KoDaTFYSh@&v1l{g9?g@~ijEpO99EO2UL1w>2gFgx z+#!yFQ$ukSwxo-r&~30d3bEgbqcA8`9EF7g#Zl-QE{?+2--)A8wW2r*k8X&g@UWdY z3Xe?rcpbI1*({ENTQkE^h!#iT#|PplY&{{4LbIykC`_v@jzX_+xBD#D(kYTxUR%~M zaTJyh5l7+cbnPg_j~j%eV6iJQaV2xS<|l^2j_99srKK1O9$8{2jQK>bqQ(zmC_J7n zhJq$VklhQB!)tsIbtZ7&hnlWI&s_uX2?@s z428qvdEc~+sVjy;Vy*9~D73mFhQiL>hM`bzju;AAzWJS*wWh_D-@u`j&*-oC!9+0>rj`&xp_}hLra4s* zL&5$VF%<6I5JREMw_+$9+bV{F&m%Duj{AtA5I9Q=g^9hzP+0G9f#1uv=zC%)yzGC1 zU(K22+>VH$5bbuHAJ4XLc8a0UFGdUnyQ5+#j870l;o(Iw6h68shQjr88~H_T8yFyl zLbau0D3rb?hCM424Jg#ZXvRM+}8W9-|sE%h6ad z6iPf6L!rxSVkmf=7DM67mc7wMY(2gaLt$fIF%-NfilOj+^;ciyvqF=^P`G(h422Gp z#84=3M+}9cpNgSyt%Mi~Ulo+zA-`=ZhC;PCF%$|n5<_8S2{9Dj`$7zbZe5ypWYt|} ziJ_41b1@XoToOZJ`b04l_H7qK;pPD`6uJe8p-`-~7z*bGh@sH6t{4i*X<{f$EF^}) z=Z(csDBDg9g@OCUP$+mr42Amj#86myPYi{9`JxB#B^m@u(&jf|5re3;?k$Ew_6;!< zzWYE7h08<5P&m0&426NOilOl4NHG*%Otqx2T;btjC_D-lL!o|%bPxHkfMjj1=q-lA z!Xz;iwjCBjVaG5r6ly*cL&1B17z)lU#Zc%TCx(Ju0WlP6C+ZtDtfLqTtsjV?U^*y< zLY@<1DD3-6421@zC2MonHZc?yE)+w-+)NCGT|bJU&_78G1*Z?iP$)V|3O5{zFs4S!pWv$DD>SUhQh)&Vklf|DTczrRbnV?tSyE@tw=Ex8Z8w= zVcpAOC>$RkhJyP@F%*`)D29TIpL7p7prDbp`T7wt6gDmuL!pX?m%cE|loUf@LPx_; z7_!eW6h7H$7z$(B8HU2!_Y6ZJaiL)->^*B33W=TfXKF*C`8mT-*xJZ26sD~+429I= zhM`bC$uJb&?rs%veBM3R+@l1>ImO ztOgF<)4aX>Q{oJ}gLdhuU_6h%wea~loMlb!Q{#S46>P!8WZ-Eh>g{gIGoUw_Pg@FF z1fdC&P3m{^cOV@Oz&U2}m>y@SC0h1)8T9c>g+-t@d5oFVj5tG_77@H*%n&}U&U zaN-U5Sz8KP5o!jng4Qt>gWdtX$*Zi*WnP@2JZX7ZpZH-g4K_jUSqOA~C(e+xLYWv3 z`owF+WC`etQ=f%vtj$$h3T3H0Iq`$}dpfLz{csi@GNZ4y6e>|^4P9X*yaykHK5jRe zDPLNgk?3k;LPyYNVNOZoEPTb^r+}Ydfk6wH7(<0ekjVD>#4m*%a2W0}Q=Wx!Kj+r! z8)hKr@me(8u%!D+tz>f?imkKD|R< z=(RR4X`EO6{a90Awzd@B>O$>(xW$x$+EQqwErrIpmO}nqOCk4Ln1?YuE+X)sNYk9~ zuS?VP{|9NB4SqXK^Wbl$X~yz~xlFPYe=AM1!M|Oarrw`DvOj-cre@i{CR0=I+9lbw zzd2RYzDGfv2Kznv|6MaR|Jz1G14-?Swfv`>$Pu)a`e8Q;M#Gyh2F8MZKNb5Q=;zLCm;?HWrhhW%-(&h=svm(nK|gMG!>6zZK7-GJ zrv3LaHLc10{H1%$zRcGT`ojRw&lLSz;kBn}n)_54M|IvIuYJFuyd+^ZbK>@MCh=o) zpxQu^=Hg;Tk|qh21nf2_YxOD-WcAK0+l&x=RzYSJAi0+{%Nx0ugbR_AsmRH^ zr}HjVj;Hnfh2KuNY?i|zx=xniE>D%|skVEnEuLz_&zYBMv8T?kn@3LObaFB$=)6lc z%2TE2oXkOVk(l<3Zw|!%+`C{!ylbcHLG>CT0PL>yCNs=x^H1>af90-3d{v+^p`4-K>^*Zfdrh zn(k)xo8YF#x~Wlas%@ki=B5U@S&JvTsqSv7XgQVSrV{wT%^Kd^%^KanO~tvXnr^B} zh^peIqTN)un+kEW)+_0zg4}c}qraOf8Ku14lm#o#O}Q{iyqg{|{-LY7=c;bG>XgST zuIhrTwP&`gwa+0}b(^t}5MCxfjrR+>2dRnyZ@Us8?6)a)PUM-t_z`#Z?V+wJz@Gs*+uK!B$;eRg$YtuJq5ZTDhv`t}5PDHE>mNuBxUh zaiyw?tBQ72;jTIfv#hHs>8gTURe-B%OUkIL@^)1gw&|)|T$Ka=`l*M$D#usxoA;2f z^-`v<_1ZFD%Z+={_6O-^(?#udQ8cl%xWUnCyNlZ5qRRRzelmOes%0)}F(N}g#jkss z?7jIE4LfZ;%|B0s6c<%Q(k8noe&Ba?QC0G(kbEjApYqG6+-WTHsarnkf{!}t zqq2O|E+4hUN3HTvX+CPYPhbPnFdvodqmq17D<9Rshx7s!?xRZjD1RSi@lg&w>Ylf{ z;;pj1)j@By$6IapR_WeqvA3G-9XP<0;vG1|)YUt1xGCN{aD)kiwXC-a@K)a5%Eenf zw5lAdI%8FbtZJ`SWm?q+>x~lgOiIm%G*}3W;XTkpu&sbqum*JI?fb9+Hkn*1E!@T* z+u+pXk_27YN;W>xd7YPL1?TV^^0-#4gqGlKFsWWy;q1LxoZTmr^# zR@Wd0Zon7C*bl)|{jSfk5`iLH#R7U)Cqr?ePp39Oftn4-M@; zc2G$U%EdvgvRCo;>R=w-fiGjgaZQXesgkBd3$xkm4tg7z%=aDsi%jOqzn{r`(R`!Z z-y*o^7sC!Jm%4s8)azM|H(Y&LQZVji>2XDXE9&3u&@scLn>rK4g_$&#se{I z|2T!YNL~)P9}C0(cnWhNM`yN9&hp=q!(4Q*^Z&<5%pZF#4B_``0;Glvi@aGNhrj+% z2J>w%E_E`N|EUb-h;Ljt<&i86|KlmlmFv24uT){{kP4ZQ0|8aJS0Dw_AsZ~!Xxkte z(jaTMPTh`SVMv8c$bkSG3quN|LpE4qSs0Qb4YKZA>FA(33qvYoLJsJ@XyPFS(jglx zHCY&vAq}!X@j?&+36KhzkOKj{B*a4sq(e4X>aaW{LmFg(ieq_5fKSpaaW83Zz3eSQ1$tk|7PUBJ$lj8`6=5Ar&$q2Lh5<7*Ze| zvcb}cg&`TzAPaPFJ|U0*sgMad5YUC?AqCPQ8!TN}9(uui*ar`x%qwJj!#LOi=ON## z+y&4N7Qucnbz@u59wxzua0Lo>=ZzT#!!q~|oO6x=Z>szPGt3M9~Xj_R`7TwWb|Y>kLcjEYK13JLBM z9ugWH9@a55xMFlzL~vqMNT<$8(NUc%7BH1EzbqXpyynodCkA2xHD;hWWtiTvmC{AR z(_Jjd?^>(L!ISuAPy3dm&9)i(qnRq@BNp>i%jZxyJPi;%??Y5&@xLlSwE3`6&jyGt z`27IUV(I#u{%`3JAtF4gMs)b|9U{br(mu9P_VLP+WYIlc<&Je75?vv>qbl}i!b2k? zBBDb(ghmG^R_IhAI6SmtSa8KoiBZ9wlR8I-gobqv4-e_|-|i5hnY)gpdgMa7Q#r$%7jIwrm ze2iZYbsgH(q&)5D6Rk*Q;a`n)Z8EzHNo3*tPBI$A@@5iQIM&n{kJdJ?wnP@X+Bd)zwVjNT z(I67eNMzx%y%JgI?A`DLpS@p6MuRwUR3ZyEw35*vMm&r;&1b`gR60X(&J=N$!n#T# z3%gI3(I7l7%4iV7V?!_US=E37mne2Em&n2y-6XQG>01(6*y@gq1~Ip>j0UmF%{PZR zdu)-u3N9Dak%h5c#_7nykWn6pK(;1f?zbt%fbC24}ccrU>^>N`3 zsD0dg@sAXX6H5O?QQ_to?B8{M{p(6s1x2G)GI8i;WumKs_Cxr{HoD?NJG54xIWibT zyO;Gk_O;35#I)BxadW0tb*~=2**3A0L>6wGB$0(boRY}GE&262Djbx^!Vy)rn5mts z5p5x&Z*!4U{w=fp0+ZH^kkKHVqh6=7;^lmLG>9Yl^l5NuE|G<^d&+1K<$Lb))3Yy^ z(IEDIv#=mF4?Br0{AT+{RDQVip&oWReVL2~(X5e-263YM9qFo|Y!QhptoW&~{@!DH zNMvD~I*+Mrn)X1C1`)PRzV>S?^8=W58y3;SL4>%j4riMG_!SWp`wOp(q&VMqVHCy0?-xWL44g*MuYILC!<09G+Y7;PgI|5W0qMSugB`;KAcmX!eRFG z8Wf+#Osq-K*BkS|ov1t-mxW1JH z7CIi3z``kWC9tr&=kR!DY136kgIJc+rzy33fqj}$3=fpR!X{mNx1hFvyo@@1=$r0@ zW7x*Wc5kI;@$S`{qQ&rTZ7AMoCxL|y&1E!*gdq}GxNnXG7G{m;(w>l2gS-M3?VBw=C5?J`zcnK`* z_Ek6nt*z5!2`s#3C!;}(DI=pn#Lumr%xCSVNnqi_g)$mMmwpmhcxrVK z10n`cJN2dn7QTK>0t-8QD5F6%3XjHSv%T9#MuQj>EP;h2`rpmO9BhOye)x+HIK+}5Ql5t#jCW9xqN>%#i}|ISa>AA1Qvd>UjhrqHIm^V z7L}Er3RZn7frULE$#4+6OPow&xptdQFQ90ja$+IHp=B~0MC7_7i>W1?&0a!rVVDFK z_BwH5DYb$R4lScdd`X6b*xFx)gBaiFpl)rg%OtSyr7QU5Z3ASe{CC z;NF(NMXoUknbRJ34l_AVk26v*W0^qvA)#O6DY;ah>p0&-_csv2M7_yGcmt-xX80Da zvNkm{?)kt)S^=@}^>O+D7v?7rsPw?Wg2tNTB=dI3ud78WY;V zAkaJTKI{g47Jg*L7j$r71eG}G1>D9G2DyP!Z<1d#fv$7ojHFT;dn6CKJna55=g)%C2-MCkLD0X zK@bZlI6ba@Ym>>V3eBc^9(DASyMWGdwx* zG!YUeagv$TqB!G*sl>$AkPKsBKIjvF2!3EjH$9p|F)I4Fb%EiauU>ts^d?U+lk1W= zBOkZ|6BKkS>q`{}~ z^um2Xubo39H+FDy zKGcU~A#;A;$xj7N(fhs%&M_re2M5OM;J{kB!GY%7;J{es&=<^1AFtmGWznae_E!$` zyDpe_&t-bIeqzhX!QeZc;813G&t7ul7I#_s;TfqIy8oC*7ieu_A~ z#Xf&moSwtHeAXhCKh+(;v%@+6x5M=Oboi<@mTyp<`%SlnHK7*h)}RjP7EQN9x&f#U z4M0E0bpzHIxWWH3paREWDolgvFavaZt)J?%VGig=t!{NQK|eZnz)si&yFoYEd*Czp z9KL|Pun%;b|26D~gTU_~OY&}iEhF`&s@vs$pr0oLUeo>OZiJgCy z+}k3y{F`ghtydY@>eI>YFG0x=Vm~ zJ+gB^3uhh{UUtuP<@B=S2j$a6~T zCp`q%h?~kC$Jy$Ttt=_`U9qLr{%+1HK&AA4GF!azU0z6!0D@0b#P#|=YM&r?}OSgyNFH1$I}MN@AvUo`aR#7HQ&0U)H1*d?i>6+_u4wAMLi9 zrhd*O+4@ai5>0)buW0IxcZsGxtCVEx*PS8R`VpH%R1c03QGIHVi0bwuL{v{TNw)rt zwIZr-nl7UHuBjrb|I|Z7^<`zH|KFpd4N?6@EncW>MLrQxz0QP3RPqcIQGHFii0YsA z;tkODV77?rOZ(+O}KUDEYOzbK;mu?R`mUp`ty^;+#kRIk}UMD;#HKFm+vE@`r)1;s-NjE zqWae@MN~iHEb00~M~SF@F`tO)FK!S~eaZk4)r-VR|G%{-h^Suerikk2ABw2{QFRg3 z-M5IS-r6jp`l~ZVRG&UjMD@hYBC0Q&Fa7_nZ6{+aClwG?z1M6})mK*#Rei-Z`Yks{Ui#NPcbGepn=``n4sZs;jP|s&9HI zs`{uEqN-1_gQ)5rEk#xL_Y+mUXsoE}XWNRZ?l4nS_3!N>y0iGn#iFXe(@0cxk3><` z?be8@esqDT>KW@rRp0xjsOmAtL{;}r7gfE^Sy9!uOcz!CYx~GStn=-!L{*<#PgM1T z!$np9x{|2s3(`bYpYX1z>iz5_U4KANQPq#9h^oHzzNqSx>^%@*Yzb9FRj=P!RP{Ay zMOB~jk*MnHzYtY@Q(sZl-~3)w^+`X9s_x!hRP_RHiK;$gkG@gMZ5CBMaGt2@6^n_g z?%P^a^&VG5RX?~&RP~*$MO9C9zROF6t?@Nc)yuRIRXxQd>G~_%imE;#SXA{Yr$kl1 z&_Pu7={-bMFLXjw^}b%`kyC8$(W0u~&Jk7J^CMB!7j6+%ec%qQs%L(8D|<<8o^g*S zimcxK1Pxm6vUXveUA9Au8d}4D>sgR^pby2Y~ozz0HgkEvr9K+FoIz zR@(GC;u`n{a+t|EHO>%CD>9)i^ny2G0qB=S9w;K3=GC%#Nh9~gJyE}=>P_m`&}_KLjGkIn52aEQxSb{2Tfet%2mLO3gPB~X#~BJ{ z4JNdK!Jvh_rJ!GEPr@x`G;3KsjEdf55^w{_8}U}kC!(p{%s3;>-^N5*C2LO@2l}|} zgk!8p&5HZEkK`w78^eeCefmQ<2>L8|>YV!`RP@@~gMJIvJFo%t%e6iWKQUu~mer%E z(8fxl+gM6&3rT=ylcFQ%nHy(_GF6$NPyA4r3d>;+oB)1B1o~)MJ(|js9nfb%>zDe( z?}z)$J_Q9f-W!#^d|LL&?o*5Gv%EhXPo%@ zOwecHHJA;X;mL_t@5UKwc{L`s0NPkdGt^hFJ`4K9>rHwQ@hQ@^KJj{!`s#fLR)Icl z*OMh#Id1v1tX_%<4WJ8*0DauJ-lpZt zmDNM3$AP{c^*y{6IE{gJT2?PaMK9bR^x8Qza$}d*vbsJbi<$GNmer@}^{;^oOewEr z^`=@@uahgQ=gpPXYcK~FjAf>ymsW0PiJm;$P+R9nWv)?qo8gl;_-k4DmqQra69e^I zQhRqvbN?-=eU;X99qhd=D?F6m>PjbG#q`L1tmeNbvpZGrwrnw0x>dgYHsjlWAeHS# zvmdXyJvQsL%jLL4Qd}35SkR?`A+ z+F7|xwDL_Pw#o;-^GY|%Pg!7AplV($r4WC<0My!+4lP3NVQJO8lu9ZRT8wc1VmUq~ z_z+p2Ay8$AMpcHgECfS2C=Yu}{hT4cs|*If&l&Pr#eQe(V*fxQRnwWhI%vb;Xqlh0 zBT;@s@J+XI?96z03y@9KM3@AVo0rIO zoWdVdVH!+_8StwSimme==8jKnO}k>YE<9tlE}%E4?s`cMy1Y&}7u{K*TR*3;R)JudOH z8S+Nre0uHx#ZS}Z8KGEB_f)C5Lll!L@BL^PiSv>tpF2RYqbVVGfMOEo<8#L+CUM?I z;{4*$UZ(J$M<*t6KH!fGPW)_2yocG(Desenct+g)orL%lx91b$yShD>5U<`Us~Wgj ztB?@Cz1*`2@v1Co@Xw^g`?>u(CEl8N&(+#FN2kMEyPt7Yzf6Z$yIl1c!P{Nc7FV@F zQsP&+s%5S^CH_{3PKlrG`bnN>cows9z?q}0!8@lVq1NsDIy<;6M|p0xN>s~TlxROOr+PxIng>L~oT^WsgV@;@RQ z%g*i}q{cJd0$%WEQ{%~ykc{{c$}k9rNQj0?KuWu+W*1(bbfhA`o$*fp2a@rw_LA#H z&jEih*)`2T9$ZvEpkg#SYV zq(Ua-z<(0otCEuo|4DrRPvW~ye1H5n{h!W!C;OemRP|3Mzmov3$aVkQ^4}BLen;L_ zp*>83kKii!C-Hg-Ltr@^0%ua*BcT&ahn;W(ijeXi53j=-I0o*dyjO--VGitpyHLC< zucI&u*25{tOUk-Azq_7aa{a1oV?<==^rdEhlUHs&JCQ|0><)F~S0JNv^*5^_O!!4g`%QD^L_KwF;m0N-0na!v zv*Xi{##z zr6G;AqM|~p$5xMujS8tyt9oqh+7)U>MAxbq85LPQ{x>t&ZQ&6iApy$A=eGkI>2P43wYMOFPmpFR2m~;WOT4Xb=Gr4=AiaDT=`CL$;;HXO3vy<-rCjA46 zV8SnzLyDbN^60fM-Oi-gbhrw%sa7U@B-b-`wOm^Rqc6c`J-RYUMcREpX$<2BnZEs^wGq0N%s)vz$9E)c;7NeUTvh;OgydK-fyIozK zFt0O_SSWMzq#ASc9e41nT)o!Y#6Da;TNBN}W_Q_!S7)1XuPT11?dXru7{Ip4Uq$0U z+UmEBUO;h3Rlt?AH7gRjh~mhN2<$jp;Hl6h6rE#p| z9--K}HoNIXF-L75cJ^OI5!|{UcBF0ci6Yo|w#=CY&_QflM*F=_F{(|z^%R4`e6bE~ zm7~2kQWTk+ACuGe(J9X@6kAs3#j~{?tl`OC*?N5M_5nr0t1jCq&IGxANYT20$43+k zr`_L4F?7zl9Ktx>U6SKJfSn)i>+u3pCyi)am?B@5Rsj@K&2@@UJRDj#kfM3-Iz=gh zPSg&f*#1szF^YxZRf|)UxLLUb#hSjAOHx!B6N&F>ySO{LG)1|uDwd)6>1=pe5|*d5 ztq@GYa;1oJO!GTej%Wv4v5loeDD3W+;WD-*uPYHoal32r3KX607Y(O~E*KO+5#(Pq zlA`Gch47hetIrk03b(bpQLqw4(GLn#rWlwfKd!MY>zrRzikbVo@M&$O*89a!c+4@| zC^j~*#8ULA?NOa#$cAGtQ+)Q*k7p?IbbfG_B75lVa}*Sb6HueTuNV2Oenuw&?Z+1qX6=7Bsoos_Nry)ws?i z6_cw-+;{C3bAd>@99Mya6mX6uZethar|>!KgDf}z-@;)y3Mb$cu*cFnJ^Q2GeC>1V zC=H$Qr(+l-Dnd0?CK(XZi3zUQ%v(Ft-2~T{Bwy~J+zor+3-}VghJ)}Od=JM!o8zZ} zgR65LC9Z+vrh`Xx1dL8Z)IkSgbDO$k8pCfoGrGtxMXFxOfE=?cj0og za(PL#3Kx&Wa&U!6pZlDD$zrKZSr-~WV`vI3pcT9XToMw9kwn=AUWIuZoSw}(lV}7Q z0`cbcsaXt5;5}Fh-18Efv4V0XaGy)e!5T_^>e68y=u@~JHo!*M1e-yh(yg!!xS!=< zb0^DK-5J>Up>EzO;NR9bk|c3xLWgU$YU62UENg%oy~fNdMULlk9z1f?WucabHC{(_8AAS zAkeFedg7rKPR}!~2=zcH>Y#Fp1n~zC5dBL)l>nZ;zw4vrcvwOg+a)s>cAnakXIcb> zP6UZc^5|WWlIOp_Imc9mKY6oP)gT5e?wPR^)j^-s-5x)?Xi4s>t9y<|T_)Fq`p^Ix z!V@EHlzCbcs&u!enpw;@JzMYxeWm?&Z_{aR^yte|wGh8&`0tadmazJ7Zulv~Gb3vK zq`609=8mc{)IaH`jJa2@--tMREM{+eXSapkKV^KKa%ywo0w-cV>uw%e{hfyyJ&T{b z|K-r^nEqX(0*0xF8NTJGtbJ+Y$(S3^ZuZ5H62#o$#}=}YUlb1S7J)N_f6m3gYRUd zuW#=1*^VnQEkASLRkr1wjOIPgb$Q@?HO685xUP??+{t)w`N|6|t6Yt_H~GEkkAm)G zcxAu;$&1~u#?0zia`-XNI~m_Zue~{Y_SKmDw?=QR^yBS}uYGR~JDzbhX6x-0VJ8pY z&Y0@GV*C`lYcT;Xt$i(F`Sj<30$5$^iw)LE^r#k48bw{qjv zw=?25<{!NArE4+g{Z5UDWB!OQD_`1C=UPnLH|E&YFMTJY{!e=^ec0h@OuzmyO*db? zlTpy;jrwQ5zZ^3%vB1WBuiedfROyS+#kXIIX@BhX`~hFy&8S^VeP}IkDW>hEE@8o! z?q<|46Y|#mlNV$1RhiQ#@f62*%s#it*DuCA?%H$uXAzGwOy!EU++FW{%)|aOD;<6C zIK#>NfLmJT>6l?F_U)Qp$&^`e)zrgovya6D-rC*M^_VI1(#u)nI*mFM<6gC7ku_E9 zGo#86yZXB47ctJ0s&vlZ&oT2(=9EvnW~FCDxX!5mT7H+zHFcM@t=Rr>Mg_->Yt=!w z%rCtUR$DRWe8$UHJH8v)$s_Zl|JUA^z(rN{VV`?vVSr&BMsQ`7O_6;SvCL3$NvSMJP0duk=iEEO;DXqfUw-fJ z?bqMKy>n;g&b>4L=RE)AIVbnt6Ry;hjflPZMu($jw7m4U<6is!D2pFFX!JH>L0?Zz z9=G4+cG>Lxy{ z7U%;x_6zDhIzax#I4ZM zX@i@^yZ6KK3o?Ewovo$3^O-F_I4WsQ4|Py@mX`jpz$WUq8w&b+ZPAQ_L$tKIw%-5H->R(+(T8x}eryVtZBb#U8vD4Bf8FLrsLjnYdTbG zKRG%eV&iQy?S`;fuWUNG#hXG7)}ZWSE;cF@n> zDIS}{UXD1u`EVFVg?C#{%dXrR)>>;Hu&9{O?i+u5`SLdh!#170bo!H4gpR*Tt8Hc- z4Li52YUKWQwPn-uCi_j>_HEb;=h73~Mc*sayxO(@ZQGM!o6iOgDP4NEtkZbAv0o`p zg`GNoZ0;?iyJhcW7O&qUoC&+Ww%OfXo9@8h9{uBBR8<(SY+V=oh*KbB2U>W2=+vUl zSy>sc_8B{3!L-$*Qqz;xhNq27&df?p&t93D^u1G(mzS@uV1$Z+Btor^kN@olL)tv#UFx$1BiAKxkgfw<1>eiO`VWD zEMa_h^4K8KvTF(X!%w<-WfBRaFPQ=tcLe)TAyMY2!wxkFCO);-vCg@z6Jv!WV-PzL-Ed zniC$OfD42pXL;N&^#x*f8PNjFnLxc$^dc5^V#K0AidY;)07O4zQ6PmZ1QxOg6N91IyVSeIOTGvlD*%{@A*u`HlLT-h?nh*`A8%WvyD|wy-%E4IAesD;6G&m%p3L^c=z{n^O*s6n7jF(g*o`xOZM-E>nV`;JR zjEOOp^fUHoS(!8F+BLqDuFTn3SLV!kT^?*#Z~pIg&ucPQ^Gzt}8(rsT=c{nHKq8)z zpItLj-_NdY+|RZuWPY|BX9QRK@3+kdboLME{D7auV%(2XNfnSw@PB-axhrDKf6v*z zL;lgGc@|~H`W7dMS&c5rTm$D@Lr8qR^F_=!MRdN(%7)H2j_IQ^w~;~!3nC+K?GQ^8 zL;0(6eTO?Bb!=)vT5<&$wjOt}u!2O{cvTQvSp|`;V3BB!MWRm-xvk>~Vmu!)*65V@ ztmO6F2wTMoA~Rdh8QQv65aX+)M|xt~xTIuw?{tK9vrD*PBD9onQ8s=hoGo8TWFj8i z;Tawrm6_%7oUQ8x=qpqb?kkx^FC!TbN1i52;35?mTG|Fw5-UZxazT&uq}0Ut?2N2x zVwNjgYU_NNm|#*dI(@oKF+--wo1r9!Dm1a1^C3KEL^!#jw-u3h`F!S8lgk4*MKW5R)-*`gOZ^a3p&QY z#=ux0?;AYOjGPdJSgi6{bTk6CHZ>!VwE#yIRTr zK6=H-pM+kxCHRH27}q5k2{0gqO43HDF-?(aEb|naW|!4tTHw2Y&{&#+6sSp(+S>^g zeptpxN%k3$QnhV|>ktd)i6||;jz?UzD@O|D$h43b6$I%^wE-rdBv3(OIE`_+N@Ehs z5f=s7Cns3-Dv8r*QaFt^jni0Xa`^6>5DjmZOOkCdNtBzF5EJ$t{t^>@j-|1@kdNcc za`}NgU^%W3>rfe(*IZWj$9y@8yKE3uj6bT()s}pv0huI|mc98LHx-k2FIXHb4@O}% zFgxI4lwf`o1Ixr#!ORu02KkvFONFp`ZpppOa#pT?FU#MQlxc%3{~WW9 zhOT(O05fWw8U|q4N8IbCB@xe*{`KYV>Q76SHa;zRzUZG{H!tsqt_`Op!NK~|l6v~* z=k+x7uPFr%|7G;g2k8Dh)4!)tY<~*+*N>^*G;6~m{f}mVBwGeQYfcqJhp2YMC6{YW z3?#AkItHkTwIFgm{?}Cr-`hZ`y?nThe>o$6?fao89yRiL?q;IY?B_+&8un)*q@MVW zzezpulV?bH>2s!nI^cP^MG{`_@G=#9;(zI@@Nvfest+Wo*}co9p7^jeQcwKC#iLF$ z)_2NeW%hiG=f-%8QnSf&Ze$*|J2*%^@do=mMYh*{mV}pvZV>|yg8l6#`D=BUc?xKD z3z+8_4a{SSw}~)D)<@&S?;_4oA9{y!pl=D8a< z{ji`&^X}sUe}pYhjEG-9G;}edXW2%I43bbqBe+mc^fa+nb)2n9G!JTM^&HPhP=x5> zjg}?InYU!cYmTT`lGzKUW&|>4Pat_0jO6tsFU|1DB(KJ!&|QM5z$8h!<#ZQ@a;RFtx-lVj#SE2`d51U#9~qRu6L<5ai8( zDiRx$VT`fi0f!b0s90S^0;~`_E-;{)&wz?mzc7GvWIzRgyT*6X0hOB$sJi0_P)CF% zaNjk1K<%CytGYgs(t!kUte`>SbmwIfrz3l1s0n1Oj^u$;83>Hm6&O5{*QqZv=3*dn zx@91DWEJ;lS_j6fN9!)hNgRqM^=TbmMhiZd%J>{__65=6S3r>1d4SK^ftZpxX!vLX;hWxE*)z?qI?ASheqzV@+)r=Z3_7?V+9((KXOxfFu3lHhf zH8;A^^>)n4>HBT2eAw5H98DjAu$pW_EjK zi#fU$Wm1&$?nns^FZPt+@TRK-heyUpQO=0>r6}i#P7)jz4Ba8-=$u9}I5b;NSS*gq zw~dwxPQw9P(_yP&#{qC^hJoJLNP?lP(cu8Yz^M!aStTgL&ejYA!R4;;!8#1I)nQ;i z9Fbf9CYt?VYzMFpq148NtTmaFn!?f+*0Ho^!1W^3_dOey78UNM?==e=_nOV^@U41& zQgLXz@p+J?b$JX+!vd6kPsh@l<~sk!(jLXq$WobNHvj)98TWKNO$)c6ELYZbn`A2B z0Sx5^Vz@7#&vx!QN&Uh!UU>y zNx7C(V6Py%N$>$0$W@anWfS5)iF(m4K+!O#&j%HzgqYu1W%; z!!Z&Ny|q9Bq7W-7-PYo;1VnqsNMXpve)5IG>(`C9NJ3EanUsULB^T{L+zVF!0gF%v7XygPJWS*a09F-1BHD;OtWRI z5`BhJ)9hAVYAr{hu405sBNDDr%3DZP$;ll;uE?>5GC71sGmholR2;b}Cu)wYQsSHm zie6^RNpDm1k(q1yuq$OJ&p~>O#J2(4GTVu!EKvJ>Xe8s1+kSv`5m$~>8kh-uAh=f) zxK|w1Xu!SVOr>nvdR;cH+}M%_OH9&nuV4A2NSxIt(_#(s>N07>{JIHFxIUFehydoI zmrcVdy4Kq3cLv7$2l`h(U~z#CfF3% zKpmUnIF-W-Cc1sXgy3mFgI$cCZoa7^|jA&=m2W1WjixyMZIAXOMhVj6{!Ep&=`0 z1S~lmWe7BxAy6soH1@bL1VW(f8t<(`AV(bny@VsO@lv_qn(MiTSMKqabpd3I!BJzS zBOkMJPp3MsHW;>_;lvlPD(q>;aAE+%311yf zwAJARUWuu#t0swAqw-kUgHWN#?9`Olm?0Sjff|`NCgmyxIViU<_EPdF&dpY9G*xP) zniu&;cg93WDSB(g$~)wbP*$Pb4e?SL0huU4F)un135a#>CaRZ$eTw5Cv3s&`1CE7dasxj>a5A6*FATJF!StY;SO6~iTF z;ibR}8#bg3NeM|wzR5oR9^S!BJSHJH5NsgW5C8ZiCC4ZGCi(?H1IMjan-JvVi`i_j zM`DsMglW*+!I8x{(}(%vix4H6ghQGeN2KtPNX4 z+SMWK2|0=pO`ZwDo_6^v#IcQtu#$*OOk=!(tJtG1W(aEz%*RPRU4cLD5xZ6YP$N}0 zpTI5wzMXNYyXlL_`1=Jvnm2%5Jg$;1vIH$D@VyQ6W14aoXArGL3b|(0pqUDZZis&1 z>L4Fa3h*8Kl0!k2pRTp42oDy(VqlLEkinT$la{TD>$C_ z6&WIs#}xPv;A6{m{Qb(kwSCVD6ZYZ`Fx}?RARm;rg z^250}IR;$~{$Rx-y&8N?9)*;ROe(nlR<5x!m9eCbAUCL#o8`zjGE+gEWa=ClDUvZc znE^6+rCgWMQ7h`fu0+NHe1bv@@TJRGa~8={M2m7VUh+rF$t20On#?CQXN_mxgT*9b z@#B}1T)DMhuACJaPhkIQ%(GDKi<&P)JpvTuq@3(lgh5?KK=Xa|eQ6n-aS7o=l_8cb6CpbtpYbAGAF&Ld;|;)P7}#~! zqcyt9diH(-^X;TRkHzTl2O!N~4nz})5jHwuUuJj55Y~|OB2%?;n7>E|DxHW!Z+!BK z<6;2FtPAmgPxg=(v4E2Olo)9E^v)R3k=V0fAr&8V(|W$n4t7DP$jvMiG6p=U^Ka4k zvE)9RMp5S9wuk8Zx5bSx2>3VEE#}`+09QDvM*JJ<%L%$6h_?|@I}r<&A9;H>p74+R zxA1PhogeUTfl+!5Om%S)#I7Fa-~I#t=7R6wQ~Nj1XfN^yVLn0Ez%v#ibU6+Bnc8#t zthA#Xm!rmCV#PkDO1 z!(#eZNut9tc(ndiJZBzqL??fxiNx}k<-E0DInTV+0iKlddftllO}S%b6!}Bms=bVP zt9RrLy;Y{dp_HRCl0)n%XHB@vOb31pno99)whU2pVX0_a!hH@uSzkQ{e`Weq{wh_j z@2~J%=NECPRL-@lcT(7iURGllVm5*(n4|C_{;EilnY3N>IN!-ckF%uS<3uu#GmkkD zC+2bFr7T};U);{lDMJJ-Pxm2g*5&|@<5uTd_L@SHk%i^{`5yGsS?RO^xsy}%g#)826m@8s*-KK9J(iuLaK*?4UrY7Or_r-#R z4|*j5HT%L_k#;Bs5&9%GJA^^g)(WlQ!lH5o)Wn2>d_%O>3Fo|&Xlxc<{l(}A)>RFi zau*vPwWvL6PK4swW);}i@>i6h2s64}IgWkj-7kNOt*m>Okv0@MzF&O;!&&vk?=e*E zKY0>^k;ivGV3>2Yq7p;do{Cc#6hYr1ge$x>>^Mq4YZPw;P4`X0d9Dw)zc~hDAU_uM zp2Ga1fq!C1-8_K3<=TT$e_=?jj6(iV2wV`&&dn&f$2|;*9edTPNYqR1hm))*;x)A! zhn>sDcjPg&?H($_V7V(8(>~$SDIW!ff?7W%h6%(Smt9!3WuO{Eb*`T&hDm0gW*G7> z_?Tlz9MR4K!?m4mmKZXfoi!NT@7N=6C@lHNUW;MwEISm%2)-GutuaI|Z)tHvlX#rin(M~okq03j>tQGccceiYfVf|9`HW>b@FtNq3F3->oLkm+wdkpWE88~3* z`Lhh|`$B{>?}UL2k)a*Z+wm|yt5CS>t~2hyjq2Mj*ivWw<%;1)w;OI4qNg_Rf?;~d z_gyix{^l4OIfSbR$|EqWC_K^~!-1T`sK6CUM;=6!T?qA@fnLh|3CU96odw;ez>vlj z^TM++z6pB^_BQMt*kahbu=ikvuvM@RU~6GW8H);RtYPz07z^2Ng>8rJMo30Bd>=v| z*f50Q%4=}FHMqPQTuluwo(9)SgKHzMCx}lju0iLH(F2C;qL`Zwz!(e*g@wVwVXR>V zA4^PKV3n{_u+y+JDnjgxm_Im+4&igya~^g9 z_9N^f>=LXR_7m)9*e|fluwP+UVAo-Pz;3{9!fwI-gx!Yy1-k>g3%duaRS|B3Aqh2< zbv4lN*dc?-VG0<#4+bz5jNK0-m@$mq7ZVtM65{>Ak3oZ9f_R_s8_?hfAl@&0^)>k3 zi}$TL#uhMk|60LX!`OYah2h&R-cJXNjxcs#+rpe-E-+V^8w~fE`ME?UWSfh(HCQiLN&rFvY8NF znn^w+B*e9xZ2gRCOzg4Y0CR*n!P>%{VJ@)sMm0(|jP9^@FhnDy^_)^n0H7pOEnrqk zHkb%F9TI6&FGF|1xW+UX`$Aw$uQwEi+LJmVI^lv*F*d_tEDP05T`xp;!RQ*_1N(Zy zdchd~>{F#uUtkr?t?$%kCH`)iJo>GT6v}BWj?XU?+$w^;35h7Cu^+eEwbb!FVc>`R zX2#ttrmGW9RrAB%6=t^|J25R}6Ft+%*Ya5Zg@XLM^DlC*l+csi&*s+dn=Q1@p7-6j znV-?zHP%0t4{#J#_B^~eEp#hw{?f?pgGbJzXO0}c=+wK6=5`NUW_o=&on!KQQO?F4 z)Hz_cW8tz7DgSCs?24JYsH$?uwUApI>4!Hx23gwgp?$6l$&zi}L|^F4`?jC(6`eRu z?i`r>DV;mV_|Jlid#Sa?dXvoyrPONqD}BB`vX8ok{yKP~)fY7OZ0esWgZ9(XoyRs6 zJ8q?$2Fd$%-@czFZdN6X{&g$;W6h4l_XixHKlPeE%f8Jv8gAA0y`5(c(CMx{ZzpGSs2iv_jYuXj?sH`A|& z^irRFB zWmM~$d-IfS868kaPG1dQPUlZ)@zIuLWz@FTufn}~A$|R~k!^hsNGCJ}F%gbMEE2M2cI{Whyp3Ph(VYsY`)mEXWd?7f#kYEBL)R{%cVD{FY~<@-(%S>K4RJh`Plv^qys8?!mCB7* zZ3`aw7VW-0_ILLmKBwVVe|)1)+%&rQV5j$G(WUfG*5@w0C#BG{3kEnxubi8(P-uH$yHe%%2~En7|Cp4v zR1l8#{-j!2OmA*E6mrgPxo|SjCTHN9b<|yUxVF^MNq$ z-T+nSr614?Ztbhw%p&2Z4Xz(w?74<&`hQnGZ=7G392>` zxBeBlb(r(*IIi@qNVN8c`;J6N@xzVXA6 z0wa@RA>vTlt?0}G`s<+!oz-KDh1{PaHb$*pMwfR>w=(&lSh()^SM#3gcPSS*^>b78 zCqm_t#NR_J7ty`VKa^RX_(a%ka%i9D`g}U?&Y?cq)tiL%)BFk@CgsuSGglYeWNsEp zN@h;?_L@smddxR#X;>n-UJ1V9zT^$sVyDfymZ2pgr8TOfvmppd6(jckEa+@@Rf>a& zKAkOTjLv%QF>qycHU(#d{~L*vL6zYdN6G@xP#+NFk@`&X0Bw|tUfV^5Bi7voJsPuY74j{BaeC&c4A6vl5;}9QP zUl$*fBR*Em=uMrx8Xz}TN&u8MN#UnIf8)^!01cB;%hXA!v6`Xu`%-Er5Zye>lNm$} zt)WLtsafeuseMfvN~z^NM*V#^wA5ms7_P6^Qj6pv`EKo3#RTBVNsjEhucyX>^(_2_ z^}6!-hr(Z>Ob4ESs0-r|6I3H&mGv${Wzo`;uzp4wJD# z087usAq>wE*2qcL#Rc64pHOf+!f36pT$173C07Z5-7~T0^hZcipOdBzE7l!lE zy{j`CGEz0N{sH5OaNvu-+Moqr_z;x{ia6=ZL`> z8KbK}0A1^a8DFSV=IAO3GyaMnswm8u&9^39bpdoqj6| zq7ON%r^iSx!-rfJeaJcuGez|go6b^B#98ew6j|8z8 zI$KYoh$lVeVpabk#?u;fb0LPV(`Mw$SsL#NSwCc3Wy#Fdu+o?NPNe8zCz2fU-NuP< zB0v50@m{zME9L~6=Vy+P+y;0%+yz?&U6Z~?r1E&!9zMb>42&+69bE4k#=@Jt2D@%P z^@}=v*3OC9sjtLmBT>>NWAy0w^rY$Q6Iziq!%~xUO0J#|DsQrg>$=hLqmqZk4<+8o WyVj%HAt`xy{J6C2U3gdVzyAYAD|6=n literal 0 HcmV?d00001 diff --git a/config/quad.yaml b/config/quad.yaml index ae9e1e2dc..538a45def 100644 --- a/config/quad.yaml +++ b/config/quad.yaml @@ -5,6 +5,8 @@ use_rk4_for_dynamics_update: false # Enable Runge-Kutta 4th order integration fo 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) diff --git a/src/config.rs b/src/config.rs index 8082e74d6..5326e78e5 100644 --- a/src/config.rs +++ b/src/config.rs @@ -27,6 +27,8 @@ pub struct Config { pub mesh: MeshConfig, /// Planner schedule configuration pub planner_schedule: Vec, + /// Rerun blueprint path + pub rerun_blueprint: String, /// Use rerun.io for recording pub use_rerun: bool, /// Render depth @@ -94,6 +96,7 @@ pub struct SimulationConfig { } #[derive(Clone, serde::Deserialize)] +#[serde(default)] /// Configuration for the quadrotor pub struct QuadrotorConfig { /// Mass of the quadrotor in kg diff --git a/src/lib.rs b/src/lib.rs index aaa60bb8a..77c9f2239 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,13 +1,39 @@ -//! # Quadrotor Simulation -//! This crate provides a comprehensive simulation environment for quadrotor drones. -//! It includes models for quadrotor dynamics, IMU simulation, various trajectory planners, -//! and a PID controller for position and attitude control. -//! ## Features -//! - Realistic quadrotor dynamics simulation -//! - IMU sensor simulation with configurable noise parameters -//! - Multiple trajectory planners including hover, minimum jerk, Lissajous curves, and circular paths -//! - PID controller for position and attitude control -//! - Integration with the `rerun` crate for visualization +//! # Peng - A Minimal Quadrotor Autonomy Framework +//! +//! A high-performance quadrotor autonomy framework written in Rust that provides +//! real-time dynamics simulation, trajectory planning, and control with modern +//! visualization capabilities. +//! +//! # Features +//! +//! ## Real-time Simulation +//! - High-fidelity quadrotor dynamics with configurable parameters +//! - IMU and depth sensor simulation +//! - Optional RK4 integration for accurate dynamics +//! +//! ## Advanced Control +//! - PID control for position and attitude with tunable gains +//! - Integral windup prevention +//! - Support for different control frequencies +//! +//! ## Rich Trajectory Planning +//! - Minimum jerk line trajectory planner +//! - Lissajous curve planner +//! - Circular trajectory planner +//! - Obstacle avoidance planner +//! - Waypoint navigation planner +//! - Landing planner +//! +//! ## Visualization & Debug +//! - Real-time 3D visualization via rerun.io +//! - Depth map rendering +//! - State telemetry logging +//! - Configurable logging frequencies +//! +//! ## Performance +//! - Memory-safe and Efficient Rust implementation +//! - Multi-threaded depth rendering +//! //! ## Example //! ``` //! use nalgebra::Vector3; @@ -65,6 +91,8 @@ pub struct Quadrotor { 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 @@ -166,6 +194,7 @@ impl Quadrotor { Ok(Self { position: Vector3::zeros(), velocity: Vector3::zeros(), + acceleration: Vector3::zeros(), orientation: UnitQuaternion::identity(), angular_velocity: Vector3::zeros(), mass, @@ -203,8 +232,8 @@ impl Quadrotor { let gravity_force = Vector3::new(0.0, 0.0, -self.mass * self.gravity); let drag_force = -self.drag_coefficient * self.velocity.norm() * self.velocity; let thrust_world = self.orientation * Vector3::new(0.0, 0.0, control_thrust); - let acceleration = (thrust_world + gravity_force + drag_force) / self.mass; - self.velocity += acceleration * self.time_step; + self.acceleration = (thrust_world + gravity_force + drag_force) / self.mass; + self.velocity += self.acceleration * self.time_step; self.position += self.velocity * self.time_step; let inertia_angular_velocity = self.inertia_matrix * self.angular_velocity; let gyroscopic_torque = self.angular_velocity.cross(&inertia_angular_velocity); @@ -323,7 +352,7 @@ impl 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 mut quadrotor = Quadrotor::new(time_step, 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 /// ]; @@ -332,7 +361,7 @@ impl Quadrotor { /// let derivative = quadrotor.state_derivative(&state, control_thrust, &control_torque); /// ``` pub fn state_derivative( - &self, + &mut self, state: &[f32], control_thrust: f32, control_torque: &Vector3, @@ -350,7 +379,7 @@ impl Quadrotor { let gravity_force = Vector3::new(0.0, 0.0, -self.mass * self.gravity); let drag_force = -self.drag_coefficient * velocity.norm() * velocity; let thrust_world = orientation * Vector3::new(0.0, 0.0, control_thrust); - let acceleration = (thrust_world + gravity_force + drag_force) / self.mass; + self.acceleration = (thrust_world + gravity_force + drag_force) / self.mass; let inertia_angular_velocity = self.inertia_matrix * angular_velocity; let gyroscopic_torque = angular_velocity.cross(&inertia_angular_velocity); @@ -358,7 +387,7 @@ impl Quadrotor { let mut derivative = [0.0; 13]; derivative[0..3].copy_from_slice(velocity.as_slice()); - derivative[3..6].copy_from_slice(acceleration.as_slice()); + derivative[3..6].copy_from_slice(self.acceleration.as_slice()); derivative[6..10].copy_from_slice(q_dot.coords.as_slice()); derivative[10..13].copy_from_slice(angular_acceleration.as_slice()); derivative @@ -379,10 +408,7 @@ impl Quadrotor { /// let (true_acceleration, true_angular_velocity) = quadrotor.read_imu().unwrap(); /// ``` pub fn read_imu(&self) -> Result<(Vector3, Vector3), SimulationError> { - let gravity_world = Vector3::new(0.0, 0.0, self.gravity); - let true_acceleration = - self.orientation.inverse() * (self.velocity / self.time_step - gravity_world); - Ok((true_acceleration, self.angular_velocity)) + Ok((self.acceleration, self.angular_velocity)) } } /// Represents an Inertial Measurement Unit (IMU) with bias and noise characteristics @@ -477,6 +503,8 @@ impl Imu { Ok(()) } /// Simulates IMU readings with added bias and noise + /// + /// The added bias and noise are based on normal distributions /// # Arguments /// * `true_acceleration` - The true acceleration vector /// * `true_angular_velocity` - The true angular velocity vector @@ -509,6 +537,9 @@ impl Imu { } } /// PID controller for quadrotor position and attitude control +/// +/// The kpid_pos and kpid_att gains are following the format of +/// porportional, derivative and integral gains /// # Example /// ``` /// use nalgebra::Vector3; @@ -2166,6 +2197,8 @@ pub struct Camera { pub aspect_ratio: f32, /// The ray directions of each pixel in the camera pub ray_directions: Vec>, + /// Depth buffer + pub depth_buffer: Vec, } /// Implementation of the camera impl Camera { @@ -2180,7 +2213,7 @@ impl Camera { /// # Example /// ``` /// use peng_quad::Camera; - /// let camera = Camera::new((800, 600), 60.0, 0.1, 100.0); + /// let camera = Camera::new((800, 600), 1.0, 5.0, 120.0); /// ``` pub fn new(resolution: (usize, usize), fov_vertical: f32, near: f32, far: f32) -> Self { let (width, height) = resolution; @@ -2198,6 +2231,8 @@ impl Camera { (width as f32 / height as f32 * (fov_vertical / 2.0).tan()).atan() * 2.0; let horizontal_focal_length = (width as f32 / 2.0) / ((fov_horizontal / 2.0).tan()); let vertical_focal_length = (height as f32 / 2.0) / ((fov_vertical / 2.0).tan()); + let depth_buffer = vec![0.0; width * height]; + Self { resolution, fov_vertical, @@ -2208,34 +2243,39 @@ impl Camera { far, aspect_ratio, ray_directions, + depth_buffer, } } + /// Renders the depth of the scene from the perspective of the quadrotor + /// + /// When the depth value is out of the near and far clipping planes, it is set to infinity + /// When the resolution is larger than 32x24, multi-threading can accelerate the rendering /// # Arguments /// * `quad_position` - The position of the quadrotor /// * `quad_orientation` - The orientation of the quadrotor /// * `maze` - The maze in the scene - /// * `depth_buffer` - The depth buffer to store the depth values + /// * `use_multi_threading` - Whether to use multi-threading to render the depth /// # Errors /// * If the depth buffer is not large enough to store the depth values /// # Example /// ``` /// use peng_quad::{Camera, Maze}; /// use nalgebra::{Vector3, UnitQuaternion}; - /// let camera = Camera::new((800, 600), 60.0, 0.1, 100.0); + /// let mut camera = Camera::new((800, 600), 60.0, 0.1, 100.0); /// let quad_position = Vector3::new(0.0, 0.0, 0.0); /// let quad_orientation = UnitQuaternion::identity(); /// let mut maze = Maze::new([-1.0, -1.0, -1.0], [1.0, 1.0, 1.0], 5, [0.1, 0.1, 0.1], [0.1, 0.5]); - /// let mut depth_buffer = vec![0.0; 800 * 600]; - /// let use_multithreading = true; - /// camera.render_depth(&quad_position, &quad_orientation, &maze, &mut depth_buffer, use_multithreading); + /// let use_multi_threading = true; + /// camera.render_depth(&quad_position, &quad_orientation, &maze, use_multi_threading); + /// let use_multi_threading = false; + /// camera.render_depth(&quad_position, &quad_orientation, &maze, use_multi_threading); /// ``` pub fn render_depth( - &self, + &mut self, quad_position: &Vector3, quad_orientation: &UnitQuaternion, maze: &Maze, - depth_buffer: &mut Vec, use_multi_threading: bool, ) -> Result<(), SimulationError> { let (width, height) = self.resolution; @@ -2246,8 +2286,7 @@ impl Camera { const CHUNK_SIZE: usize = 64; if use_multi_threading { - depth_buffer.reserve((total_pixels - depth_buffer.capacity()).max(0)); - depth_buffer + self.depth_buffer .par_chunks_mut(CHUNK_SIZE) .enumerate() .try_for_each(|(chunk_idx, chunk)| { @@ -2257,101 +2296,113 @@ impl Camera { if ray_idx >= total_pixels { break; } - let direction = rotation_camera_to_world * self.ray_directions[ray_idx]; - *depth = self.ray_cast( + *depth = ray_cast( quad_position, &rotation_world_to_camera, - &direction, + &(rotation_camera_to_world * self.ray_directions[ray_idx]), maze, + self.near, + self.far, )?; } Ok::<(), SimulationError>(()) })?; } else { - depth_buffer.clear(); - depth_buffer.reserve((total_pixels - depth_buffer.capacity()).max(0)); for i in 0..total_pixels { - depth_buffer.push(self.ray_cast( + self.depth_buffer[i] = ray_cast( quad_position, &rotation_world_to_camera, &(rotation_camera_to_world * self.ray_directions[i]), maze, - )?); + self.near, + self.far, + )?; } } Ok(()) } - /// Casts a ray from the camera origin in the given direction - /// # Arguments - /// * `origin` - The origin of the ray - /// * `rotation_world_to_camera` - The rotation matrix from world to camera coordinates - /// * `direction` - The direction of the ray - /// * `maze` - The maze in the scene - /// # Returns - /// * The distance to the closest obstacle hit by the ray - /// # Errors - /// * If the ray does not hit any obstacles - /// # Example - /// ``` - /// use peng_quad::{Camera, Maze}; - /// use nalgebra::{Vector3, Matrix3}; - /// let camera = Camera::new((800, 600), 60.0, 0.1, 100.0); - /// let origin = Vector3::new(0.0, 0.0, 0.0); - /// let rotation_world_to_camera = Matrix3::identity(); - /// let direction = Vector3::new(1.0, 0.0, 0.0); - /// let mut maze = Maze::new([-1.0, -1.0, -1.0], [1.0, 1.0, 1.0], 5, [0.1, 0.1, 0.1], [0.1, 0.5]); - /// let distance = camera.ray_cast(&origin, &rotation_world_to_camera, &direction, &maze); - /// ``` - pub fn ray_cast( - &self, - origin: &Vector3, - rotation_world_to_camera: &Matrix3, - direction: &Vector3, - maze: &Maze, - ) -> Result { - let mut closest_hit = self.far; - // Inline tube intersection - for axis in 0..3 { - if direction[axis].abs() > f32::EPSILON { - for &bound in &[maze.lower_bounds[axis], maze.upper_bounds[axis]] { - let t = (bound - origin[axis]) / direction[axis]; - if t > self.near && t < closest_hit { - let intersection_point = origin + direction * t; - if (0..3).all(|i| { - i == axis - || (intersection_point[i] >= maze.lower_bounds[i] - && intersection_point[i] <= maze.upper_bounds[i]) - }) { - closest_hit = t; - } - } +} + +/// Casts a ray from the camera origin in the given direction +/// # Arguments +/// * `origin` - The origin of the ray +/// * `rotation_world_to_camera` - The rotation matrix from world to camera coordinates +/// * `direction` - The direction of the ray +/// * `maze` - The maze in the scene +/// * `near` - The minimum distance to consider +/// * `far` - The maximum distance to consider +/// # Returns +/// * The distance to the closest obstacle hit by the ray +/// # Errors +/// * If the ray does not hit any obstacles +/// # Example +/// ``` +/// use peng_quad::{ray_cast, Maze}; +/// use nalgebra::{Vector3, Matrix3}; +/// let origin = Vector3::new(0.0, 0.0, 0.0); +/// let rotation_world_to_camera = Matrix3::identity(); +/// let direction = Vector3::new(0.0, 0.0, 1.0); +/// let maze = Maze::new([-1.0, -1.0, -1.0], [1.0, 1.0, 1.0], 5, [0.1, 0.1, 0.1], [0.1, 0.5]); +/// let near = 0.1; +/// let far = 100.0; +/// let distance = ray_cast(&origin, &rotation_world_to_camera, &direction, &maze, near, far); +/// ``` +pub fn ray_cast( + origin: &Vector3, + rotation_world_to_camera: &Matrix3, + direction: &Vector3, + maze: &Maze, + near: f32, + far: f32, +) -> Result { + let mut closest_hit = far; + // Inline tube intersection + for axis in 0..3 { + let t_near = if direction[axis] > 0.0 { + (maze.upper_bounds[axis] - origin[axis]) / direction[axis] + } else { + (maze.lower_bounds[axis] - origin[axis]) / direction[axis] + }; + if t_near > near && t_near < closest_hit { + let intersection_point = origin + direction * t_near; + let mut valid = true; + for i in 0..3 { + if i != axis + && (intersection_point[i] < maze.lower_bounds[i] + || intersection_point[i] > maze.upper_bounds[i]) + { + valid = false; + break; } } - } - // Early exit if we've hit a wall closer than any possible obstacle - if closest_hit <= self.near { - return Ok(f32::INFINITY); - } - // Inline sphere intersection - for obstacle in &maze.obstacles { - let oc = origin - obstacle.position; - let b = oc.dot(direction); - let c = oc.dot(&oc) - obstacle.radius * obstacle.radius; - let discriminant = b * b - c; - if discriminant >= 0.0 { - // let t = -b - discriminant.sqrt(); - let t = -b - fast_sqrt(discriminant); - if t > self.near && t < closest_hit { - closest_hit = t; - } + if valid { + closest_hit = t_near; } } - if closest_hit < self.far { - Ok((rotation_world_to_camera * direction * closest_hit).x) - } else { - Ok(f32::INFINITY) + } + // Early exit if we've hit a wall closer than any possible obstacle + if closest_hit <= near { + return Ok(f32::INFINITY); + } + // Inline sphere intersection + for obstacle in &maze.obstacles { + let oc = origin - obstacle.position; + let b = oc.dot(direction); + let c = oc.dot(&oc) - obstacle.radius * obstacle.radius; + let discriminant = b * b - c; + if discriminant >= 0.0 { + // let t = -b - discriminant.sqrt(); + let t = -b - fast_sqrt(discriminant); + if t > near && t < closest_hit { + closest_hit = t; + } } } + if closest_hit < far { + Ok((rotation_world_to_camera * direction * closest_hit).x) + } else { + Ok(f32::INFINITY) + } } /// Logs simulation data to the rerun recording stream /// # Arguments @@ -2378,7 +2429,7 @@ impl Camera { /// ``` pub fn log_data( rec: &rerun::RecordingStream, - quad: &Quadrotor, + quad_state: &QuadrotorState, desired_position: &Vector3, desired_velocity: &Vector3, measured_accel: &Vector3, @@ -2407,21 +2458,25 @@ 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), @@ -2589,7 +2644,7 @@ pub fn log_trajectory( )?; Ok(()) } -/// log mesh data to the rerun recording stream +/// Log mesh data to the rerun recording stream /// # Arguments /// * `rec` - The rerun::RecordingStream instance /// * `division` - The number of divisions in the mesh @@ -2636,50 +2691,77 @@ pub fn log_mesh( )?; Ok(()) } -/// log depth image data to the rerun recording stream +/// Log depth image data to the rerun recording stream +/// +/// When the depth value is `f32::INFINITY`, the pixel is considered invalid and logged as black +/// When the resolution is larger than 32x24, multi-threading can accelerate the rendering /// # Arguments /// * `rec` - The rerun::RecordingStream instance -/// * `depth_image` - The depth image data -/// * `resolution` - The width and height of the depth image -/// * `min_depth` - The minimum depth value -/// * `max_depth` - The maximum depth value +/// * `cam` - The Camera instance +/// * `use_multi_threading` - Whether to use multithreading to log the depth image /// # Errors /// * If the data cannot be logged to the recording stream /// # Example /// ```no_run -/// use peng_quad::log_depth_image; +/// use peng_quad::{log_depth_image, Camera}; /// let rec = rerun::RecordingStreamBuilder::new("log.rerun").connect().unwrap(); -/// let depth_image = vec![0.0; 640 * 480]; -/// log_depth_image(&rec, &depth_image, (640usize, 480usize), 0.0, 1.0).unwrap(); +/// let camera = Camera::new((640, 480), 0.1, 100.0, 60.0); +/// let use_multi_threading = false; +/// log_depth_image(&rec, &camera, use_multi_threading).unwrap(); +/// let use_multi_threading = true; +/// log_depth_image(&rec, &camera, use_multi_threading).unwrap(); /// ``` pub fn log_depth_image( rec: &rerun::RecordingStream, - depth_image: &[f32], - resolution: (usize, usize), - min_depth: f32, - max_depth: f32, + cam: &Camera, + use_multi_threading: bool, ) -> Result<(), SimulationError> { - let (width, height) = resolution; - let mut image = rerun::external::ndarray::Array::zeros((height, width, 3)); + let (width, height) = (cam.resolution.0, cam.resolution.1); + let (min_depth, max_depth) = (cam.near, cam.far); + let depth_image = &cam.depth_buffer; + let mut image: rerun::external::ndarray::Array = + rerun::external::ndarray::Array::zeros((height, width, 3)); let depth_range = max_depth - min_depth; - image - .axis_iter_mut(rerun::external::ndarray::Axis(0)) - .enumerate() - .for_each(|(y, mut row)| { - 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); - color_map_fn(normalized_depth * 255.0) - } else { - (0, 0, 0) - }; - (pixel[0], pixel[1], pixel[2]) = color; - } - }); + let scale_factor = 255.0 / depth_range; + if use_multi_threading { + const CHUNK_SIZE: usize = 32; + image + .as_slice_mut() + .expect("Failed to get mutable slice of image") + .par_chunks_exact_mut(CHUNK_SIZE * 3) + .enumerate() + .for_each(|(chunk_index, chunk)| { + let start_index = chunk_index * 32; + for (i, pixel) in chunk.chunks_exact_mut(3).enumerate() { + let idx = start_index + i; + let depth = depth_image[idx]; + let color = if depth.is_finite() { + let normalized_depth = + ((depth - min_depth) * scale_factor).clamp(0.0, 255.0); + color_map_fn(normalized_depth) + } else { + (0, 0, 0) + }; + (pixel[0], pixel[1], pixel[2]) = color; + } + }); + } else { + for (index, pixel) in image + .as_slice_mut() + .expect("Failed to get mutable slice of image") + .chunks_exact_mut(3) + .enumerate() + { + let depth = depth_image[index]; + let color = if depth.is_finite() { + let normalized_depth = ((depth - min_depth) * scale_factor).clamp(0.0, 255.0); + color_map_fn(normalized_depth) + } else { + (0, 0, 0) + }; + (pixel[0], pixel[1], pixel[2]) = color; + } + } let rerun_image = rerun::Image::from_color_model_and_tensor(rerun::ColorModel::RGB, image) .map_err(|e| SimulationError::OtherError(format!("Failed to create rerun image: {}", e)))?; rec.log("world/quad/cam/depth", &rerun_image)?; @@ -2692,12 +2774,11 @@ pub fn log_depth_image( /// * `cam_position` - The position vector of the camera (aligns with the quad) /// * `cam_orientation` - The orientation quaternion of quad /// * `cam_transform` - The transform matrix between quad and camera alignment -/// * `depth_image` - The depth image data /// # Errors /// * If the data cannot be logged to the recording stream /// # Example /// ```no_run -/// use peng_quad::{pinhole_depth, Camera}; +/// use peng_quad::{log_pinhole_depth, Camera}; /// use nalgebra::{Vector3, UnitQuaternion}; /// let rec = rerun::RecordingStreamBuilder::new("log.rerun").connect().unwrap(); /// let depth_image = vec![ 0.0f32 ; 640 * 480]; @@ -2705,16 +2786,17 @@ pub fn log_depth_image( /// let cam_orientation = UnitQuaternion::identity(); /// let cam_transform = [0.0, 0.0, 1.0, -1.0, 0.0, 0.0, 0.0, -1.0, 0.0]; /// let camera = Camera::new((800, 600), 60.0, 0.1, 100.0); -/// pinhole_depth(&rec, &camera, cam_position, cam_orientation, cam_transform, &depth_image).unwrap(); +/// log_pinhole_depth(&rec, &camera, cam_position, cam_orientation, cam_transform).unwrap(); +/// ``` -pub fn pinhole_depth( +pub fn log_pinhole_depth( rec: &rerun::RecordingStream, cam: &Camera, cam_position: Vector3, cam_orientation: UnitQuaternion, cam_transform: [f32; 9], - depth_image: &[f32], ) -> Result<(), SimulationError> { + let depth_image = &cam.depth_buffer; let (width, height) = cam.resolution; let pinhole_camera = rerun::Pinhole::from_focal_length_and_resolution( (cam.horizontal_focal_length, cam.vertical_focal_length), diff --git a/src/main.rs b/src/main.rs index e63314a7f..36e7d47e3 100644 --- a/src/main.rs +++ b/src/main.rs @@ -10,19 +10,26 @@ mod liftoff_quad; /// Main function for the simulation 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 = std::env::args().collect(); if args.len() != 2 { - log::warn!("Usage: {} .", args[0]); - log::warn!("Loading default configuration: config/quad.yaml"); + println!( + "[\x1b[33mWARN\x1b[0m peng_quad] Usage: {} .", + args[0] + ); + println!("[\x1b[33mWARN\x1b[0m peng_quad] Loading default configuration: config/quad.yaml"); } else { - log::info!("Loading configuration: {}", args[1]); + println!( + "[\x1b[32mINFO\x1b[0m peng_quad] Loading configuration: {}", + args[1] + ); config_str = &args[1]; } - let config = config::Config::from_yaml(config_str).expect("Failed to load configuration"); + let config = config::Config::from_yaml(config_str).expect("Failed to load configuration."); + println!( + "[\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, @@ -53,7 +60,7 @@ fn main() -> Result<(), SimulationError> { config.maze.obstacles_velocity_bounds, config.maze.obstacles_radius_bounds, ); - let camera = Camera::new( + let mut camera = Camera::new( config.camera.resolution, config.camera.fov_vertical.to_radians(), config.camera.near, @@ -71,14 +78,23 @@ fn main() -> Result<(), SimulationError> { params: step.params.clone(), }) .collect(); - log::info!("Use rerun.io: {}", config.use_rerun); let rec = if config.use_rerun { - rerun::spawn(&rerun::SpawnOptions::default())?; - Some(rerun::RecordingStreamBuilder::new("Peng").connect()?) + let _rec = rerun::RecordingStreamBuilder::new("Peng").spawn()?; + rerun::Logger::new(_rec.clone()) + .with_path_prefix("logs") + .with_filter(rerun::default_log_filter()) + .init() + .unwrap(); + Some(_rec) } else { + env_logger::builder() + .parse_env(env_logger::Env::default().default_filter_or("info")) + .init(); None }; + log::info!("Use rerun.io: {}", config.use_rerun); if let Some(rec) = &rec { + 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)?; @@ -128,50 +144,43 @@ fn main() -> Result<(), SimulationError> { quad.time_step, ); let _ = quad.control(i, &config, thrust, &torque); + let quad_state = quad.observe()?; imu.update(quad.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, - &mut depth_buffer, config.use_multithreading_depth_rendering, )?; } if let Some(rec) = &rec { rec.set_time_seconds("timestamp", time); - if trajectory.add_point(quad.position) { + if trajectory.add_point(quad_state.position) { log_trajectory(rec, &trajectory)?; } log_joy(rec, thrust, &torque)?; log_data( rec, - &quad, + &quad_state, &desired_position, &desired_velocity, - &_measured_accel, - &_measured_gyro, + &measured_accel, + &measured_gyro, thrust, &torque, )?; if config.render_depth { - log_depth_image( - rec, - &depth_buffer, - camera.resolution, - camera.near, - camera.far, - )?; - pinhole_depth( + log_depth_image(rec, &camera, config.use_multithreading_depth_rendering)?; + log_pinhole_depth( rec, &camera, - quad.position, - quad.orientation, + quad_state.position, + quad_state.orientation, config.camera.rotation_transform, - &depth_buffer, )?; } log_maze_obstacles(rec, &maze)?; @@ -183,5 +192,6 @@ fn main() -> Result<(), SimulationError> { break; } } + log::logger().flush(); Ok(()) } From ca9aa3c87d8f1ab652a2351bf7fd2d16a2c4e87a Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Tue, 12 Nov 2024 02:47:01 -0800 Subject: [PATCH 17/54] update liftoff loop strange quirk where rebind to port is required to get new position data --- src/liftoff_quad.rs | 77 +++++++++++++++++++++++++++------------------ 1 file changed, 47 insertions(+), 30 deletions(-) diff --git a/src/liftoff_quad.rs b/src/liftoff_quad.rs index 945f291ec..5edf449cf 100644 --- a/src/liftoff_quad.rs +++ b/src/liftoff_quad.rs @@ -154,53 +154,69 @@ impl LiftoffPacket { } } +fn quaternion_ruf_to_ned(ruf_quat: UnitQuaternion) -> UnitQuaternion { + // Step 1: Flip the handedness by negating the Z component of the RUF quaternion. + let flipped_quat = Quaternion::new(ruf_quat.w, ruf_quat.i, ruf_quat.j, -ruf_quat.k); + + // Step 2: Define a 90-degree rotation around the Y-axis to align X (Right) to X (North) + let rotation_y = + UnitQuaternion::from_axis_angle(&Vector3::y_axis(), std::f32::consts::FRAC_PI_2); + + // Step 3: Define a -90-degree rotation around the X-axis to align Z (Forward) to Z (Down) + let rotation_x = + UnitQuaternion::from_axis_angle(&Vector3::x_axis(), -std::f32::consts::FRAC_PI_2); + + // Step 4: Combine the handedness-adjusted quaternion with the rotation transformations + // Apply the Y rotation first, then the X rotation + rotation_x * rotation_y * UnitQuaternion::new_normalize(flipped_quat) +} + +/// Translate Unity coordinates to NED coordinates +pub fn vector3_ruf_to_ned(unity_position: Vector3) -> Vector3 { + Vector3::new(unity_position[2], unity_position[0], -unity_position[1]) +} + async fn feedback_loop( address: &str, - timeout: Duration, - max_retry_delay: Duration, data_lock: Arc>>, ) -> Result<(), SimulationError> { - let mut buf = [0; 256]; let mut current_wait = Duration::from_secs(0); let mut delay = Duration::from_secs(2); + let max_wait = Duration::from_secs(60 * 5); + let max_delay = Duration::from_secs(30); loop { + let mut buf = [0; 128]; match UdpSocket::bind(address) { - // Bind successful Ok(socket) => { socket .set_read_timeout(Some(Duration::from_secs(15))) .map_err(|e| SimulationError::OtherError(e.to_string()))?; - current_wait = Duration::from_secs(0); - delay = Duration::from_secs(1); - // Read loop - loop { - match socket.recv_from(&mut buf) { - Ok((len, _)) => { - let mut cursor = std::io::Cursor::new(&buf[..len]); - // TODO: more robust handling of packet parsing errors during resets - let sample = LiftoffPacket::read_be(&mut cursor) - .expect("Failed to read LiftoffPacket"); + match socket.recv_from(&mut buf) { + Ok((len, _)) => { + let mut cursor = std::io::Cursor::new(&buf[..len]); + // TODO: more robust handling of packet parsing errors during resets + if let Ok(sample) = LiftoffPacket::read(&mut cursor) { + // TODO: RUF to NED orientation + // TODO: apply transformation to position vector let mut data_lock = data_lock.lock().await; *data_lock = Some(sample); } - Err(_) => { - if current_wait >= timeout { - return Err(SimulationError::OtherError( - "Bind loop exceeded max wait time".into(), - )); - } - current_wait += delay; - sleep( - delay - + Duration::from_millis( - (rand::random::() * 1000.0) as u64, - ), - ) - .await; - delay = (delay * 2).min(max_retry_delay); - // break; + current_wait = Duration::from_secs(0); + delay = Duration::from_secs(2); + } + Err(e) => { + if current_wait >= max_wait { + return Err(SimulationError::OtherError( + "Bind loop exceeded max wait time".into(), + )); } + current_wait += delay; + sleep( + delay + Duration::from_millis((rand::random::() * 1000.0) as u64), + ) + .await; + delay = (delay * 2).min(max_delay); } } } @@ -211,6 +227,7 @@ async fn feedback_loop( } } } + Ok(()) } fn normalize(value: f32, min: f32, max: f32) -> f32 { From 3fd791b29271796ab250f7dbe1b3120f9c1d0550 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Tue, 12 Nov 2024 20:06:14 -0800 Subject: [PATCH 18/54] liftoff quad gets observe implementation, RUF to NED translations --- src/liftoff_quad.rs | 55 ++++++++++++++++++--------------------------- 1 file changed, 22 insertions(+), 33 deletions(-) diff --git a/src/liftoff_quad.rs b/src/liftoff_quad.rs index 5edf449cf..97486adab 100644 --- a/src/liftoff_quad.rs +++ b/src/liftoff_quad.rs @@ -6,6 +6,7 @@ use peng_quad::quadrotor::{QuadrotorInterface, QuadrotorState}; use peng_quad::SimulationError; use rand; use serialport::{available_ports, SerialPort, SerialPortBuilder, SerialPortType}; +use std::fmt::format; use std::net::UdpSocket; use std::sync::Arc; use tokio::sync::Mutex; @@ -63,17 +64,18 @@ impl QuadrotorInterface for LiftoffQuad { } /// Observe the current state of the quadrotor - /// # Returns - /// A tuple containing the position, velocity, orientation, and angular velocity of the quadrotor + /// Returns a tuple containing the position, velocity, orientation, and angular velocity of the quadrotor. fn observe(&mut self) -> Result { let mut data_lock = tokio::runtime::Handle::current().block_on(self.shared_data.lock()); while let Some(sample) = data_lock.take() { // update the last state value self.last_state = self.state.clone(); - // store the sample values into templ variables - let position = Vector3::from_row_slice(&sample.position); + // Store the sample values into temp variables + // Convert the position from Unity coordinates to NED coordinates + let position = vector3_ruf_to_ned(Vector3::from_row_slice(&sample.position)); let velocity = (position - self.last_state.position) / self.time_step; - let orientation = sample.attitude_quaternion(); + // Convert the orientation from Unity RUF to NED coordinates + let orientation = quaternion_ruf_to_ned(sample.attitude_quaternion()); let delta_q = self.last_state.orientation * orientation.conjugate(); let angle = 2.0 * delta_q.scalar().acos(); let axis = delta_q @@ -86,14 +88,8 @@ impl QuadrotorInterface for LiftoffQuad { orientation, angular_velocity, }; - return Ok(QuadrotorState { - position, - velocity, - orientation, - angular_velocity, - }); } - todo!("implement state feedback from Liftoff UDP") + Ok(self.state.clone()) } } @@ -103,16 +99,10 @@ impl LiftoffQuad { vehicle_config: QuadrotorConfig, liftoff_config: LiftoffConfiguration, ) -> Result { - let shared_data = Arc::new(Mutex::new(None)); + let shared_data: Arc>> = Arc::new(Mutex::new(None)); let shared_data_clone = Arc::clone(&shared_data); tokio::spawn(async move { - let _ = feedback_loop( - &liftoff_config.ip_address.to_string(), - liftoff_config.connection_timeout, - liftoff_config.max_retry_delay, - shared_data_clone, - ) - .await; + let _ = feedback_loop(&liftoff_config, shared_data_clone).await; }); let writer = Writer::new("COM3".to_string(), 115200) .map_err(|e| SimulationError::OtherError(e.to_string()))?; @@ -177,17 +167,17 @@ pub fn vector3_ruf_to_ned(unity_position: Vector3) -> Vector3 { } async fn feedback_loop( - address: &str, + liftoff_config: &LiftoffConfiguration, data_lock: Arc>>, ) -> Result<(), SimulationError> { let mut current_wait = Duration::from_secs(0); let mut delay = Duration::from_secs(2); - let max_wait = Duration::from_secs(60 * 5); - let max_delay = Duration::from_secs(30); + let max_wait = liftoff_config.connection_timeout; + let max_delay = liftoff_config.max_retry_delay; loop { let mut buf = [0; 128]; - match UdpSocket::bind(address) { + match UdpSocket::bind(liftoff_config.ip_address.to_string()) { Ok(socket) => { socket .set_read_timeout(Some(Duration::from_secs(15))) @@ -197,8 +187,6 @@ async fn feedback_loop( let mut cursor = std::io::Cursor::new(&buf[..len]); // TODO: more robust handling of packet parsing errors during resets if let Ok(sample) = LiftoffPacket::read(&mut cursor) { - // TODO: RUF to NED orientation - // TODO: apply transformation to position vector let mut data_lock = data_lock.lock().await; *data_lock = Some(sample); } @@ -207,9 +195,10 @@ async fn feedback_loop( } Err(e) => { if current_wait >= max_wait { - return Err(SimulationError::OtherError( - "Bind loop exceeded max wait time".into(), - )); + return Err(SimulationError::OtherError(format!( + "Bind loop exceeded max wait time {}", + e.to_string(), + ))); } current_wait += delay; sleep( @@ -221,13 +210,13 @@ async fn feedback_loop( } } Err(e) => { - return Err(SimulationError::OtherError( - "Bind loop exceeded max wait time".into(), - )); + return Err(SimulationError::OtherError(format!( + "Bind loop exceeded max wait time {}", + e.to_string() + ))); } } } - Ok(()) } fn normalize(value: f32, min: f32, max: f32) -> f32 { From 893f36e1e7f64cfcbc37f77b367b4020c1fbd3f9 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Tue, 12 Nov 2024 20:40:57 -0800 Subject: [PATCH 19/54] don't pass whole quad to objects, just the state --- src/main.rs | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/src/main.rs b/src/main.rs index 36e7d47e3..a4976acad 100644 --- a/src/main.rs +++ b/src/main.rs @@ -105,6 +105,7 @@ fn main() -> Result<(), SimulationError> { let frame_time = Duration::from_secs_f32(1.0 / config.simulation.simulation_frequency as f32); 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 { @@ -118,14 +119,14 @@ fn main() -> Result<(), SimulationError> { 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, )?; @@ -133,18 +134,18 @@ fn main() -> Result<(), SimulationError> { &desired_position, &desired_velocity, desired_yaw, - &quad.position, - &quad.velocity, + &quad_state.position, + &quad_state.velocity, quad.time_step, ); let torque = controller.compute_attitude_control( &calculated_desired_orientation, - &quad.orientation, - &quad.angular_velocity, + &quad_state.orientation, + &quad_state.angular_velocity, quad.time_step, ); let _ = quad.control(i, &config, thrust, &torque); - let quad_state = quad.observe()?; + quad_state = quad.observe()?; imu.update(quad.time_step)?; let (true_accel, true_gyro) = quad.read_imu()?; let (measured_accel, measured_gyro) = imu.read(true_accel, true_gyro)?; @@ -179,7 +180,7 @@ fn main() -> Result<(), SimulationError> { rec, &camera, quad_state.position, - quad_state.orientation, + quad.orientation, config.camera.rotation_transform, )?; } From d36e01672cfc39d6704aa03f4ae7145c43fc95a3 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Tue, 12 Nov 2024 20:41:26 -0800 Subject: [PATCH 20/54] control inputs to rc happens in control method --- src/lib.rs | 31 ++++++++++---------- src/liftoff_quad.rs | 70 +++++++++++++++++++-------------------------- 2 files changed, 44 insertions(+), 57 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 77c9f2239..5fdbf9cf4 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1806,7 +1806,7 @@ pub fn update_planner( step: usize, time: f32, simulation_frequency: usize, - quad: &Quadrotor, + quad_state: &QuadrotorState, obstacles: &[Obstacle], planner_config: &[PlannerStepConfig], ) -> Result<(), SimulationError> { @@ -1815,7 +1815,7 @@ pub fn update_planner( .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)?); + planner_manager.set_planner(create_planner(planner_step, quad_state, time, obstacles)?); } Ok(()) } @@ -1856,29 +1856,29 @@ pub fn update_planner( /// ``` pub fn create_planner( step: &PlannerStepConfig, - quad: &Quadrotor, + 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")?, })), "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")?, })), @@ -1886,18 +1886,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")?, @@ -1908,7 +1908,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() @@ -1927,7 +1927,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() @@ -1954,10 +1954,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: {}", @@ -2788,7 +2788,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, diff --git a/src/liftoff_quad.rs b/src/liftoff_quad.rs index 97486adab..133ad172e 100644 --- a/src/liftoff_quad.rs +++ b/src/liftoff_quad.rs @@ -47,18 +47,40 @@ pub struct LiftoffQuad { impl QuadrotorInterface for LiftoffQuad { fn control( &mut self, - step_number: usize, + _: usize, config: &peng_quad::config::Config, thrust: f32, torque: &Vector3, ) -> Result<(), SimulationError> { // Given thrust and torque, calculate the control inputs + let roll_torque = torque[0]; + let pitch_torque = torque[1]; + let yaw_torque = torque[2]; + + let max_torques = config.quadrotor.max_torques(); + + // Normalize inputs + let normalized_thrust = normalize(thrust, 0.0, config.quadrotor.max_thrust_kg); + let normalized_roll = normalize(roll_torque, -max_torques.0, max_torques.0); + let normalized_pitch = normalize(pitch_torque, -max_torques.1, max_torques.1); + let normalized_yaw = normalize(yaw_torque, -max_torques.2, max_torques.2); + + // Scale to RC commands + let throttle_command = scale_throttle(normalized_thrust); + let aileron_command = scale_control(normalized_roll); + let elevator_command = scale_control(normalized_pitch); + let rudder_command = scale_control(normalized_yaw); + + let cyberrc_data = cyberrc::RcData { + throttle: throttle_command, + aileron: aileron_command, + elevator: elevator_command, + rudder: rudder_command, + arm: 0, + mode: 0, + }; self.writer - .write(control_inputs_to_rc_commands( - config.quadrotor.clone(), - thrust, - torque, - )) + .write(cyberrc_data) .map_err(|e| SimulationError::OtherError(e.to_string()))?; Ok(()) } @@ -123,7 +145,7 @@ impl LiftoffQuad { #[binrw] #[br(little)] #[derive(Debug)] -struct LiftoffPacket { +pub struct LiftoffPacket { timestamp: f32, position: [f32; 3], // TODO: binrw read to quaternion @@ -235,37 +257,3 @@ fn scale_throttle(thrust: f32) -> i32 { fn scale_control(value: f32) -> i32 { scale_to_rc_command(value, 1000.0, 2000.0, 1500.0) } - -fn control_inputs_to_rc_commands( - quadrotor_config: QuadrotorConfig, - thrust: f32, - torque: &Vector3, -) -> cyberrc::RcData { - let roll_torque = torque[0]; - let pitch_torque = torque[1]; - let yaw_torque = torque[2]; - - // TODO: what cases will we have this be non zero - let max_torques = quadrotor_config.max_torques(); - - // Normalize inputs - let normalized_thrust = normalize(thrust, 0.0, quadrotor_config.max_thrust_kg); - let normalized_roll = normalize(roll_torque, -max_torques.0, max_torques.0); - let normalized_pitch = normalize(pitch_torque, -max_torques.1, max_torques.1); - let normalized_yaw = normalize(yaw_torque, -max_torques.2, max_torques.2); - - // Scale to RC commands - let throttle_command = scale_throttle(normalized_thrust); - let aileron_command = scale_control(normalized_roll); - let elevator_command = scale_control(normalized_pitch); - let rudder_command = scale_control(normalized_yaw); - - cyberrc::RcData { - throttle: throttle_command, - aileron: aileron_command, - elevator: elevator_command, - rudder: rudder_command, - arm: 0, - mode: 0, - } -} From 994f32df15181b3c05a589fcf75e05ad4d903d9e Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Fri, 15 Nov 2024 00:56:27 -0800 Subject: [PATCH 21/54] latest config --- config/liftoff_quad.yaml | 19 +++-- config/quad.yaml | 5 +- src/config.rs | 101 +++++++++++++++++--------- tests/testdata/test_config_base.yaml | 2 + tests/testdata/test_liftoff_base.yaml | 2 + 5 files changed, 85 insertions(+), 44 deletions(-) diff --git a/config/liftoff_quad.yaml b/config/liftoff_quad.yaml index e7ba6d8fe..a81bbca43 100644 --- a/config/liftoff_quad.yaml +++ b/config/liftoff_quad.yaml @@ -1,25 +1,28 @@ 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) + 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: - mass: 1.3 # Mass of the quadrotor (kg) + type: "Liftoff" + mass: .131 # Mass of the quadrotor (kg) gravity: 9.81 # Gravitational acceleration (m/s^2) - drag_coefficient: 0.000 # Aerodynamic drag coefficient + drag_coefficient: 0.800 # 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: - ip_address: 0.0.0.0:9001 + 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 pid_controller: pos_gains: # PID gains for position control diff --git a/config/quad.yaml b/config/quad.yaml index 538a45def..504aad9f5 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/src/config.rs b/src/config.rs index 5326e78e5..9f5a4bfb1 100644 --- a/src/config.rs +++ b/src/config.rs @@ -14,7 +14,7 @@ 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 @@ -35,40 +35,16 @@ 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 pub real_time: bool, - /// Vehicle specific configuration - pub vehicle_configuration: Option, } #[derive(serde::Deserialize)] #[serde(tag = "type")] /// Vehicle Specifig configuration -pub enum VehicleConfigurations { - LiftoffConfiguration(LiftoffConfiguration), -} - -#[derive(serde::Deserialize)] -#[serde(default)] -/// Configurations for controlling drones in Liftoff -pub struct LiftoffConfiguration { - pub ip_address: String, - pub connection_timeout: tokio::time::Duration, - pub max_retry_delay: tokio::time::Duration, -} - -impl Default for LiftoffConfiguration { - fn default() -> Self { - LiftoffConfiguration { - 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), - } - } +pub enum QuadrotorConfigurations { + Peng(QuadrotorConfig), + Liftoff(LiftoffQuadrotorConfig), } #[derive(serde::Deserialize)] @@ -82,7 +58,7 @@ pub struct PlannerStep { pub params: serde_yaml::Value, } -#[derive(serde::Deserialize)] +#[derive(Clone, serde::Deserialize)] /// Configuration for the simulation pub struct SimulationConfig { /// Control frequency in Hz @@ -93,6 +69,10 @@ 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(Clone, serde::Deserialize)] @@ -152,6 +132,54 @@ impl QuadrotorConfig { } } +#[derive(Clone, 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, + pub ip_address: String, + pub connection_timeout: tokio::time::Duration, + pub max_retry_delay: tokio::time::Duration, +} + +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), + } + } +} + +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 { @@ -248,13 +276,18 @@ mod tests { #[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!(config.quadrotor.mass, 1.3); - assert_eq!(config.quadrotor.gravity, 9.81); - assert_eq!(config.quadrotor.drag_coefficient, 0.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]); assert_eq!(config.pid_controller.pos_max_int, [10.0, 10.0, 10.0]); @@ -265,8 +298,8 @@ mod tests { #[test] fn test_liftoff_config() { let config = Config::from_yaml("tests/testdata/test_liftoff_base.yaml").unwrap(); - let liftoff_config = match config.vehicle_configuration { - Some(VehicleConfigurations::LiftoffConfiguration(liftoff_config)) => liftoff_config, + 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/tests/testdata/test_config_base.yaml b/tests/testdata/test_config_base.yaml index ae9e1e2dc..538a45def 100644 --- a/tests/testdata/test_config_base.yaml +++ b/tests/testdata/test_config_base.yaml @@ -5,6 +5,8 @@ use_rk4_for_dynamics_update: false # Enable Runge-Kutta 4th order integration fo 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) diff --git a/tests/testdata/test_liftoff_base.yaml b/tests/testdata/test_liftoff_base.yaml index 0f6d80fd3..ae4d56fd0 100644 --- a/tests/testdata/test_liftoff_base.yaml +++ b/tests/testdata/test_liftoff_base.yaml @@ -5,6 +5,8 @@ use_rk4_for_dynamics_update: false # Enable Runge-Kutta 4th order integration fo 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) From 001494a01c544bf0cf8c63e60b3aed4279e1be59 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Fri, 15 Nov 2024 00:57:57 -0800 Subject: [PATCH 22/54] quadrotor factory in main --- src/main.rs | 55 ++++++++++++++++++++++++++++++++++++----------------- 1 file changed, 38 insertions(+), 17 deletions(-) diff --git a/src/main.rs b/src/main.rs index a4976acad..16f475e3f 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,4 +1,5 @@ #![feature(thread_sleep_until)] +use liftoff_quad::LiftoffQuad; use nalgebra::Vector3; use peng_quad::quadrotor::QuadrotorInterface; use peng_quad::*; @@ -8,9 +9,10 @@ use std::time::Instant; mod liftoff_quad; +// #[tokio::main] /// Main function for the simulation fn main() -> Result<(), SimulationError> { - let mut config_str = "config/quad.yaml"; + let mut config_str = "config/liftoff_quad.yaml"; let args: Vec = std::env::args().collect(); if args.len() != 2 { println!( @@ -30,13 +32,32 @@ 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 time_step = 1.0 / config.simulation.simulation_frequency as f32; + // TODO: quadrotor factory + let (mut quad, mass, gravity): (Box, f32, f32) = match config.quadrotor + { + config::QuadrotorConfigurations::Peng(quadrotor_config) => ( + Box::new(Quadrotor::new( + 1.0 / config.simulation.simulation_frequency as f32, + config.simulation.clone(), + quadrotor_config.mass, + quadrotor_config.gravity, + quadrotor_config.drag_coefficient, + quadrotor_config.inertia_matrix, + )?), + quadrotor_config.mass, + quadrotor_config.gravity, + ), + config::QuadrotorConfigurations::Liftoff(liftoff_quadrotor_config) => ( + Box::new(LiftoffQuad::new( + 1.0 / config.simulation.simulation_frequency as f32, + liftoff_quadrotor_config.clone(), + )?), + liftoff_quadrotor_config.mass, + liftoff_quadrotor_config.gravity, + ), + }; + let _pos_gains = config.pid_controller.pos_gains; let _att_gains = config.pid_controller.att_gains; let mut controller = PIDController::new( @@ -44,8 +65,8 @@ fn main() -> Result<(), SimulationError> { [_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, + mass, + gravity, ); let mut imu = Imu::new( config.imu.accel_noise_std, @@ -112,8 +133,8 @@ fn main() -> Result<(), SimulationError> { 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, @@ -136,17 +157,17 @@ fn main() -> Result<(), SimulationError> { desired_yaw, &quad_state.position, &quad_state.velocity, - quad.time_step, + time_step, ); let torque = controller.compute_attitude_control( &calculated_desired_orientation, &quad_state.orientation, &quad_state.angular_velocity, - quad.time_step, + time_step, ); - let _ = quad.control(i, &config, thrust, &torque); + let _ = quad.control(i, thrust, &torque); quad_state = quad.observe()?; - imu.update(quad.time_step)?; + imu.update(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 / config.simulation.log_frequency) == 0 { @@ -180,7 +201,7 @@ fn main() -> Result<(), SimulationError> { rec, &camera, quad_state.position, - quad.orientation, + quad_state.orientation, config.camera.rotation_transform, )?; } From feaa7b13f1f00eeb8cb17e914a72ca899024e3d7 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Fri, 15 Nov 2024 00:58:26 -0800 Subject: [PATCH 23/54] liftoff quad observation wip --- src/liftoff_quad.rs | 204 +++++++++++++++++++++++++++----------------- 1 file changed, 128 insertions(+), 76 deletions(-) diff --git a/src/liftoff_quad.rs b/src/liftoff_quad.rs index 133ad172e..1e9464090 100644 --- a/src/liftoff_quad.rs +++ b/src/liftoff_quad.rs @@ -1,25 +1,16 @@ use binrw::{binrw, BinRead}; use cyber_rc::{cyberrc, Writer}; -use nalgebra::{Matrix4, Quaternion, Unit, UnitQuaternion, Vector3}; -use peng_quad::config::{LiftoffConfiguration, QuadrotorConfig}; +use nalgebra::{AbstractRotation, Matrix4, Quaternion, Unit, UnitQuaternion, Vector3}; +use peng_quad::config::{self, LiftoffQuadrotorConfig, QuadrotorConfig, QuadrotorConfigurations}; use peng_quad::quadrotor::{QuadrotorInterface, QuadrotorState}; use peng_quad::SimulationError; use rand; use serialport::{available_ports, SerialPort, SerialPortBuilder, SerialPortType}; -use std::fmt::format; use std::net::UdpSocket; use std::sync::Arc; use tokio::sync::Mutex; use tokio::time::{sleep, Duration}; -#[rustfmt::skip] -const MOTOR_MIXING_MATRIX: Matrix4 = Matrix4::new( - 1.0, 1.0, 1.0, 1.0, - -1.0, 1.0, -1.0, 1.0, - -1.0, -1.0, 1.0, 1.0, - 1.0, -1.0, -1.0, 1.0, -); - /// Represents a quadrotor in the game Liftoff /// # Example /// ``` @@ -35,7 +26,7 @@ pub struct LiftoffQuad { /// Simulation time step in seconds pub time_step: f32, /// Configured physical parameters - pub config: QuadrotorConfig, + pub config: LiftoffQuadrotorConfig, /// Previous Thrust pub previous_thrust: f32, /// Previous Torque @@ -48,22 +39,17 @@ impl QuadrotorInterface for LiftoffQuad { fn control( &mut self, _: usize, - config: &peng_quad::config::Config, thrust: f32, torque: &Vector3, ) -> Result<(), SimulationError> { // Given thrust and torque, calculate the control inputs - let roll_torque = torque[0]; - let pitch_torque = torque[1]; - let yaw_torque = torque[2]; - - let max_torques = config.quadrotor.max_torques(); + let max_torques = self.config.max_torques(); // Normalize inputs - let normalized_thrust = normalize(thrust, 0.0, config.quadrotor.max_thrust_kg); - let normalized_roll = normalize(roll_torque, -max_torques.0, max_torques.0); - let normalized_pitch = normalize(pitch_torque, -max_torques.1, max_torques.1); - let normalized_yaw = normalize(yaw_torque, -max_torques.2, max_torques.2); + let normalized_thrust = normalize(thrust, 0.0, self.config.max_thrust_kg); + let normalized_roll = normalize(torque[0], -max_torques.0, max_torques.0); + let normalized_pitch = normalize(torque[2], -max_torques.1, max_torques.1); + let normalized_yaw = normalize(torque[3], -max_torques.2, max_torques.2); // Scale to RC commands let throttle_command = scale_throttle(normalized_thrust); @@ -92,53 +78,75 @@ impl QuadrotorInterface for LiftoffQuad { while let Some(sample) = data_lock.take() { // update the last state value self.last_state = self.state.clone(); - // Store the sample values into temp variables - // Convert the position from Unity coordinates to NED coordinates - let position = vector3_ruf_to_ned(Vector3::from_row_slice(&sample.position)); - let velocity = (position - self.last_state.position) / self.time_step; - // Convert the orientation from Unity RUF to NED coordinates - let orientation = quaternion_ruf_to_ned(sample.attitude_quaternion()); - let delta_q = self.last_state.orientation * orientation.conjugate(); - let angle = 2.0 * delta_q.scalar().acos(); - let axis = delta_q - .axis() - .unwrap_or(Unit::new_normalize(Vector3::::zeros())); - let angular_velocity = axis.scale(angle / self.time_step); + let attitude_quaternion = sample.attitude_quaternion(); + // The position is in Unity inertial coordinates, so we transform to NED inertial + // coordinates. + let sample_position = sample.position(); + // Calculate the body-frame velocity by rotating the inertial velocity using the + // attitude quaternion. + let v_body = attitude_quaternion + .transform_vector(&((sample_position - self.last_state.position) / self.time_step)); + + let omega_body = sample.pqr(); + let acceleration_body = self.body_acceleration(attitude_quaternion, omega_body, v_body); self.state = QuadrotorState { - position, - velocity, - orientation, - angular_velocity, + position: sample_position, + velocity: v_body, + acceleration: acceleration_body, + orientation: attitude_quaternion, + angular_velocity: omega_body, }; } Ok(self.state.clone()) } + + fn read_imu(&self) -> Result<(Vector3, Vector3), SimulationError> { + todo!("") + } } impl LiftoffQuad { - fn new( - time_step: f32, - vehicle_config: QuadrotorConfig, - liftoff_config: LiftoffConfiguration, - ) -> Result { + pub fn new(time_step: f32, config: LiftoffQuadrotorConfig) -> Result { let shared_data: Arc>> = Arc::new(Mutex::new(None)); let shared_data_clone = Arc::clone(&shared_data); - tokio::spawn(async move { - let _ = feedback_loop(&liftoff_config, shared_data_clone).await; - }); + let config_clone = config.clone(); + // tokio::spawn(async move { + // let _ = feedback_loop(config_clone, shared_data_clone).await; + // }); let writer = Writer::new("COM3".to_string(), 115200) .map_err(|e| SimulationError::OtherError(e.to_string()))?; Ok(Self { writer: writer, state: QuadrotorState::default(), last_state: QuadrotorState::default(), - config: vehicle_config.clone(), + config, time_step, previous_thrust: 0.0, previous_torque: Vector3::zeros(), shared_data: Arc::new(Mutex::new(None)), }) } + + // Function to calculate body-frame acceleration for NED coordinates + fn body_acceleration( + &self, + q_inertial_to_body: UnitQuaternion, // Unit quaternion rotation from inertial to body + omega_body: Vector3, // Angular velocity in body frame + velocity_body: Vector3, // Linear velocity in body frame + ) -> Vector3 { + // Step 1: Calculate thrust acceleration (negative Z in NED body frame) + let thrust_acceleration = Vector3::new(0.0, 0.0, -self.previous_thrust / self.config.mass); + + // Step 2: Rotate the gravity vector to the body frame using the quaternion `apply` + let gravity_inertial = Vector3::new(0.0, 0.0, self.config.gravity); // NED gravity vector in inertial frame + let gravity_body = q_inertial_to_body.transform_vector(&gravity_inertial); // Rotate gravity vector to body frame + + // Step 3: Calculate rotational (Coriolis) effects + let rotational_acceleration = omega_body.cross(&velocity_body); + + // Step 4: Combine all terms + thrust_acceleration + gravity_body - rotational_acceleration + } } // TODO: configure packet based on the content of the Liftoff config file @@ -147,49 +155,57 @@ impl LiftoffQuad { #[derive(Debug)] pub struct LiftoffPacket { timestamp: f32, + // x, y, z position: [f32; 3], - // TODO: binrw read to quaternion + // x, y, z, w attitude: [f32; 4], + // pitch, roll, yaw - q, p, r + gyro: [f32; 3], motor_num: u8, #[br(count = motor_num)] motor_rpm: Vec, } impl LiftoffPacket { + /// Returns the angular velocity p, q, r + /// These are the roll rate, pitch rate, and yaw rate respectively + pub fn pqr(&self) -> Vector3 { + Vector3::new(self.gyro[1], self.gyro[0], self.gyro[2]) + } + + /// Returns the attitude quaternion in the NED frame pub fn attitude_quaternion(&self) -> UnitQuaternion { - UnitQuaternion::from_quaternion(Quaternion::new( + // The quaternion is in the Unity right-up-forward (RUF) frame + let ruf_quat = UnitQuaternion::from_quaternion(Quaternion::new( + self.attitude[3], self.attitude[0], self.attitude[1], self.attitude[2], - self.attitude[3], - )) - } -} - -fn quaternion_ruf_to_ned(ruf_quat: UnitQuaternion) -> UnitQuaternion { - // Step 1: Flip the handedness by negating the Z component of the RUF quaternion. - let flipped_quat = Quaternion::new(ruf_quat.w, ruf_quat.i, ruf_quat.j, -ruf_quat.k); + )); + // Flip the handedness by negating the Z component of the RUF quaternion. + let flipped_quat = Quaternion::new(ruf_quat.w, ruf_quat.i, ruf_quat.j, -ruf_quat.k); - // Step 2: Define a 90-degree rotation around the Y-axis to align X (Right) to X (North) - let rotation_y = - UnitQuaternion::from_axis_angle(&Vector3::y_axis(), std::f32::consts::FRAC_PI_2); + // Define a 90-degree rotation around the Y-axis to align X (Right) to X (North) + let rotation_y = + UnitQuaternion::from_axis_angle(&Vector3::y_axis(), std::f32::consts::FRAC_PI_2); - // Step 3: Define a -90-degree rotation around the X-axis to align Z (Forward) to Z (Down) - let rotation_x = - UnitQuaternion::from_axis_angle(&Vector3::x_axis(), -std::f32::consts::FRAC_PI_2); + // Define a -90-degree rotation around the X-axis to align Z (Forward) to Z (Down) + let rotation_x = + UnitQuaternion::from_axis_angle(&Vector3::x_axis(), -std::f32::consts::FRAC_PI_2); - // Step 4: Combine the handedness-adjusted quaternion with the rotation transformations - // Apply the Y rotation first, then the X rotation - rotation_x * rotation_y * UnitQuaternion::new_normalize(flipped_quat) -} + // Combine the handedness-adjusted quaternion with the rotation transformations + // Apply the Y rotation first, then the X rotation + rotation_x * rotation_y * UnitQuaternion::new_normalize(flipped_quat) + } -/// Translate Unity coordinates to NED coordinates -pub fn vector3_ruf_to_ned(unity_position: Vector3) -> Vector3 { - Vector3::new(unity_position[2], unity_position[0], -unity_position[1]) + /// Translate Unity coordinates to NED coordinates + pub fn position(&self) -> Vector3 { + Vector3::new(self.position[2], self.position[0], -self.position[1]) + } } async fn feedback_loop( - liftoff_config: &LiftoffConfiguration, + liftoff_config: LiftoffQuadrotorConfig, data_lock: Arc>>, ) -> Result<(), SimulationError> { let mut current_wait = Duration::from_secs(0); @@ -241,19 +257,55 @@ async fn feedback_loop( } } +/// Scale a value from a given range to a new range +/// # Example +/// ``` +/// let value = 0.0; +/// let min = -10.0; +/// let max = 10.0; +/// let scaled_value = normalize(value, min, max); +/// assert_eq!(scaled_value, 0.5); +/// ``` fn normalize(value: f32, min: f32, max: f32) -> f32 { (value - min) / (max - min) } -fn scale_to_rc_command(value: f32, min_output: f32, max_output: f32, center: f32) -> i32 { - let scaled_value = value * (max_output - center) + center; - scaled_value.clamp(min_output, max_output) as i32 +// TODO clean up all the type casts +// TODO assert value is normalized +fn scale_to_rc_command(value: f32, min: i32, max: i32, center: i32) -> i32 { + // assert value is in the range 0 to 1 + // Scale normalized value to the range between min and max + let range = max - min; + let scaled_value = min as f32 + value * range as f32; + + // Calculate the offset from the center within the range + let center_offset = scaled_value - (min as f32 + (range as f32) / 2.0); + + // Adjust the result around the center + (center as f32 + center_offset) as i32 + // scaled_value.clamp(max_output, min_output) as i32 } +/// Scale a thrust value to a throttle command +/// Throttle is inverted from Xinput to Liftoff +/// # Example +/// ``` +/// let thrust = 0.0; +/// let throttle = scale_throttle(thrust); +/// assert_eq!(throttle, 0); +/// ``` fn scale_throttle(thrust: f32) -> i32 { - scale_to_rc_command(thrust, 1000.0, 2000.0, 1500.0) + scale_to_rc_command(thrust, 32768, -32768, 32768) } fn scale_control(value: f32) -> i32 { - scale_to_rc_command(value, 1000.0, 2000.0, 1500.0) + scale_to_rc_command(value, -32768, 32767, 0) } + +#[rustfmt::skip] +const MOTOR_MIXING_MATRIX: Matrix4 = Matrix4::new( + 1.0, 1.0, 1.0, 1.0, + -1.0, 1.0, -1.0, 1.0, + -1.0, -1.0, 1.0, 1.0, + 1.0, -1.0, -1.0, 1.0, +); From cdf5586a2b157fe687120702a62062a43dff6757 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Fri, 15 Nov 2024 01:00:33 -0800 Subject: [PATCH 24/54] lib --- src/lib.rs | 76 ++++++++++++++++++++++++++---------------------- src/quadrotor.rs | 14 +++++++-- 2 files changed, 54 insertions(+), 36 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 5fdbf9cf4..415806317 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -113,21 +113,19 @@ pub struct Quadrotor { pub previous_thrust: f32, /// Previous Torque pub previous_torque: Vector3, + /// Config + pub config: config::SimulationConfig, } impl QuadrotorInterface for Quadrotor { fn control( &mut self, step_number: usize, - config: &config::Config, thrust: f32, torque: &Vector3, ) -> Result<(), SimulationError> { - if step_number - % (config.simulation.simulation_frequency / config.simulation.control_frequency) - == 0 - { - if config.use_rk4_for_dynamics_control { + 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); @@ -137,7 +135,7 @@ impl QuadrotorInterface for Quadrotor { } 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); @@ -149,10 +147,31 @@ impl QuadrotorInterface for Quadrotor { return Ok(QuadrotorState { position: self.position, velocity: self.velocity, + acceleration: self.acceleration, orientation: self.orientation, angular_velocity: self.angular_velocity, }); } + + //TODO: replace this method in main with observe + /// 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(); + /// ``` + fn read_imu(&self) -> Result<(Vector3, Vector3), SimulationError> { + Ok((self.acceleration, self.angular_velocity)) + } } /// Implementation of the Quadrotor struct @@ -177,8 +196,10 @@ impl Quadrotor { /// 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); /// ``` + /// 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, @@ -192,6 +213,7 @@ impl Quadrotor { "Failed to invert inertia matrix".to_string(), ))?; Ok(Self { + config, position: Vector3::zeros(), velocity: Vector3::zeros(), acceleration: Vector3::zeros(), @@ -392,24 +414,6 @@ impl Quadrotor { 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 @@ -1777,7 +1781,7 @@ pub struct PlannerStepConfig { /// * If the planner could not be created /// # Example /// ``` -/// use peng_quad::{PlannerManager, Quadrotor, Obstacle, PlannerStepConfig, update_planner}; +/// 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); @@ -1787,7 +1791,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, 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, @@ -1799,7 +1804,7 @@ 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, @@ -1831,7 +1836,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::{PlannerType, Quadrotor, Obstacle, PlannerStepConfig, create_planner, +/// quadrotor::QuadrotorInterface}; /// use nalgebra::Vector3; /// let step = PlannerStepConfig { /// step: 0, @@ -1846,9 +1852,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, 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"), @@ -2415,17 +2422,18 @@ 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::{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 quad = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap(); +/// let quad_state = quad.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, 0.0, &Vector3::new(0.0, 0.0, 0.0)).unwrap(); /// ``` pub fn log_data( rec: &rerun::RecordingStream, diff --git a/src/quadrotor.rs b/src/quadrotor.rs index cbc8d3cc2..09fb22b34 100644 --- a/src/quadrotor.rs +++ b/src/quadrotor.rs @@ -1,4 +1,4 @@ -use crate::config::Config; +use crate::config::QuadrotorConfigurations; use crate::SimulationError; use nalgebra::{UnitQuaternion, Vector3}; @@ -8,6 +8,8 @@ pub struct QuadrotorState { 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 @@ -17,7 +19,15 @@ pub struct QuadrotorState { /// 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 { - fn control(&mut self, step_number: usize, config: &Config, thrust: f32, torque: &Vector3) -> Result<(), SimulationError>; + // TODO remove config from control + fn control( + &mut self, + step_number: usize, + thrust: f32, + torque: &Vector3, + ) -> Result<(), SimulationError>; fn observe(&mut self) -> Result; + + fn read_imu(&self) -> Result<(Vector3, Vector3), SimulationError>; } From a9af9763161af93324f99ee21543b8bcf1d90f95 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sat, 16 Nov 2024 00:28:31 -0800 Subject: [PATCH 25/54] add serial port and baud rate to liftoff quadrotor config --- src/config.rs | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/config.rs b/src/config.rs index 9f5a4bfb1..5e41002a0 100644 --- a/src/config.rs +++ b/src/config.rs @@ -39,7 +39,7 @@ pub struct Config { pub real_time: bool, } -#[derive(serde::Deserialize)] +#[derive(Debug, serde::Deserialize)] #[serde(tag = "type")] /// Vehicle Specifig configuration pub enum QuadrotorConfigurations { @@ -75,7 +75,7 @@ pub struct SimulationConfig { pub use_rk4_for_dynamics_update: bool, } -#[derive(Clone, serde::Deserialize)] +#[derive(Clone, Debug, serde::Deserialize)] #[serde(default)] /// Configuration for the quadrotor pub struct QuadrotorConfig { @@ -132,7 +132,7 @@ impl QuadrotorConfig { } } -#[derive(Clone, serde::Deserialize)] +#[derive(Clone, Debug, serde::Deserialize)] #[serde(default)] /// Configuration for the quadrotor pub struct LiftoffQuadrotorConfig { @@ -151,6 +151,8 @@ pub struct LiftoffQuadrotorConfig { 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 { @@ -165,6 +167,8 @@ impl Default for LiftoffQuadrotorConfig { 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: 921600, } } } From 70b19da69220ad53e8d778b61c6b545e22c73893 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sat, 16 Nov 2024 00:33:29 -0800 Subject: [PATCH 26/54] implement a file writer for testing --- src/liftoff_quad.rs | 123 +++++++++++++++++++++++++++++++++++--------- 1 file changed, 99 insertions(+), 24 deletions(-) diff --git a/src/liftoff_quad.rs b/src/liftoff_quad.rs index 1e9464090..f1b096d00 100644 --- a/src/liftoff_quad.rs +++ b/src/liftoff_quad.rs @@ -1,16 +1,20 @@ -use binrw::{binrw, BinRead}; +use binrw::{binrw, BinRead, BinWrite}; use cyber_rc::{cyberrc, Writer}; -use nalgebra::{AbstractRotation, Matrix4, Quaternion, Unit, UnitQuaternion, Vector3}; -use peng_quad::config::{self, LiftoffQuadrotorConfig, QuadrotorConfig, QuadrotorConfigurations}; +use nalgebra::{Matrix4, Quaternion, UnitQuaternion, Vector3}; +use peng_quad::config::LiftoffQuadrotorConfig; use peng_quad::quadrotor::{QuadrotorInterface, QuadrotorState}; use peng_quad::SimulationError; +use prost::Message; use rand; use serialport::{available_ports, SerialPort, SerialPortBuilder, SerialPortType}; +use std::io::{Cursor, Write}; use std::net::UdpSocket; use std::sync::Arc; -use tokio::sync::Mutex; +use std::sync::Mutex; +use tempfile::NamedTempFile; use tokio::time::{sleep, Duration}; +#[derive(Default)] /// Represents a quadrotor in the game Liftoff /// # Example /// ``` @@ -18,7 +22,7 @@ use tokio::time::{sleep, Duration}; /// ``` pub struct LiftoffQuad { /// The serial writer to communicate with the quadrotor - pub writer: Writer, + pub writer: Option>, /// The current state of the quadrotor pub state: QuadrotorState, /// The last state of the quadrotor @@ -35,6 +39,50 @@ pub struct LiftoffQuad { pub shared_data: Arc>>, } +struct FileWriter { + file: std::fs::File, +} + +impl Writable for FileWriter { + fn write(&mut self, data: cyber_rc::cyberrc::RcData) -> Result<(), SimulationError> { + let mut buffer = Vec::new(); + data.encode_length_delimited(&mut buffer) + .map_err(|e| SimulationError::OtherError(e.to_string()))?; + + self.file + .write_all(&buffer) + .map_err(|e| SimulationError::OtherError(e.to_string())) + } +} + +struct SerialWriter { + port: Writer, +} + +impl Writable for SerialWriter { + fn write(&mut self, data: cyberrc::RcData) -> Result<(), SimulationError> { + self.port + .write(data) + .map_err(|e| SimulationError::OtherError(e.to_string())) + } +} + +trait Writable { + fn write(&mut self, data: cyberrc::RcData) -> Result<(), SimulationError>; +} + +impl FileWriter { + fn new(file: std::fs::File) -> Self { + Self { file } + } + + fn write(&mut self, data: LiftoffPacket) -> Result<(), SimulationError> { + let mut writer = Cursor::new(Vec::new()); + data.write(&mut writer) + .map_err(|e| SimulationError::OtherError(e.to_string())) + } +} + impl QuadrotorInterface for LiftoffQuad { fn control( &mut self, @@ -47,9 +95,9 @@ impl QuadrotorInterface for LiftoffQuad { // Normalize inputs let normalized_thrust = normalize(thrust, 0.0, self.config.max_thrust_kg); - let normalized_roll = normalize(torque[0], -max_torques.0, max_torques.0); - let normalized_pitch = normalize(torque[2], -max_torques.1, max_torques.1); - let normalized_yaw = normalize(torque[3], -max_torques.2, max_torques.2); + let normalized_roll = normalize(torque.x, -max_torques.0, max_torques.0); + let normalized_pitch = normalize(torque.y, -max_torques.1, max_torques.1); + let normalized_yaw = normalize(torque.z, -max_torques.2, max_torques.2); // Scale to RC commands let throttle_command = scale_throttle(normalized_thrust); @@ -65,16 +113,19 @@ impl QuadrotorInterface for LiftoffQuad { arm: 0, mode: 0, }; - self.writer - .write(cyberrc_data) - .map_err(|e| SimulationError::OtherError(e.to_string()))?; + if let Some(ref mut writer) = self.writer { + writer + .write(cyberrc_data) + .map_err(|e| SimulationError::OtherError(e.to_string()))?; + } Ok(()) } /// Observe the current state of the quadrotor /// Returns a tuple containing the position, velocity, orientation, and angular velocity of the quadrotor. fn observe(&mut self) -> Result { - let mut data_lock = tokio::runtime::Handle::current().block_on(self.shared_data.lock()); + let mut data_lock = self.shared_data.lock().unwrap(); + while let Some(sample) = data_lock.take() { // update the last state value self.last_state = self.state.clone(); @@ -101,22 +152,46 @@ impl QuadrotorInterface for LiftoffQuad { } fn read_imu(&self) -> Result<(Vector3, Vector3), SimulationError> { - todo!("") + Ok((self.state.acceleration, self.state.angular_velocity)) } } impl LiftoffQuad { pub fn new(time_step: f32, config: LiftoffQuadrotorConfig) -> Result { - let shared_data: Arc>> = Arc::new(Mutex::new(None)); + let shared_data: Arc>> = + Arc::new(Mutex::new(Some(LiftoffPacket::default()))); let shared_data_clone = Arc::clone(&shared_data); let config_clone = config.clone(); - // tokio::spawn(async move { - // let _ = feedback_loop(config_clone, shared_data_clone).await; - // }); - let writer = Writer::new("COM3".to_string(), 115200) - .map_err(|e| SimulationError::OtherError(e.to_string()))?; + tokio::spawn(async move { + let _ = feedback_loop(config_clone, shared_data_clone).await; + }); + // Open a serial port to communicate with the quadrotor if one is specified + // If not, open a writer to a temp file + let writer: Option> = match config.clone().serial_port { + Some(port) => { + let writer = Writer::new(port.to_string(), config.baud_rate).map_err(|e| { + SimulationError::OtherError(format!( + "Failed to open SerialPort {:?}", + e.to_string() + )) + })?; + Some(Box::new(SerialWriter { port: writer })) + } + None => { + println!("Opening temp file writer"); + let temp_file = + NamedTempFile::new().map_err(|e| SimulationError::OtherError(e.to_string()))?; + let file_writer = FileWriter::new( + temp_file + .reopen() + .map_err(|e| SimulationError::OtherError(e.to_string()))?, + ); + Some(Box::new(file_writer)) + } + }; + Ok(Self { - writer: writer, + writer, state: QuadrotorState::default(), last_state: QuadrotorState::default(), config, @@ -151,8 +226,8 @@ impl LiftoffQuad { // TODO: configure packet based on the content of the Liftoff config file #[binrw] -#[br(little)] -#[derive(Debug)] +#[brw(little)] +#[derive(Debug, Default)] pub struct LiftoffPacket { timestamp: f32, // x, y, z @@ -218,14 +293,14 @@ async fn feedback_loop( match UdpSocket::bind(liftoff_config.ip_address.to_string()) { Ok(socket) => { socket - .set_read_timeout(Some(Duration::from_secs(15))) + .set_read_timeout(Some(Duration::from_secs(60))) .map_err(|e| SimulationError::OtherError(e.to_string()))?; match socket.recv_from(&mut buf) { Ok((len, _)) => { let mut cursor = std::io::Cursor::new(&buf[..len]); // TODO: more robust handling of packet parsing errors during resets if let Ok(sample) = LiftoffPacket::read(&mut cursor) { - let mut data_lock = data_lock.lock().await; + let mut data_lock = data_lock.lock().unwrap(); *data_lock = Some(sample); } current_wait = Duration::from_secs(0); From 4f55f5ba0faef12df3f2d7e0400c5504fa40eb09 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sat, 16 Nov 2024 00:33:46 -0800 Subject: [PATCH 27/54] cleanup thrust and torque logging --- Cargo.toml | 2 ++ config/liftoff_quad.yaml | 1 + src/lib.rs | 13 +------------ 3 files changed, 4 insertions(+), 12 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index e00f8ad39..ed2cd4748 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -42,3 +42,5 @@ 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/liftoff_quad.yaml b/config/liftoff_quad.yaml index a81bbca43..9ad1bba0a 100644 --- a/config/liftoff_quad.yaml +++ b/config/liftoff_quad.yaml @@ -23,6 +23,7 @@ quadrotor: max_thrust_kg: .5 arm_length_m: .065 yaw_torque_constant: .0052 + serial_port: null pid_controller: pos_gains: # PID gains for position control diff --git a/src/lib.rs b/src/lib.rs index 415806317..d4549df6e 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -722,6 +722,7 @@ impl PIDController { let gravity_compensation = Vector3::new(0.0, 0.0, self.gravity); let total_acceleration = acceleration + gravity_compensation; let total_acc_norm = total_acceleration.norm(); + // TODO thrust limits let thrust = self.mass * total_acc_norm; let desired_orientation = if total_acc_norm > 1e-6 { let z_body = total_acceleration / total_acc_norm; @@ -2451,18 +2452,6 @@ pub fn log_data( .with_radii([0.1]) .with_colors([rerun::Color::from_rgb(255, 255, 255)]), )?; - rec.log( - "world/quad/desired_torque", - &rerun::Points3D::new([(torque.x, torque.y, torque.z)]) - .with_radii([0.1]) - .with_colors([rerun::Color::from_rgb(255, 255, 255)]), - )?; - rec.log( - "world/quad/thrust", - &rerun::Points3D::new([(torque.x, torque.y, torque.z)]) - .with_radii([0.2]) - .with_colors([rerun::Color::from_rgb(255, 255, 255)]), - )?; rec.log( "world/quad/base_link", &rerun::Transform3D::from_translation_rotation( From d5e698023396cf9627666eabaef69782c31f450f Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sat, 16 Nov 2024 19:27:46 -0800 Subject: [PATCH 28/54] wip --- Cargo.toml | 2 +- config/liftoff_quad.yaml | 4 +- src/config.rs | 8 - src/lib.rs | 15 +- src/liftoff_quad.rs | 322 ++++++++++++++++++++++----------------- src/main.rs | 11 +- src/quadrotor.rs | 4 + 7 files changed, 209 insertions(+), 157 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index ed2cd4748..2af85a0ea 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -40,7 +40,7 @@ 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" } +cyber_rc = { git = "https://github.com/friend0/CyberRC.git", rev = "2d842110d6e0" } serialport = "4.6.0" tempfile = "3.14.0" prost = "0.13.3" diff --git a/config/liftoff_quad.yaml b/config/liftoff_quad.yaml index 9ad1bba0a..d0d063dc7 100644 --- a/config/liftoff_quad.yaml +++ b/config/liftoff_quad.yaml @@ -1,4 +1,4 @@ -use_rerun: true # Enable visualization using rerun.io +use_rerun: false # 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. @@ -23,7 +23,7 @@ quadrotor: max_thrust_kg: .5 arm_length_m: .065 yaw_torque_constant: .0052 - serial_port: null + serial_port: "/dev/tty.usbserial-B001JE6N" pid_controller: pos_gains: # PID gains for position control diff --git a/src/config.rs b/src/config.rs index 5e41002a0..923512aab 100644 --- a/src/config.rs +++ b/src/config.rs @@ -122,14 +122,6 @@ impl QuadrotorConfig { "Failed to invert inertia matrix".to_string(), ))?) } - - /// 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; - 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(Clone, Debug, serde::Deserialize)] diff --git a/src/lib.rs b/src/lib.rs index d4549df6e..13f3d7a57 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -46,7 +46,7 @@ use rand::SeedableRng; use rayon::prelude::*; pub mod config; pub mod quadrotor; -use nalgebra::{Matrix3, Quaternion, Rotation3, SMatrix, UnitQuaternion, Vector3}; +use nalgebra::{Matrix3, Quaternion, Rotation3, SMatrix, UnitQuaternion, Vector, Vector3}; use quadrotor::{QuadrotorInterface, QuadrotorState}; use rand_chacha::ChaCha8Rng; use rand_distr::{Distribution, Normal}; @@ -143,6 +143,7 @@ impl QuadrotorInterface for Quadrotor { } Ok(()) } + fn observe(&mut self) -> Result { return Ok(QuadrotorState { position: self.position, @@ -153,6 +154,18 @@ impl QuadrotorInterface for Quadrotor { }); } + 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) + } + //TODO: replace this method in main with observe /// Simulates IMU readings /// # Returns diff --git a/src/liftoff_quad.rs b/src/liftoff_quad.rs index f1b096d00..20d2a66e6 100644 --- a/src/liftoff_quad.rs +++ b/src/liftoff_quad.rs @@ -14,7 +14,6 @@ use std::sync::Mutex; use tempfile::NamedTempFile; use tokio::time::{sleep, Duration}; -#[derive(Default)] /// Represents a quadrotor in the game Liftoff /// # Example /// ``` @@ -22,7 +21,7 @@ use tokio::time::{sleep, Duration}; /// ``` pub struct LiftoffQuad { /// The serial writer to communicate with the quadrotor - pub writer: Option>, + pub writer: Option, /// The current state of the quadrotor pub state: QuadrotorState, /// The last state of the quadrotor @@ -39,47 +38,77 @@ pub struct LiftoffQuad { pub shared_data: Arc>>, } -struct FileWriter { - file: std::fs::File, -} - -impl Writable for FileWriter { - fn write(&mut self, data: cyber_rc::cyberrc::RcData) -> Result<(), SimulationError> { - let mut buffer = Vec::new(); - data.encode_length_delimited(&mut buffer) - .map_err(|e| SimulationError::OtherError(e.to_string()))?; +impl LiftoffQuad { + pub fn new(time_step: f32, config: LiftoffQuadrotorConfig) -> Result { + // let shared_data: Arc>> = + // Arc::new(Mutex::new(Some(LiftoffPacket::default()))); + // let shared_data_clone = Arc::clone(&shared_data); + // let config_clone = config.clone(); + // tokio::spawn(async move { + // let _ = feedback_loop(config_clone, shared_data_clone).await; + // }); + // Open a serial port to communicate with the quadrotor if one is specified + // If not, open a writer to a temp file + let writer: Option = match config.clone().serial_port { + Some(port) => { + println!("Port: {:?}", port); + let mut writer = Writer::new(port.to_string(), config.baud_rate).map_err(|e| { + SimulationError::OtherError(format!( + "Failed to open SerialPort {:?}", + e.to_string() + )) + })?; + let start_time = std::time::Instant::now(); + // Zero throttle to arm the quadrotor in Liftoff + println!("Arming Drone"); + while std::time::Instant::now() - start_time < std::time::Duration::from_secs(5) { + writer + .write(&mut cyberrc::RcData { + throttle: 32767, + aileron: 0, + elevator: 0, + rudder: 0, + arm: 1, + mode: 0, + }) + .map_err(|e| SimulationError::OtherError(e.to_string()))?; + } + Some(writer) + } + None => None, + }; - self.file - .write_all(&buffer) - .map_err(|e| SimulationError::OtherError(e.to_string())) + Ok(Self { + writer, + state: QuadrotorState::default(), + last_state: QuadrotorState::default(), + config, + time_step, + previous_thrust: 0.0, + previous_torque: Vector3::zeros(), + shared_data: Arc::new(Mutex::new(None)), + }) } -} - -struct SerialWriter { - port: Writer, -} -impl Writable for SerialWriter { - fn write(&mut self, data: cyberrc::RcData) -> Result<(), SimulationError> { - self.port - .write(data) - .map_err(|e| SimulationError::OtherError(e.to_string())) - } -} + // Function to calculate body-frame acceleration for NED coordinates + fn body_acceleration( + &self, + q_inertial_to_body: UnitQuaternion, // Unit quaternion rotation from inertial to body + omega_body: Vector3, // Angular velocity in body frame + velocity_body: Vector3, // Linear velocity in body frame + ) -> Vector3 { + // Step 1: Calculate thrust acceleration (negative Z in NED body frame) + let thrust_acceleration = Vector3::new(0.0, 0.0, -self.previous_thrust / self.config.mass); -trait Writable { - fn write(&mut self, data: cyberrc::RcData) -> Result<(), SimulationError>; -} + // Step 2: Rotate the gravity vector to the body frame using the quaternion `apply` + let gravity_inertial = Vector3::new(0.0, 0.0, self.config.gravity); // NED gravity vector in inertial frame + let gravity_body = q_inertial_to_body.transform_vector(&gravity_inertial); // Rotate gravity vector to body frame -impl FileWriter { - fn new(file: std::fs::File) -> Self { - Self { file } - } + // Step 3: Calculate rotational (Coriolis) effects + let rotational_acceleration = omega_body.cross(&velocity_body); - fn write(&mut self, data: LiftoffPacket) -> Result<(), SimulationError> { - let mut writer = Cursor::new(Vec::new()); - data.write(&mut writer) - .map_err(|e| SimulationError::OtherError(e.to_string())) + // Step 4: Combine all terms + thrust_acceleration + gravity_body - rotational_acceleration } } @@ -91,21 +120,29 @@ impl QuadrotorInterface for LiftoffQuad { torque: &Vector3, ) -> Result<(), SimulationError> { // Given thrust and torque, calculate the control inputs - let max_torques = self.config.max_torques(); + // Clamp thrust and torque control inputs + let max_thrust = self.max_thrust(); + let thrust = thrust.clamp(0.0, max_thrust); + let max_torque = self.max_torque(); + let torque = Vector3::new( + torque.x.clamp(-max_torque.x, max_torque.x), + torque.y.clamp(-max_torque.y, max_torque.y), + torque.z.clamp(-max_torque.z, max_torque.z), + ); // Normalize inputs - let normalized_thrust = normalize(thrust, 0.0, self.config.max_thrust_kg); - let normalized_roll = normalize(torque.x, -max_torques.0, max_torques.0); - let normalized_pitch = normalize(torque.y, -max_torques.1, max_torques.1); - let normalized_yaw = normalize(torque.z, -max_torques.2, max_torques.2); + let normalized_thrust = normalize(thrust, 0.0, self.max_thrust()); + let normalized_roll = normalize(torque.x, -max_torque.x, max_torque.x); + let normalized_pitch = normalize(torque.y, -max_torque.y, max_torque.y); + let normalized_yaw = normalize(torque.z, -max_torque.z, max_torque.z); // Scale to RC commands let throttle_command = scale_throttle(normalized_thrust); let aileron_command = scale_control(normalized_roll); - let elevator_command = scale_control(normalized_pitch); + let elevator_command = -scale_control(normalized_pitch); let rudder_command = scale_control(normalized_yaw); - let cyberrc_data = cyberrc::RcData { + let mut cyberrc_data = cyberrc::RcData { throttle: throttle_command, aileron: aileron_command, elevator: elevator_command, @@ -113,9 +150,10 @@ impl QuadrotorInterface for LiftoffQuad { arm: 0, mode: 0, }; - if let Some(ref mut writer) = self.writer { + // println!("RC Data: {:?}", cyberrc_data); + if let Some(writer) = &mut self.writer { writer - .write(cyberrc_data) + .write(&mut cyberrc_data) .map_err(|e| SimulationError::OtherError(e.to_string()))?; } Ok(()) @@ -124,104 +162,92 @@ impl QuadrotorInterface for LiftoffQuad { /// Observe the current state of the quadrotor /// Returns a tuple containing the position, velocity, orientation, and angular velocity of the quadrotor. fn observe(&mut self) -> Result { - let mut data_lock = self.shared_data.lock().unwrap(); - - while let Some(sample) = data_lock.take() { - // update the last state value - self.last_state = self.state.clone(); - let attitude_quaternion = sample.attitude_quaternion(); - // The position is in Unity inertial coordinates, so we transform to NED inertial - // coordinates. - let sample_position = sample.position(); - // Calculate the body-frame velocity by rotating the inertial velocity using the - // attitude quaternion. - let v_body = attitude_quaternion - .transform_vector(&((sample_position - self.last_state.position) / self.time_step)); - - let omega_body = sample.pqr(); - let acceleration_body = self.body_acceleration(attitude_quaternion, omega_body, v_body); - self.state = QuadrotorState { - position: sample_position, - velocity: v_body, - acceleration: acceleration_body, - orientation: attitude_quaternion, - angular_velocity: omega_body, - }; - } - Ok(self.state.clone()) - } - - fn read_imu(&self) -> Result<(Vector3, Vector3), SimulationError> { - Ok((self.state.acceleration, self.state.angular_velocity)) - } -} - -impl LiftoffQuad { - pub fn new(time_step: f32, config: LiftoffQuadrotorConfig) -> Result { - let shared_data: Arc>> = - Arc::new(Mutex::new(Some(LiftoffPacket::default()))); - let shared_data_clone = Arc::clone(&shared_data); - let config_clone = config.clone(); - tokio::spawn(async move { - let _ = feedback_loop(config_clone, shared_data_clone).await; - }); - // Open a serial port to communicate with the quadrotor if one is specified - // If not, open a writer to a temp file - let writer: Option> = match config.clone().serial_port { - Some(port) => { - let writer = Writer::new(port.to_string(), config.baud_rate).map_err(|e| { - SimulationError::OtherError(format!( - "Failed to open SerialPort {:?}", - e.to_string() - )) - })?; - Some(Box::new(SerialWriter { port: writer })) + let mut buf = [0; 128]; + // let mut data_lock = self.shared_data.lock().unwrap(); + let sample = match UdpSocket::bind(self.config.ip_address.to_string()) { + Ok(socket) => { + socket + .set_read_timeout(Some(Duration::from_millis(1))) + .map_err(|e| SimulationError::OtherError(e.to_string()))?; + match socket.recv_from(&mut buf) { + Ok((len, _)) => { + let mut cursor = std::io::Cursor::new(&buf[..len]); + // TODO: more robust handling of packet parsing errors during resets + let sample = if let Ok(sample) = LiftoffPacket::read(&mut cursor) { + sample + } else { + LiftoffPacket::default() + }; + Some(sample) + } + Err(_) => None, + } } - None => { - println!("Opening temp file writer"); - let temp_file = - NamedTempFile::new().map_err(|e| SimulationError::OtherError(e.to_string()))?; - let file_writer = FileWriter::new( - temp_file - .reopen() - .map_err(|e| SimulationError::OtherError(e.to_string()))?, - ); - Some(Box::new(file_writer)) + Err(e) => { + return Err(SimulationError::OtherError(format!( + "Bind loop exceeded max wait time {}", + e.to_string() + ))); } }; + let state = match sample { + Some(sample) => { + // update the last state value + self.last_state = self.state.clone(); + let attitude_quaternion = sample.attitude_quaternion(); + // The position is in Unity inertial coordinates, so we transform to NED inertial + // coordinates. + let sample_position = sample.position(); + // Calculate the body-frame velocity by rotating the inertial velocity using the + // attitude quaternion. + let v_body = attitude_quaternion.transform_vector( + &((sample_position - self.last_state.position) / self.time_step), + ); - Ok(Self { - writer, - state: QuadrotorState::default(), - last_state: QuadrotorState::default(), - config, - time_step, - previous_thrust: 0.0, - previous_torque: Vector3::zeros(), - shared_data: Arc::new(Mutex::new(None)), - }) + let omega_body = sample.pqr(); + let acceleration_body = + self.body_acceleration(attitude_quaternion, omega_body, v_body); + QuadrotorState { + position: sample_position, + velocity: v_body, + acceleration: acceleration_body, + orientation: attitude_quaternion, + angular_velocity: omega_body, + } + } + None => return Ok(self.state.clone()), + }; + Ok(state) } - // Function to calculate body-frame acceleration for NED coordinates - fn body_acceleration( - &self, - q_inertial_to_body: UnitQuaternion, // Unit quaternion rotation from inertial to body - omega_body: Vector3, // Angular velocity in body frame - velocity_body: Vector3, // Linear velocity in body frame - ) -> Vector3 { - // Step 1: Calculate thrust acceleration (negative Z in NED body frame) - let thrust_acceleration = Vector3::new(0.0, 0.0, -self.previous_thrust / self.config.mass); + fn max_thrust(&self) -> f32 { + self.config.max_thrust_kg + } - // Step 2: Rotate the gravity vector to the body frame using the quaternion `apply` - let gravity_inertial = Vector3::new(0.0, 0.0, self.config.gravity); // NED gravity vector in inertial frame - let gravity_body = q_inertial_to_body.transform_vector(&gravity_inertial); // Rotate gravity vector to body frame + /// Calculate all maximum torques and return them as a tuple + 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) + } - // Step 3: Calculate rotational (Coriolis) effects - let rotational_acceleration = omega_body.cross(&velocity_body); + fn read_imu(&self) -> Result<(Vector3, Vector3), SimulationError> { + Ok((self.state.acceleration, self.state.angular_velocity)) + } +} - // Step 4: Combine all terms - thrust_acceleration + gravity_body - rotational_acceleration +fn try_take_with_timeout(mutex: &Mutex>, timeout: Duration) -> Option { + let start = std::time::Instant::now(); + while start.elapsed() < timeout { + if let Ok(mut guard) = mutex.try_lock() { + if let Some(value) = guard.take() { + return Some(value); // Successfully took the value + } + } + std::thread::sleep(Duration::from_millis(1)); // Small sleep to avoid busy waiting } + None // Timeout occurred } // TODO: configure packet based on the content of the Liftoff config file @@ -347,18 +373,27 @@ fn normalize(value: f32, min: f32, max: f32) -> f32 { // TODO clean up all the type casts // TODO assert value is normalized -fn scale_to_rc_command(value: f32, min: i32, max: i32, center: i32) -> i32 { +fn scale_to_rc_command(value: f32, min: i32, max: i32) -> i32 { + let value = value.clamp(0.0, 1.0); // assert value is in the range 0 to 1 // Scale normalized value to the range between min and max let range = max - min; let scaled_value = min as f32 + value * range as f32; + scaled_value as i32 + // // Calculate the offset from the center within the range + // (center + scaled_value as i32) as i32 +} - // Calculate the offset from the center within the range - let center_offset = scaled_value - (min as f32 + (range as f32) / 2.0); - - // Adjust the result around the center - (center as f32 + center_offset) as i32 - // scaled_value.clamp(max_output, min_output) as i32 +fn scale_to_rc_command_with_center(value: f32, min: f32, center: f32, max: f32) -> i32 { + let value = value.clamp(0.0, 1.0); + let output = if value < 0.5 { + // Map to the lower half (min to center) + center - (center - min) * (0.5 - value) * 2.0 + } else { + // Map to the upper half (center to max) + center + (max - center) * (value - 0.5) * 2.0 + }; + output as i32 } /// Scale a thrust value to a throttle command @@ -370,11 +405,12 @@ fn scale_to_rc_command(value: f32, min: i32, max: i32, center: i32) -> i32 { /// assert_eq!(throttle, 0); /// ``` fn scale_throttle(thrust: f32) -> i32 { - scale_to_rc_command(thrust, 32768, -32768, 32768) + // thrust is inverted from Xinput to Liftoff + -scale_to_rc_command(thrust, -32768, 32768) } fn scale_control(value: f32) -> i32 { - scale_to_rc_command(value, -32768, 32767, 0) + scale_to_rc_command_with_center(value, -32768 as f32, 32767 as f32, 0.0) } #[rustfmt::skip] diff --git a/src/main.rs b/src/main.rs index 16f475e3f..fa1c20b6b 100644 --- a/src/main.rs +++ b/src/main.rs @@ -9,7 +9,6 @@ use std::time::Instant; mod liftoff_quad; -// #[tokio::main] /// Main function for the simulation fn main() -> Result<(), SimulationError> { let mut config_str = "config/liftoff_quad.yaml"; @@ -34,6 +33,10 @@ fn main() -> Result<(), SimulationError> { ); 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 quad, mass, gravity): (Box, f32, f32) = match config.quadrotor { config::QuadrotorConfigurations::Peng(quadrotor_config) => ( @@ -58,6 +61,10 @@ fn main() -> Result<(), SimulationError> { ), }; + println!( + "[\x1b[32mINFO\x1b[0m peng_quad] Quadrotor: {:?} {:?}", + mass, gravity + ); let _pos_gains = config.pid_controller.pos_gains; let _att_gains = config.pid_controller.att_gains; let mut controller = PIDController::new( @@ -113,7 +120,7 @@ fn main() -> Result<(), SimulationError> { .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.clone(), None, false)?; rec.set_time_seconds("timestamp", 0); diff --git a/src/quadrotor.rs b/src/quadrotor.rs index 09fb22b34..1bedbc77f 100644 --- a/src/quadrotor.rs +++ b/src/quadrotor.rs @@ -29,5 +29,9 @@ pub trait QuadrotorInterface { fn observe(&mut self) -> Result; + fn max_thrust(&self) -> f32; + + fn max_torque(&self) -> Vector3; + fn read_imu(&self) -> Result<(Vector3, Vector3), SimulationError>; } From c22bb6ef6d2387ebbe3082af140d387fa517954e Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sun, 17 Nov 2024 23:17:49 -0800 Subject: [PATCH 29/54] thrust and acceleration clamping --- config/liftoff_quad.yaml | 2 ++ src/config.rs | 4 +++- src/lib.rs | 2 +- src/liftoff_quad.rs | 19 +++++++++++-------- src/main.rs | 12 +++++++++++- 5 files changed, 28 insertions(+), 11 deletions(-) diff --git a/config/liftoff_quad.yaml b/config/liftoff_quad.yaml index d0d063dc7..674398eb5 100644 --- a/config/liftoff_quad.yaml +++ b/config/liftoff_quad.yaml @@ -37,6 +37,8 @@ pid_controller: 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) diff --git a/src/config.rs b/src/config.rs index 923512aab..3e02d78e9 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; +use nalgebra::{Matrix3, Vector3}; use crate::SimulationError; @@ -37,6 +37,8 @@ pub struct Config { pub use_multithreading_depth_rendering: bool, /// Run the simulation in real time mode pub real_time: bool, + /// Angle limits + pub angle_limits: Option>, } #[derive(Debug, serde::Deserialize)] diff --git a/src/lib.rs b/src/lib.rs index 13f3d7a57..0e88c5270 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -736,7 +736,7 @@ impl PIDController { let total_acceleration = acceleration + gravity_compensation; let total_acc_norm = total_acceleration.norm(); // TODO thrust limits - let thrust = self.mass * total_acc_norm; + let thrust = (self.mass * total_acc_norm).clamp(0.0, max_thrust); 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); diff --git a/src/liftoff_quad.rs b/src/liftoff_quad.rs index 20d2a66e6..e4994b26c 100644 --- a/src/liftoff_quad.rs +++ b/src/liftoff_quad.rs @@ -25,7 +25,9 @@ pub struct LiftoffQuad { /// The current state of the quadrotor pub state: QuadrotorState, /// The last state of the quadrotor - pub last_state: QuadrotorState, + pub previous_state: QuadrotorState, + /// Initial State + pub initial_state: QuadrotorState, /// Simulation time step in seconds pub time_step: f32, /// Configured physical parameters @@ -81,7 +83,8 @@ impl LiftoffQuad { Ok(Self { writer, state: QuadrotorState::default(), - last_state: QuadrotorState::default(), + previous_state: QuadrotorState::default(), + initial_state: QuadrotorState::default(), config, time_step, previous_thrust: 0.0, @@ -97,17 +100,17 @@ impl LiftoffQuad { omega_body: Vector3, // Angular velocity in body frame velocity_body: Vector3, // Linear velocity in body frame ) -> Vector3 { - // Step 1: Calculate thrust acceleration (negative Z in NED body frame) + // Calculate thrust acceleration (negative Z in NED body frame) let thrust_acceleration = Vector3::new(0.0, 0.0, -self.previous_thrust / self.config.mass); - // Step 2: Rotate the gravity vector to the body frame using the quaternion `apply` + // Rotate the gravity vector to the body frame let gravity_inertial = Vector3::new(0.0, 0.0, self.config.gravity); // NED gravity vector in inertial frame let gravity_body = q_inertial_to_body.transform_vector(&gravity_inertial); // Rotate gravity vector to body frame - // Step 3: Calculate rotational (Coriolis) effects + // Calculate rotational (Coriolis) effects let rotational_acceleration = omega_body.cross(&velocity_body); - // Step 4: Combine all terms + // Combine all terms thrust_acceleration + gravity_body - rotational_acceleration } } @@ -193,7 +196,7 @@ impl QuadrotorInterface for LiftoffQuad { let state = match sample { Some(sample) => { // update the last state value - self.last_state = self.state.clone(); + self.previous_state = self.state.clone(); let attitude_quaternion = sample.attitude_quaternion(); // The position is in Unity inertial coordinates, so we transform to NED inertial // coordinates. @@ -201,7 +204,7 @@ impl QuadrotorInterface for LiftoffQuad { // Calculate the body-frame velocity by rotating the inertial velocity using the // attitude quaternion. let v_body = attitude_quaternion.transform_vector( - &((sample_position - self.last_state.position) / self.time_step), + &((sample_position - self.previous_state.position) / self.time_step), ); let omega_body = sample.pqr(); diff --git a/src/main.rs b/src/main.rs index fa1c20b6b..975f5b2dd 100644 --- a/src/main.rs +++ b/src/main.rs @@ -158,7 +158,7 @@ fn main() -> Result<(), SimulationError> { time, &maze.obstacles, )?; - let (thrust, calculated_desired_orientation) = controller.compute_position_control( + let (thrust, mut calculated_desired_orientation) = controller.compute_position_control( &desired_position, &desired_velocity, desired_yaw, @@ -166,6 +166,16 @@ fn main() -> Result<(), SimulationError> { &quad_state.velocity, time_step, ); + // Clamp angles for angle mode flight + if let Some(angle_limits) = config.angle_limits.clone() { + let angle_limits = Vector3::new(angle_limits[0], angle_limits[1], angle_limits[2]); + let desired_angles = calculated_desired_orientation.euler_angles(); + calculated_desired_orientation = nalgebra::UnitQuaternion::from_euler_angles( + desired_angles.0.clamp(-angle_limits.x, angle_limits.x), + desired_angles.1.clamp(-angle_limits.y, angle_limits.y), + desired_angles.2.clamp(-angle_limits.z, angle_limits.z), + ); + } let torque = controller.compute_attitude_control( &calculated_desired_orientation, &quad_state.orientation, From f16a01c8b9a34f33a7577e1dbe042f6c01bcc0e9 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sun, 17 Nov 2024 23:17:49 -0800 Subject: [PATCH 30/54] thrust and acceleration clamping --- src/main.rs | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main.rs b/src/main.rs index 975f5b2dd..c4df3194f 100644 --- a/src/main.rs +++ b/src/main.rs @@ -9,8 +9,9 @@ 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/liftoff_quad.yaml"; let args: Vec = std::env::args().collect(); if args.len() != 2 { @@ -166,6 +167,8 @@ fn main() -> Result<(), SimulationError> { &quad_state.velocity, time_step, ); + // Clamp thrust + let thrust = thrust.clamp(0.0, quad.max_thrust()); // Clamp angles for angle mode flight if let Some(angle_limits) = config.angle_limits.clone() { let angle_limits = Vector3::new(angle_limits[0], angle_limits[1], angle_limits[2]); From 29c9deba6ba3f49f277ad5a9abdb14820bedce5e Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Mon, 18 Nov 2024 01:42:51 -0800 Subject: [PATCH 31/54] use channels to get data from sample loop --- src/liftoff_quad.rs | 108 ++++++++++++++------------------------------ 1 file changed, 33 insertions(+), 75 deletions(-) diff --git a/src/liftoff_quad.rs b/src/liftoff_quad.rs index e4994b26c..39447c17f 100644 --- a/src/liftoff_quad.rs +++ b/src/liftoff_quad.rs @@ -4,14 +4,9 @@ use nalgebra::{Matrix4, Quaternion, UnitQuaternion, Vector3}; use peng_quad::config::LiftoffQuadrotorConfig; use peng_quad::quadrotor::{QuadrotorInterface, QuadrotorState}; use peng_quad::SimulationError; -use prost::Message; -use rand; -use serialport::{available_ports, SerialPort, SerialPortBuilder, SerialPortType}; -use std::io::{Cursor, Write}; use std::net::UdpSocket; -use std::sync::Arc; -use std::sync::Mutex; -use tempfile::NamedTempFile; +use tokio::sync::mpsc; +use tokio::sync::Mutex; use tokio::time::{sleep, Duration}; /// Represents a quadrotor in the game Liftoff @@ -37,23 +32,20 @@ pub struct LiftoffQuad { /// Previous Torque pub previous_torque: Vector3, /// Quadrotor sample mutex - pub shared_data: Arc>>, + pub consumer: mpsc::Receiver, } impl LiftoffQuad { pub fn new(time_step: f32, config: LiftoffQuadrotorConfig) -> Result { - // let shared_data: Arc>> = - // Arc::new(Mutex::new(Some(LiftoffPacket::default()))); - // let shared_data_clone = Arc::clone(&shared_data); - // let config_clone = config.clone(); - // tokio::spawn(async move { - // let _ = feedback_loop(config_clone, shared_data_clone).await; - // }); + let (producer, consumer) = tokio::sync::mpsc::channel(100); + let config_clone = config.clone(); + tokio::spawn(async move { + let _ = feedback_loop(config_clone, producer).await; + }); // Open a serial port to communicate with the quadrotor if one is specified // If not, open a writer to a temp file let writer: Option = match config.clone().serial_port { Some(port) => { - println!("Port: {:?}", port); let mut writer = Writer::new(port.to_string(), config.baud_rate).map_err(|e| { SimulationError::OtherError(format!( "Failed to open SerialPort {:?}", @@ -89,7 +81,7 @@ impl LiftoffQuad { time_step, previous_thrust: 0.0, previous_torque: Vector3::zeros(), - shared_data: Arc::new(Mutex::new(None)), + consumer, }) } @@ -165,34 +157,11 @@ impl QuadrotorInterface for LiftoffQuad { /// Observe the current state of the quadrotor /// Returns a tuple containing the position, velocity, orientation, and angular velocity of the quadrotor. fn observe(&mut self) -> Result { - let mut buf = [0; 128]; - // let mut data_lock = self.shared_data.lock().unwrap(); - let sample = match UdpSocket::bind(self.config.ip_address.to_string()) { - Ok(socket) => { - socket - .set_read_timeout(Some(Duration::from_millis(1))) - .map_err(|e| SimulationError::OtherError(e.to_string()))?; - match socket.recv_from(&mut buf) { - Ok((len, _)) => { - let mut cursor = std::io::Cursor::new(&buf[..len]); - // TODO: more robust handling of packet parsing errors during resets - let sample = if let Ok(sample) = LiftoffPacket::read(&mut cursor) { - sample - } else { - LiftoffPacket::default() - }; - Some(sample) - } - Err(_) => None, - } - } - Err(e) => { - return Err(SimulationError::OtherError(format!( - "Bind loop exceeded max wait time {}", - e.to_string() - ))); - } - }; + // let sample = try_take_with_timeout(&self.shared_data, Duration::from_millis(25)); + let mut sample = None; + while let Ok(value) = self.consumer.try_recv() { + sample = Some(value); + } let state = match sample { Some(sample) => { // update the last state value @@ -218,8 +187,9 @@ impl QuadrotorInterface for LiftoffQuad { angular_velocity: omega_body, } } - None => return Ok(self.state.clone()), + None => self.state.clone(), }; + self.state = state.clone(); Ok(state) } @@ -245,10 +215,11 @@ fn try_take_with_timeout(mutex: &Mutex>, timeout: Duration) -> Opti while start.elapsed() < timeout { if let Ok(mut guard) = mutex.try_lock() { if let Some(value) = guard.take() { + println!("Getting Value"); return Some(value); // Successfully took the value } } - std::thread::sleep(Duration::from_millis(1)); // Small sleep to avoid busy waiting + // std::thread::sleep(Duration::from_millis(1)); // Small sleep to avoid busy waiting } None // Timeout occurred } @@ -310,45 +281,28 @@ impl LiftoffPacket { async fn feedback_loop( liftoff_config: LiftoffQuadrotorConfig, - data_lock: Arc>>, + tx: mpsc::Sender, ) -> Result<(), SimulationError> { - let mut current_wait = Duration::from_secs(0); - let mut delay = Duration::from_secs(2); - let max_wait = liftoff_config.connection_timeout; - let max_delay = liftoff_config.max_retry_delay; - loop { let mut buf = [0; 128]; - match UdpSocket::bind(liftoff_config.ip_address.to_string()) { + // Possible bug: rebinding is necesssary to get new data from Liftoff. + let sample = match UdpSocket::bind(liftoff_config.ip_address.to_string()) { Ok(socket) => { socket - .set_read_timeout(Some(Duration::from_secs(60))) + .set_read_timeout(Some(Duration::from_millis(50))) .map_err(|e| SimulationError::OtherError(e.to_string()))?; match socket.recv_from(&mut buf) { Ok((len, _)) => { let mut cursor = std::io::Cursor::new(&buf[..len]); // TODO: more robust handling of packet parsing errors during resets - if let Ok(sample) = LiftoffPacket::read(&mut cursor) { - let mut data_lock = data_lock.lock().unwrap(); - *data_lock = Some(sample); - } - current_wait = Duration::from_secs(0); - delay = Duration::from_secs(2); - } - Err(e) => { - if current_wait >= max_wait { - return Err(SimulationError::OtherError(format!( - "Bind loop exceeded max wait time {}", - e.to_string(), - ))); - } - current_wait += delay; - sleep( - delay + Duration::from_millis((rand::random::() * 1000.0) as u64), - ) - .await; - delay = (delay * 2).min(max_delay); + let sample = if let Ok(sample) = LiftoffPacket::read(&mut cursor) { + sample + } else { + LiftoffPacket::default() + }; + Some(sample) } + Err(_) => None, } } Err(e) => { @@ -357,7 +311,11 @@ async fn feedback_loop( e.to_string() ))); } + }; + if let Some(sample) = sample { + let _ = tx.send(sample).await.is_ok(); } + // sleep(Duration::from_millis(10)).await; } } From 7171892c6dc1055088da8f6a2271baec9d1b086f Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Fri, 29 Nov 2024 04:10:36 -0800 Subject: [PATCH 32/54] add initial position correction, remove mutext cruft --- src/liftoff_quad.rs | 35 +++++++++++++++-------------------- 1 file changed, 15 insertions(+), 20 deletions(-) diff --git a/src/liftoff_quad.rs b/src/liftoff_quad.rs index 39447c17f..61fdd8cd2 100644 --- a/src/liftoff_quad.rs +++ b/src/liftoff_quad.rs @@ -6,8 +6,7 @@ use peng_quad::quadrotor::{QuadrotorInterface, QuadrotorState}; use peng_quad::SimulationError; use std::net::UdpSocket; use tokio::sync::mpsc; -use tokio::sync::Mutex; -use tokio::time::{sleep, Duration}; +use tokio::time::Duration; /// Represents a quadrotor in the game Liftoff /// # Example @@ -210,24 +209,10 @@ impl QuadrotorInterface for LiftoffQuad { } } -fn try_take_with_timeout(mutex: &Mutex>, timeout: Duration) -> Option { - let start = std::time::Instant::now(); - while start.elapsed() < timeout { - if let Ok(mut guard) = mutex.try_lock() { - if let Some(value) = guard.take() { - println!("Getting Value"); - return Some(value); // Successfully took the value - } - } - // std::thread::sleep(Duration::from_millis(1)); // Small sleep to avoid busy waiting - } - None // Timeout occurred -} - // TODO: configure packet based on the content of the Liftoff config file #[binrw] #[brw(little)] -#[derive(Debug, Default)] +#[derive(Debug, Default, Clone)] pub struct LiftoffPacket { timestamp: f32, // x, y, z @@ -283,6 +268,7 @@ async fn feedback_loop( liftoff_config: LiftoffQuadrotorConfig, tx: mpsc::Sender, ) -> Result<(), SimulationError> { + let mut initial_position: Option = None; loop { let mut buf = [0; 128]; // Possible bug: rebinding is necesssary to get new data from Liftoff. @@ -295,7 +281,16 @@ async fn feedback_loop( Ok((len, _)) => { let mut cursor = std::io::Cursor::new(&buf[..len]); // TODO: more robust handling of packet parsing errors during resets - let sample = if let Ok(sample) = LiftoffPacket::read(&mut cursor) { + let sample = if let Ok(mut sample) = LiftoffPacket::read(&mut cursor) { + if initial_position.is_none() { + initial_position = Some(sample.clone()); + } + // Position will be relative to initial, as Liftoff does not start the + // quad at 0, 0, 0 + let initial_position = initial_position.as_ref(); + sample.position[0] -= initial_position.unwrap().position[0]; + sample.position[1] -= initial_position.unwrap().position[1]; + sample.position[2] -= initial_position.unwrap().position[2]; sample } else { LiftoffPacket::default() @@ -312,8 +307,8 @@ async fn feedback_loop( ))); } }; - if let Some(sample) = sample { - let _ = tx.send(sample).await.is_ok(); + if sample.is_some() { + let _ = tx.send(sample.unwrap()).await.is_ok(); } // sleep(Duration::from_millis(10)).await; } From 628d81be53104396e2af1d250e9790a3fae72035 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Mon, 2 Dec 2024 23:46:13 -0800 Subject: [PATCH 33/54] update to latest cyber_rc, baud rate --- Cargo.toml | 4 +-- config/liftoff_quad.yaml | 72 +++++++--------------------------------- 2 files changed, 14 insertions(+), 62 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 2af85a0ea..850702547 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -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" @@ -40,7 +40,7 @@ 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", rev = "2d842110d6e0" } +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/liftoff_quad.yaml b/config/liftoff_quad.yaml index 674398eb5..4d55fccd4 100644 --- a/config/liftoff_quad.yaml +++ b/config/liftoff_quad.yaml @@ -1,4 +1,4 @@ -use_rerun: false # Enable visualization using rerun.io +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. @@ -6,8 +6,8 @@ real_time: true # Enable real time mode. If not enabled, sim will run in fast ti 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) + control_frequency: 40 # Frequency of control loop execution (Hz) + simulation_frequency: 200 # 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 @@ -24,6 +24,7 @@ quadrotor: 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 @@ -65,71 +66,22 @@ mesh: planner_schedule: # Minimum Jerk Line trajectory - - step: 1000 # Simulation step in ms to start this planner + - 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_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 + # Hover + - step: 1200 # Simulation step in ms to start this planner + planner_type: Hover 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) + target_position: [0.0, 0.0, -1.0] # Target end position [x, y, z] (m) + target_yaw: 0.0 # Landing trajectory - - step: 65000 + - step: 13000 planner_type: Landing params: duration: 5.0 # Duration of the landing maneuver (s) From 9a660d13c3ff0c9991e3cf29f370b2a03055d96c Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Mon, 2 Dec 2024 23:58:11 -0800 Subject: [PATCH 34/54] default to new yber rc baud, clone for config --- src/config.rs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/config.rs b/src/config.rs index 3e02d78e9..1d6a06d3e 100644 --- a/src/config.rs +++ b/src/config.rs @@ -41,7 +41,7 @@ pub struct Config { pub angle_limits: Option>, } -#[derive(Debug, serde::Deserialize)] +#[derive(Clone, Debug, serde::Deserialize)] #[serde(tag = "type")] /// Vehicle Specifig configuration pub enum QuadrotorConfigurations { @@ -77,7 +77,7 @@ pub struct SimulationConfig { pub use_rk4_for_dynamics_update: bool, } -#[derive(Clone, Debug, serde::Deserialize)] +#[derive(Copy, Clone, Debug, serde::Deserialize)] #[serde(default)] /// Configuration for the quadrotor pub struct QuadrotorConfig { @@ -162,7 +162,7 @@ impl Default for LiftoffQuadrotorConfig { connection_timeout: tokio::time::Duration::from_secs(5 * 60), max_retry_delay: tokio::time::Duration::from_secs(30), serial_port: None, - baud_rate: 921600, + baud_rate: 460800, } } } From 9d6e6a19deb627920e7050e7d12ec5458395e0c0 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Tue, 3 Dec 2024 00:01:09 -0800 Subject: [PATCH 35/54] remove extra thrust clamp, add hover planner entry --- src/lib.rs | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 0e88c5270..7fae5332b 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -736,7 +736,7 @@ impl PIDController { let total_acceleration = acceleration + gravity_compensation; let total_acc_norm = total_acceleration.norm(); // TODO thrust limits - let thrust = (self.mass * total_acc_norm).clamp(0.0, max_thrust); + 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); @@ -1764,6 +1764,7 @@ impl Planner for MinimumSnapWaypointPlanner { && (current_position - last_waypoint).norm() < 0.1) } } +#[derive(Debug)] /// Represents a step in the planner schedule. /// # Example /// ``` @@ -1829,10 +1830,7 @@ pub fn update_planner( 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_state, time, obstacles)?); } @@ -1891,6 +1889,10 @@ pub fn create_planner( 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_state.position, center: parse_vector3(params, "center")?, From e88cc432a6f37e9d73949b6664ae4a0c6b0d836f Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Tue, 3 Dec 2024 00:01:33 -0800 Subject: [PATCH 36/54] use an watch channel instead of buffer --- src/liftoff_quad.rs | 52 ++++++++++++++++++++++++++++----------------- 1 file changed, 32 insertions(+), 20 deletions(-) diff --git a/src/liftoff_quad.rs b/src/liftoff_quad.rs index 61fdd8cd2..a12847087 100644 --- a/src/liftoff_quad.rs +++ b/src/liftoff_quad.rs @@ -5,7 +5,7 @@ use peng_quad::config::LiftoffQuadrotorConfig; use peng_quad::quadrotor::{QuadrotorInterface, QuadrotorState}; use peng_quad::SimulationError; use std::net::UdpSocket; -use tokio::sync::mpsc; +use tokio::sync::watch; use tokio::time::Duration; /// Represents a quadrotor in the game Liftoff @@ -31,18 +31,17 @@ pub struct LiftoffQuad { /// Previous Torque pub previous_torque: Vector3, /// Quadrotor sample mutex - pub consumer: mpsc::Receiver, + pub consumer: watch::Receiver>, } impl LiftoffQuad { pub fn new(time_step: f32, config: LiftoffQuadrotorConfig) -> Result { - let (producer, consumer) = tokio::sync::mpsc::channel(100); + let (producer, mut consumer) = watch::channel(None::); let config_clone = config.clone(); tokio::spawn(async move { let _ = feedback_loop(config_clone, producer).await; }); // Open a serial port to communicate with the quadrotor if one is specified - // If not, open a writer to a temp file let writer: Option = match config.clone().serial_port { Some(port) => { let mut writer = Writer::new(port.to_string(), config.baud_rate).map_err(|e| { @@ -54,6 +53,15 @@ impl LiftoffQuad { let start_time = std::time::Instant::now(); // Zero throttle to arm the quadrotor in Liftoff println!("Arming Drone"); + writer + .serial_port + .set_timeout(Duration::from_millis(50)) + .map_err(|e| { + SimulationError::OtherError(format!( + "Failed to set timeout {:?}", + e.to_string() + )) + })?; while std::time::Instant::now() - start_time < std::time::Duration::from_secs(5) { writer .write(&mut cyberrc::RcData { @@ -65,10 +73,15 @@ impl LiftoffQuad { mode: 0, }) .map_err(|e| SimulationError::OtherError(e.to_string()))?; + std::thread::sleep(Duration::from_millis(100)); } + Some(writer) } - None => None, + None => { + println!("No serial port specified, writing to temp file"); + None + } }; Ok(Self { @@ -144,7 +157,6 @@ impl QuadrotorInterface for LiftoffQuad { arm: 0, mode: 0, }; - // println!("RC Data: {:?}", cyberrc_data); if let Some(writer) = &mut self.writer { writer .write(&mut cyberrc_data) @@ -156,11 +168,8 @@ impl QuadrotorInterface for LiftoffQuad { /// Observe the current state of the quadrotor /// Returns a tuple containing the position, velocity, orientation, and angular velocity of the quadrotor. fn observe(&mut self) -> Result { - // let sample = try_take_with_timeout(&self.shared_data, Duration::from_millis(25)); - let mut sample = None; - while let Ok(value) = self.consumer.try_recv() { - sample = Some(value); - } + let sample = self.consumer.borrow_and_update().clone(); + let state = match sample { Some(sample) => { // update the last state value @@ -221,6 +230,8 @@ pub struct LiftoffPacket { attitude: [f32; 4], // pitch, roll, yaw - q, p, r gyro: [f32; 3], + // throttle, yaw, pitch, roll + input: [f32; 4], motor_num: u8, #[br(count = motor_num)] motor_rpm: Vec, @@ -243,19 +254,20 @@ impl LiftoffPacket { self.attitude[2], )); // Flip the handedness by negating the Z component of the RUF quaternion. + // The orientation is now LUF let flipped_quat = Quaternion::new(ruf_quat.w, ruf_quat.i, ruf_quat.j, -ruf_quat.k); - // Define a 90-degree rotation around the Y-axis to align X (Right) to X (North) - let rotation_y = - UnitQuaternion::from_axis_angle(&Vector3::y_axis(), std::f32::consts::FRAC_PI_2); + // // Define a 90-degree rotation around the Y-axis to align X (Right) to X (North) + // let rotation_y = + // UnitQuaternion::from_axis_angle(&Vector3::y_axis(), std::f32::consts::FRAC_PI_2); - // Define a -90-degree rotation around the X-axis to align Z (Forward) to Z (Down) - let rotation_x = - UnitQuaternion::from_axis_angle(&Vector3::x_axis(), -std::f32::consts::FRAC_PI_2); + // Define a 90-degree rotation around the X-axis to align Z (Forward) to Z (Down) + // let rotation_x = + // UnitQuaternion::from_axis_angle(&Vector3::x_axis(), std::f32::consts::FRAC_PI_2); // Combine the handedness-adjusted quaternion with the rotation transformations // Apply the Y rotation first, then the X rotation - rotation_x * rotation_y * UnitQuaternion::new_normalize(flipped_quat) + UnitQuaternion::new_normalize(flipped_quat) } /// Translate Unity coordinates to NED coordinates @@ -266,7 +278,7 @@ impl LiftoffPacket { async fn feedback_loop( liftoff_config: LiftoffQuadrotorConfig, - tx: mpsc::Sender, + tx: watch::Sender>, ) -> Result<(), SimulationError> { let mut initial_position: Option = None; loop { @@ -308,7 +320,7 @@ async fn feedback_loop( } }; if sample.is_some() { - let _ = tx.send(sample.unwrap()).await.is_ok(); + let _ = tx.send(sample).is_ok(); } // sleep(Duration::from_millis(10)).await; } From 330ae599da4421fc395f58804a0b09936fbd5890 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sun, 8 Dec 2024 01:09:26 -0800 Subject: [PATCH 37/54] operable control frequencies may be a bit too aggressive --- config/liftoff_quad.yaml | 10 ++-- src/main.rs | 114 +++++++++++++++++++++++---------------- 2 files changed, 72 insertions(+), 52 deletions(-) diff --git a/config/liftoff_quad.yaml b/config/liftoff_quad.yaml index 4d55fccd4..d396fdf4d 100644 --- a/config/liftoff_quad.yaml +++ b/config/liftoff_quad.yaml @@ -6,8 +6,8 @@ real_time: true # Enable real time mode. If not enabled, sim will run in fast ti rerun_blueprint: "config/peng_default_blueprint.rbl" simulation: - control_frequency: 40 # Frequency of control loop execution (Hz) - simulation_frequency: 200 # Frequency of physics simulation updates (Hz) + 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 @@ -69,7 +69,7 @@ planner_schedule: - 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_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) @@ -77,11 +77,11 @@ planner_schedule: - 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_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: 5.0 # Duration of the landing maneuver (s) + duration: 10.0 # Duration of the landing maneuver (s) diff --git a/src/main.rs b/src/main.rs index c4df3194f..e2e1392c2 100644 --- a/src/main.rs +++ b/src/main.rs @@ -12,7 +12,7 @@ mod liftoff_quad; #[tokio::main] /// Main function for the simulation async fn main() -> Result<(), SimulationError> { - let mut config_str = "config/liftoff_quad.yaml"; + let mut config_str = "config/quad.yaml"; let args: Vec = std::env::args().collect(); if args.len() != 2 { println!( @@ -38,44 +38,6 @@ async fn main() -> Result<(), SimulationError> { "[\x1b[32mINFO\x1b[0m peng_quad] Using quadrotor: {:?}", config.quadrotor ); - let (mut quad, mass, gravity): (Box, f32, f32) = match config.quadrotor - { - config::QuadrotorConfigurations::Peng(quadrotor_config) => ( - Box::new(Quadrotor::new( - 1.0 / config.simulation.simulation_frequency as f32, - config.simulation.clone(), - quadrotor_config.mass, - quadrotor_config.gravity, - quadrotor_config.drag_coefficient, - quadrotor_config.inertia_matrix, - )?), - quadrotor_config.mass, - quadrotor_config.gravity, - ), - config::QuadrotorConfigurations::Liftoff(liftoff_quadrotor_config) => ( - Box::new(LiftoffQuad::new( - 1.0 / config.simulation.simulation_frequency as f32, - liftoff_quadrotor_config.clone(), - )?), - liftoff_quadrotor_config.mass, - liftoff_quadrotor_config.gravity, - ), - }; - - println!( - "[\x1b[32mINFO\x1b[0m peng_quad] Quadrotor: {:?} {:?}", - mass, 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, - gravity, - ); let mut imu = Imu::new( config.imu.accel_noise_std, config.imu.gyro_noise_std, @@ -117,7 +79,7 @@ async 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 }; @@ -129,11 +91,48 @@ 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 + { + config::QuadrotorConfigurations::Peng(quadrotor_config) => ( + Box::new(Quadrotor::new( + 1.0 / config.simulation.simulation_frequency as f32, + config.simulation.clone(), + quadrotor_config.mass, + quadrotor_config.gravity, + quadrotor_config.drag_coefficient, + quadrotor_config.inertia_matrix, + )?), + quadrotor_config.mass, + quadrotor_config.gravity, + ), + config::QuadrotorConfigurations::Liftoff(ref liftoff_quadrotor_config) => ( + Box::new(LiftoffQuad::new( + 1.0 / config.simulation.simulation_frequency as f32, + liftoff_quadrotor_config.clone(), + )?), + liftoff_quadrotor_config.mass, + liftoff_quadrotor_config.gravity, + ), + }; + + println!( + "[\x1b[32mINFO\x1b[0m peng_quad] Quadrotor: {:?} {:?}", + mass, 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, + 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 @@ -159,6 +158,14 @@ async fn main() -> Result<(), SimulationError> { time, &maze.obstacles, )?; + let (roll, pitch, yaw) = quad_state.orientation.euler_angles(); + + println!( + "Quad Error: {:?}, {:?}, {:?}", + desired_position, + quad_state.position, + (roll.to_degrees(), pitch.to_degrees(), yaw.to_degrees()) + ); let (thrust, mut calculated_desired_orientation) = controller.compute_position_control( &desired_position, &desired_velocity, @@ -199,15 +206,24 @@ async fn main() -> Result<(), SimulationError> { config.use_multithreading_depth_rendering, )?; } + if let Some(rec) = &rec { rec.set_time_seconds("timestamp", time); - if trajectory.add_point(quad_state.position) { + let mut rerun_quad_state = quad_state.clone(); + if let config::QuadrotorConfigurations::Liftoff(_) = config.quadrotor.clone() { + rerun_quad_state.position = Vector3::new( + quad_state.position.x, + -quad_state.position.y, + -quad_state.position.z, + ); + } + if trajectory.add_point(rerun_quad_state.position) { log_trajectory(rec, &trajectory)?; } - log_joy(rec, thrust, &torque)?; + // log_joy(rec, thrust, &torque)?; log_data( rec, - &quad_state, + &rerun_quad_state, &desired_position, &desired_velocity, &measured_accel, @@ -215,13 +231,17 @@ async fn main() -> Result<(), SimulationError> { thrust, &torque, )?; + let rotation = nalgebra::UnitQuaternion::from_axis_angle( + &Vector3::z_axis(), + std::f32::consts::FRAC_PI_2, + ); if config.render_depth { log_depth_image(rec, &camera, config.use_multithreading_depth_rendering)?; log_pinhole_depth( rec, &camera, - quad_state.position, - quad_state.orientation, + rerun_quad_state.position, + rotation * quad_state.orientation, config.camera.rotation_transform, )?; } From 2b8e148933ed2b2e9df08cd5816bad94a991b073 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sun, 8 Dec 2024 01:09:55 -0800 Subject: [PATCH 38/54] add a time to the quadrotor state --- src/quadrotor.rs | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/quadrotor.rs b/src/quadrotor.rs index 1bedbc77f..c0a2a48e1 100644 --- a/src/quadrotor.rs +++ b/src/quadrotor.rs @@ -5,6 +5,9 @@ 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, From db93275a62272c6fc916a0e5fc4abd27fd35ce07 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sun, 8 Dec 2024 01:11:56 -0800 Subject: [PATCH 39/54] remove debug, add guard on control output for times prior to first planner --- src/main.rs | 72 +++++++++++++++++++++-------------------------------- 1 file changed, 29 insertions(+), 43 deletions(-) diff --git a/src/main.rs b/src/main.rs index e2e1392c2..de36c229d 100644 --- a/src/main.rs +++ b/src/main.rs @@ -93,25 +93,26 @@ async fn main() -> Result<(), SimulationError> { } let (mut quad, mass, gravity): (Box, f32, f32) = match config.quadrotor { - config::QuadrotorConfigurations::Peng(quadrotor_config) => ( + config::QuadrotorConfigurations::Peng(quad_config) => ( Box::new(Quadrotor::new( 1.0 / config.simulation.simulation_frequency as f32, config.simulation.clone(), - quadrotor_config.mass, - quadrotor_config.gravity, - quadrotor_config.drag_coefficient, - quadrotor_config.inertia_matrix, + quad_config.mass, + quad_config.gravity, + quad_config.drag_coefficient, + quad_config.inertia_matrix, )?), - quadrotor_config.mass, - quadrotor_config.gravity, + quad_config.mass, + quad_config.gravity, ), - config::QuadrotorConfigurations::Liftoff(ref liftoff_quadrotor_config) => ( + config::QuadrotorConfigurations::Liftoff(ref liftoff_quad_config) => ( Box::new(LiftoffQuad::new( - 1.0 / config.simulation.simulation_frequency as f32, - liftoff_quadrotor_config.clone(), + 1.0 / config.simulation.control_frequency as f32, + config.simulation.clone(), + liftoff_quad_config.clone(), )?), - liftoff_quadrotor_config.mass, - liftoff_quadrotor_config.gravity, + liftoff_quad_config.mass, + liftoff_quad_config.gravity, ), }; @@ -158,15 +159,7 @@ async fn main() -> Result<(), SimulationError> { time, &maze.obstacles, )?; - let (roll, pitch, yaw) = quad_state.orientation.euler_angles(); - - println!( - "Quad Error: {:?}, {:?}, {:?}", - desired_position, - quad_state.position, - (roll.to_degrees(), pitch.to_degrees(), yaw.to_degrees()) - ); - let (thrust, mut calculated_desired_orientation) = controller.compute_position_control( + let (mut thrust, mut calculated_desired_orientation) = controller.compute_position_control( &desired_position, &desired_velocity, desired_yaw, @@ -174,25 +167,17 @@ async fn main() -> Result<(), SimulationError> { &quad_state.velocity, time_step, ); - // Clamp thrust - let thrust = thrust.clamp(0.0, quad.max_thrust()); - // Clamp angles for angle mode flight - if let Some(angle_limits) = config.angle_limits.clone() { - let angle_limits = Vector3::new(angle_limits[0], angle_limits[1], angle_limits[2]); - let desired_angles = calculated_desired_orientation.euler_angles(); - calculated_desired_orientation = nalgebra::UnitQuaternion::from_euler_angles( - desired_angles.0.clamp(-angle_limits.x, angle_limits.x), - desired_angles.1.clamp(-angle_limits.y, angle_limits.y), - desired_angles.2.clamp(-angle_limits.z, angle_limits.z), - ); - } - let torque = controller.compute_attitude_control( + + let mut torque = controller.compute_attitude_control( &calculated_desired_orientation, &quad_state.orientation, - &quad_state.angular_velocity, + &(quad_state.angular_velocity / (2.0 * std::f32::consts::PI)), time_step, ); - let _ = quad.control(i, thrust, &torque); + 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()?; @@ -214,7 +199,7 @@ async fn main() -> Result<(), SimulationError> { rerun_quad_state.position = Vector3::new( quad_state.position.x, -quad_state.position.y, - -quad_state.position.z, + quad_state.position.z, ); } if trajectory.add_point(rerun_quad_state.position) { @@ -227,13 +212,14 @@ async fn main() -> Result<(), SimulationError> { &desired_position, &desired_velocity, &measured_accel, - &measured_gyro, + &quad_state.angular_velocity, thrust, &torque, )?; let rotation = nalgebra::UnitQuaternion::from_axis_angle( &Vector3::z_axis(), - std::f32::consts::FRAC_PI_2, + // std::f32::consts::FRAC_PI_2, + 0.0, ); if config.render_depth { log_depth_image(rec, &camera, config.use_multithreading_depth_rendering)?; @@ -249,10 +235,10 @@ async fn main() -> Result<(), SimulationError> { } } i += 1; - if time >= config.simulation.duration { - log::info!("Complete Simulation"); - break; - } + // if time >= config.simulation.duration { + // log::info!("Complete Simulation"); + // break; + // } } log::logger().flush(); Ok(()) From 0257c287338598a2c907d53e68da0a6c11b2b8c4 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sun, 8 Dec 2024 01:23:50 -0800 Subject: [PATCH 40/54] time added to quadrotor state, determine yaw from axis angle --- src/lib.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 7fae5332b..f8b8024e3 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -146,6 +146,7 @@ impl QuadrotorInterface for Quadrotor { fn observe(&mut self) -> Result { return Ok(QuadrotorState { + time: 0.0, position: self.position, velocity: self.velocity, acceleration: self.acceleration, @@ -735,11 +736,10 @@ impl PIDController { let gravity_compensation = Vector3::new(0.0, 0.0, self.gravity); let total_acceleration = acceleration + gravity_compensation; let total_acc_norm = total_acceleration.norm(); - // TODO thrust limits 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); From d5dab06a93e4160f855a74a107d70742ac8b4077 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sun, 8 Dec 2024 01:33:17 -0800 Subject: [PATCH 41/54] derive defulat for simulation config, add a comment on IP --- src/config.rs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/config.rs b/src/config.rs index 1d6a06d3e..702cc4b95 100644 --- a/src/config.rs +++ b/src/config.rs @@ -60,7 +60,7 @@ pub struct PlannerStep { pub params: serde_yaml::Value, } -#[derive(Clone, serde::Deserialize)] +#[derive(Clone, Default, serde::Deserialize)] /// Configuration for the simulation pub struct SimulationConfig { /// Control frequency in Hz @@ -142,6 +142,7 @@ pub struct LiftoffQuadrotorConfig { 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, From dda0a2304a6bbf8d4d511176cc027e7a13c4c02d Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sun, 8 Dec 2024 01:38:46 -0800 Subject: [PATCH 42/54] fix bug in control scaling, implement fast feedback loop, correct attitude handling --- src/liftoff_quad.rs | 367 +++++++++++++++++++++++++++----------------- 1 file changed, 226 insertions(+), 141 deletions(-) diff --git a/src/liftoff_quad.rs b/src/liftoff_quad.rs index a12847087..c11196ed7 100644 --- a/src/liftoff_quad.rs +++ b/src/liftoff_quad.rs @@ -1,7 +1,7 @@ use binrw::{binrw, BinRead, BinWrite}; use cyber_rc::{cyberrc, Writer}; use nalgebra::{Matrix4, Quaternion, UnitQuaternion, Vector3}; -use peng_quad::config::LiftoffQuadrotorConfig; +use peng_quad::config::{LiftoffQuadrotorConfig, SimulationConfig}; use peng_quad::quadrotor::{QuadrotorInterface, QuadrotorState}; use peng_quad::SimulationError; use std::net::UdpSocket; @@ -21,9 +21,11 @@ pub struct LiftoffQuad { /// The last state of the quadrotor pub previous_state: QuadrotorState, /// Initial State - pub initial_state: QuadrotorState, + pub initial_state: Option, /// Simulation time step in seconds pub time_step: f32, + /// Config + pub simulation_config: SimulationConfig, /// Configured physical parameters pub config: LiftoffQuadrotorConfig, /// Previous Thrust @@ -31,15 +33,22 @@ pub struct LiftoffQuad { /// Previous Torque pub previous_torque: Vector3, /// Quadrotor sample mutex - pub consumer: watch::Receiver>, + pub producer: watch::Sender>>, + /// Quadrotor sample mutex + pub consumer: watch::Receiver>>, } impl LiftoffQuad { - pub fn new(time_step: f32, config: LiftoffQuadrotorConfig) -> Result { - let (producer, mut consumer) = watch::channel(None::); + pub fn new( + time_step: f32, + simulation_config: SimulationConfig, + config: LiftoffQuadrotorConfig, + ) -> Result { + let (producer, mut consumer) = watch::channel(None::>); let config_clone = config.clone(); + let producer_clone = producer.clone(); tokio::spawn(async move { - let _ = feedback_loop(config_clone, producer).await; + let _ = feedback_loop_fast(&config_clone.ip_address, producer_clone).await; }); // Open a serial port to communicate with the quadrotor if one is specified let writer: Option = match config.clone().serial_port { @@ -83,17 +92,18 @@ impl LiftoffQuad { None } }; - Ok(Self { writer, state: QuadrotorState::default(), previous_state: QuadrotorState::default(), - initial_state: QuadrotorState::default(), + initial_state: None, + simulation_config, config, time_step, previous_thrust: 0.0, previous_torque: Vector3::zeros(), consumer, + producer, }) } @@ -117,50 +127,67 @@ impl LiftoffQuad { // Combine all terms thrust_acceleration + gravity_body - rotational_acceleration } + + fn ensure_initial_is_set(&mut self, state: QuadrotorState) { + self.initial_state.get_or_insert(state); + } } impl QuadrotorInterface for LiftoffQuad { fn control( &mut self, - _: usize, + step_number: usize, thrust: f32, torque: &Vector3, ) -> Result<(), SimulationError> { - // Given thrust and torque, calculate the control inputs - // Clamp thrust and torque control inputs - let max_thrust = self.max_thrust(); - let thrust = thrust.clamp(0.0, max_thrust); - let max_torque = self.max_torque(); - let torque = Vector3::new( - torque.x.clamp(-max_torque.x, max_torque.x), - torque.y.clamp(-max_torque.y, max_torque.y), - torque.z.clamp(-max_torque.z, max_torque.z), - ); - - // Normalize inputs - let normalized_thrust = normalize(thrust, 0.0, self.max_thrust()); - let normalized_roll = normalize(torque.x, -max_torque.x, max_torque.x); - let normalized_pitch = normalize(torque.y, -max_torque.y, max_torque.y); - let normalized_yaw = normalize(torque.z, -max_torque.z, max_torque.z); - - // Scale to RC commands - let throttle_command = scale_throttle(normalized_thrust); - let aileron_command = scale_control(normalized_roll); - let elevator_command = -scale_control(normalized_pitch); - let rudder_command = scale_control(normalized_yaw); - - let mut cyberrc_data = cyberrc::RcData { - throttle: throttle_command, - aileron: aileron_command, - elevator: elevator_command, - rudder: rudder_command, - arm: 0, - mode: 0, - }; - if let Some(writer) = &mut self.writer { - writer - .write(&mut cyberrc_data) - .map_err(|e| SimulationError::OtherError(e.to_string()))?; + if step_number + % (self.simulation_config.simulation_frequency + / self.simulation_config.control_frequency) + == 0 + { + // Given thrust and torque, calculate the control inputs + // Clamp thrust and torque control inputs + let max_thrust = self.max_thrust(); + // let thrust = thrust.clamp(0.0, 5.0); + let max_torque = self.max_torque(); + println!("Pitch Torque: {:?}", torque.y); + let torque = Vector3::new( + torque.x.clamp(-max_torque.x, max_torque.x), + torque.y.clamp(-10.0, 10.0), + // torque.z.clamp(-max_torque.z, max_torque.z), + torque.z.clamp(-6.0, 6.0), + ); + + // Normalize inputs + let normalized_thrust = normalize(thrust, 0.0, 5.0); + let normalized_roll = normalize(torque.x, -max_torque.x, max_torque.x); + let normalized_pitch = normalize(torque.y, -12.0, 12.0); + let normalized_yaw = normalize(torque.z, -6.0, 6.0); + println!( + "Thrust: {:?}, Normalized Thrust: {:?} Timestamp: {:?}", + thrust, normalized_thrust, self.state.time + ); + + // Scale to RC commands + let throttle_command = scale_throttle(normalized_thrust); + let aileron_command = scale_control(normalized_roll); + let elevator_command = -scale_control(normalized_pitch); + let rudder_command = scale_control(normalized_yaw); + + let mut cyberrc_data = cyberrc::RcData { + throttle: throttle_command, + // elevator: 0, // elevator_command, + aileron: aileron_command, + elevator: elevator_command, + rudder: rudder_command, + arm: 0, + mode: 0, + }; + if let Some(writer) = &mut self.writer { + writer + .write(&mut cyberrc_data) + .map_err(|e| SimulationError::OtherError(e.to_string()))?; + } } Ok(()) } @@ -168,37 +195,78 @@ impl QuadrotorInterface for LiftoffQuad { /// Observe the current state of the quadrotor /// Returns a tuple containing the position, velocity, orientation, and angular velocity of the quadrotor. fn observe(&mut self) -> Result { - let sample = self.consumer.borrow_and_update().clone(); - - let state = match sample { - Some(sample) => { - // update the last state value - self.previous_state = self.state.clone(); - let attitude_quaternion = sample.attitude_quaternion(); - // The position is in Unity inertial coordinates, so we transform to NED inertial - // coordinates. - let sample_position = sample.position(); - // Calculate the body-frame velocity by rotating the inertial velocity using the - // attitude quaternion. - let v_body = attitude_quaternion.transform_vector( - &((sample_position - self.previous_state.position) / self.time_step), - ); - - let omega_body = sample.pqr(); - let acceleration_body = - self.body_acceleration(attitude_quaternion, omega_body, v_body); - QuadrotorState { - position: sample_position, - velocity: v_body, - acceleration: acceleration_body, - orientation: attitude_quaternion, - angular_velocity: omega_body, - } + // TODO: if there is not a new packet, return the old state + if !self + .consumer + .has_changed() + .map_err(|e| SimulationError::OtherError(e.to_string()))? + { + return Ok(self.state.clone()); + } + self.previous_state = self.state.clone(); + let packet = self.consumer.borrow_and_update().clone(); + let sample = match packet { + Some(packet) => { + let mut cursor = std::io::Cursor::new(packet); + LiftoffPacket::read(&mut cursor) + .map_err(|e| SimulationError::OtherError(e.to_string())) } - None => self.state.clone(), + None => Err(SimulationError::OtherError("No packet".to_string())), + }?; + + let initial_state = self.initial_state.get_or_insert_with(|| { + QuadrotorState { + time: sample.timestamp, + position: sample.position(), + velocity: Vector3::zeros(), + acceleration: Vector3::zeros(), + // Store the initial yaw as the initial attitude so that we can adjust for it on + // subsequent samples + orientation: (|| -> UnitQuaternion { + let attitude = sample.attitude_quaternion(); + println!("Initial Yaw: {:?}", attitude.euler_angles().2.to_degrees()); + UnitQuaternion::new_normalize(Quaternion::new( + (1.0 - (attitude.k * attitude.k)).sqrt(), + 0.0, + 0.0, + -attitude.k, + )) + })(), + // orientation: sample.attitude_quaternion(), + angular_velocity: Vector3::zeros(), + } + }); + + // Adjust the sample by the initial state + // Adjust for the initial yaw - we take the starting yaw of the vehicle to be 0 + let attitude_quaternion = + initial_state.orientation.clone().conjugate() * sample.attitude_quaternion(); + // Asjut fot the initial position - we take the starting location of the vehicle to be the + // origin + let mut sample_position = sample.position(); + sample_position[0] -= initial_state.position[0]; + sample_position[1] -= initial_state.position[1]; + sample_position[2] -= initial_state.position[2]; + + // Calculate the body-frame velocity by rotating the inertial velocity using the + // attitude quaternion. + let v_body = attitude_quaternion.transform_vector( + &((sample_position - self.previous_state.position) + / (sample.timestamp - self.previous_state.time)), + ); + // sample_position[2] = -sample_position[2]; + + let omega_body = sample.pqr(); + let acceleration_body = self.body_acceleration(attitude_quaternion, omega_body, v_body); + self.state = QuadrotorState { + time: sample.timestamp as f32, + position: sample_position, + velocity: v_body, + acceleration: acceleration_body, + orientation: attitude_quaternion, + angular_velocity: omega_body, }; - self.state = state.clone(); - Ok(state) + Ok(self.state.clone()) } fn max_thrust(&self) -> f32 { @@ -209,7 +277,7 @@ impl QuadrotorInterface for LiftoffQuad { 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; + let yaw_torque = 8.0 * motor_thrust; Vector3::new(max_rp_torque, max_rp_torque, yaw_torque) } @@ -246,83 +314,45 @@ impl LiftoffPacket { /// Returns the attitude quaternion in the NED frame pub fn attitude_quaternion(&self) -> UnitQuaternion { - // The quaternion is in the Unity right-up-forward (RUF) frame - let ruf_quat = UnitQuaternion::from_quaternion(Quaternion::new( - self.attitude[3], - self.attitude[0], - self.attitude[1], - self.attitude[2], - )); - // Flip the handedness by negating the Z component of the RUF quaternion. - // The orientation is now LUF - let flipped_quat = Quaternion::new(ruf_quat.w, ruf_quat.i, ruf_quat.j, -ruf_quat.k); - - // // Define a 90-degree rotation around the Y-axis to align X (Right) to X (North) - // let rotation_y = - // UnitQuaternion::from_axis_angle(&Vector3::y_axis(), std::f32::consts::FRAC_PI_2); - - // Define a 90-degree rotation around the X-axis to align Z (Forward) to Z (Down) - // let rotation_x = - // UnitQuaternion::from_axis_angle(&Vector3::x_axis(), std::f32::consts::FRAC_PI_2); - - // Combine the handedness-adjusted quaternion with the rotation transformations - // Apply the Y rotation first, then the X rotation - UnitQuaternion::new_normalize(flipped_quat) + // Cast the attitude as a unit quaternion, and flip the handedness by inverting the z + // axis. + let r = UnitQuaternion::from_quaternion( + Quaternion::new( + self.attitude[3], + self.attitude[0], + self.attitude[1], + -self.attitude[2], + ) + .normalize(), + ); + let euler = r.euler_angles(); + UnitQuaternion::from_euler_angles(euler.2, euler.0, -euler.1) } - /// Translate Unity coordinates to NED coordinates + /// TODO: this is NEU + /// Translate Unity coordinates in RUF to NED coordinates pub fn position(&self) -> Vector3 { - Vector3::new(self.position[2], self.position[0], -self.position[1]) + Vector3::new(self.position[2], self.position[0], self.position[1]) } } -async fn feedback_loop( - liftoff_config: LiftoffQuadrotorConfig, - tx: watch::Sender>, +async fn feedback_loop_fast( + address: &str, + tx: watch::Sender>>, ) -> Result<(), SimulationError> { - let mut initial_position: Option = None; + let socket = tokio::net::UdpSocket::bind(address.to_string()) + .await + .map_err(|e| SimulationError::OtherError(e.to_string()))?; + let mut buf = [0; 2048]; loop { - let mut buf = [0; 128]; - // Possible bug: rebinding is necesssary to get new data from Liftoff. - let sample = match UdpSocket::bind(liftoff_config.ip_address.to_string()) { - Ok(socket) => { - socket - .set_read_timeout(Some(Duration::from_millis(50))) + match socket.recv_from(&mut buf).await { + Ok((len, _)) => { + let _ = tx + .send(Some(buf[..len].to_vec())) .map_err(|e| SimulationError::OtherError(e.to_string()))?; - match socket.recv_from(&mut buf) { - Ok((len, _)) => { - let mut cursor = std::io::Cursor::new(&buf[..len]); - // TODO: more robust handling of packet parsing errors during resets - let sample = if let Ok(mut sample) = LiftoffPacket::read(&mut cursor) { - if initial_position.is_none() { - initial_position = Some(sample.clone()); - } - // Position will be relative to initial, as Liftoff does not start the - // quad at 0, 0, 0 - let initial_position = initial_position.as_ref(); - sample.position[0] -= initial_position.unwrap().position[0]; - sample.position[1] -= initial_position.unwrap().position[1]; - sample.position[2] -= initial_position.unwrap().position[2]; - sample - } else { - LiftoffPacket::default() - }; - Some(sample) - } - Err(_) => None, - } - } - Err(e) => { - return Err(SimulationError::OtherError(format!( - "Bind loop exceeded max wait time {}", - e.to_string() - ))); } - }; - if sample.is_some() { - let _ = tx.send(sample).is_ok(); + Err(_) => (), } - // sleep(Duration::from_millis(10)).await; } } @@ -378,7 +408,7 @@ fn scale_throttle(thrust: f32) -> i32 { } fn scale_control(value: f32) -> i32 { - scale_to_rc_command_with_center(value, -32768 as f32, 32767 as f32, 0.0) + scale_to_rc_command_with_center(value, -32768 as f32, 0.0, 32737 as f32) } #[rustfmt::skip] @@ -388,3 +418,58 @@ const MOTOR_MIXING_MATRIX: Matrix4 = Matrix4::new( -1.0, -1.0, 1.0, 1.0, 1.0, -1.0, -1.0, 1.0, ); + +#[cfg(test)] +mod tests { + + use super::*; + use std::f32::consts::FRAC_PI_2; + + /// Checks if two `Vector3` instances are element-wise close within the given tolerances. + fn is_close(this: &Vector3, other: &Vector3, rel_tol: f32, abs_tol: f32) -> bool { + fn is_close_scalar(a: f32, b: f32, rel_tol: f32, abs_tol: f32) -> bool { + (a - b).abs() <= (rel_tol * b.abs()).max(abs_tol) + } + + is_close_scalar(this.x, other.x, rel_tol, abs_tol) + && is_close_scalar(this.y, other.y, rel_tol, abs_tol) + && is_close_scalar(this.z, other.z, rel_tol, abs_tol) + } + + #[test] + fn test_acceleration_body() { + let quad = LiftoffQuad::new( + 0.01, + SimulationConfig::default(), + LiftoffQuadrotorConfig::default(), + ) + .unwrap(); + let omega = Vector3::zeros(); + let v = Vector3::zeros(); + + let q = UnitQuaternion::from_euler_angles(0.0, 0.0, 0.0); + let a = quad.body_acceleration(q, omega, v); + assert_eq!(a, Vector3::new(0.0, 0.0, quad.config.gravity)); + + // Roll onto the right side, gravity should align with y direction to the "east" side of + // the drone + let q = UnitQuaternion::from_euler_angles(-FRAC_PI_2, 0.0, 0.0); + let a = quad.body_acceleration(q, omega, v); + assert!(is_close( + &a, + &Vector3::new(0.0, quad.config.gravity, 0.0), + 1e-6, + 1e-6 + )); + + // Pitch forward 90 degrees, gravity should align with x direction down the nose + let q = UnitQuaternion::from_euler_angles(0.0, FRAC_PI_2, 0.0); + let a = quad.body_acceleration(q, omega, v); + assert!(is_close( + &a, + &Vector3::new(quad.config.gravity, 0.0, 0.0), + 1e-6, + 1e-6 + )); + } +} From c3aef62305429b79a89ea2849ef1fd91aa68b150 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sat, 14 Dec 2024 13:48:12 -0800 Subject: [PATCH 43/54] pull liftoff quad references from quadrotor interface implementation --- config/liftoff_quad.yaml | 87 --------------------------------------- src/config.rs | 88 ++++++++-------------------------------- src/main.rs | 24 ++++------- 3 files changed, 27 insertions(+), 172 deletions(-) delete mode 100644 config/liftoff_quad.yaml diff --git a/config/liftoff_quad.yaml b/config/liftoff_quad.yaml deleted file mode 100644 index d396fdf4d..000000000 --- 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 702cc4b95..c1904a424 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 de36c229d..262a7b1ef 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; From 8078c23d6f9270dc66f9e107dbfb0820b1c810bd Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sat, 14 Dec 2024 13:49:18 -0800 Subject: [PATCH 44/54] complete removal of liftoff implementation --- src/liftoff_quad.rs | 475 -------------------------------------------- src/main.rs | 10 - 2 files changed, 485 deletions(-) delete mode 100644 src/liftoff_quad.rs diff --git a/src/liftoff_quad.rs b/src/liftoff_quad.rs deleted file mode 100644 index c11196ed7..000000000 --- a/src/liftoff_quad.rs +++ /dev/null @@ -1,475 +0,0 @@ -use binrw::{binrw, BinRead, BinWrite}; -use cyber_rc::{cyberrc, Writer}; -use nalgebra::{Matrix4, Quaternion, UnitQuaternion, Vector3}; -use peng_quad::config::{LiftoffQuadrotorConfig, SimulationConfig}; -use peng_quad::quadrotor::{QuadrotorInterface, QuadrotorState}; -use peng_quad::SimulationError; -use std::net::UdpSocket; -use tokio::sync::watch; -use tokio::time::Duration; - -/// Represents a quadrotor in the game Liftoff -/// # Example -/// ``` -/// let quad = LiftoffQuad{ } -/// ``` -pub struct LiftoffQuad { - /// The serial writer to communicate with the quadrotor - pub writer: Option, - /// The current state of the quadrotor - pub state: QuadrotorState, - /// The last state of the quadrotor - pub previous_state: QuadrotorState, - /// Initial State - pub initial_state: Option, - /// Simulation time step in seconds - pub time_step: f32, - /// Config - pub simulation_config: SimulationConfig, - /// Configured physical parameters - pub config: LiftoffQuadrotorConfig, - /// Previous Thrust - pub previous_thrust: f32, - /// Previous Torque - pub previous_torque: Vector3, - /// Quadrotor sample mutex - pub producer: watch::Sender>>, - /// Quadrotor sample mutex - pub consumer: watch::Receiver>>, -} - -impl LiftoffQuad { - pub fn new( - time_step: f32, - simulation_config: SimulationConfig, - config: LiftoffQuadrotorConfig, - ) -> Result { - let (producer, mut consumer) = watch::channel(None::>); - let config_clone = config.clone(); - let producer_clone = producer.clone(); - tokio::spawn(async move { - let _ = feedback_loop_fast(&config_clone.ip_address, producer_clone).await; - }); - // Open a serial port to communicate with the quadrotor if one is specified - let writer: Option = match config.clone().serial_port { - Some(port) => { - let mut writer = Writer::new(port.to_string(), config.baud_rate).map_err(|e| { - SimulationError::OtherError(format!( - "Failed to open SerialPort {:?}", - e.to_string() - )) - })?; - let start_time = std::time::Instant::now(); - // Zero throttle to arm the quadrotor in Liftoff - println!("Arming Drone"); - writer - .serial_port - .set_timeout(Duration::from_millis(50)) - .map_err(|e| { - SimulationError::OtherError(format!( - "Failed to set timeout {:?}", - e.to_string() - )) - })?; - while std::time::Instant::now() - start_time < std::time::Duration::from_secs(5) { - writer - .write(&mut cyberrc::RcData { - throttle: 32767, - aileron: 0, - elevator: 0, - rudder: 0, - arm: 1, - mode: 0, - }) - .map_err(|e| SimulationError::OtherError(e.to_string()))?; - std::thread::sleep(Duration::from_millis(100)); - } - - Some(writer) - } - None => { - println!("No serial port specified, writing to temp file"); - None - } - }; - Ok(Self { - writer, - state: QuadrotorState::default(), - previous_state: QuadrotorState::default(), - initial_state: None, - simulation_config, - config, - time_step, - previous_thrust: 0.0, - previous_torque: Vector3::zeros(), - consumer, - producer, - }) - } - - // Function to calculate body-frame acceleration for NED coordinates - fn body_acceleration( - &self, - q_inertial_to_body: UnitQuaternion, // Unit quaternion rotation from inertial to body - omega_body: Vector3, // Angular velocity in body frame - velocity_body: Vector3, // Linear velocity in body frame - ) -> Vector3 { - // Calculate thrust acceleration (negative Z in NED body frame) - let thrust_acceleration = Vector3::new(0.0, 0.0, -self.previous_thrust / self.config.mass); - - // Rotate the gravity vector to the body frame - let gravity_inertial = Vector3::new(0.0, 0.0, self.config.gravity); // NED gravity vector in inertial frame - let gravity_body = q_inertial_to_body.transform_vector(&gravity_inertial); // Rotate gravity vector to body frame - - // Calculate rotational (Coriolis) effects - let rotational_acceleration = omega_body.cross(&velocity_body); - - // Combine all terms - thrust_acceleration + gravity_body - rotational_acceleration - } - - fn ensure_initial_is_set(&mut self, state: QuadrotorState) { - self.initial_state.get_or_insert(state); - } -} - -impl QuadrotorInterface for LiftoffQuad { - fn control( - &mut self, - step_number: usize, - thrust: f32, - torque: &Vector3, - ) -> Result<(), SimulationError> { - if step_number - % (self.simulation_config.simulation_frequency - / self.simulation_config.control_frequency) - == 0 - { - // Given thrust and torque, calculate the control inputs - // Clamp thrust and torque control inputs - let max_thrust = self.max_thrust(); - // let thrust = thrust.clamp(0.0, 5.0); - let max_torque = self.max_torque(); - println!("Pitch Torque: {:?}", torque.y); - let torque = Vector3::new( - torque.x.clamp(-max_torque.x, max_torque.x), - torque.y.clamp(-10.0, 10.0), - // torque.z.clamp(-max_torque.z, max_torque.z), - torque.z.clamp(-6.0, 6.0), - ); - - // Normalize inputs - let normalized_thrust = normalize(thrust, 0.0, 5.0); - let normalized_roll = normalize(torque.x, -max_torque.x, max_torque.x); - let normalized_pitch = normalize(torque.y, -12.0, 12.0); - let normalized_yaw = normalize(torque.z, -6.0, 6.0); - println!( - "Thrust: {:?}, Normalized Thrust: {:?} Timestamp: {:?}", - thrust, normalized_thrust, self.state.time - ); - - // Scale to RC commands - let throttle_command = scale_throttle(normalized_thrust); - let aileron_command = scale_control(normalized_roll); - let elevator_command = -scale_control(normalized_pitch); - let rudder_command = scale_control(normalized_yaw); - - let mut cyberrc_data = cyberrc::RcData { - throttle: throttle_command, - // elevator: 0, // elevator_command, - aileron: aileron_command, - elevator: elevator_command, - rudder: rudder_command, - arm: 0, - mode: 0, - }; - if let Some(writer) = &mut self.writer { - writer - .write(&mut cyberrc_data) - .map_err(|e| SimulationError::OtherError(e.to_string()))?; - } - } - Ok(()) - } - - /// Observe the current state of the quadrotor - /// Returns a tuple containing the position, velocity, orientation, and angular velocity of the quadrotor. - fn observe(&mut self) -> Result { - // TODO: if there is not a new packet, return the old state - if !self - .consumer - .has_changed() - .map_err(|e| SimulationError::OtherError(e.to_string()))? - { - return Ok(self.state.clone()); - } - self.previous_state = self.state.clone(); - let packet = self.consumer.borrow_and_update().clone(); - let sample = match packet { - Some(packet) => { - let mut cursor = std::io::Cursor::new(packet); - LiftoffPacket::read(&mut cursor) - .map_err(|e| SimulationError::OtherError(e.to_string())) - } - None => Err(SimulationError::OtherError("No packet".to_string())), - }?; - - let initial_state = self.initial_state.get_or_insert_with(|| { - QuadrotorState { - time: sample.timestamp, - position: sample.position(), - velocity: Vector3::zeros(), - acceleration: Vector3::zeros(), - // Store the initial yaw as the initial attitude so that we can adjust for it on - // subsequent samples - orientation: (|| -> UnitQuaternion { - let attitude = sample.attitude_quaternion(); - println!("Initial Yaw: {:?}", attitude.euler_angles().2.to_degrees()); - UnitQuaternion::new_normalize(Quaternion::new( - (1.0 - (attitude.k * attitude.k)).sqrt(), - 0.0, - 0.0, - -attitude.k, - )) - })(), - // orientation: sample.attitude_quaternion(), - angular_velocity: Vector3::zeros(), - } - }); - - // Adjust the sample by the initial state - // Adjust for the initial yaw - we take the starting yaw of the vehicle to be 0 - let attitude_quaternion = - initial_state.orientation.clone().conjugate() * sample.attitude_quaternion(); - // Asjut fot the initial position - we take the starting location of the vehicle to be the - // origin - let mut sample_position = sample.position(); - sample_position[0] -= initial_state.position[0]; - sample_position[1] -= initial_state.position[1]; - sample_position[2] -= initial_state.position[2]; - - // Calculate the body-frame velocity by rotating the inertial velocity using the - // attitude quaternion. - let v_body = attitude_quaternion.transform_vector( - &((sample_position - self.previous_state.position) - / (sample.timestamp - self.previous_state.time)), - ); - // sample_position[2] = -sample_position[2]; - - let omega_body = sample.pqr(); - let acceleration_body = self.body_acceleration(attitude_quaternion, omega_body, v_body); - self.state = QuadrotorState { - time: sample.timestamp as f32, - position: sample_position, - velocity: v_body, - acceleration: acceleration_body, - orientation: attitude_quaternion, - angular_velocity: omega_body, - }; - Ok(self.state.clone()) - } - - fn max_thrust(&self) -> f32 { - self.config.max_thrust_kg - } - - /// Calculate all maximum torques and return them as a tuple - 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 = 8.0 * motor_thrust; - Vector3::new(max_rp_torque, max_rp_torque, yaw_torque) - } - - fn read_imu(&self) -> Result<(Vector3, Vector3), SimulationError> { - Ok((self.state.acceleration, self.state.angular_velocity)) - } -} - -// TODO: configure packet based on the content of the Liftoff config file -#[binrw] -#[brw(little)] -#[derive(Debug, Default, Clone)] -pub struct LiftoffPacket { - timestamp: f32, - // x, y, z - position: [f32; 3], - // x, y, z, w - attitude: [f32; 4], - // pitch, roll, yaw - q, p, r - gyro: [f32; 3], - // throttle, yaw, pitch, roll - input: [f32; 4], - motor_num: u8, - #[br(count = motor_num)] - motor_rpm: Vec, -} - -impl LiftoffPacket { - /// Returns the angular velocity p, q, r - /// These are the roll rate, pitch rate, and yaw rate respectively - pub fn pqr(&self) -> Vector3 { - Vector3::new(self.gyro[1], self.gyro[0], self.gyro[2]) - } - - /// Returns the attitude quaternion in the NED frame - pub fn attitude_quaternion(&self) -> UnitQuaternion { - // Cast the attitude as a unit quaternion, and flip the handedness by inverting the z - // axis. - let r = UnitQuaternion::from_quaternion( - Quaternion::new( - self.attitude[3], - self.attitude[0], - self.attitude[1], - -self.attitude[2], - ) - .normalize(), - ); - let euler = r.euler_angles(); - UnitQuaternion::from_euler_angles(euler.2, euler.0, -euler.1) - } - - /// TODO: this is NEU - /// Translate Unity coordinates in RUF to NED coordinates - pub fn position(&self) -> Vector3 { - Vector3::new(self.position[2], self.position[0], self.position[1]) - } -} - -async fn feedback_loop_fast( - address: &str, - tx: watch::Sender>>, -) -> Result<(), SimulationError> { - let socket = tokio::net::UdpSocket::bind(address.to_string()) - .await - .map_err(|e| SimulationError::OtherError(e.to_string()))?; - let mut buf = [0; 2048]; - loop { - match socket.recv_from(&mut buf).await { - Ok((len, _)) => { - let _ = tx - .send(Some(buf[..len].to_vec())) - .map_err(|e| SimulationError::OtherError(e.to_string()))?; - } - Err(_) => (), - } - } -} - -/// Scale a value from a given range to a new range -/// # Example -/// ``` -/// let value = 0.0; -/// let min = -10.0; -/// let max = 10.0; -/// let scaled_value = normalize(value, min, max); -/// assert_eq!(scaled_value, 0.5); -/// ``` -fn normalize(value: f32, min: f32, max: f32) -> f32 { - (value - min) / (max - min) -} - -// TODO clean up all the type casts -// TODO assert value is normalized -fn scale_to_rc_command(value: f32, min: i32, max: i32) -> i32 { - let value = value.clamp(0.0, 1.0); - // assert value is in the range 0 to 1 - // Scale normalized value to the range between min and max - let range = max - min; - let scaled_value = min as f32 + value * range as f32; - scaled_value as i32 - // // Calculate the offset from the center within the range - // (center + scaled_value as i32) as i32 -} - -fn scale_to_rc_command_with_center(value: f32, min: f32, center: f32, max: f32) -> i32 { - let value = value.clamp(0.0, 1.0); - let output = if value < 0.5 { - // Map to the lower half (min to center) - center - (center - min) * (0.5 - value) * 2.0 - } else { - // Map to the upper half (center to max) - center + (max - center) * (value - 0.5) * 2.0 - }; - output as i32 -} - -/// Scale a thrust value to a throttle command -/// Throttle is inverted from Xinput to Liftoff -/// # Example -/// ``` -/// let thrust = 0.0; -/// let throttle = scale_throttle(thrust); -/// assert_eq!(throttle, 0); -/// ``` -fn scale_throttle(thrust: f32) -> i32 { - // thrust is inverted from Xinput to Liftoff - -scale_to_rc_command(thrust, -32768, 32768) -} - -fn scale_control(value: f32) -> i32 { - scale_to_rc_command_with_center(value, -32768 as f32, 0.0, 32737 as f32) -} - -#[rustfmt::skip] -const MOTOR_MIXING_MATRIX: Matrix4 = Matrix4::new( - 1.0, 1.0, 1.0, 1.0, - -1.0, 1.0, -1.0, 1.0, - -1.0, -1.0, 1.0, 1.0, - 1.0, -1.0, -1.0, 1.0, -); - -#[cfg(test)] -mod tests { - - use super::*; - use std::f32::consts::FRAC_PI_2; - - /// Checks if two `Vector3` instances are element-wise close within the given tolerances. - fn is_close(this: &Vector3, other: &Vector3, rel_tol: f32, abs_tol: f32) -> bool { - fn is_close_scalar(a: f32, b: f32, rel_tol: f32, abs_tol: f32) -> bool { - (a - b).abs() <= (rel_tol * b.abs()).max(abs_tol) - } - - is_close_scalar(this.x, other.x, rel_tol, abs_tol) - && is_close_scalar(this.y, other.y, rel_tol, abs_tol) - && is_close_scalar(this.z, other.z, rel_tol, abs_tol) - } - - #[test] - fn test_acceleration_body() { - let quad = LiftoffQuad::new( - 0.01, - SimulationConfig::default(), - LiftoffQuadrotorConfig::default(), - ) - .unwrap(); - let omega = Vector3::zeros(); - let v = Vector3::zeros(); - - let q = UnitQuaternion::from_euler_angles(0.0, 0.0, 0.0); - let a = quad.body_acceleration(q, omega, v); - assert_eq!(a, Vector3::new(0.0, 0.0, quad.config.gravity)); - - // Roll onto the right side, gravity should align with y direction to the "east" side of - // the drone - let q = UnitQuaternion::from_euler_angles(-FRAC_PI_2, 0.0, 0.0); - let a = quad.body_acceleration(q, omega, v); - assert!(is_close( - &a, - &Vector3::new(0.0, quad.config.gravity, 0.0), - 1e-6, - 1e-6 - )); - - // Pitch forward 90 degrees, gravity should align with x direction down the nose - let q = UnitQuaternion::from_euler_angles(0.0, FRAC_PI_2, 0.0); - let a = quad.body_acceleration(q, omega, v); - assert!(is_close( - &a, - &Vector3::new(quad.config.gravity, 0.0, 0.0), - 1e-6, - 1e-6 - )); - } -} diff --git a/src/main.rs b/src/main.rs index 262a7b1ef..b21330daf 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,5 +1,4 @@ #![feature(thread_sleep_until)] -use liftoff_quad::LiftoffQuad; use nalgebra::Vector3; use peng_quad::quadrotor::QuadrotorInterface; use peng_quad::*; @@ -7,8 +6,6 @@ use std::thread; use std::time::Duration; use std::time::Instant; -mod liftoff_quad; - #[tokio::main] /// Main function for the simulation async fn main() -> Result<(), SimulationError> { @@ -189,13 +186,6 @@ async fn main() -> Result<(), SimulationError> { if let Some(rec) = &rec { rec.set_time_seconds("timestamp", time); let mut rerun_quad_state = quad_state.clone(); - if let config::QuadrotorConfigurations::Liftoff(_) = config.quadrotor.clone() { - rerun_quad_state.position = Vector3::new( - quad_state.position.x, - -quad_state.position.y, - quad_state.position.z, - ); - } if trajectory.add_point(rerun_quad_state.position) { log_trajectory(rec, &trajectory)?; } From e9f4d26200ae09bac5400c274e9bc07cf09760d4 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sat, 14 Dec 2024 14:12:14 -0800 Subject: [PATCH 45/54] resolve imu updates --- src/config.rs | 14 +++++++------- src/lib.rs | 39 --------------------------------------- 2 files changed, 7 insertions(+), 46 deletions(-) diff --git a/src/config.rs b/src/config.rs index c1904a424..d10265fde 100644 --- a/src/config.rs +++ b/src/config.rs @@ -41,13 +41,6 @@ pub struct Config { pub angle_limits: Option>, } -#[derive(Clone, Debug, serde::Deserialize)] -#[serde(tag = "type")] -/// Vehicle Specifig configuration -pub enum QuadrotorConfigurations { - Peng(QuadrotorConfig), -} - #[derive(serde::Deserialize)] /// Configuration for a planner step pub struct PlannerStep { @@ -92,6 +85,13 @@ impl Default for SimulationConfig { } } +#[derive(Clone, Debug, serde::Deserialize)] +#[serde(tag = "type")] +/// Vehicle Specifig configuration +pub enum QuadrotorConfigurations { + Peng(QuadrotorConfig), +} + #[derive(Copy, Clone, Debug, serde::Deserialize)] #[serde(default)] /// Configuration for the quadrotor diff --git a/src/lib.rs b/src/lib.rs index 752bb88c2..404337dc5 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -166,26 +166,6 @@ impl QuadrotorInterface for Quadrotor { let yaw_torque = 2.0 * 0.005 * motor_thrust; Vector3::new(max_rp_torque, max_rp_torque, yaw_torque) } - - //TODO: replace this method in main with observe - /// 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(); - /// ``` - fn read_imu(&self) -> Result<(Vector3, Vector3), SimulationError> { - Ok((self.acceleration, self.angular_velocity)) - } } /// Implementation of the Quadrotor struct @@ -428,25 +408,6 @@ impl Quadrotor { 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 From 5fec6d68726fd9c123c69c19eae21c39411b8b64 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sat, 14 Dec 2024 16:00:16 -0800 Subject: [PATCH 46/54] liftoff requirements for ubuntu --- .github/workflows/CI.yml | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/.github/workflows/CI.yml b/.github/workflows/CI.yml index 7efbe3b7c..6fe879cb5 100644 --- a/.github/workflows/CI.yml +++ b/.github/workflows/CI.yml @@ -33,6 +33,22 @@ jobs: restore-keys: | ${{ runner.os }}-cargo-build- + - name: Run only 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 + - name: Build run: cargo build --verbose - name: Run tests From 0ba912092e71a1dd36e004dca2a6a648fdae23b2 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sat, 14 Dec 2024 16:04:11 -0800 Subject: [PATCH 47/54] fixup imu --- src/lib.rs | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/lib.rs b/src/lib.rs index 404337dc5..f8b8024e3 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -166,6 +166,26 @@ impl QuadrotorInterface for Quadrotor { let yaw_torque = 2.0 * 0.005 * motor_thrust; Vector3::new(max_rp_torque, max_rp_torque, yaw_torque) } + + //TODO: replace this method in main with observe + /// 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(); + /// ``` + fn read_imu(&self) -> Result<(Vector3, Vector3), SimulationError> { + Ok((self.acceleration, self.angular_velocity)) + } } /// Implementation of the Quadrotor struct From baf2f09a9b319ac48561bd4cc8f4732493497e6e Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sat, 14 Dec 2024 16:05:24 -0800 Subject: [PATCH 48/54] fixup gh workflow formatting --- .github/workflows/CI.yml | 27 ++++++++++++++------------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/.github/workflows/CI.yml b/.github/workflows/CI.yml index 6fe879cb5..1e8cdcc94 100644 --- a/.github/workflows/CI.yml +++ b/.github/workflows/CI.yml @@ -35,19 +35,20 @@ jobs: - name: Run only 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 + 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 - name: Build run: cargo build --verbose From d8531fe3f71db97eeabe9a6107bb5273ca20d6af Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sat, 14 Dec 2024 16:15:21 -0800 Subject: [PATCH 49/54] libudev --- .github/workflows/CI.yml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/.github/workflows/CI.yml b/.github/workflows/CI.yml index 1e8cdcc94..177037cfb 100644 --- a/.github/workflows/CI.yml +++ b/.github/workflows/CI.yml @@ -33,7 +33,7 @@ jobs: restore-keys: | ${{ runner.os }}-cargo-build- - - name: Run only on Ubuntu + - name: Install rerun requirements on Ubuntu if: matrix.os == 'ubuntu-latest' run: | sudo apt-get -y install \ @@ -48,7 +48,8 @@ jobs: libxcb-shape0-dev \ libxcb-xfixes0-dev \ libxkbcommon-dev \ - patchelf + patchelf \ + libudev-dev - name: Build run: cargo build --verbose From 93eac00b67ad10b1953feb6ece95df61ef991b6b Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sat, 14 Dec 2024 16:39:45 -0800 Subject: [PATCH 50/54] cmake proto --- .github/workflows/CI.yml | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/.github/workflows/CI.yml b/.github/workflows/CI.yml index 177037cfb..37ee901cf 100644 --- a/.github/workflows/CI.yml +++ b/.github/workflows/CI.yml @@ -49,7 +49,24 @@ jobs: libxcb-xfixes0-dev \ libxkbcommon-dev \ patchelf \ - libudev-dev + libudev-dev \ + cmake \ + protobuf-compiler + + - name: Install Protobuf and CMake on macOS + if: runner.os == 'macos-latest' + run: | + brew install cmake protobuf + protoc --version + cmake --version + + - name: Install Protobuf on Windows + if: runner.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 From 7b70034bfa51683dc1a3d2c678c56714fb1075cb Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sat, 14 Dec 2024 17:01:04 -0800 Subject: [PATCH 51/54] runner --- .github/workflows/CI.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/CI.yml b/.github/workflows/CI.yml index 37ee901cf..ae18eb375 100644 --- a/.github/workflows/CI.yml +++ b/.github/workflows/CI.yml @@ -54,14 +54,14 @@ jobs: protobuf-compiler - name: Install Protobuf and CMake on macOS - if: runner.os == 'macos-latest' + if: matrix.os == 'macos-latest' run: | brew install cmake protobuf protoc --version cmake --version - name: Install Protobuf on Windows - if: runner.os == 'windows-latest' + if: matrix.os == 'windows-latest' run: | choco install cmake --installargs '"ADD_CMAKE_TO_PATH=System"' choco install protoc From cbaa1116221979c7f20b86d7ad3eba7b0ebaf017 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sat, 14 Dec 2024 18:48:05 -0800 Subject: [PATCH 52/54] liftoff configs, and fixup test cases --- src/config.rs | 1 + src/lib.rs | 43 +++++++++++++++++---------- src/main.rs | 1 - tests/testdata/test_config_base.yaml | 6 ++-- tests/testdata/test_liftoff_base.yaml | 1 + 5 files changed, 33 insertions(+), 19 deletions(-) diff --git a/src/config.rs b/src/config.rs index d10265fde..7e01e9594 100644 --- a/src/config.rs +++ b/src/config.rs @@ -90,6 +90,7 @@ impl Default for SimulationConfig { /// Vehicle Specifig configuration pub enum QuadrotorConfigurations { Peng(QuadrotorConfig), + Liftoff(QuadrotorConfig), } #[derive(Copy, Clone, Debug, serde::Deserialize)] diff --git a/src/lib.rs b/src/lib.rs index f8b8024e3..0c0e28c37 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -36,11 +36,12 @@ //! //! ## 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::*; @@ -80,11 +81,12 @@ pub enum 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 Quadrotor { /// Current position of the quadrotor in 3D space @@ -167,7 +169,6 @@ impl QuadrotorInterface for Quadrotor { Vector3::new(max_rp_torque, max_rp_torque, yaw_torque) } - //TODO: replace this method in main with observe /// Simulates IMU readings /// # Returns /// * A tuple containing the true acceleration and angular velocity of the quadrotor @@ -175,12 +176,14 @@ impl QuadrotorInterface for Quadrotor { /// * 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, mass, gravity, drag_coefficient, inertia_matrix).unwrap(); + /// 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> { @@ -203,12 +206,13 @@ impl Quadrotor { /// * 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( @@ -250,12 +254,13 @@ impl Quadrotor { /// * `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); @@ -284,11 +289,12 @@ impl Quadrotor { /// * `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); @@ -334,12 +340,13 @@ impl Quadrotor { /// * `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] { @@ -355,12 +362,13 @@ impl Quadrotor { /// * `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 /// ]; @@ -383,12 +391,13 @@ impl Quadrotor { /// 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 /// ]; @@ -1796,6 +1805,7 @@ pub struct PlannerStepConfig { /// * If the planner could not be created /// # Example /// ``` +/// use peng_quad::config; /// use peng_quad::{PlannerManager, Quadrotor, Obstacle, PlannerStepConfig, update_planner, quadrotor::QuadrotorInterface}; /// use nalgebra::Vector3; /// let simulation_frequency = 1000; @@ -1806,7 +1816,7 @@ 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 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 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 { @@ -1848,8 +1858,8 @@ pub fn update_planner( /// * If the planner type is not recognized /// # Example /// ``` -/// use peng_quad::{PlannerType, Quadrotor, Obstacle, PlannerStepConfig, create_planner, -/// quadrotor::QuadrotorInterface}; +/// use peng_quad::config; +/// use peng_quad::{PlannerType, Quadrotor, Obstacle, PlannerStepConfig, create_planner, quadrotor::QuadrotorInterface}; /// use nalgebra::Vector3; /// let step = PlannerStepConfig { /// step: 0, @@ -1864,7 +1874,7 @@ 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 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 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_state, time, &obstacles).unwrap(); @@ -2438,13 +2448,14 @@ pub fn ray_cast( /// * If the data cannot be logged to the recording stream /// # Example /// ```no_run +/// 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 mut quad = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap(); -/// let quad_state = quad.observe().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); diff --git a/src/main.rs b/src/main.rs index 5e80a44a7..f676b0358 100644 --- a/src/main.rs +++ b/src/main.rs @@ -9,7 +9,6 @@ use std::time::Instant; #[tokio::main] /// Main function for the simulation async fn main() -> Result<(), SimulationError> { - let mut config_str = "config/quad.yaml"; let args: Vec = std::env::args().collect(); if args.len() != 2 { diff --git a/tests/testdata/test_config_base.yaml b/tests/testdata/test_config_base.yaml index 538a45def..203007381 100644 --- a/tests/testdata/test_config_base.yaml +++ b/tests/testdata/test_config_base.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,12 @@ 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) + 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 diff --git a/tests/testdata/test_liftoff_base.yaml b/tests/testdata/test_liftoff_base.yaml index ae4d56fd0..9bd19dacd 100644 --- a/tests/testdata/test_liftoff_base.yaml +++ b/tests/testdata/test_liftoff_base.yaml @@ -14,6 +14,7 @@ simulation: 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 From a608d31fc4b0fcbc206b9532f04a0805c8339efb Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sat, 14 Dec 2024 19:33:32 -0800 Subject: [PATCH 53/54] clippy fixes --- src/config.rs | 5 ++--- src/lib.rs | 18 +++++++++--------- src/quadrotor.rs | 1 - 3 files changed, 11 insertions(+), 13 deletions(-) diff --git a/src/config.rs b/src/config.rs index 7e01e9594..1c7fea84c 100644 --- a/src/config.rs +++ b/src/config.rs @@ -130,12 +130,11 @@ impl QuadrotorConfig { } pub fn inverse_inertia_matrix(&self) -> Result, SimulationError> { - Ok(self - .inertia_matrix() + self.inertia_matrix() .try_inverse() .ok_or(SimulationError::NalgebraError( "Failed to invert inertia matrix".to_string(), - ))?) + )) } } diff --git a/src/lib.rs b/src/lib.rs index 0c0e28c37..a60541470 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -47,7 +47,7 @@ use rand::SeedableRng; use rayon::prelude::*; pub mod config; pub mod quadrotor; -use nalgebra::{Matrix3, Quaternion, Rotation3, SMatrix, UnitQuaternion, Vector, Vector3}; +use nalgebra::{Matrix3, Quaternion, Rotation3, SMatrix, UnitQuaternion, Vector3}; use quadrotor::{QuadrotorInterface, QuadrotorState}; use rand_chacha::ChaCha8Rng; use rand_distr::{Distribution, Normal}; @@ -128,9 +128,9 @@ impl QuadrotorInterface for Quadrotor { ) -> 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); + 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; @@ -147,14 +147,14 @@ impl QuadrotorInterface for Quadrotor { } fn observe(&mut self) -> Result { - return Ok(QuadrotorState { + 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 { @@ -1835,7 +1835,7 @@ pub fn update_planner( planner_manager: &mut PlannerManager, step: usize, time: f32, - simulation_frequency: usize, + _simulation_frequency: usize, quad_state: &QuadrotorState, obstacles: &[Obstacle], planner_config: &[PlannerStepConfig], @@ -2469,7 +2469,7 @@ pub fn log_data( desired_velocity: &Vector3, measured_accel: &Vector3, measured_gyro: &Vector3, - thrust: f32, + _thrust: f32, torque: &Vector3, ) -> Result<(), SimulationError> { rec.log( @@ -2894,8 +2894,8 @@ pub fn color_map_fn(gray: f32) -> (u8, u8, u8) { /// ``` pub fn log_joy( rec: &rerun::RecordingStream, - thrust: f32, - torque: &Vector3, + _thrust: f32, + _torque: &Vector3, ) -> Result<(), SimulationError> { let num_points = 100; let radius = 1.0; diff --git a/src/quadrotor.rs b/src/quadrotor.rs index c0a2a48e1..bc834af4f 100644 --- a/src/quadrotor.rs +++ b/src/quadrotor.rs @@ -1,4 +1,3 @@ -use crate::config::QuadrotorConfigurations; use crate::SimulationError; use nalgebra::{UnitQuaternion, Vector3}; From f5eb3b370c6f280139f8bf617fa32154f1498d97 Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sat, 14 Dec 2024 19:33:32 -0800 Subject: [PATCH 54/54] clippy fixes --- src/lib.rs | 3 +-- src/main.rs | 19 +++++++++---------- 2 files changed, 10 insertions(+), 12 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index a60541470..2353a2452 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -2460,7 +2460,7 @@ pub fn ray_cast( /// 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_state, &desired_position, &desired_velocity, &measured_accel, &measured_gyro, 0.0, &Vector3::new(0.0, 0.0, 0.0)).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, @@ -2469,7 +2469,6 @@ pub fn log_data( desired_velocity: &Vector3, measured_accel: &Vector3, measured_gyro: &Vector3, - _thrust: f32, torque: &Vector3, ) -> Result<(), SimulationError> { rec.log( diff --git a/src/main.rs b/src/main.rs index f676b0358..0f5f62b6a 100644 --- a/src/main.rs +++ b/src/main.rs @@ -56,7 +56,7 @@ async 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() @@ -151,7 +151,7 @@ async fn main() -> Result<(), SimulationError> { time, &maze.obstacles, )?; - let (mut thrust, mut calculated_desired_orientation) = controller.compute_position_control( + let (thrust, calculated_desired_orientation) = controller.compute_position_control( &desired_position, &desired_velocity, desired_yaw, @@ -160,7 +160,7 @@ async fn main() -> Result<(), SimulationError> { time_step, ); - let mut torque = controller.compute_attitude_control( + let torque = controller.compute_attitude_control( &calculated_desired_orientation, &quad_state.orientation, &(quad_state.angular_velocity / (2.0 * std::f32::consts::PI)), @@ -173,7 +173,7 @@ async fn main() -> Result<(), SimulationError> { 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( @@ -186,7 +186,7 @@ async fn main() -> Result<(), SimulationError> { if let Some(rec) = &rec { rec.set_time_seconds("timestamp", time); - let mut rerun_quad_state = quad_state.clone(); + let rerun_quad_state = quad_state.clone(); if trajectory.add_point(rerun_quad_state.position) { log_trajectory(rec, &trajectory)?; } @@ -198,7 +198,6 @@ async fn main() -> Result<(), SimulationError> { &desired_velocity, &measured_accel, &quad_state.angular_velocity, - thrust, &torque, )?; let rotation = nalgebra::UnitQuaternion::from_axis_angle( @@ -220,10 +219,10 @@ async fn main() -> Result<(), SimulationError> { } } i += 1; - // if time >= config.simulation.duration { - // log::info!("Complete Simulation"); - // break; - // } + if time >= config.simulation.duration { + log::info!("Complete Simulation"); + break; + } } log::logger().flush(); Ok(())