From ace98da729676e694012940a68a9198279acef75 Mon Sep 17 00:00:00 2001 From: Ryan A Rodriguez Date: Sun, 3 Nov 2024 17:43:28 -0800 Subject: [PATCH] add the quadrotor trait, stub implementations for RC, liftoff quad --- src/lib.rs | 102 +++++++++++++++++++++++++++++++------------- src/liftoff_quad.rs | 60 ++++++++++++++++++++++++++ src/main.rs | 3 ++ src/rc_quad.rs | 60 ++++++++++++++++++++++++++ 4 files changed, 196 insertions(+), 29 deletions(-) create mode 100644 src/liftoff_quad.rs create mode 100644 src/rc_quad.rs diff --git a/src/lib.rs b/src/lib.rs index 6f9201331..8df38648e 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -48,6 +48,29 @@ pub enum SimulationError { #[error("Other error: {0}")] 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 /// ``` @@ -83,6 +106,56 @@ pub struct SimulatedQuadrotor { /// Previous Torque pub previous_torque: Vector3, } + +impl Quadrotor for SimulatedQuadrotor { + fn control( + &mut self, + step_number: usize, + config: &config::Config, + thrust: f32, + torque: &Vector3, + ) { + if step_number + % (config.simulation.simulation_frequency / config.simulation.control_frequency) + == 0 + { + if config.use_rk4_for_dynamics_control { + self.update_dynamics_with_controls_rk4(thrust, &torque); + } else { + self.update_dynamics_with_controls_euler(thrust, &torque); + } + self.previous_thrust = thrust; + self.previous_torque = *torque; + } else { + let previous_thrust = self.previous_thrust; + let previous_torque = self.previous_torque; + if config.use_rk4_for_dynamics_update { + self.update_dynamics_with_controls_rk4(previous_thrust, &previous_torque); + } else { + self.update_dynamics_with_controls_euler(previous_thrust, &previous_torque); + } + } + } + fn observe( + &self, + ) -> Result< + ( + Vector3, + Vector3, + UnitQuaternion, + Vector3, + ), + SimulationError, + > { + return Ok(( + self.position, + self.velocity, + self.orientation, + self.angular_velocity, + )); + } +} + /// Implementation of the Quadrotor struct impl SimulatedQuadrotor { /// Creates a new Quadrotor with default parameters @@ -135,35 +208,6 @@ impl SimulatedQuadrotor { }) } - pub fn control( - &mut self, - step_number: usize, - config: &config::Config, - thrust: f32, - torque: &Vector3, - ) { - if step_number - % (config.simulation.simulation_frequency / config.simulation.control_frequency) - == 0 - { - if config.use_rk4_for_dynamics_control { - self.update_dynamics_with_controls_rk4(thrust, &torque); - } else { - self.update_dynamics_with_controls_euler(thrust, &torque); - } - self.previous_thrust = thrust; - self.previous_torque = *torque; - } else { - let previous_thrust = self.previous_thrust; - let previous_torque = self.previous_torque; - if config.use_rk4_for_dynamics_update { - self.update_dynamics_with_controls_rk4(previous_thrust, &previous_torque); - } else { - self.update_dynamics_with_controls_euler(previous_thrust, &previous_torque); - } - } - } - /// Updates the quadrotor's dynamics with control inputs /// # Arguments /// * `control_thrust` - The total thrust force applied to the quadrotor diff --git a/src/liftoff_quad.rs b/src/liftoff_quad.rs new file mode 100644 index 000000000..84cd3d77f --- /dev/null +++ b/src/liftoff_quad.rs @@ -0,0 +1,60 @@ +use nalgebra::{Matrix3, Quaternion, Rotation3, SMatrix, UnitQuaternion, Vector3}; +use peng_quad::{Quadrotor, SimulationError}; + +/// Represents an RC quadrotor +/// # Example +/// ``` +/// use nalgebra::Vector3; +/// use peng_quad::Quadrotor; +/// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01); +/// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977]; +/// let quadrotor = RCQuad::new(time_step, mass, inertia_matrix); +/// ``` +pub struct LiftoffQuad { + /// Current position of the quadrotor in 3D space + pub position: Vector3, + /// Current velocity of the quadrotor + pub velocity: Vector3, + /// Current orientation of the quadrotor + pub orientation: UnitQuaternion, + /// Current angular velocity of the quadrotor + pub angular_velocity: Vector3, + /// Simulation time step in seconds + pub time_step: f32, + /// Mass of the quadrotor in kg + pub mass: f32, + /// Inertia matrix of the quadrotor + pub inertia_matrix: Matrix3, + /// Inverse of the inertia matrix + pub inertia_matrix_inv: Matrix3, + /// Previous Thrust + pub previous_thrust: f32, + /// Previous Torque + pub previous_torque: Vector3, +} + +impl Quadrotor for LiftoffQuad { + fn control( + &mut self, + step_number: usize, + config: &peng_quad::config::Config, + thrust: f32, + torque: &Vector3, + ) { + todo!("implement control outputs to CyberRC - gamepad mode") + } + + fn observe( + &self, + ) -> Result< + ( + Vector3, + Vector3, + UnitQuaternion, + Vector3, + ), + SimulationError, + > { + todo!("implement state feedback from Liftoff UDP") + } +} diff --git a/src/main.rs b/src/main.rs index 925d47ebe..99eb5890e 100644 --- a/src/main.rs +++ b/src/main.rs @@ -5,6 +5,9 @@ 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> { env_logger::builder() diff --git a/src/rc_quad.rs b/src/rc_quad.rs new file mode 100644 index 000000000..727d424b6 --- /dev/null +++ b/src/rc_quad.rs @@ -0,0 +1,60 @@ +use nalgebra::{Matrix3, Quaternion, Rotation3, SMatrix, UnitQuaternion, Vector3}; +use peng_quad::{Quadrotor, SimulationError}; + +/// Represents an RC quadrotor +/// # Example +/// ``` +/// use nalgebra::Vector3; +/// use peng_quad::Quadrotor; +/// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01); +/// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977]; +/// let quadrotor = RCQuad::new(time_step, mass, inertia_matrix); +/// ``` +pub struct RCQuad { + /// Current position of the quadrotor in 3D space + pub position: Vector3, + /// Current velocity of the quadrotor + pub velocity: Vector3, + /// Current orientation of the quadrotor + pub orientation: UnitQuaternion, + /// Current angular velocity of the quadrotor + pub angular_velocity: Vector3, + /// Simulation time step in seconds + pub time_step: f32, + /// Mass of the quadrotor in kg + pub mass: f32, + /// Inertia matrix of the quadrotor + pub inertia_matrix: Matrix3, + /// Inverse of the inertia matrix + pub inertia_matrix_inv: Matrix3, + /// Previous Thrust + pub previous_thrust: f32, + /// Previous Torque + pub previous_torque: Vector3, +} + +impl Quadrotor for RCQuad { + fn control( + &mut self, + step_number: usize, + config: &peng_quad::config::Config, + thrust: f32, + torque: &Vector3, + ) { + todo!("implement control outputs to CyberRC - PPM mode") + } + + fn observe( + &self, + ) -> Result< + ( + Vector3, + Vector3, + UnitQuaternion, + Vector3, + ), + SimulationError, + > { + todo!("implement state feedback from Vicon") + } +}