From 282a15a373c3feef88604cbff5d48d0db932f57c Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Tue, 23 Dec 2025 11:42:58 -0500 Subject: [PATCH 1/6] kebab package naming --- CMakeLists.txt | 2 +- Cargo.toml | 5 +-- README.md | 4 +-- analysis/Cargo.toml | 7 +++++ .../Cargo.toml | 4 +-- .../src/lib.rs | 0 ateam-controls/Cargo.toml | 15 +++++++++ ateam-controls/src/lib.rs | 31 +++++++++++++++++++ ateam_controls/Cargo.toml | 6 ---- ateam_controls/src/lib.rs | 14 --------- .../include/ateam_controls/ateam_controls.h | 12 ------- 11 files changed, 61 insertions(+), 39 deletions(-) create mode 100644 analysis/Cargo.toml rename {ateam_controls_c => ateam-controls-c}/Cargo.toml (50%) rename {ateam_controls_c => ateam-controls-c}/src/lib.rs (100%) create mode 100644 ateam-controls/Cargo.toml create mode 100644 ateam-controls/src/lib.rs delete mode 100644 ateam_controls/Cargo.toml delete mode 100644 ateam_controls/src/lib.rs delete mode 100644 ateam_controls_c/include/ateam_controls/ateam_controls.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 8dcb60f..b837d0e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,7 +23,7 @@ add_custom_target( add_library(ateam_controls STATIC IMPORTED) set_target_properties( ateam_controls PROPERTIES - INTERFACE_INCLUDE_DIRECTORIES "${CMAKE_CURRENT_SOURCE_DIR}/ateam_controls_c/include" + INTERFACE_INCLUDE_DIRECTORIES "${CMAKE_CURRENT_SOURCE_DIR}/ateam-controls-c/include" IMPORTED_LOCATION ${IMPORTED_LIB_PATH} IMPORTED_GLOBAL TRUE ) diff --git a/Cargo.toml b/Cargo.toml index 589e90c..ef3b7ee 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,7 @@ [workspace] members = [ - "ateam_controls", - "ateam_controls_c", + "ateam-controls", + "ateam-controls-c", + "analysis", ] resolver = "3" diff --git a/README.md b/README.md index 536dada..e51080f 100644 --- a/README.md +++ b/README.md @@ -7,9 +7,9 @@ The shared code is written primarily in Rust with C interface bindings. ## Contents * analysis * Docs and scripts used to design and understand our motion controls -* ateam_controls +* ateam-controls * The primary Rust crate for shared code -* ateam_controls_c +* ateam-controls-c * Crate for building C-compatible library and headers * cpp_tests * Tests for the C-compatible bindings diff --git a/analysis/Cargo.toml b/analysis/Cargo.toml new file mode 100644 index 0000000..2da9021 --- /dev/null +++ b/analysis/Cargo.toml @@ -0,0 +1,7 @@ +[package] +name = "analysis" +version = "0.1.0" +edition = "2024" + +[dependencies] +ateam-controls = { path = "../ateam-controls", features = ["std"] } diff --git a/ateam_controls_c/Cargo.toml b/ateam-controls-c/Cargo.toml similarity index 50% rename from ateam_controls_c/Cargo.toml rename to ateam-controls-c/Cargo.toml index 9c1aed8..2617c08 100644 --- a/ateam_controls_c/Cargo.toml +++ b/ateam-controls-c/Cargo.toml @@ -1,5 +1,5 @@ [package] -name = "ateam_controls_c" +name = "ateam-controls-c" version = "0.1.0" edition = "2024" @@ -7,4 +7,4 @@ edition = "2024" crate-type = ["staticlib"] [dependencies] -ateam_controls = { path = "../ateam_controls" } +ateam-controls = { path = "../ateam-controls", features = ["std"] } diff --git a/ateam_controls_c/src/lib.rs b/ateam-controls-c/src/lib.rs similarity index 100% rename from ateam_controls_c/src/lib.rs rename to ateam-controls-c/src/lib.rs diff --git a/ateam-controls/Cargo.toml b/ateam-controls/Cargo.toml new file mode 100644 index 0000000..b282a16 --- /dev/null +++ b/ateam-controls/Cargo.toml @@ -0,0 +1,15 @@ +[package] +name = "ateam-controls" +version = "0.1.0" +edition = "2024" + +[features] +default = [] +std = [] + +[dependencies] +libm = "0.2" +nalgebra = { version = "0.34.0", default-features = false, features = [ + "libm", + "macros", +] } diff --git a/ateam-controls/src/lib.rs b/ateam-controls/src/lib.rs new file mode 100644 index 0000000..0ab60bc --- /dev/null +++ b/ateam-controls/src/lib.rs @@ -0,0 +1,31 @@ +#![cfg_attr(not(feature = "std"), no_std)] + +use nalgebra; + +pub mod ctypes; +pub mod trajectory_params; +pub mod robot_physical_params; +pub mod bangbang_trajectory; +pub mod robot_model; + +pub type Vector3f = nalgebra::Vector3; +pub type Vector4f = nalgebra::Vector4; +pub type Matrix3x4f = nalgebra::Matrix3x4; +pub type Matrix4x3f = nalgebra::Matrix4x3; +pub type Matrix3f = nalgebra::Matrix3; +pub type Rotation3f = nalgebra::Rotation3; + +pub fn add(left: u64, right: u64) -> u64 { + left + right +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn it_works() { + let result = add(2, 2); + assert_eq!(result, 4); + } +} diff --git a/ateam_controls/Cargo.toml b/ateam_controls/Cargo.toml deleted file mode 100644 index 9218ba2..0000000 --- a/ateam_controls/Cargo.toml +++ /dev/null @@ -1,6 +0,0 @@ -[package] -name = "ateam_controls" -version = "0.1.0" -edition = "2024" - -[dependencies] diff --git a/ateam_controls/src/lib.rs b/ateam_controls/src/lib.rs deleted file mode 100644 index b93cf3f..0000000 --- a/ateam_controls/src/lib.rs +++ /dev/null @@ -1,14 +0,0 @@ -pub fn add(left: u64, right: u64) -> u64 { - left + right -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn it_works() { - let result = add(2, 2); - assert_eq!(result, 4); - } -} diff --git a/ateam_controls_c/include/ateam_controls/ateam_controls.h b/ateam_controls_c/include/ateam_controls/ateam_controls.h deleted file mode 100644 index f6a3ae9..0000000 --- a/ateam_controls_c/include/ateam_controls/ateam_controls.h +++ /dev/null @@ -1,12 +0,0 @@ -#ifndef ATEAM_CONTROLS_C__ATEAM_CONTROLS_H__ -#define ATEAM_CONTROLS_C__ATEAM_CONTROLS_H__ - -#include - -extern "C" { - -uint64_t ateam_controls_add(uint64_t left, uint64_t right); - -} - -#endif // ATEAM_CONTROLS_C__ATEAM_CONTROLS_H__ From fd38384b88da545a11de5e0715f9a366c718c148 Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Tue, 23 Dec 2025 11:45:20 -0500 Subject: [PATCH 2/6] bangbang trajectory rust implementation --- ateam-controls/src/bangbang_trajectory.rs | 261 ++++++++++++++++++++++ ateam-controls/src/lib.rs | 7 + ateam-controls/src/trajectory_params.rs | 9 + 3 files changed, 277 insertions(+) create mode 100644 ateam-controls/src/bangbang_trajectory.rs create mode 100644 ateam-controls/src/trajectory_params.rs diff --git a/ateam-controls/src/bangbang_trajectory.rs b/ateam-controls/src/bangbang_trajectory.rs new file mode 100644 index 0000000..4dd3965 --- /dev/null +++ b/ateam-controls/src/bangbang_trajectory.rs @@ -0,0 +1,261 @@ +use core::f32::consts::PI; +use libm::{cosf, sinf, sqrtf}; +use crate::{trajectory_params::*}; +use crate::{Vector3f, RigidBodyState}; + + +#[repr(C)] +#[derive(Clone, Copy, Default, Debug)] +/// sdd1: t1 -> t2, sdd2: t2 -> t3, sdd3: t3 -> t4 +pub struct BangBangTraj1D { + pub sdd1: f32, + pub sdd2: f32, + pub sdd3: f32, + pub t1: f32, + pub t2: f32, + pub t3: f32, + pub t4: f32, +} + +impl BangBangTraj1D { + pub fn time_shift(&mut self, dt: f32) { + self.t1 += dt; + self.t2 += dt; + self.t3 += dt; + self.t4 += dt; + } +} + +#[repr(C)] +#[derive(Clone, Copy, Default, Debug)] +pub struct BangBangTraj3D { + pub x: BangBangTraj1D, + pub y: BangBangTraj1D, + pub z: BangBangTraj1D, +} + +impl BangBangTraj3D { + pub fn time_shift(&mut self, dt: f32) { + self.x.time_shift(dt); + self.y.time_shift(dt); + self.z.time_shift(dt); + } + + pub fn get_end_time(&self) -> f32 { + self.x.t4.max(self.y.t4).max(self.z.t4) + } +} + +pub fn compute_optimal_bangbang_traj_3d(init_state: RigidBodyState, target_pose: Vector3f) -> BangBangTraj3D { + let mut alpha = PI / 4.0; + let mut increment = PI / 8.0; + let precision = 0.0000001; + let mut x_traj; + let mut y_traj; + loop { + let cos_alpha = cosf(alpha); + let sin_alpha = sinf(alpha); + x_traj = compute_bangbang_traj_1d(init_state.pose.x, init_state.twist.x, target_pose.x, cos_alpha * MAX_TRANSLATIONAL_VELOCITY, cos_alpha * MAX_TRANSLATIONAL_ACCELERATION); + y_traj = compute_bangbang_traj_1d(init_state.pose.y, init_state.twist.y, target_pose.y, sin_alpha * MAX_TRANSLATIONAL_VELOCITY, sin_alpha * MAX_TRANSLATIONAL_ACCELERATION); + // x_traj = compute_bangbang_traj_1d(init_state.pose.x, init_state.twist.x, target_pose.x, (PI / 4.0).sin() * MAX_TRANSLATIONAL_VELOCITY, cos_alpha * MAX_TRANSLATIONAL_ACCELERATION); + // y_traj = compute_bangbang_traj_1d(init_state.pose.y, init_state.twist.y, target_pose.y, (PI / 4.0).cos() * MAX_TRANSLATIONAL_VELOCITY, sin_alpha * MAX_TRANSLATIONAL_ACCELERATION); + if x_traj.t4 > y_traj.t4 { + alpha -= increment; + } else { + alpha += increment; + } + if increment <= precision { + break + } + increment *= 0.5; + } + let traj = BangBangTraj3D { + x: x_traj, + y: y_traj, + z: compute_bangbang_traj_1d(init_state.pose.z, init_state.twist.z, target_pose.z, MAX_ROTATIONAL_VELOCITY, MAX_ROTATIONAL_ACCELERATION), + }; + return traj; +} + +pub fn compute_bangbang_traj_3d_state_at_t(traj: BangBangTraj3D, current_state: RigidBodyState, current_time: f32, t: f32) -> RigidBodyState { + let (x_f, xd_f) = compute_bangbang_traj_1d_state_at_t(traj.x, current_state.pose.x, current_state.twist.x, current_time, t); + let (y_f, yd_f) = compute_bangbang_traj_1d_state_at_t(traj.y, current_state.pose.y, current_state.twist.y, current_time, t); + let (z_f, zd_f) = compute_bangbang_traj_1d_state_at_t(traj.z, current_state.pose.z, current_state.twist.z, current_time, t); + RigidBodyState { + pose: Vector3f::new(x_f, y_f, z_f), + twist: Vector3f::new(xd_f, yd_f, zd_f) + } +} + +pub fn compute_bangbang_traj_3d_accel_at_t(traj: BangBangTraj3D, t: f32) -> Vector3f { + Vector3f::new( + compute_bangbang_traj_1d_accel_at_t(traj.x, t), + compute_bangbang_traj_1d_accel_at_t(traj.y, t), + compute_bangbang_traj_1d_accel_at_t(traj.z, t), + ) +} + +pub fn compute_bangbang_traj_1d_accel_at_t(traj: BangBangTraj1D, t: f32) -> f32 { + if t >= traj.t3 { + return traj.sdd3; + } + if t >= traj.t2 { + return traj.sdd2; + } + if t >= traj.t1 { + return traj.sdd1; + } + panic!("Tried to use a trajectory that hasn't started yet!") +} + +/// Takes the initial velocity sd0, the desired positive change in position ds, and the bang-bang acceleration sdd +/// Returns the positive acceleration time T1, the negative acceleration time T2, and the peak velocity reached Vpeak +fn compute_positive_triangular_profile(sd0: f32, ds: f32, sdd: f32) -> (f32, f32, f32) { + if sd0 < 0.0 || ds < 0.0 || sdd < 0.0 { + panic!("compute_positive_triangular_profile: All values should be positive") + } + + let t2 = sqrtf((sdd * ds + 0.5 * sd0 * sd0) / (sdd * sdd)); + let t1 = t2 - sd0 / sdd; + let vpeak = sdd * t2; + (t1, t2, vpeak) +} + +/// Takes the initial velocity sd0, the desired positive change in position ds, the bang-bang acceleration sdd, and the max velocity contraint sd_max +/// Returns the acceleration or deceleration time T1, the coasting time T2, the negative acceleration time T3, and the acceleration used for T1 (sdd1) +fn compute_positive_trapezoidal_profile(sd0: f32, ds: f32, sdd: f32, sd_max: f32) -> (f32, f32, f32, f32) { + if sd0 < 0.0 || ds <= 0.0 || sdd <= 0.0 || sd_max <= 0.0 { // allow sd0 to be zero + panic!("compute_positive_triangular_profile: All values should be positive") + } + + // The first period of time can either be acceleration to max velocity or deceleration to max velocity + let t1; + let d1; + let sdd1; + if sd0 > sd_max { + t1 = (sd0 - sd_max) / sdd; // deceleration time + d1 = sd_max * t1 + 0.5 * (sd0 - sd_max) * t1; // distance traveled during deceleration + sdd1 = -sdd; + } else { + t1 = (sd_max - sd0) / sdd; // acceleration time + d1 = sd0 * t1 + 0.5 * (sd_max - sd0) * t1; // distance traveled during acceleration + sdd1 = sdd; + } + let t3 = sd_max / sdd; // breaking time + let d3 = 0.5 * sd_max * t3; // distance traveled during the deceleration + let d2 = ds - d1 - d3; // distance traveled during the coast + let t2 = d2 / sd_max; // coasting time + if t1 < 0.0 || t2 < 0.0 || t3 < 0.0 { + panic!("compute_positive_trapezoidal_profile: No solution found") + } + return (t1, t2, t3, sdd1) +} + +/// Returns the time it took to break and the resulting position +fn compute_break(s0: f32, sd0: f32, sdd: f32) -> (f32, f32) { + let time_to_rest = sd0.abs() / sdd.abs(); + let sf = s0 + 0.5 * sd0 * time_to_rest; + (time_to_rest, sf) +} + +fn compute_bangbang_traj_1d(s0: f32, sd0: f32, s_trg: f32, sd_max: f32, sdd_max: f32) -> BangBangTraj1D { + if sdd_max <= 0.0 || sd_max <= 0.0 { + panic!("compute_optimal_traj1d: Can't compute trajectory when max velocity or acceleration is 0.0") + } + + let mut traj = BangBangTraj1D::default(); + let mut s = s0; + let mut sd = sd0; + + // First check if s will need to turn around (change sign in velocity) + if sd != 0.0 { + // a. check if initial velocity is in the wrong direction + // b. check if full break will overshoot s_trg + let (break_time, s_after_full_break) = compute_break(s, sd, sdd_max); + // Check if full break position is outside the segment between s0 and s_trg + if s_after_full_break != s_trg && (s_after_full_break - s).is_sign_positive() == (s_after_full_break - s_trg).is_sign_positive() { + // The trajectory requires a full break to rest immediately to turn around + traj.sdd1 = - sd.signum() * sdd_max; // acceleration should be in opposite direction of initial velocity + traj.t2 += break_time; + traj.t3 += break_time; + traj.t4 += break_time; + s = s_after_full_break; + sd = 0.0; + } + } + + // Solve triangular profile and check if unconstrained velocity is greater than sd_max + let ds = s_trg - s; + let direction = ds.signum(); + let (t1, t2, vpeak) = compute_positive_triangular_profile(sd.abs(), ds.abs(), sdd_max); + if vpeak < sd_max { + + // accelerate towards target + traj.sdd1 = direction * sdd_max; // NOTE: this could have already been set if break to zero was added in the beginning of the trajectory, but the acceleration should be in the same direction here if that is the case + traj.t2 += t1; + traj.t3 += t1; + traj.t4 += t1; + + // Skip the coasting time period + // traj.sdd2 = 0.0; + // traj.t3 += 0.0; + // traj.t4 += 0.0; + + // break away from target + traj.sdd3 = - direction * sdd_max; + traj.t4 += t2; + + } else { + // Solve trapezoidal profile + let (t1, t2, t3, sdd1) = compute_positive_trapezoidal_profile(sd.abs(), ds.abs(), sdd_max, sd_max); + + // accelerate or decelerate to max velocity + traj.sdd1 = direction * sdd1; // NOTE: this could have already been set if break to zero was added in the beginning of the trajectory, but the acceleration should be in the same direction here if that is the case + traj.t2 += t1; + traj.t3 += t1; + traj.t4 += t1; + + // coast + traj.sdd2 = 0.0; + traj.t3 += t2; + traj.t4 += t2; + + // break away from target + traj.sdd3 = - direction * sdd_max; + traj.t4 += t3; + } + + traj +} + +fn compute_bangbang_traj_1d_state_at_t(traj: BangBangTraj1D, s: f32, sd: f32, current_time: f32, t: f32) -> (f32, f32) { + let mut s = s; + let mut sd = sd; + let mut current_time = current_time; + + if t < current_time { + panic!("Unable to compute state of trajectory in the past"); + } + if traj.t1 > current_time { + panic!("Unable to compute state of trajectory that hasn't started"); + } + + for (part_end_time, part_acceleration) in [(traj.t2, traj.sdd1), (traj.t3, traj.sdd2), (traj.t4, traj.sdd3)] { + if current_time < part_end_time { + let time_in_part; + if t < part_end_time { + time_in_part = t - current_time; + } else { + time_in_part = part_end_time - current_time; + } + s += sd * time_in_part + 0.5 * part_acceleration * time_in_part * time_in_part; // v * t + 1/2 * a * t^2 + sd += part_acceleration * time_in_part; // a * t + if t < part_end_time { + return (s, sd) // reached the desired time + } else { + current_time = part_end_time; + } + } + } + return (s, sd) // t was equal to or after the trajectory end time +} diff --git a/ateam-controls/src/lib.rs b/ateam-controls/src/lib.rs index 0ab60bc..20c3d3c 100644 --- a/ateam-controls/src/lib.rs +++ b/ateam-controls/src/lib.rs @@ -15,6 +15,13 @@ pub type Matrix4x3f = nalgebra::Matrix4x3; pub type Matrix3f = nalgebra::Matrix3; pub type Rotation3f = nalgebra::Rotation3; + +#[derive(Clone, Copy, Default, Debug)] +pub struct RigidBodyState { + pub pose: Vector3f, + pub twist: Vector3f, +} + pub fn add(left: u64, right: u64) -> u64 { left + right } diff --git a/ateam-controls/src/trajectory_params.rs b/ateam-controls/src/trajectory_params.rs new file mode 100644 index 0000000..6f9f601 --- /dev/null +++ b/ateam-controls/src/trajectory_params.rs @@ -0,0 +1,9 @@ +pub use core::f32::consts::PI; + + +pub const ALLOWABLE_ERROR_POS: f32 = 0.001; // m +pub const ALLOWABLE_ERROR_VEL: f32 = 0.001; // m/s +pub const MAX_TRANSLATIONAL_ACCELERATION: f32 = 3.0; // m/s^2 +pub const MAX_TRANSLATIONAL_VELOCITY: f32 = 3.0; // m/s +pub const MAX_ROTATIONAL_ACCELERATION: f32 = 2.0 * PI; // rad/s^2 +pub const MAX_ROTATIONAL_VELOCITY: f32 = 2.0 * PI; // rad/s \ No newline at end of file From fa3d7f8f65220e1533300c14fe6a4c3d78e9069a Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Tue, 23 Dec 2025 11:47:22 -0500 Subject: [PATCH 3/6] robot model rust implementation --- ateam-controls/src/robot_model.rs | 130 ++++++++++++++++++++ ateam-controls/src/robot_physical_params.rs | 9 ++ 2 files changed, 139 insertions(+) create mode 100644 ateam-controls/src/robot_model.rs create mode 100644 ateam-controls/src/robot_physical_params.rs diff --git a/ateam-controls/src/robot_model.rs b/ateam-controls/src/robot_model.rs new file mode 100644 index 0000000..d8f9b02 --- /dev/null +++ b/ateam-controls/src/robot_model.rs @@ -0,0 +1,130 @@ +use libm::{sinf, cosf}; +use crate::{Matrix3f, Matrix3x4f, Matrix4x3f, Vector3f, Vector4f}; +use crate::robot_physical_params::*; + +/// Pose: Vector3 +/// x - x linear position (m) +/// y - y linear position (m) +/// z - z angular orientation (rad) +/// Twist: Vector3 +/// x - x linear velocity (m/s) +/// y - y linear velocity (m/s) +/// z - angular velocity around z axis (rad/s) +/// Accel: Vector3 +/// x - x linear acceleration (m/s^2) +/// y - y linear acceleration (m/s^2) +/// z - angular aceleration around z axis (rad/s^2) +/// Wheel Velocities: Vector4 +/// x - front left angular velocity (rad/s) +/// y - back left angular velocity (rad/s) +/// z - back right angular velocity (rad/s) +/// w - front right angular velocity (rad/s) +/// Wheel Torques: Vector4 +/// x - front left torque (N*m) +/// y - back left torque (N*m) +/// z - back right torque (N*m) +/// w - front right torque (N*m) + +/// Rotation matrix around z axis by theta radians +fn z_rotation_mat(theta: f32) -> Matrix3f { + let c = cosf(theta); + let s = sinf(theta); + Matrix3f::new( + c , -s , 0.0, + s , c , 0.0, + 0.0, 0.0, 1.0 + ) +} + +/// Pseudo matrix inverse +fn pinv_3x4(a: Matrix3x4f) -> Matrix4x3f { + let a_t = a.clone().transpose(); + let a_at = a * a_t.clone(); + let a_at_inv = a_at.try_inverse().unwrap(); + let a_inv = a_t * a_at_inv; + a_inv +} + +pub fn transform_frame_global2robot_pose(robot_pose: Vector3f, pose: Vector3f) -> Vector3f { + z_rotation_mat(-robot_pose.z) * (pose - robot_pose) +} + +pub fn transform_frame_robot2global_pose(robot_pose: Vector3f, pose: Vector3f) -> Vector3f { + z_rotation_mat(robot_pose.z) * pose + robot_pose +} + +pub fn transform_frame_global2robot_twist(robot_pose: Vector3f, twist: Vector3f) -> Vector3f { + z_rotation_mat(-robot_pose.z) * twist +} + +pub fn transform_frame_robot2global_twist(robot_pose: Vector3f, twist: Vector3f) -> Vector3f { + z_rotation_mat(robot_pose.z) * twist +} + +pub fn transform_frame_global2robot_accel(robot_pose: Vector3f, accel: Vector3f) -> Vector3f { + z_rotation_mat(-robot_pose.z) * accel +} + +pub fn transform_frame_robot2global_accel(robot_pose: Vector3f, accel: Vector3f) -> Vector3f { + z_rotation_mat(robot_pose.z) * accel +} + +pub struct RobotModel { + pub wheel_transform_mat: Matrix3x4f, + pub wheel_transform_mat_inv: Matrix4x3f, + pub wheel_radius: f32, + pub body_inirtia: Matrix3f, + pub body_inirtia_inv: Matrix3f, +} + +impl RobotModel { + /// alpha: angle between robot y axis and segment between robot origin and robot front left wheel, same as angle between robot -y axis and segment between robot origin and robot front right wheel + /// beta: angle between robot y axis and segment between robot origin and robot back left wheel, same as angle between robot -y axis and segment between robot origin and robot back right wheel + /// l: distance from the robot center to each wheel + /// r: radius of each wheel + /// m: robot body mass + /// i: robot body moment of inirtia around z axis + pub fn new(alpha: f32, beta: f32, l: f32, r: f32, m: f32, i: f32) -> RobotModel { + let wheel_transform_mat = Matrix3x4f::new( + -cosf(alpha), -cosf(beta), cosf(beta), cosf(alpha), + sinf(alpha), -sinf(beta), -sinf(beta), sinf(alpha), + l , l , l , l + ); + let body_inirtia = Matrix3f::new( + m , 0., 0., + 0., m , 0., + 0., 0., i , + ); + RobotModel { + wheel_transform_mat, + wheel_transform_mat_inv: pinv_3x4(wheel_transform_mat), + wheel_radius: r, + body_inirtia, + body_inirtia_inv: body_inirtia.try_inverse().unwrap(), + } + } + + pub fn new_from_constants() -> RobotModel { + RobotModel::new(WHEEL_ANGLE_ALPHA, WHEEL_ANGLE_BETA, WHEEL_DISTANCE, WHEEL_RADIUS, BODY_MASS, BODY_MOMENT_Z) + } + + /// Calculate robot frame twist from wheel velocities + pub fn wheel_velocities_to_twist(&self, wheel_velocities: Vector4f) -> Vector3f { + self.wheel_transform_mat_inv.transpose() * wheel_velocities + } + + /// Calculate wheel velocities from robot frame twist + pub fn twist_to_wheel_velocities(&self, twist: Vector3f) -> Vector4f { + self.wheel_transform_mat.transpose() * twist + } + + /// Calculate robot frame accel from wheel torques + pub fn wheel_torques_to_accel(&self, torques: Vector4f) -> Vector3f { + self.body_inirtia_inv * self.wheel_transform_mat * torques / self.wheel_radius + } + + // Calculate wheel torques from robot frame accel + pub fn accel_to_wheel_torques(&self, accel: Vector3f) -> Vector4f { + self.wheel_radius * self.wheel_transform_mat_inv * self.body_inirtia * accel + } +} diff --git a/ateam-controls/src/robot_physical_params.rs b/ateam-controls/src/robot_physical_params.rs new file mode 100644 index 0000000..26ae3c6 --- /dev/null +++ b/ateam-controls/src/robot_physical_params.rs @@ -0,0 +1,9 @@ +use core::f32::consts::PI; + + +pub const BODY_MASS: f32 = 2.0; // kg +pub const BODY_MOMENT_Z: f32 = 0.0009; // kg * m^2 // scalar moment of inertia around the z axis // I = 0.5 * M * R^2 // 0.0009 = 0.5 * 2 * 0.03^2 +pub const WHEEL_ANGLE_ALPHA: f32 = PI / 6.0; // rad +pub const WHEEL_ANGLE_BETA: f32 = PI / 4.0; // rad +pub const WHEEL_DISTANCE: f32 = 0.0814; // m // Distance from center of robot to center of wheel +pub const WHEEL_RADIUS: f32 = 0.030; // m \ No newline at end of file From fac3f72bc63b344a127293cee58f0afed157863f Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Tue, 23 Dec 2025 11:48:27 -0500 Subject: [PATCH 4/6] trajectory sim analysis --- Cargo.lock | 161 ++++++++++++++++++++++++++- analysis/Cargo.toml | 5 + analysis/src/bin/trajectory_sim.rs | 86 ++++++++++++++ analysis/trajectory_sim_visualize.py | 127 +++++++++++++++++++++ 4 files changed, 376 insertions(+), 3 deletions(-) create mode 100644 analysis/src/bin/trajectory_sim.rs create mode 100644 analysis/trajectory_sim_visualize.py diff --git a/Cargo.lock b/Cargo.lock index d5b7ddd..9056410 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -3,12 +3,167 @@ version = 4 [[package]] -name = "ateam_controls" +name = "analysis" version = "0.1.0" +dependencies = [ + "ateam-controls", +] + +[[package]] +name = "approx" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cab112f0a86d568ea0e627cc1d6be74a1e9cd55214684db5561995f6dad897c6" +dependencies = [ + "num-traits", +] + +[[package]] +name = "ateam-controls" +version = "0.1.0" +dependencies = [ + "libm", + "nalgebra", +] [[package]] -name = "ateam_controls_c" +name = "ateam-controls-c" version = "0.1.0" dependencies = [ - "ateam_controls", + "ateam-controls", +] + +[[package]] +name = "autocfg" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c08606f8c3cbf4ce6ec8e28fb0014a2c086708fe954eaa885384a6165172e7e8" + +[[package]] +name = "libm" +version = "0.2.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f9fbbcab51052fe104eb5e5d351cf728d30a5be1fe14d9be8a3b097481fb97de" + +[[package]] +name = "nalgebra" +version = "0.34.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c4d5b3eff5cd580f93da45e64715e8c20a3996342f1e466599cf7a267a0c2f5f" +dependencies = [ + "approx", + "nalgebra-macros", + "num-complex", + "num-rational", + "num-traits", + "simba", + "typenum", +] + +[[package]] +name = "nalgebra-macros" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "973e7178a678cfd059ccec50887658d482ce16b0aa9da3888ddeab5cd5eb4889" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "num-complex" +version = "0.4.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "73f88a1307638156682bada9d7604135552957b7818057dcef22705b4d509495" +dependencies = [ + "num-traits", +] + +[[package]] +name = "num-integer" +version = "0.1.46" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7969661fd2958a5cb096e56c8e1ad0444ac2bbcd0061bd28660485a44879858f" +dependencies = [ + "num-traits", ] + +[[package]] +name = "num-rational" +version = "0.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f83d14da390562dca69fc84082e73e548e1ad308d24accdedd2720017cb37824" +dependencies = [ + "num-integer", + "num-traits", +] + +[[package]] +name = "num-traits" +version = "0.2.19" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" +dependencies = [ + "autocfg", + "libm", +] + +[[package]] +name = "paste" +version = "1.0.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" + +[[package]] +name = "proc-macro2" +version = "1.0.101" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "89ae43fd86e4158d6db51ad8e2b80f313af9cc74f5c0e03ccb87de09998732de" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "quote" +version = "1.0.41" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ce25767e7b499d1b604768e7cde645d14cc8584231ea6b295e9c9eb22c02e1d1" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "simba" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c99284beb21666094ba2b75bbceda012e610f5479dfcc2d6e2426f53197ffd95" +dependencies = [ + "approx", + "num-complex", + "num-traits", + "paste", +] + +[[package]] +name = "syn" +version = "2.0.107" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2a26dbd934e5451d21ef060c018dae56fc073894c5a7896f882928a76e6d081b" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "typenum" +version = "1.19.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "562d481066bde0658276a35467c4af00bdc6ee726305698a55b86e61d7ad82bb" + +[[package]] +name = "unicode-ident" +version = "1.0.19" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f63a545481291138910575129486daeaf8ac54aee4387fe7906919f7830c7d9d" diff --git a/analysis/Cargo.toml b/analysis/Cargo.toml index 2da9021..1392017 100644 --- a/analysis/Cargo.toml +++ b/analysis/Cargo.toml @@ -5,3 +5,8 @@ edition = "2024" [dependencies] ateam-controls = { path = "../ateam-controls", features = ["std"] } + +[[bin]] +name = "trajectory_sim" +test = false +harness = false \ No newline at end of file diff --git a/analysis/src/bin/trajectory_sim.rs b/analysis/src/bin/trajectory_sim.rs new file mode 100644 index 0000000..2cc6a3b --- /dev/null +++ b/analysis/src/bin/trajectory_sim.rs @@ -0,0 +1,86 @@ +use ateam_controls::{RigidBodyState, Vector3f}; +use ateam_controls::robot_model::{RobotModel, transform_frame_global2robot_accel, transform_frame_robot2global_accel, transform_frame_global2robot_twist}; +use ateam_controls::bangbang_trajectory::{compute_optimal_bangbang_traj_3d, compute_bangbang_traj_3d_accel_at_t}; +use ateam_controls::robot_physical_params::*; +use ateam_controls::trajectory_params::*; +use core::f32::consts::PI; + +fn main() { + let model = RobotModel::new( + WHEEL_ANGLE_ALPHA, + WHEEL_ANGLE_BETA, + WHEEL_DISTANCE, + WHEEL_RADIUS, + BODY_MASS, + BODY_MOMENT_Z, + ); + + let mut current_state = RigidBodyState { + pose: Vector3f::new(0.5, 0.5, 0.0), + twist: Vector3f::new(1.0, -1.0, 0.0), + }; + + let target_pose = Vector3f::new(0.5, 1.5, PI / 2.0); + + // Timing constants + // let control_period = 0.02; // 50 Hz controller + // let simulation_period = 0.001; // 1000 Hz physics engine + let control_period = 0.001; // 1 KHz controller + let simulation_period = 0.00001; // 100 KHz physics engine + let steps_per_control_cycle = (control_period / simulation_period) as usize; + + let mut total_sim_time = 0.0; + + // Print CSV header for the Python visualizer + println!("time,x,y,theta,vx,vy,vtheta,ax,ay,atheta,wv1,wv2,wv3,wv4,wt1,wt2,wt3,wt4"); + + while total_sim_time < 5.0 { + + // --- ON-ROBOT PROCESSING --- + // 1. Generate new trajectory from current state + let traj = compute_optimal_bangbang_traj_3d(current_state, target_pose); + + // 2. Determine desired global acceleration at this instant + let desired_global_accel = compute_bangbang_traj_3d_accel_at_t(traj, 0.0); + + // 3. Convert to robot frame and calculate wheel torques + let robot_accel_cmd = transform_frame_global2robot_accel(current_state.pose, desired_global_accel); + + let wheel_torques = model.accel_to_wheel_torques(robot_accel_cmd); + + // --- PHYSICS SIMULATION --- + for _ in 0..steps_per_control_cycle { + // Apply torques to find actual acceleration + let robot_accel = model.wheel_torques_to_accel(wheel_torques); + + let global_accel = transform_frame_robot2global_accel(current_state.pose, robot_accel); + + // Integrate + current_state.pose += current_state.twist * simulation_period + 0.5 * global_accel * simulation_period * simulation_period; // dx = v*t + 0.5*a*t^2 + current_state.twist += global_accel * simulation_period; // dv = a*t + + // Calculate wheel velocities for telemetry + let robot_twist = transform_frame_global2robot_twist(current_state.pose, current_state.twist); + let wheel_velocities = model.twist_to_wheel_velocities(robot_twist); + + // Log data at simulation frequency for smooth video + println!( + "{:.3},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4}", + total_sim_time, + current_state.pose.x, current_state.pose.y, current_state.pose.z, + current_state.twist.x, current_state.twist.y, current_state.twist.z, + global_accel.x, global_accel.y, global_accel.z, + wheel_velocities.x, wheel_velocities.y, wheel_velocities.z, wheel_velocities.w, + wheel_torques.x, wheel_torques.y, wheel_torques.z, wheel_torques.w + ); + + total_sim_time += simulation_period; + + // Check if arrived + if (current_state.pose - target_pose).norm() < ALLOWABLE_ERROR_POS + && current_state.twist.norm() < ALLOWABLE_ERROR_VEL { + return; + } + } + } +} \ No newline at end of file diff --git a/analysis/trajectory_sim_visualize.py b/analysis/trajectory_sim_visualize.py new file mode 100644 index 0000000..532d847 --- /dev/null +++ b/analysis/trajectory_sim_visualize.py @@ -0,0 +1,127 @@ +import pandas as pd +import matplotlib.pyplot as plt +import matplotlib.animation as animation +import numpy as np +import sys + + +data = None + +def create_plots(): + ############### Static 3x3 plot: pos/vel/acc for x, y, theta ############### + plot_fig, axes = plt.subplots(3, 3, figsize=(12, 9), sharex=True) + + # Column 0: X + axes[0, 0].plot(data['time'], data['x'], color='C0') + axes[0, 0].set_title('X Position') + axes[0, 0].grid(True) + + axes[1, 0].plot(data['time'], data['vx'], color='C1') + axes[1, 0].set_title('X Velocity') + axes[1, 0].grid(True) + + axes[2, 0].plot(data['time'], data['ax'], color='C2') + axes[2, 0].set_title('X Acceleration') + axes[2, 0].grid(True) + + # Column 1: Y + axes[0, 1].plot(data['time'], data['y'], color='C0') + axes[0, 1].set_title('Y Position') + axes[0, 1].grid(True) + + axes[1, 1].plot(data['time'], data['vy'], color='C1') + axes[1, 1].set_title('Y Velocity') + axes[1, 1].grid(True) + + axes[2, 1].plot(data['time'], data['ay'], color='C2') + axes[2, 1].set_title('Y Acceleration') + axes[2, 1].grid(True) + + # Column 2: Theta + axes[0, 2].plot(data['time'], data['theta'], color='C0') + axes[0, 2].set_title('Theta (rad)') + axes[0, 2].grid(True) + + axes[1, 2].plot(data['time'], data['vtheta'], color='C1') + axes[1, 2].set_title('Theta Velocity (rad/s)') + axes[1, 2].grid(True) + + axes[2, 2].plot(data['time'], data['atheta'], color='C2') + axes[2, 2].set_title('Theta Acceleration (rad/s^2)') + axes[2, 2].grid(True) + + # Label x-axis on bottom row + for ax in axes[2, :]: + ax.set_xlabel('Time (s)') + + plot_fig.tight_layout() + plot_fig.savefig('sim_plots.png', dpi=200) + print('Saved static 3x3 plot as sim_plots.png') + +def create_animation(): + frame_fig, ax = plt.subplots(figsize=(10, 8)) + ax.set_xlim(-0.5, 2.0) + ax.set_ylim(-0.5, 2.0) + ax.set_aspect('equal') + ax.grid(True, linestyle='--', alpha=0.6) + + # Robot body and direction marker + robot_body = plt.Circle((0, 0), 0.1, color='blue', alpha=0.3) + heading_line, = ax.plot([], [], 'k-', lw=2) + ax.add_patch(robot_body) + + # Vectors (Velocity = Green, Acceleration = Blue) + vel_arrow = ax.quiver(0, 0, 0, 0, color='green', scale=5, label='Velocity') + accel_arrow = ax.quiver(0, 0, 0, 0, color='blue', scale=10, label='Accel') + + # Telemetry Text + telemetry_box = ax.text(2.0, 1.8, "", fontsize=9, family='monospace', + bbox=dict(facecolor='white', alpha=0.8)) + + def update(i): + row = data.iloc[i] + + # Update Robot Position + robot_body.center = (row['x'], row['y']) + + # Update Heading line + hx = row['x'] + 0.1 * np.cos(row['theta']) + hy = row['y'] + 0.1 * np.sin(row['theta']) + heading_line.set_data([row['x'], hx], [row['y'], hy]) + + # Update Vectors + vel_arrow.set_offsets([row['x'], row['y']]) + vel_arrow.set_UVC(row['vx'], row['vy']) + + accel_arrow.set_offsets([row['x'], row['y']]) + accel_arrow.set_UVC(row['ax'], row['ay']) + + # Update Telemetry HUD + txt = (f"Time: {row['time']:.2f}s\n" + f"Wheel Vels (rad/s):\n" + f" FL: {row['wv1']:>6.2f} FR: {row['wv4']:>6.2f}\n" + f" BL: {row['wv2']:>6.2f} BR: {row['wv3']:>6.2f}\n" + f"Wheel Torques (Nm):\n" + f" FL: {row['wt1']:>6.2f} FR: {row['wt4']:>6.2f}\n" + f" BL: {row['wt2']:>6.2f} BR: {row['wt3']:>6.2f}") + telemetry_box.set_text(txt) + + return robot_body, heading_line, vel_arrow, accel_arrow, telemetry_box + + downsample_factor = 1000 + ani = animation.FuncAnimation(frame_fig, update, frames=range(0, len(data), downsample_factor), interval=downsample_factor) + + plt.legend(loc='lower left') + plt.title("Robot Trajectory Analysis Simulation") + + # Save video + ani.save('sim_animation.mp4', writer='ffmpeg', fps=50) + print("Video saved as sim_animation.mp4") + + +if __name__ == "__main__": + # Load simulation data + # data = pd.read_csv(sys.stdin) + data = pd.read_csv('sim_data.csv') # For testing purposes + create_plots() + create_animation() From f29c0ff0af4789aa4bf7aece97350da928640222 Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Tue, 23 Dec 2025 11:48:57 -0500 Subject: [PATCH 5/6] c bindings and ctests --- .../include/ateam_controls/ateam_controls.h | 84 +++++++ ateam-controls-c/src/lib.rs | 94 ++++++++ ateam-controls/src/ctypes.rs | 223 ++++++++++++++++++ cpp_tests/CMakeLists.txt | 2 + cpp_tests/test_bangbang.cpp | 111 +++++++++ cpp_tests/test_robot_model.cpp | 134 +++++++++++ 6 files changed, 648 insertions(+) create mode 100644 ateam-controls-c/include/ateam_controls/ateam_controls.h create mode 100644 ateam-controls/src/ctypes.rs create mode 100644 cpp_tests/test_bangbang.cpp create mode 100644 cpp_tests/test_robot_model.cpp diff --git a/ateam-controls-c/include/ateam_controls/ateam_controls.h b/ateam-controls-c/include/ateam_controls/ateam_controls.h new file mode 100644 index 0000000..11bac68 --- /dev/null +++ b/ateam-controls-c/include/ateam_controls/ateam_controls.h @@ -0,0 +1,84 @@ +#ifndef ATEAM_CONTROLS_C__ATEAM_CONTROLS_H__ +#define ATEAM_CONTROLS_C__ATEAM_CONTROLS_H__ + +#include + +extern "C" { + +typedef struct Vector3C { + float x; + float y; + float z; +} Vector3C_t; + +typedef struct Vector4C { + float x; + float y; + float z; + float w; +} Vector4C_t; + +typedef struct Matrix3C { + float data[9]; +} Matrix3C_t; + +typedef struct Matrix3x4C { + float data[12]; +} Matrix3x4C_t; + +typedef struct Matrix4x3C { + float data[12]; +} Matrix4x3C_t; + +typedef struct RigidBodyStateC { + Vector3C pose; + Vector3C twist; +} RigidBodyStateC_t; + +typedef struct RobotModelC { + Matrix3x4C wheel_transform_mat; + Matrix4x3C wheel_transform_mat_inv; + float wheel_radius; + Matrix3C body_inirtia; + Matrix3C body_inirtia_inv; +} RobotModelC_t; + +typedef struct BangBangTraj1D { + float sdd1; + float sdd2; + float sdd3; + float t1; + float t2; + float t3; + float t4; +} BangBangTraj1D_t; + +typedef struct BangBangTraj3D { + BangBangTraj1D_t x_traj; + BangBangTraj1D_t y_traj; + BangBangTraj1D_t z_traj; +} BangBangTraj3D_t; + +uint64_t ateam_controls_add(uint64_t left, uint64_t right); +Vector3C_t ateam_controls_transform_frame_global2robot_pose(Vector3C_t robot_pose, Vector3C_t pose); +Vector3C_t ateam_controls_transform_frame_robot2global_pose(Vector3C_t robot_pose, Vector3C_t pose); +Vector3C_t ateam_controls_transform_frame_global2robot_twist(Vector3C_t robot_pose, Vector3C_t twist); +Vector3C_t ateam_controls_transform_frame_robot2global_twist(Vector3C_t robot_pose, Vector3C_t twist); +Vector3C_t ateam_controls_transform_frame_global2robot_accel(Vector3C_t robot_pose, Vector3C_t accel); +Vector3C_t ateam_controls_transform_frame_robot2global_accel(Vector3C_t robot_pose, Vector3C_t accel); +RobotModelC_t ateam_controls_robot_model_new_from_constants(); +Vector3C_t ateam_controls_robot_model_wheel_velocities_to_twist(RobotModelC_t robot_model, Vector4C wheel_velocities); +Vector4C ateam_controls_robot_model_twist_to_wheel_velocities(RobotModelC_t robot_model, Vector3C_t twist); +Vector3C_t ateam_controls_robot_model_wheel_torques_to_accel(RobotModelC_t robot_model, Vector4C torques); +Vector4C ateam_controls_robot_model_accel_to_wheel_torques(RobotModelC_t robot_model, Vector3C_t accel); +void ateam_controls_bangbang_traj_1d_time_shift(BangBangTraj1D_t* traj, float dt); +void ateam_controls_bangbang_traj_3d_time_shift(BangBangTraj3D_t* traj, float dt); +float ateam_controls_bangbang_traj_3d_get_end_time(BangBangTraj3D_t traj); +BangBangTraj3D_t ateam_controls_compute_optimal_bangbang_traj_3d(RigidBodyStateC_t init_state, Vector3C_t target_pose); +RigidBodyStateC_t ateam_controls_compute_bangbang_traj_3d_state_at_t(BangBangTraj3D_t traj, RigidBodyStateC_t current_state, float current_time, float t); +Vector3C_t ateam_controls_compute_bangbang_traj_3d_accel_at_t(BangBangTraj3D_t traj, float t); +float ateam_controls_compute_bangbang_traj_1d_accel_at_t(BangBangTraj1D_t traj, float t); + +} + +#endif // ATEAM_CONTROLS_C__ATEAM_CONTROLS_H__ diff --git a/ateam-controls-c/src/lib.rs b/ateam-controls-c/src/lib.rs index 75ba659..cdbfb47 100644 --- a/ateam-controls-c/src/lib.rs +++ b/ateam-controls-c/src/lib.rs @@ -1,6 +1,100 @@ use ateam_controls; +use ateam_controls::ctypes::*; +use ateam_controls::bangbang_trajectory::*; +use ateam_controls::robot_model::*; + #[unsafe(no_mangle)] pub extern "C" fn ateam_controls_add(left: u64, right: u64) -> u64 { ateam_controls::add(left, right) } + +#[unsafe(no_mangle)] +pub extern "C" fn ateam_controls_transform_frame_global2robot_pose(robot_pose: Vector3C, pose: Vector3C) -> Vector3C { + transform_frame_global2robot_pose(robot_pose.into(), pose.into()).into() +} + +#[unsafe(no_mangle)] +pub extern "C" fn ateam_controls_transform_frame_robot2global_pose(robot_pose: Vector3C, pose: Vector3C) -> Vector3C { + transform_frame_robot2global_pose(robot_pose.into(), pose.into()).into() +} + +#[unsafe(no_mangle)] +pub extern "C" fn ateam_controls_transform_frame_global2robot_twist(robot_pose: Vector3C, twist: Vector3C) -> Vector3C { + transform_frame_global2robot_twist(robot_pose.into(), twist.into()).into() +} + +#[unsafe(no_mangle)] +pub extern "C" fn ateam_controls_transform_frame_robot2global_twist(robot_pose: Vector3C, twist: Vector3C) -> Vector3C { + transform_frame_robot2global_twist(robot_pose.into(), twist.into()).into() +} + +#[unsafe(no_mangle)] +pub extern "C" fn ateam_controls_transform_frame_global2robot_accel(robot_pose: Vector3C, accel: Vector3C) -> Vector3C { + transform_frame_global2robot_accel(robot_pose.into(), accel.into()).into() +} + +#[unsafe(no_mangle)] +pub extern "C" fn ateam_controls_transform_frame_robot2global_accel(robot_pose: Vector3C, accel: Vector3C) -> Vector3C { + transform_frame_robot2global_accel(robot_pose.into(), accel.into()).into() +} + +#[unsafe(no_mangle)] +pub extern "C" fn ateam_controls_robot_model_new_from_constants() -> RobotModelC { + RobotModel::new_from_constants().into() +} + +#[unsafe(no_mangle)] +pub extern "C" fn ateam_controls_robot_model_wheel_velocities_to_twist(robot_model: RobotModelC, wheel_velocities: Vector4C) -> Vector3C { + RobotModel::from(robot_model).wheel_velocities_to_twist(wheel_velocities.into()).into() +} + +#[unsafe(no_mangle)] +pub extern "C" fn ateam_controls_robot_model_twist_to_wheel_velocities(robot_model: RobotModelC, twist: Vector3C) -> Vector4C { + RobotModel::from(robot_model).twist_to_wheel_velocities(twist.into()).into() +} + +#[unsafe(no_mangle)] +pub extern "C" fn ateam_controls_robot_model_wheel_torques_to_accel(robot_model: RobotModelC, torques: Vector4C) -> Vector3C { + RobotModel::from(robot_model).wheel_torques_to_accel(torques.into()).into() +} + +#[unsafe(no_mangle)] +pub extern "C" fn ateam_controls_robot_model_accel_to_wheel_torques(robot_model: RobotModelC, accel: Vector3C) -> Vector4C { + RobotModel::from(robot_model).accel_to_wheel_torques(accel.into()).into() +} + +#[unsafe(no_mangle)] +pub extern "C" fn ateam_controls_bangbang_traj_1d_time_shift(traj: *mut BangBangTraj1D, dt: f32) { + unsafe {(&mut *traj).time_shift(dt)}; +} + +#[unsafe(no_mangle)] +pub extern "C" fn ateam_controls_bangbang_traj_3d_time_shift(traj: *mut BangBangTraj3D, dt: f32) { + unsafe {(&mut *traj).time_shift(dt)}; +} + +#[unsafe(no_mangle)] +pub extern "C" fn ateam_controls_bangbang_traj_3d_get_end_time(traj: BangBangTraj3D) -> f32 { + traj.get_end_time() +} + +#[unsafe(no_mangle)] +pub extern "C" fn ateam_controls_compute_optimal_bangbang_traj_3d(init_state: RigidBodyStateC, target_pose: Vector3C) -> BangBangTraj3D { + compute_optimal_bangbang_traj_3d(init_state.into(), target_pose.into()) +} + +#[unsafe(no_mangle)] +pub extern "C" fn ateam_controls_compute_bangbang_traj_3d_state_at_t(traj: BangBangTraj3D, current_state: RigidBodyStateC, current_time: f32, t: f32) -> RigidBodyStateC { + compute_bangbang_traj_3d_state_at_t(traj, current_state.into(), current_time, t).into() +} + +#[unsafe(no_mangle)] +pub extern "C" fn ateam_controls_compute_bangbang_traj_3d_accel_at_t(traj: BangBangTraj3D, t: f32) -> Vector3C { + compute_bangbang_traj_3d_accel_at_t(traj, t).into() +} + +#[unsafe(no_mangle)] +pub extern "C" fn ateam_controls_compute_bangbang_traj_1d_accel_at_t(traj: BangBangTraj1D, t: f32) -> f32 { + compute_bangbang_traj_1d_accel_at_t(traj, t) +} diff --git a/ateam-controls/src/ctypes.rs b/ateam-controls/src/ctypes.rs new file mode 100644 index 0000000..1748f3c --- /dev/null +++ b/ateam-controls/src/ctypes.rs @@ -0,0 +1,223 @@ +use nalgebra::{Vector3, Vector4, Matrix3, Matrix3x4, Matrix4x3}; +use crate::{RigidBodyState, robot_model::RobotModel}; + +#[repr(C)] +#[derive(Clone, Copy, Default, Debug)] +pub struct Vector3C { + pub x: f32, + pub y: f32, + pub z: f32, +} + +impl From> for Vector3C { + fn from(v: Vector3) -> Self { + Vector3C {x: v.x, y: v.y, z: v.z} + } +} + +impl From for Vector3 { + fn from(v: Vector3C) -> Self { + Self::new(v.x, v.y, v.z) + } +} + +#[repr(C)] +#[derive(Clone, Copy, Default, Debug)] +pub struct Vector4C { + pub x: f32, + pub y: f32, + pub z: f32, + pub w: f32, +} + +impl From> for Vector4C { + fn from(v: Vector4) -> Self { + Vector4C {x: v.x, y: v.y, z: v.z, w: v.w} + } +} + +impl From for Vector4 { + fn from(v: Vector4C) -> Self { + Self::new(v.x, v.y, v.z, v.w) + } +} + +#[repr(C)] +#[derive(Clone, Copy, Default, Debug)] +pub struct Matrix3C { + pub data: [f32; 9], +} + +impl Matrix3C { + /// Create a new Matrix3C from a flat array in column-major order + pub fn new(data: [f32; 9]) -> Self { + Matrix3C { data } + } + + pub fn get(&self, row: usize, col: usize) -> f32 { + self.data[col * 3 + row] + } + + pub fn set(&mut self, row: usize, col: usize, value: f32) { + self.data[col * 3 + row] = value; + } +} + +impl From> for Matrix3C { + fn from(mat: Matrix3) -> Self { + let mut data = [0.0f32; 9]; + for i in 0..3 { + for j in 0..3 { + data[j * 3 + i] = mat[(i, j)]; + } + } + Matrix3C { data } + } +} + +impl From for Matrix3 { + fn from(mat: Matrix3C) -> Self { + Matrix3::from_column_slice(&mat.data) + } +} + + +#[repr(C)] +#[derive(Clone, Copy, Default, Debug)] +pub struct Matrix3x4C { + pub data: [f32; 12], +} + +impl Matrix3x4C { + /// Create a new Matrix3x4C from a flat array in column-major order + pub fn new(data: [f32; 12]) -> Self { + Matrix3x4C { data } + } + + pub fn get(&self, row: usize, col: usize) -> f32 { + // 3 rows, 4 cols + self.data[col * 3 + row] + } + + pub fn set(&mut self, row: usize, col: usize, value: f32) { + self.data[col * 3 + row] = value; + } +} + +impl From> for Matrix3x4C { + fn from(mat: Matrix3x4) -> Self { + let mut data = [0.0f32; 12]; + for i in 0..3 { + for j in 0..4 { + data[j * 3 + i] = mat[(i, j)]; + } + } + Matrix3x4C { data } + } +} + +impl From for Matrix3x4 { + fn from(mat: Matrix3x4C) -> Self { + Matrix3x4::from_column_slice(&mat.data) + } +} + + +#[repr(C)] +#[derive(Clone, Copy, Default, Debug)] +pub struct Matrix4x3C { + pub data: [f32; 12], +} + +impl Matrix4x3C { + /// Create a new Matrix4x3C from a flat array in column-major order + pub fn new(data: [f32; 12]) -> Self { + Matrix4x3C { data } + } + + pub fn get(&self, row: usize, col: usize) -> f32 { + // 4 rows, 3 cols + self.data[col * 4 + row] + } + + pub fn set(&mut self, row: usize, col: usize, value: f32) { + self.data[col * 4 + row] = value; + } +} + +impl From> for Matrix4x3C { + fn from(mat: Matrix4x3) -> Self { + let mut data = [0.0f32; 12]; + for i in 0..4 { + for j in 0..3 { + data[j * 4 + i] = mat[(i, j)]; + } + } + Matrix4x3C { data } + } +} + +impl From for Matrix4x3 { + fn from(mat: Matrix4x3C) -> Self { + Matrix4x3::from_column_slice(&mat.data) + } +} + +#[repr(C)] +#[derive(Clone, Copy, Default, Debug)] +pub struct RigidBodyStateC { + pub pose: Vector3C, + pub twist: Vector3C, +} + +impl From for RigidBodyStateC { + fn from(state: RigidBodyState) -> Self { + Self { + pose: state.pose.into(), + twist: state.twist.into(), + } + } +} + +impl From for RigidBodyState { + fn from(state: RigidBodyStateC) -> Self { + Self { + pose: state.pose.into(), + twist: state.twist.into(), + } + } +} + +#[repr(C)] +#[derive(Clone, Copy, Default, Debug)] +pub struct RobotModelC { + wheel_transform_mat: Matrix3x4C, + wheel_transform_mat_inv: Matrix4x3C, + wheel_radius: f32, + body_inirtia: Matrix3C, + body_inirtia_inv: Matrix3C, +} + +impl From for RobotModelC { + fn from(robot_model: RobotModel) -> Self { + Self { + wheel_transform_mat: robot_model.wheel_transform_mat.into(), + wheel_transform_mat_inv: robot_model.wheel_transform_mat_inv.into(), + wheel_radius: robot_model.wheel_radius.into(), + body_inirtia: robot_model.body_inirtia.into(), + body_inirtia_inv: robot_model.body_inirtia_inv.into(), + } + } +} + +impl From for RobotModel { + fn from(robot_model: RobotModelC) -> Self { + Self { + wheel_transform_mat: robot_model.wheel_transform_mat.into(), + wheel_transform_mat_inv: robot_model.wheel_transform_mat_inv.into(), + wheel_radius: robot_model.wheel_radius.into(), + body_inirtia: robot_model.body_inirtia.into(), + body_inirtia_inv: robot_model.body_inirtia_inv.into(), + } + } +} \ No newline at end of file diff --git a/cpp_tests/CMakeLists.txt b/cpp_tests/CMakeLists.txt index 9fe502d..7672d28 100644 --- a/cpp_tests/CMakeLists.txt +++ b/cpp_tests/CMakeLists.txt @@ -4,6 +4,8 @@ include(GoogleTest) add_executable( cpp_tests test_add.cpp + test_bangbang.cpp + test_robot_model.cpp ) target_link_libraries( cpp_tests diff --git a/cpp_tests/test_bangbang.cpp b/cpp_tests/test_bangbang.cpp new file mode 100644 index 0000000..b53f4d4 --- /dev/null +++ b/cpp_tests/test_bangbang.cpp @@ -0,0 +1,111 @@ +#include +#include + +TEST(AteamControls, BangBangX) { + RigidBodyStateC_t init_state = { + .pose = {0.0f, 0.0f, 0.0f}, + .twist = {0.0f, 0.0f, 0.0f} + }; + Vector3C_t target_pose = { + .x = 1.0f, + .y = 0.0f, + .z = 0.0f + }; + BangBangTraj3D_t traj = ateam_controls_compute_optimal_bangbang_traj_3d(init_state, target_pose); + float end_time = ateam_controls_bangbang_traj_3d_get_end_time(traj); + RigidBodyStateC_t final_state = ateam_controls_compute_bangbang_traj_3d_state_at_t(traj, init_state, 0.0f, end_time); + EXPECT_NEAR(final_state.pose.x, target_pose.x, 1e-6f); + EXPECT_NEAR(final_state.pose.y, target_pose.y, 1e-6f); + EXPECT_NEAR(final_state.pose.z, target_pose.z, 1e-6f); +} + +// Test Y direction bang-bang trajectory achieves the target pose +TEST(AteamControls, BangBangY) { + RigidBodyStateC_t init_state = { + .pose = {0.0f, 0.0f, 0.0f}, + .twist = {0.0f, 0.0f, 0.0f} + }; + Vector3C_t target_pose = { + .x = 0.0f, + .y = 1.0f, + .z = 0.0f + }; + BangBangTraj3D_t traj = ateam_controls_compute_optimal_bangbang_traj_3d(init_state, target_pose); + float end_time = ateam_controls_bangbang_traj_3d_get_end_time(traj); + RigidBodyStateC_t final_state = ateam_controls_compute_bangbang_traj_3d_state_at_t(traj, init_state, 0.0f, end_time); + EXPECT_NEAR(final_state.pose.x, target_pose.x, 1e-6f); + EXPECT_NEAR(final_state.pose.y, target_pose.y, 1e-6f); + EXPECT_NEAR(final_state.pose.z, target_pose.z, 1e-6f); +} + +// Test Z direction bang-bang trajectory meets the target pose +TEST(AteamControls, BangBangZ) { + RigidBodyStateC_t init_state = { + .pose = {0.0f, 0.0f, 0.0f}, + .twist = {0.0f, 0.0f, 0.0f} + }; + Vector3C_t target_pose = { + .x = 0.0f, + .y = 0.0f, + .z = 1.0f + }; + BangBangTraj3D_t traj = ateam_controls_compute_optimal_bangbang_traj_3d(init_state, target_pose); + float end_time = ateam_controls_bangbang_traj_3d_get_end_time(traj); + RigidBodyStateC_t final_state = ateam_controls_compute_bangbang_traj_3d_state_at_t(traj, init_state, 0.0f, end_time); + EXPECT_NEAR(final_state.pose.x, target_pose.x, 1e-6f); + EXPECT_NEAR(final_state.pose.y, target_pose.y, 1e-6f); + EXPECT_NEAR(final_state.pose.z, target_pose.z, 1e-6f); +} + +// Test symmetric XYZ bang-bang trajectory meets the target pose +TEST(AteamControls, BangBangXYZ) { + RigidBodyStateC_t init_state = { + .pose = {0.0f, 0.0f, 0.0f}, + .twist = {0.0f, 0.0f, 0.0f} + }; + Vector3C_t target_pose = { + .x = 1.0f, + .y = 1.0f, + .z = 1.0f + }; + BangBangTraj3D_t traj = ateam_controls_compute_optimal_bangbang_traj_3d(init_state, target_pose); + float end_time = ateam_controls_bangbang_traj_3d_get_end_time(traj); + RigidBodyStateC_t final_state = ateam_controls_compute_bangbang_traj_3d_state_at_t(traj, init_state, 0.0f, end_time); + EXPECT_NEAR(final_state.pose.x, target_pose.x, 1e-6f); + EXPECT_NEAR(final_state.pose.y, target_pose.y, 1e-6f); + EXPECT_NEAR(final_state.pose.z, target_pose.z, 1e-6f); +} + +// Test that the initial and final accelerations are equal and opposite +TEST(AteamControls, BangBangAcceleration) { + RigidBodyStateC_t init_state = { + .pose = {0.0f, 0.0f, 0.0f}, + .twist = {0.0f, 0.0f, 0.0f} + }; + Vector3C_t target_pose = { + .x = 1.0f, + .y = 0.0f, + .z = 1.0f + }; + BangBangTraj3D_t traj = ateam_controls_compute_optimal_bangbang_traj_3d(init_state, target_pose); + EXPECT_NEAR(traj.x_traj.sdd1, -traj.x_traj.sdd3, 1e-6f); +} + +// Test that time shifting a trajectory works +TEST(AteamControls, BangBangTimeShift) { + RigidBodyStateC_t init_state = { + .pose = {0.0f, 0.0f, 0.0f}, + .twist = {0.0f, 0.0f, 0.0f} + }; + Vector3C_t target_pose = { + .x = 1.0f, + .y = 0.0f, + .z = 0.0f + }; + BangBangTraj3D_t traj = ateam_controls_compute_optimal_bangbang_traj_3d(init_state, target_pose); + float shift_amount = 1.0f; + float initial_end_time = ateam_controls_bangbang_traj_3d_get_end_time(traj); + ateam_controls_bangbang_traj_3d_time_shift(&traj, shift_amount); + float shifted_end_time = ateam_controls_bangbang_traj_3d_get_end_time(traj); + EXPECT_NEAR(shifted_end_time - initial_end_time, shift_amount, 1e-6f); +} diff --git a/cpp_tests/test_robot_model.cpp b/cpp_tests/test_robot_model.cpp new file mode 100644 index 0000000..67c566d --- /dev/null +++ b/cpp_tests/test_robot_model.cpp @@ -0,0 +1,134 @@ +#include +#include +#include + +// Test global to robot pose transform at origin with no rotation +TEST(RobotModel, TransformGlobal2RobotPoseAtOrigin) { + Vector3C_t robot_pose = {0.0f, 0.0f, 0.0f}; + Vector3C_t global_pose = {1.0f, 0.0f, 0.0f}; + Vector3C_t robot_frame_pose = ateam_controls_transform_frame_global2robot_pose(robot_pose, global_pose); + EXPECT_NEAR(robot_frame_pose.x, 1.0f, 1e-6f); + EXPECT_NEAR(robot_frame_pose.y, 0.0f, 1e-6f); + EXPECT_NEAR(robot_frame_pose.z, 0.0f, 1e-6f); +} + +// Test robot to global pose transform at origin with no rotation +TEST(RobotModel, TransformRobot2GlobalPoseAtOrigin) { + Vector3C_t robot_pose = {0.0f, 0.0f, 0.0f}; + Vector3C_t robot_frame_pose = {1.0f, 0.0f, 0.0f}; + Vector3C_t global_pose = ateam_controls_transform_frame_robot2global_pose(robot_pose, robot_frame_pose); + EXPECT_NEAR(global_pose.x, 1.0f, 1e-6f); + EXPECT_NEAR(global_pose.y, 0.0f, 1e-6f); + EXPECT_NEAR(global_pose.z, 0.0f, 1e-6f); +} + +// Test that pose transforms are inverses of each other +TEST(RobotModel, PoseTransformsAreInverses) { + Vector3C_t robot_pose = {1.0f, 2.0f, 0.0f}; + Vector3C_t global_pose = {3.0f, 4.0f, 0.0f}; + + Vector3C_t to_robot = ateam_controls_transform_frame_global2robot_pose(robot_pose, global_pose); + Vector3C_t back_to_global = ateam_controls_transform_frame_robot2global_pose(robot_pose, to_robot); + + EXPECT_NEAR(back_to_global.x, global_pose.x, 1e-5f); + EXPECT_NEAR(back_to_global.y, global_pose.y, 1e-5f); + EXPECT_NEAR(back_to_global.z, global_pose.z, 1e-5f); +} + +// Test global to robot twist transform at origin with no rotation +TEST(RobotModel, TransformGlobal2RobotTwistNoRotation) { + Vector3C_t robot_pose = {0.0f, 0.0f, 0.0f}; + Vector3C_t global_twist = {1.0f, 0.0f, 0.0f}; + Vector3C_t robot_twist = ateam_controls_transform_frame_global2robot_twist(robot_pose, global_twist); + EXPECT_NEAR(robot_twist.x, 1.0f, 1e-6f); + EXPECT_NEAR(robot_twist.y, 0.0f, 1e-6f); + EXPECT_NEAR(robot_twist.z, 0.0f, 1e-6f); +} + +// Test robot to global twist transform at origin with no rotation +TEST(RobotModel, TransformRobot2GlobalTwistNoRotation) { + Vector3C_t robot_pose = {0.0f, 0.0f, 0.0f}; + Vector3C_t robot_twist = {1.0f, 0.0f, 0.0f}; + Vector3C_t global_twist = ateam_controls_transform_frame_robot2global_twist(robot_pose, robot_twist); + EXPECT_NEAR(global_twist.x, 1.0f, 1e-6f); + EXPECT_NEAR(global_twist.y, 0.0f, 1e-6f); + EXPECT_NEAR(global_twist.z, 0.0f, 1e-6f); +} + +// Test that twist transforms are inverses of each other +TEST(RobotModel, TwistTransformsAreInverses) { + Vector3C_t robot_pose = {0.0f, 0.0f, 0.0f}; + Vector3C_t global_twist = {1.0f, 2.0f, 0.0f}; + + Vector3C_t to_robot = ateam_controls_transform_frame_global2robot_twist(robot_pose, global_twist); + Vector3C_t back_to_global = ateam_controls_transform_frame_robot2global_twist(robot_pose, to_robot); + + EXPECT_NEAR(back_to_global.x, global_twist.x, 1e-5f); + EXPECT_NEAR(back_to_global.y, global_twist.y, 1e-5f); + EXPECT_NEAR(back_to_global.z, global_twist.z, 1e-5f); +} + +// Test global to robot accel transform at origin with no rotation +TEST(RobotModel, TransformGlobal2RobotAccelNoRotation) { + Vector3C_t robot_pose = {0.0f, 0.0f, 0.0f}; + Vector3C_t global_accel = {1.0f, 0.0f, 0.0f}; + Vector3C_t robot_accel = ateam_controls_transform_frame_global2robot_accel(robot_pose, global_accel); + EXPECT_NEAR(robot_accel.x, 1.0f, 1e-6f); + EXPECT_NEAR(robot_accel.y, 0.0f, 1e-6f); + EXPECT_NEAR(robot_accel.z, 0.0f, 1e-6f); +} + +// Test robot to global accel transform at origin with no rotation +TEST(RobotModel, TransformRobot2GlobalAccelNoRotation) { + Vector3C_t robot_pose = {0.0f, 0.0f, 0.0f}; + Vector3C_t robot_accel = {1.0f, 0.0f, 0.0f}; + Vector3C_t global_accel = ateam_controls_transform_frame_robot2global_accel(robot_pose, robot_accel); + EXPECT_NEAR(global_accel.x, 1.0f, 1e-6f); + EXPECT_NEAR(global_accel.y, 0.0f, 1e-6f); + EXPECT_NEAR(global_accel.z, 0.0f, 1e-6f); +} + +// Test that accel transforms are inverses of each other +TEST(RobotModel, AccelTransformsAreInverses) { + Vector3C_t robot_pose = {0.0f, 0.0f, 0.0f}; + Vector3C_t global_accel = {1.0f, 2.0f, 0.0f}; + + Vector3C_t to_robot = ateam_controls_transform_frame_global2robot_accel(robot_pose, global_accel); + Vector3C_t back_to_global = ateam_controls_transform_frame_robot2global_accel(robot_pose, to_robot); + + EXPECT_NEAR(back_to_global.x, global_accel.x, 1e-5f); + EXPECT_NEAR(back_to_global.y, global_accel.y, 1e-5f); + EXPECT_NEAR(back_to_global.z, global_accel.z, 1e-5f); +} + +// Test robot model initialization +TEST(RobotModel, RobotModelNewFromConstants) { + RobotModelC_t robot_model = ateam_controls_robot_model_new_from_constants(); + EXPECT_GT(robot_model.wheel_radius, 0.0f); +} + +// Test wheel velocities to twist conversion and back +TEST(RobotModel, WheelVelocitiesToTwistAndBack) { + RobotModelC_t robot_model = ateam_controls_robot_model_new_from_constants(); + Vector3C_t twist = {1.0f, 0.0f, 0.0f}; + + Vector4C wheel_velocities = ateam_controls_robot_model_twist_to_wheel_velocities(robot_model, twist); + Vector3C_t twist_back = ateam_controls_robot_model_wheel_velocities_to_twist(robot_model, wheel_velocities); + + EXPECT_NEAR(twist_back.x, twist.x, 1e-4f); + EXPECT_NEAR(twist_back.y, twist.y, 1e-4f); + EXPECT_NEAR(twist_back.z, twist.z, 1e-4f); +} + +// Test accel to wheel torques conversion and back +TEST(RobotModel, AccelToWheelTorquesAndBack) { + RobotModelC_t robot_model = ateam_controls_robot_model_new_from_constants(); + Vector3C_t accel = {1.0f, 0.0f, 0.0f}; + + Vector4C torques = ateam_controls_robot_model_accel_to_wheel_torques(robot_model, accel); + Vector3C_t accel_back = ateam_controls_robot_model_wheel_torques_to_accel(robot_model, torques); + + EXPECT_NEAR(accel_back.x, accel.x, 1e-4f); + EXPECT_NEAR(accel_back.y, accel.y, 1e-4f); + EXPECT_NEAR(accel_back.z, accel.z, 1e-4f); +} From f05df10e2a1a5d45ae56c760bf003c3d97bf9a26 Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Sun, 28 Dec 2025 15:29:33 -0500 Subject: [PATCH 6/6] wheel radius in twist calculation and move python visualization script --- analysis/{ => scripts}/trajectory_sim_visualize.py | 4 ++-- ateam-controls/src/robot_model.rs | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) rename analysis/{ => scripts}/trajectory_sim_visualize.py (97%) diff --git a/analysis/trajectory_sim_visualize.py b/analysis/scripts/trajectory_sim_visualize.py similarity index 97% rename from analysis/trajectory_sim_visualize.py rename to analysis/scripts/trajectory_sim_visualize.py index 532d847..0074ef8 100644 --- a/analysis/trajectory_sim_visualize.py +++ b/analysis/scripts/trajectory_sim_visualize.py @@ -121,7 +121,7 @@ def update(i): if __name__ == "__main__": # Load simulation data - # data = pd.read_csv(sys.stdin) - data = pd.read_csv('sim_data.csv') # For testing purposes + data = pd.read_csv(sys.stdin) + # data = pd.read_csv('sim_data.csv') # For testing purposes create_plots() create_animation() diff --git a/ateam-controls/src/robot_model.rs b/ateam-controls/src/robot_model.rs index d8f9b02..38d222c 100644 --- a/ateam-controls/src/robot_model.rs +++ b/ateam-controls/src/robot_model.rs @@ -110,12 +110,12 @@ impl RobotModel { /// Calculate robot frame twist from wheel velocities pub fn wheel_velocities_to_twist(&self, wheel_velocities: Vector4f) -> Vector3f { - self.wheel_transform_mat_inv.transpose() * wheel_velocities + self.wheel_radius * self.wheel_transform_mat_inv.transpose() * wheel_velocities } /// Calculate wheel velocities from robot frame twist pub fn twist_to_wheel_velocities(&self, twist: Vector3f) -> Vector4f { - self.wheel_transform_mat.transpose() * twist + self.wheel_transform_mat.transpose() * twist / self.wheel_radius } /// Calculate robot frame accel from wheel torques