Skip to content

Commit

Permalink
operable control frequencies
Browse files Browse the repository at this point in the history 
may be a bit too aggressive
  • Loading branch information
@friend0
friend0 committed Dec 8, 2024
1 parent e88cc43 commit 330ae59
Show file tree
Hide file tree
Showing 2 changed files with 72 additions and 52 deletions.
10 changes: 5 additions & 5 deletions config/liftoff_quad.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -69,19 +69,19 @@ 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)

# 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_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)
114 changes: 67 additions & 47 deletions src/main.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<String> = std::env::args().collect();
if args.len() != 2 {
println!(
Expand All @@ -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<dyn QuadrotorInterface>, 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,
Expand Down Expand Up @@ -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
};
Expand All @@ -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<dyn QuadrotorInterface>, 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
Expand All @@ -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,
Expand Down Expand Up @@ -199,29 +206,42 @@ 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,
&measured_gyro,
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,
)?;
}
Expand Down

0 comments on commit 330ae59

Please sign in to comment.