thrust and acceleration clamping

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)

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<Vec<f32>>,
 }
 
 #[derive(Debug, serde::Deserialize)]

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);

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<f32>,                // Angular velocity in body frame
         velocity_body: Vector3<f32>,             // Linear velocity in body frame
     ) -> Vector3<f32> {
-        // 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,15 +196,15 @@ 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.
                 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),
+                    &((sample_position - self.previous_state.position) / self.time_step),
                 );
 
                 let omega_body = sample.pqr();

src/main.rs

@@ -158,14 +158,24 @@ 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,
             &quad_state.position,
             &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,