before cargo fix
This commit is contained in:
franchioping
parent
79734a5394
commit
13f87452ec
10
Cargo.toml
10
Cargo.toml
|
|
@ -17,11 +17,13 @@ serde = {version = "1.0.228", features = ["serde_derive"]}
|
|||
toml = "0.9.8"
|
||||
|
||||
|
||||
|
||||
[[bin]]
|
||||
name = "batch"
|
||||
path = "src/main_batch.rs"
|
||||
name = "test"
|
||||
path = "src/main_test.rs"
|
||||
|
||||
|
||||
[profile.dev.package.rapier3d]
|
||||
# [profile.dev.package.rapier3d]
|
||||
# opt-level = 3
|
||||
|
||||
[profile.dev.package."*"]
|
||||
opt-level = 3
|
||||
|
|
|
|||
12
analyze.py
12
analyze.py
|
|
@ -124,12 +124,12 @@ class SimVisualizer:
|
|||
self.ax.plot(
|
||||
self.df["time"], self.df[f"mot_{coord}"].clip(-1, 1), label=f"mot_{coord}"
|
||||
)
|
||||
self.ax.plot(
|
||||
self.df["time"],
|
||||
self.df[f"dmot_{coord}"].clip(-1, 1),
|
||||
label=f"desired mot_{coord}",
|
||||
linestyle="--",
|
||||
)
|
||||
# self.ax.plot(
|
||||
# self.df["time"],
|
||||
# self.df[f"dmot_{coord}"].clip(-1, 1),
|
||||
# label=f"desired mot_{coord}",
|
||||
# linestyle="--",
|
||||
# )
|
||||
|
||||
self.ax.set_xlabel("Time (s)")
|
||||
self.ax.set_ylabel("Value")
|
||||
|
|
|
|||
|
|
@ -34,7 +34,7 @@ pub struct SimulationConfig {
|
|||
// The order of this Vec defines the stack (e.g., [Angle, Rate])
|
||||
pub layers: Vec<LayerConfig>,
|
||||
|
||||
/// Maps [Pitch, Yaw, Roll] to each of the 4 motors
|
||||
/// Maps [Roll, Yaw, Pitch] to each of the 4 motors
|
||||
pub motor_map: [[f32; 3]; 4],
|
||||
|
||||
// Motors & Physics
|
||||
|
|
|
|||
|
|
@ -82,7 +82,7 @@ impl Drone {
|
|||
*/
|
||||
// .linear_damping(0.2) // Damps velocity slowly
|
||||
.angular_damping(0.1) // Damps angular velocity slowly
|
||||
.gyroscopic_forces_enabled(true)
|
||||
.gyroscopic_forces_enabled(false)
|
||||
.build(),
|
||||
);
|
||||
let width = 0.40;
|
||||
|
|
|
|||
|
|
@ -8,71 +8,21 @@ use crate::drone::JoystickInput;
|
|||
|
||||
use crate::config::*;
|
||||
|
||||
pub mod mixer;
|
||||
pub mod modules;
|
||||
|
||||
use mixer::MotorMixer;
|
||||
use modules::{ControllerModule, PidProcessor};
|
||||
|
||||
pub struct DroneState {
|
||||
pub rotation: na::UnitQuaternion<f32>,
|
||||
pub angular_velocity: na::Vector3<f32>,
|
||||
}
|
||||
|
||||
pub enum ControllerModule {
|
||||
Rate {
|
||||
processor: PidProcessor,
|
||||
max_rate: f32,
|
||||
},
|
||||
Angle {
|
||||
processor: PidProcessor,
|
||||
max_angle: f32,
|
||||
},
|
||||
}
|
||||
|
||||
impl ControllerModule {
|
||||
/// Takes the current setpoint (from joystick or previous layer) and
|
||||
/// returns the command for the next layer.
|
||||
pub fn process(
|
||||
&mut self,
|
||||
setpoint: na::Vector3<f32>,
|
||||
state: &DroneState,
|
||||
dt: f32,
|
||||
is_first_layer: bool,
|
||||
) -> na::Vector3<f32> {
|
||||
match self {
|
||||
ControllerModule::Angle {
|
||||
processor,
|
||||
max_angle,
|
||||
} => {
|
||||
// Setpoint is -1.0..1.0, scale it to target Radians
|
||||
let target_angles = setpoint * *max_angle;
|
||||
|
||||
let (r, p, y) = state.rotation.euler_angles();
|
||||
let current_angles = na::vector![r, y, p];
|
||||
|
||||
// Output of Angle PID = Desired Angular Velocity
|
||||
processor.update(target_angles, current_angles, dt)
|
||||
}
|
||||
|
||||
ControllerModule::Rate {
|
||||
processor,
|
||||
max_rate,
|
||||
} => {
|
||||
// If Rate is the start of the chain (Acro mode), scale the joystick.
|
||||
// If it's the second layer, the setpoint is already a velocity from the Angle layer.
|
||||
let target_velocity = if is_first_layer {
|
||||
setpoint * *max_rate
|
||||
} else {
|
||||
setpoint
|
||||
};
|
||||
let rot = state.rotation;
|
||||
let current = rot.inverse().transform_vector(&state.angular_velocity);
|
||||
|
||||
// Output of Rate PID = Desired Torque/Correction Force
|
||||
processor.update(target_velocity, current, dt)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub struct StackedController {
|
||||
modules: Vec<ControllerModule>,
|
||||
config: SimulationConfig,
|
||||
mixer: MotorMixer,
|
||||
|
||||
// State
|
||||
drone_state: DroneState,
|
||||
|
|
@ -106,6 +56,11 @@ impl StackedController {
|
|||
}
|
||||
|
||||
Self {
|
||||
mixer: MotorMixer {
|
||||
motor_map: config.motor_map,
|
||||
min_throttle: 0.0,
|
||||
max_throttle: 1.0,
|
||||
},
|
||||
modules,
|
||||
config,
|
||||
input: JoystickInput::default(),
|
||||
|
|
@ -134,46 +89,18 @@ impl DroneController for StackedController {
|
|||
fn get_motor_throttles(&mut self) -> [f32; 4] {
|
||||
let dt = (self.current_time - self.last_time).max(0.0001);
|
||||
|
||||
// Input normalized -1.0 to 1.0 from joystick
|
||||
let mut setpoint = vector![
|
||||
self.input.roll_input,
|
||||
self.input.yaw_input,
|
||||
self.input.pitch_input,
|
||||
];
|
||||
|
||||
// --- CASCADED PROCESSING ---
|
||||
// If Layer 0 is Angle, setpoint becomes Desired Rate.
|
||||
// If Layer 1 is Rate, it takes that Desired Rate and outputs Torque.
|
||||
//
|
||||
|
||||
for (i, module) in self.modules.iter_mut().enumerate() {
|
||||
let is_first_layer = i == 0;
|
||||
setpoint = module.process(setpoint, &self.drone_state, dt, is_first_layer);
|
||||
}
|
||||
|
||||
// --- MOTOR MIXER ---
|
||||
let mut motors = [0.0; 4];
|
||||
for i in 0..4 {
|
||||
let map = self.config.motor_map[i];
|
||||
motors[i] = self.input.throttle_input
|
||||
+ (map[0] * setpoint.x) // Roll
|
||||
+ (map[1] * setpoint.y) // Yaw
|
||||
+ (map[2] * setpoint.z); // Pitch
|
||||
}
|
||||
|
||||
// Clamp while keeping relative values
|
||||
let max = motors
|
||||
.iter()
|
||||
.copied()
|
||||
.max_by(|a, b| a.total_cmp(b))
|
||||
.unwrap_or(0.0);
|
||||
if max > 1.0 {
|
||||
for v in motors.iter_mut() {
|
||||
*v /= max;
|
||||
}
|
||||
}
|
||||
|
||||
motors
|
||||
return self.mixer.mix(self.input.throttle_input, setpoint).0;
|
||||
}
|
||||
|
||||
fn as_any(&self) -> &dyn Any {
|
||||
|
|
@ -183,39 +110,3 @@ impl DroneController for StackedController {
|
|||
self
|
||||
}
|
||||
}
|
||||
|
||||
pub struct PidProcessor {
|
||||
kp: na::Vector3<f32>,
|
||||
ki: na::Vector3<f32>,
|
||||
kd: na::Vector3<f32>,
|
||||
integral: na::Vector3<f32>,
|
||||
last_error: na::Vector3<f32>,
|
||||
}
|
||||
|
||||
impl PidProcessor {
|
||||
fn new(c: &PidConfig) -> Self {
|
||||
Self {
|
||||
kp: c.p_vec(),
|
||||
ki: c.i_vec(),
|
||||
kd: c.d_vec(),
|
||||
integral: na::Vector3::zeros(),
|
||||
last_error: na::Vector3::zeros(),
|
||||
}
|
||||
}
|
||||
|
||||
fn update(
|
||||
&mut self,
|
||||
target: na::Vector3<f32>,
|
||||
current: na::Vector3<f32>,
|
||||
dt: f32,
|
||||
) -> na::Vector3<f32> {
|
||||
let error = target - current;
|
||||
self.integral += error * dt;
|
||||
let derivative = (error - self.last_error) / dt;
|
||||
self.last_error = error;
|
||||
|
||||
error.component_mul(&self.kp)
|
||||
+ self.integral.component_mul(&self.ki)
|
||||
+ derivative.component_mul(&self.kd)
|
||||
}
|
||||
}
|
||||
|
|
|
|||
173
src/main.rs
173
src/main.rs
|
|
@ -1,54 +1,111 @@
|
|||
use macroquad::prelude as mq;
|
||||
use rapier3d::prelude::*;
|
||||
|
||||
mod engine;
|
||||
use engine::*;
|
||||
|
||||
mod camera;
|
||||
mod drone;
|
||||
mod rendering;
|
||||
|
||||
mod config;
|
||||
mod drone;
|
||||
mod helpers;
|
||||
mod rendering;
|
||||
mod simulation;
|
||||
|
||||
mod graphics_util;
|
||||
|
||||
use crate::{drone::fpvcontroller::JoystickInput, rendering::Renderer};
|
||||
use crate::drone::input::*;
|
||||
use crate::simulation::{SimMode, Simulation};
|
||||
|
||||
fn window_conf() -> mq::Conf {
|
||||
mq::Conf {
|
||||
window_title: "RustDroneSim".to_owned(),
|
||||
window_resizable: true,
|
||||
// fullscreen: true,
|
||||
platform: mq::miniquad::conf::Platform {
|
||||
// linux_backend: mq::miniquad::conf::LinuxBackend::WaylandOnly,
|
||||
..Default::default()
|
||||
},
|
||||
..Default::default()
|
||||
}
|
||||
use crate::config::SimulationConfig;
|
||||
use helpers::list_files;
|
||||
use std::fs;
|
||||
|
||||
const INPUTS_DIR: &str = "inputs/";
|
||||
const CONFIGS_DIR: &str = "configurations/";
|
||||
const RESULTS_DIR: &str = "results/";
|
||||
|
||||
use std::env;
|
||||
use std::path::PathBuf;
|
||||
use std::thread::JoinHandle;
|
||||
|
||||
fn main() {
|
||||
run_batch();
|
||||
}
|
||||
|
||||
#[macroquad::main(window_conf)]
|
||||
async fn main() {
|
||||
/* World Setup */
|
||||
let mut world = World::new(600.0);
|
||||
fn run_batch() {
|
||||
let _ = fs::remove_dir_all(RESULTS_DIR);
|
||||
let _ = fs::create_dir_all(RESULTS_DIR);
|
||||
|
||||
world.register_free_collider(
|
||||
ColliderBuilder::cuboid(30.0, 1.0, 30.0)
|
||||
.translation(vector![0.0, -2.0, 0.0])
|
||||
.restitution(0.5)
|
||||
.build(),
|
||||
None,
|
||||
let input_files = list_files(INPUTS_DIR);
|
||||
let config_files = list_files(CONFIGS_DIR);
|
||||
|
||||
let mut handles: Vec<JoinHandle<()>> = Default::default();
|
||||
|
||||
for input_path in input_files {
|
||||
for config_path in &config_files {
|
||||
let cp = config_path.clone();
|
||||
let ip = input_path.clone();
|
||||
handles.push(std::thread::spawn(move || {
|
||||
run(&ip, &cp);
|
||||
}));
|
||||
}
|
||||
}
|
||||
for handle in handles {
|
||||
handle.join().unwrap();
|
||||
}
|
||||
|
||||
println!("All simulations completed.");
|
||||
}
|
||||
|
||||
fn run(input_path: &PathBuf, config_path: &PathBuf) {
|
||||
let input_name = input_path.file_stem().unwrap().to_string_lossy();
|
||||
|
||||
let inputs = InputRecording::load_inputs_from_csv(input_path.to_str().unwrap())
|
||||
.expect("Failed to load input recording");
|
||||
let config_name = config_path.file_stem().unwrap().to_string_lossy();
|
||||
|
||||
let config: SimulationConfig =
|
||||
toml::from_str(&fs::read_to_string(config_path).unwrap()).expect("Invalid config file");
|
||||
|
||||
println!(
|
||||
"Running simulation: input={} config={}",
|
||||
input_name, config_name
|
||||
);
|
||||
|
||||
let mut drone_controller = drone::pidcontroller::PIDController::default();
|
||||
drone_controller.set_input(JoystickInput {
|
||||
throttle_input: 0.2,
|
||||
yaw_input: 0.2,
|
||||
roll_input: 0.2,
|
||||
pitch_input: 0.0,
|
||||
});
|
||||
let mut drone_obj = drone::Drone::new(
|
||||
let mut world = World::new(config.tickrate);
|
||||
|
||||
let drone = drone::Drone::new(
|
||||
&mut world,
|
||||
Box::new(drone_controller),
|
||||
Box::new(drone::stacked::StackedController::new(config.clone())),
|
||||
drone::MotorCharacteristics {
|
||||
max_thrust: config.max_thrust,
|
||||
max_torque: config.max_torque,
|
||||
time_constant: config.time_constant,
|
||||
..Default::default()
|
||||
},
|
||||
config.mass,
|
||||
);
|
||||
|
||||
let result_file = format!("{}/{}_{}.csv", RESULTS_DIR, input_name, config_name);
|
||||
|
||||
let mut sim = Simulation::new(
|
||||
drone,
|
||||
world,
|
||||
SimMode::Playback(inputs.clone(), 0.0),
|
||||
Some(result_file),
|
||||
config.drone_tick_rate,
|
||||
);
|
||||
|
||||
sim.run().unwrap();
|
||||
}
|
||||
|
||||
async fn run_record(output: String) {
|
||||
println!("Recording inputs to {}", output);
|
||||
|
||||
let tickrate = 60000.0;
|
||||
let mut world = World::new(tickrate);
|
||||
|
||||
let drone = drone::Drone::new(
|
||||
&mut world,
|
||||
Box::new(drone::pidcontroller::PIDController {
|
||||
..Default::default()
|
||||
}),
|
||||
drone::MotorCharacteristics {
|
||||
max_thrust: 2.6,
|
||||
max_torque: 0.5,
|
||||
|
|
@ -57,39 +114,13 @@ async fn main() {
|
|||
0.350,
|
||||
);
|
||||
|
||||
/* Renderer Setup */
|
||||
let camera = camera::AttachedCamera::new(
|
||||
drone_obj.rb_handle,
|
||||
vector![1.0, 0.0, 0.0],
|
||||
vector![0.5, 0.0, 0.0],
|
||||
let mut sim = Simulation::new(
|
||||
drone,
|
||||
world,
|
||||
SimMode::Record(InputRecording::default(), output),
|
||||
None,
|
||||
600,
|
||||
);
|
||||
let mut renderer = Renderer::new(Box::new(camera));
|
||||
|
||||
renderer.light.set_location(
|
||||
vector![70.0, 150.0, -90.0].into(),
|
||||
vector![-0.4, -0.7, 0.6].into(),
|
||||
);
|
||||
renderer.update_light(&world);
|
||||
|
||||
loop {
|
||||
renderer.update_camera(&world);
|
||||
|
||||
if mq::is_key_pressed(mq::KeyCode::Q) {
|
||||
break;
|
||||
}
|
||||
|
||||
// Physics
|
||||
world.step();
|
||||
let _ = clearscreen::clear();
|
||||
drone_obj.process_tick(&mut world);
|
||||
|
||||
// Rendering
|
||||
renderer.draw(&mut world);
|
||||
|
||||
if world.tick % (10) == 0 {
|
||||
// renderer.update_light(&world);
|
||||
// world.clear_ofb();
|
||||
mq::next_frame().await;
|
||||
}
|
||||
}
|
||||
sim.run_and_render().await.unwrap();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,126 +0,0 @@
|
|||
mod engine;
|
||||
use engine::*;
|
||||
mod camera;
|
||||
mod config;
|
||||
mod drone;
|
||||
mod helpers;
|
||||
mod rendering;
|
||||
mod simulation;
|
||||
|
||||
mod graphics_util;
|
||||
|
||||
use crate::drone::input::*;
|
||||
use crate::simulation::{SimMode, Simulation};
|
||||
|
||||
use crate::config::SimulationConfig;
|
||||
use helpers::list_files;
|
||||
use std::fs;
|
||||
|
||||
const INPUTS_DIR: &str = "inputs/";
|
||||
const CONFIGS_DIR: &str = "configurations/";
|
||||
const RESULTS_DIR: &str = "results/";
|
||||
|
||||
use std::env;
|
||||
use std::path::PathBuf;
|
||||
use std::thread::JoinHandle;
|
||||
|
||||
fn main() {
|
||||
run_batch();
|
||||
}
|
||||
|
||||
fn run_batch() {
|
||||
let _ = fs::remove_dir_all(RESULTS_DIR);
|
||||
let _ = fs::create_dir_all(RESULTS_DIR);
|
||||
|
||||
let input_files = list_files(INPUTS_DIR);
|
||||
let config_files = list_files(CONFIGS_DIR);
|
||||
|
||||
let mut handles: Vec<JoinHandle<()>> = Default::default();
|
||||
|
||||
for input_path in input_files {
|
||||
for config_path in &config_files {
|
||||
let cp = config_path.clone();
|
||||
let ip = input_path.clone();
|
||||
handles.push(std::thread::spawn(move || {
|
||||
run(&ip, &cp);
|
||||
}));
|
||||
}
|
||||
}
|
||||
for handle in handles {
|
||||
handle.join().unwrap();
|
||||
}
|
||||
|
||||
println!("All simulations completed.");
|
||||
}
|
||||
|
||||
fn run(input_path: &PathBuf, config_path: &PathBuf) {
|
||||
let input_name = input_path.file_stem().unwrap().to_string_lossy();
|
||||
|
||||
let inputs = InputRecording::load_inputs_from_csv(input_path.to_str().unwrap())
|
||||
.expect("Failed to load input recording");
|
||||
let config_name = config_path.file_stem().unwrap().to_string_lossy();
|
||||
|
||||
let config: SimulationConfig =
|
||||
toml::from_str(&fs::read_to_string(config_path).unwrap()).expect("Invalid config file");
|
||||
|
||||
println!(
|
||||
"Running simulation: input={} config={}",
|
||||
input_name, config_name
|
||||
);
|
||||
|
||||
let mut world = World::new(config.tickrate);
|
||||
|
||||
let drone = drone::Drone::new(
|
||||
&mut world,
|
||||
Box::new(drone::stacked::StackedController::new(config.clone())),
|
||||
drone::MotorCharacteristics {
|
||||
max_thrust: config.max_thrust,
|
||||
max_torque: config.max_torque,
|
||||
time_constant: config.time_constant,
|
||||
..Default::default()
|
||||
},
|
||||
config.mass,
|
||||
);
|
||||
|
||||
let result_file = format!("{}/{}_{}.csv", RESULTS_DIR, input_name, config_name);
|
||||
|
||||
let mut sim = Simulation::new(
|
||||
drone,
|
||||
world,
|
||||
SimMode::Playback(inputs.clone(), 0.0),
|
||||
Some(result_file),
|
||||
config.drone_tick_rate,
|
||||
);
|
||||
|
||||
sim.run().unwrap();
|
||||
}
|
||||
|
||||
async fn run_record(output: String) {
|
||||
println!("Recording inputs to {}", output);
|
||||
|
||||
let tickrate = 60000.0;
|
||||
let mut world = World::new(tickrate);
|
||||
|
||||
let drone = drone::Drone::new(
|
||||
&mut world,
|
||||
Box::new(drone::pidcontroller::PIDController {
|
||||
..Default::default()
|
||||
}),
|
||||
drone::MotorCharacteristics {
|
||||
max_thrust: 2.6,
|
||||
max_torque: 0.5,
|
||||
..Default::default()
|
||||
},
|
||||
0.350,
|
||||
);
|
||||
|
||||
let mut sim = Simulation::new(
|
||||
drone,
|
||||
world,
|
||||
SimMode::Record(InputRecording::default(), output),
|
||||
None,
|
||||
600,
|
||||
);
|
||||
|
||||
sim.run_and_render().await.unwrap();
|
||||
}
|
||||
Loading…
Reference in New Issue