From d5e698023396cf9627666eabaef69782c31f450f Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sat, 16 Nov 2024 19:27:46 -0800 Subject: [PATCH] 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 ed2cd474..2af85a0e 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 9ad1bba0..d0d063dc 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 5e41002a..923512aa 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 d4549df6..13f3d7a5 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 f1b096d0..20d2a66e 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 16f475e3..fa1c20b6 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 09fb22b3..1bedbc77 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>; }