From c22bb6ef6d2387ebbe3082af140d387fa517954e Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sun, 17 Nov 2024 23:17:49 -0800 Subject: [PATCH] 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 d0d063dc..674398eb 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 923512aa..3e02d78e 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 13f3d7a5..0e88c527 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 20d2a66e..e4994b26 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 fa1c20b6..975f5b2d 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,