Cansat
/
RustPhysicsMQ
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

changed the vertical axis from y to z

This commit is contained in:
franchioping 2026-03-02 18:40:33 +00:00
parent 55fbc7830a
commit 3790a7c337
19 changed files with 215 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
configurations/gen.py
View File

@ -1,11 +1,14 @@
import os
import subprocess
subprocess.run("rm -rf final && mkdir final", shell=True)
FOLDER_PREFIX = "configurations/"
MOT_FOLDER = FOLDER_PREFIX + "mot/"
SIM_FOLDER = FOLDER_PREFIX + "sim/"
PID_FOLDER = FOLDER_PREFIX + "pid_cont/"
FINAL_FOLDER = FOLDER_PREFIX + "final/"
subprocess.run(f"rm -rf {FINAL_FOLDER} && mkdir {FINAL_FOLDER}", shell=True)
MOT_FOLDER = "mot/"
SIM_FOLDER = "sim/"
PID_FOLDER = "pid_cont/"
mot_files = os.listdir(MOT_FOLDER)
sim_files = os.listdir(SIM_FOLDER)
@ -22,7 +25,7 @@ for case in prod:
with open(PID_FOLDER + case[2], "r") as f3:
settings = f1.read() + "\n" + f2.read() + "\n" + f3.read()
with open(
"final/" + "".join(case).replace(".toml", "_").removesuffix("_") + ".toml",
FINAL_FOLDER + "".join(case).replace(".toml", "_").removesuffix("_") + ".toml",
"w",
) as f:
f.write(settings)

9
configurations/mot/mot_std.toml
View File

@ -5,9 +5,10 @@ mass = 0.350
time_constant = 0.00
# roll, pitch, yaw
motor_map = [
[-1.0, 1.0, 1.0], # Front Right
[1.0, -1.0, 1.0], # Front Left
[1.0, 1.0, -1.0], # Rear Left
[-1.0, -1.0, -1.0], # Rear Right
[1.0, -1.0, 1.0], # Front Right
[-1.0, -1.0, -1.0], # Front Left
[-1.0, 1.0, 1.0], # Rear Left
[1.0, 1.0, -1.0], # Rear Right
]

12
configurations/mot/mot_tc.toml
View File

@ -1,12 +0,0 @@
max_thrust = 2.6
max_torque = 0.5
mass = 0.350
time_constant = 0.01
motor_map = [
[-1.0, 1.0, 1.0], # Front Right
[1.0, -1.0, 1.0], # Front Left
[1.0, 1.0, -1.0], # Rear Left
[-1.0, -1.0, -1.0], # Rear Right
]

6
configurations/pid_cont/rate_025.toml
View File

@ -12,9 +12,9 @@ stack = { max_rate = 3.14, rotation_pid = { kp = [
0.0,
0.0,
], frequency = 50.0 }, rate_pid = { kp = [
0.03,
0.3,
0.03,
0.1,
0.1,
1.0,
], ki = [
0.0,
0.0,

26
configurations/pid_cont/rate_030.toml Normal file
View File

@ -0,0 +1,26 @@
stack = { max_rate = 3.14, rotation_pid = { kp = [
1.5,
1.5,
1.5,
], ki = [
0.0,
0.0,
0.0,
], kd = [
0.0,
0.0,
0.0,
], frequency = 50.0 }, rate_pid = { kp = [
0.2,
0.2,
2.0,
], ki = [
0.0,
0.0,
0.0,
], kd = [
0.0,
0.0,
0.0,
], frequency = 600.0 } }

4
configurations/pid_cont/rate_03.toml → configurations/pid_cont_old/rate_03_x.toml
View File

@ -13,8 +13,8 @@ stack = { max_rate = 3.14, rotation_pid = { kp = [
0.0,
], frequency = 50.0 }, rate_pid = { kp = [
0.03,
0.3,
0.03,
0.0,
0.0,
], ki = [
0.0,
0.0,

12
configurations/pid_cont/rate_02.toml → configurations/pid_cont_old/rate_03_y.toml
View File

@ -1,8 +1,8 @@
stack = { max_rate = 3.14, rotation_pid = { kp = [
0.5,
0.5,
0.5,
1.0,
1.0,
1.0,
], ki = [
0.0,
0.0,
@ -11,10 +11,10 @@ stack = { max_rate = 3.14, rotation_pid = { kp = [
0.0,
0.0,
0.0,
], frequency = 100.0 }, rate_pid = { kp = [
0.03,
0.3,
], frequency = 50.0 }, rate_pid = { kp = [
0.0,
0.03,
0.0,
], ki = [
0.0,
0.0,

26
configurations/pid_cont_old/rate_03_z.toml Normal file
View File

@ -0,0 +1,26 @@
stack = { max_rate = 3.14, rotation_pid = { kp = [
1.0,
1.0,
1.0,
], ki = [
0.0,
0.0,
0.0,
], kd = [
0.0,
0.0,
0.0,
], frequency = 50.0 }, rate_pid = { kp = [
0.0,
0.0,
0.3,
], ki = [
0.0,
0.0,
0.0,
], kd = [
0.0,
0.0,
0.0,
], frequency = 600.0 } }

92
inputs/not/rot_all.json Normal file
View File

@ -0,0 +1,92 @@
{
"records": [
{
"input": {
"joystick": {
"throttle_input": 0,
"roll_input": 0,
"yaw_input": 0,
"pitch_input": 0
},
"position": {
"lat": 0,
"long": 0,
"alt": 0
},
"rotation": {
"roll": 0,
"yaw": 0,
"pitch": 0
},
"mode": "Rotation"
},
"time": 0
},
{
"input": {
"joystick": {
"throttle_input": 0,
"roll_input": 0,
"yaw_input": 0,
"pitch_input": 0
},
"position": {
"lat": 0,
"long": 0,
"alt": 0
},
"rotation": {
"roll": 3.14,
"yaw": 3.1,
"pitch": 1.5
},
"mode": "Rotation"
},
"time": 3
},
{
"input": {
"joystick": {
"throttle_input": 0,
"roll_input": 0,
"yaw_input": 0,
"pitch_input": 0
},
"position": {
"lat": 0,
"long": 0,
"alt": 0
},
"rotation": {
"roll": -1.5,
"yaw": 0.8,
"pitch": 0.4
},
"mode": "Rotation"
},
"time": 10
},
{
"input": {
"joystick": {
"throttle_input": 0,
"roll_input": 0,
"yaw_input": 0,
"pitch_input": 0
},
"position": {
"lat": 0,
"long": 0,
"alt": 0
},
"rotation": {
"roll": 1.5,
"yaw": 0.1,
"pitch": 2
},
"mode": "Rotation"
},
"time": 20
}
]
}

0
inputs/rot_x.json → inputs/not/rot_x.json
View File

0
inputs/rot_y.json → inputs/not/rot_y.json
View File

0
inputs/rot_z.json → inputs/not/rot_z.json
View File

4
src/config.rs
View File

@ -2,7 +2,7 @@ use nalgebra::{vector, Vector3};
use rapier3d::prelude::*;
use serde::Deserialize;
#[derive(Debug, Deserialize, Clone)]
#[derive(Default, Debug, Deserialize, Clone)]
pub struct PidConfig {
pub kp: [f32; 3],
pub ki: [f32; 3],
@ -26,8 +26,10 @@ impl PidConfig {
#[derive(Debug, Deserialize, Clone)]
pub struct ControllerStackConfig {
/// PID for the rotation (angle → angular rate) layer
#[serde(default)]
pub rotation_pid: PidConfig,
#[serde(default)]
pub acceleration_pid: PidConfig,
/// PID for the angular rate (angular rate → torque) layer

16
src/drone.rs
View File

@ -34,10 +34,10 @@ impl Default for MotorCharacteristics {
* 2 --- 3
*/
relative_motor_positions: [
rp::point![5.0, 0.0, 5.0],
rp::point![5.0, 0.0, -5.0],
rp::point![-5.0, 0.0, -5.0],
rp::point![-5.0, 0.0, 5.0],
rp::point![5.0, 5.0, 0.0],
rp::point![5.0, -5.0, 0.0],
rp::point![-5.0, -5.0, 0.0],
rp::point![-5.0, 5.0, 0.0],
],
max_thrust: 2.6,
max_torque: 0.5,
@ -70,7 +70,7 @@ impl Drone {
) -> Drone {
let drone_rb_handle = world.register_body(
rp::RigidBodyBuilder::dynamic()
.translation(rp::vector![0.0, 10.0, 0.0])
.translation(rp::vector![0.0, 0.0, 10.0])
.rotation(rp::vector![0.0, 0.0, 0.0])
/*
* These damping values keep the simulation more realistic,
@ -164,7 +164,7 @@ impl Drone {
// Apply RB's rotation to point at RB's Up
//
// motor_position is relative, transform it by the RB's position first
let mut thrust_force = nalgebra::Vector3::new(0.0, thrust, 0.0);
let mut thrust_force = nalgebra::Vector3::new(0.0, 0.0, thrust);
thrust_force = drone_rb.rotation().transform_vector(&thrust_force);
drone_rb.add_force_at_point(
thrust_force,
@ -173,9 +173,9 @@ impl Drone {
);
let torque = if i % 2 == 0 {
rp::vector![0.0, torque, 0.0]
rp::vector![0.0, 0.0, torque]
} else {
rp::vector![0.0, -torque, 0.0]
rp::vector![0.0, 0.0, -torque]
};
drone_rb.add_torque(drone_rb.rotation().transform_vector(&torque), true);
}

4
src/drone/input.rs
View File

@ -44,9 +44,13 @@ pub enum ModeInput {
#[derive(Default, Clone, Copy, PartialEq, serde::Serialize, serde::Deserialize)]
pub struct Input {
#[serde(default)]
pub joystick: JoystickInput,
#[serde(default)]
pub acceleration: AccelerationInput,
#[serde(default)]
pub rotation: RotationInput,
#[serde(default)]
pub position: PositionInput,
pub mode: ModeInput,
}

4
src/drone/stacked.rs
View File

@ -86,8 +86,8 @@ impl DroneController for StackedController {
// println!("Rotation!");
Rotation(na::vector![
self.input.rotation.roll,
self.input.rotation.yaw,
self.input.rotation.pitch,
self.input.rotation.yaw,
])
};
self.rotation_rt
@ -96,8 +96,8 @@ impl DroneController for StackedController {
AngularRate(
na::vector![
self.input.joystick.roll_input,
self.input.joystick.yaw_input,
self.input.joystick.pitch_input,
self.input.joystick.yaw_input,
] * self.config.stack.max_rate,
)
};

10
src/drone/stacked/mixer.rs
View File

@ -12,7 +12,7 @@ impl Default for MotorMixingMode {
}
pub struct MotorMixer {
pub motor_map: [[f32; 3]; 4], // roll, yaw, pitch
pub motor_map: [[f32; 3]; 4], // roll, pitch, yaw
pub min_throttle: f32,
pub max_throttle: f32,
pub mixing_mode: MotorMixingMode,
@ -77,12 +77,14 @@ impl MotorMixer {
scale = scale.clamp(min_scale, 1.0);
let max_delta = scale * delta.into_iter().reduce(f32::max).unwrap_or(0.0);
let min_delta = scale * delta.into_iter().reduce(f32::min).unwrap_or(0.0);
let mut max_delta = scale * delta.into_iter().reduce(f32::max).unwrap_or(0.0);
let mut min_delta = scale * delta.into_iter().reduce(f32::min).unwrap_or(0.0);
let delta_dif = max_delta - min_delta;
if delta_dif > throttle_range_len {
scale *= throttle_range_len / delta_dif;
scale *= throttle_range_len / delta_dif * 0.9;
max_delta *= throttle_range_len / delta_dif * 0.9;
min_delta *= throttle_range_len / delta_dif * 0.9;
}
let lim_throttle = throttle.clamp(

4
src/engine.rs
View File

@ -38,7 +38,7 @@ pub struct ColliderExtraData {
impl World {
pub fn new(dt: f32) -> Self {
let gravity: na::Vector3<f32> = rp::vector![0.0, -9.81, 0.0];
let gravity: na::Vector3<f32> = rp::vector![0.0, 0.0, -9.81];
let mut integration_parameters = rp::IntegrationParameters::default();
integration_parameters.set_inv_dt(dt);
let physics_pipeline = rp::PhysicsPipeline::new();
@ -153,7 +153,7 @@ impl World {
impl Default for World {
fn default() -> Self {
let gravity: na::Vector3<f32> = rp::vector![0.0, -9.81, 0.0];
let gravity: na::Vector3<f32> = rp::vector![0.0, 0.0, -9.81];
let mut integration_parameters = rp::IntegrationParameters::default();
integration_parameters.set_inv_dt(600.0);
let physics_pipeline = rp::PhysicsPipeline::new();

33
src/simulation.rs
View File

@ -6,7 +6,7 @@ use std::error::Error;
use crate::{
drone::{
controller::DroneController,
input::{Input, InputRecording},
input::{Input, InputRecording, ModeInput},
Drone,
},
engine::World,
@ -161,8 +161,8 @@ impl Simulation {
let mut target_angular_vel: na::Vector3<f32> = na::vector![
current_input.joystick.roll_input,
current_input.joystick.yaw_input,
current_input.joystick.pitch_input,
current_input.joystick.yaw_input,
] * 3.14;
let mut target_mot: na::Vector3<f32> = Default::default();
@ -172,19 +172,30 @@ impl Simulation {
.as_mut_any()
.downcast_mut::<crate::drone::stacked::StackedController>()
{
target_angular_vel = cont.rotation_rt.last_output.unwrap_or_default().0;
if current_input.mode != ModeInput::Acro {
target_angular_vel = cont.rotation_rt.last_output.unwrap_or_default().0;
}
target_mot = cont.rate_rt.last_output.unwrap_or_default().0;
}
/*
# roll, pitch, yaw
motor_map = [
[1.0, -1.0, 1.0], # Front Right
[-1.0, -1.0, -1.0], # Front Left
[-1.0, 1.0, 1.0], # Rear Left
[1.0, 1.0, -1.0], # Rear Right
]
*/
let applied_motor_diff: na::Vector3<f32> = na::vector![
(self.drone.current_throttles[1] + self.drone.current_throttles[2]
- self.drone.current_throttles[0]
- self.drone.current_throttles[3]),
(self.drone.current_throttles[0] + self.drone.current_throttles[2]
(self.drone.current_throttles[0] + self.drone.current_throttles[3]
- self.drone.current_throttles[1]
- self.drone.current_throttles[3]),
- self.drone.current_throttles[2]),
(self.drone.current_throttles[2] + self.drone.current_throttles[3]
- self.drone.current_throttles[0]
- self.drone.current_throttles[1])
- self.drone.current_throttles[1]),
(self.drone.current_throttles[0] + self.drone.current_throttles[2]
- self.drone.current_throttles[1]
- self.drone.current_throttles[3])
] / 2.0;
if let Some(logger) = &mut self.logger {
@ -201,8 +212,8 @@ impl Simulation {
rot_current: self.drone.get_rot(&self.world).scaled_axis().into(),
rot_target: na::UnitQuaternion::from_scaled_axis(vector![
current_input.rotation.roll,
current_input.rotation.yaw,
current_input.rotation.pitch
current_input.rotation.pitch,
current_input.rotation.yaw
])
.scaled_axis()
.into(),