From 10e2ea3f0f3db20eb885beb06d54bf8ed6144811 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 14 Jul 2024 11:19:14 -0700 Subject: [PATCH] Remove common fields from TrajoptLib swerve module (#531) --- src-tauri/src/main.rs | 25 ++++-------- trajoptlib/examples/Swerve/src/Main.cpp | 8 ++-- trajoptlib/examples/swerve.rs | 37 ++++------------- .../trajopt/drivetrain/SwerveDrivetrain.hpp | 22 +++++++--- .../trajopt/drivetrain/SwerveModule.hpp | 31 -------------- trajoptlib/src/RustFFI.cpp | 13 +++--- trajoptlib/src/SwerveTrajectoryGenerator.cpp | 40 +++++++++---------- trajoptlib/src/lib.rs | 12 +++--- trajoptlib/test/src/ObstacleTest.cpp | 13 +++--- 9 files changed, 75 insertions(+), 126 deletions(-) delete mode 100644 trajoptlib/include/trajopt/drivetrain/SwerveModule.hpp diff --git a/src-tauri/src/main.rs b/src-tauri/src/main.rs index 3634973a72..dc540aa71b 100644 --- a/src-tauri/src/main.rs +++ b/src-tauri/src/main.rs @@ -11,7 +11,7 @@ use tauri::{ api::{dialog::blocking::FileDialogBuilder, file}, Manager, }; -use trajoptlib::{HolonomicTrajectory, Pose2d, SwerveDrivetrain, SwerveModule, SwervePathBuilder}; +use trajoptlib::{HolonomicTrajectory, Pose2d, SwerveDrivetrain, SwervePathBuilder, Translation2d}; #[derive(Clone, serde::Serialize, Debug)] struct OpenFileEventPayload<'a> { @@ -410,34 +410,25 @@ async fn generate_trajectory( let drivetrain = SwerveDrivetrain { mass: config.mass, moi: config.rotationalInertia, + wheel_radius: config.wheelRadius, + wheel_max_angular_velocity: config.wheelMaxVelocity, + wheel_max_torque: config.wheelMaxTorque, modules: vec![ - SwerveModule { + Translation2d { x: half_wheel_base, y: half_track_width, - wheel_radius: config.wheelRadius, - wheel_max_angular_velocity: config.wheelMaxVelocity, - wheel_max_torque: config.wheelMaxTorque, }, - SwerveModule { + Translation2d { x: half_wheel_base, y: -half_track_width, - wheel_radius: config.wheelRadius, - wheel_max_angular_velocity: config.wheelMaxVelocity, - wheel_max_torque: config.wheelMaxTorque, }, - SwerveModule { + Translation2d { x: -half_wheel_base, y: half_track_width, - wheel_radius: config.wheelRadius, - wheel_max_angular_velocity: config.wheelMaxVelocity, - wheel_max_torque: config.wheelMaxTorque, }, - SwerveModule { + Translation2d { x: -half_wheel_base, y: -half_track_width, - wheel_radius: config.wheelRadius, - wheel_max_angular_velocity: config.wheelMaxVelocity, - wheel_max_torque: config.wheelMaxTorque, }, ], }; diff --git a/trajoptlib/examples/Swerve/src/Main.cpp b/trajoptlib/examples/Swerve/src/Main.cpp index b73de7d407..c43f959789 100644 --- a/trajoptlib/examples/Swerve/src/Main.cpp +++ b/trajoptlib/examples/Swerve/src/Main.cpp @@ -17,10 +17,10 @@ int main() { trajopt::SwerveDrivetrain swerveDrivetrain{ .mass = 45, .moi = 6, - .modules = {{{+0.6, +0.6}, 0.04, 70, 2}, - {{+0.6, -0.6}, 0.04, 70, 2}, - {{-0.6, +0.6}, 0.04, 70, 2}, - {{-0.6, -0.6}, 0.04, 70, 2}}}; + .wheelRadius = 0.04, + .wheelMaxAngularVelocity = 70, + .wheelMaxTorque = 2, + .modules = {{+0.6, +0.6}, {+0.6, -0.6}, {-0.6, +0.6}, {-0.6, -0.6}}}; trajopt::LinearVelocityMaxMagnitudeConstraint zeroLinearVelocity{0.0}; trajopt::AngularVelocityMaxMagnitudeConstraint zeroAngularVelocity{0.0}; diff --git a/trajoptlib/examples/swerve.rs b/trajoptlib/examples/swerve.rs index 92a98fa739..3086626e87 100644 --- a/trajoptlib/examples/swerve.rs +++ b/trajoptlib/examples/swerve.rs @@ -1,38 +1,17 @@ -use trajoptlib::{SwerveDrivetrain, SwerveModule, SwervePathBuilder}; +use trajoptlib::{SwerveDrivetrain, SwervePathBuilder, Translation2d}; fn main() { let drivetrain = SwerveDrivetrain { mass: 45.0, moi: 6.0, + wheel_radius: 0.04, + wheel_max_angular_velocity: 70.0, + wheel_max_torque: 2.0, modules: vec![ - SwerveModule { - x: 0.6, - y: 0.6, - wheel_radius: 0.04, - wheel_max_angular_velocity: 70.0, - wheel_max_torque: 2.0, - }, - SwerveModule { - x: 0.6, - y: -0.6, - wheel_radius: 0.04, - wheel_max_angular_velocity: 70.0, - wheel_max_torque: 2.0, - }, - SwerveModule { - x: -0.6, - y: 0.6, - wheel_radius: 0.04, - wheel_max_angular_velocity: 70.0, - wheel_max_torque: 2.0, - }, - SwerveModule { - x: -0.6, - y: -0.6, - wheel_radius: 0.04, - wheel_max_angular_velocity: 70.0, - wheel_max_torque: 2.0, - }, + Translation2d { x: 0.6, y: 0.6 }, + Translation2d { x: 0.6, y: -0.6 }, + Translation2d { x: -0.6, y: 0.6 }, + Translation2d { x: -0.6, y: -0.6 }, ], }; diff --git a/trajoptlib/include/trajopt/drivetrain/SwerveDrivetrain.hpp b/trajoptlib/include/trajopt/drivetrain/SwerveDrivetrain.hpp index 720570788c..26c5eb5efd 100644 --- a/trajoptlib/include/trajopt/drivetrain/SwerveDrivetrain.hpp +++ b/trajoptlib/include/trajopt/drivetrain/SwerveDrivetrain.hpp @@ -4,7 +4,7 @@ #include -#include "trajopt/drivetrain/SwerveModule.hpp" +#include "trajopt/geometry/Translation2.hpp" #include "trajopt/util/SymbolExports.hpp" namespace trajopt { @@ -16,13 +16,25 @@ namespace trajopt { * be four. The order the swerve modules are listed does not matter. */ struct TRAJOPT_DLLEXPORT SwerveDrivetrain { - /// the mass of the robot (kg) + /// The mass of the robot (kg). double mass; - /// the moment of inertia of the robot about the origin (kg−m²) + + /// The moment of inertia of the robot about the origin (kg−m²). double moi; - /// The list of swerve modules that make the robot move, usually one in each + + /// Radius of wheel (meters). + double wheelRadius; + + /// Maximum angular velocity of wheel (rad/s). + double wheelMaxAngularVelocity; + + /// Maximum torque (N−m) applied to wheel. + double wheelMaxTorque; + + /// Translation (meters) of each swerve module from the origin of the robot + /// coordinate system to the center of the module. There's usually one in each /// corner. - std::vector modules; + std::vector modules; }; } // namespace trajopt diff --git a/trajoptlib/include/trajopt/drivetrain/SwerveModule.hpp b/trajoptlib/include/trajopt/drivetrain/SwerveModule.hpp deleted file mode 100644 index 0a7bc87659..0000000000 --- a/trajoptlib/include/trajopt/drivetrain/SwerveModule.hpp +++ /dev/null @@ -1,31 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include "trajopt/geometry/Translation2.hpp" -#include "trajopt/util/SymbolExports.hpp" - -namespace trajopt { - -/** - * This class represents a single swerve module in a swerve drivetrain. - * - * It is defined by the module diagonal, which is the line connecting the origin - * of the robot coordinate system to the center of the module. The wheel radius, - * max speed, and max torque must also be specified per module. - */ -struct TRAJOPT_DLLEXPORT SwerveModule { - /// Translation (meters) of swerve module in robot frame. - Translation2d translation; - - /// Radius of wheel (meters). - double wheelRadius; - - /// Maximum angular velocity of wheel (rad/s). - double wheelMaxAngularVelocity; - - /// Maximum torque (N−m) applied to wheel. - double wheelMaxTorque; -}; - -} // namespace trajopt diff --git a/trajoptlib/src/RustFFI.cpp b/trajoptlib/src/RustFFI.cpp index b8d3fcafba..6fd084aa21 100644 --- a/trajoptlib/src/RustFFI.cpp +++ b/trajoptlib/src/RustFFI.cpp @@ -14,7 +14,6 @@ #include "trajopt/constraint/LinearVelocityDirectionConstraint.hpp" #include "trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp" #include "trajopt/constraint/PointAtConstraint.hpp" -#include "trajopt/drivetrain/SwerveModule.hpp" #include "trajopt/trajectory/HolonomicTrajectory.hpp" #include "trajopt/trajectory/HolonomicTrajectorySample.hpp" #include "trajopt/util/Cancellation.hpp" @@ -23,17 +22,15 @@ namespace trajopt::rsffi { void SwervePathBuilder::set_drivetrain(const SwerveDrivetrain& drivetrain) { - std::vector cppModules; + std::vector cppModules; for (const auto& module : drivetrain.modules) { - cppModules.push_back( - trajopt::SwerveModule{{module.x, module.y}, - module.wheel_radius, - module.wheel_max_angular_velocity, - module.wheel_max_torque}); + cppModules.emplace_back(module.x, module.y); } path_builder.SetDrivetrain(trajopt::SwerveDrivetrain{ - drivetrain.mass, drivetrain.moi, std::move(cppModules)}); + drivetrain.mass, drivetrain.moi, drivetrain.wheel_radius, + drivetrain.wheel_max_angular_velocity, drivetrain.wheel_max_torque, + std::move(cppModules)}); } void SwervePathBuilder::set_control_interval_counts( diff --git a/trajoptlib/src/SwerveTrajectoryGenerator.cpp b/trajoptlib/src/SwerveTrajectoryGenerator.cpp index 173e752950..68ba5264b5 100644 --- a/trajoptlib/src/SwerveTrajectoryGenerator.cpp +++ b/trajoptlib/src/SwerveTrajectoryGenerator.cpp @@ -107,27 +107,25 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator( double minWidth = INFINITY; for (size_t i = 1; i < path.drivetrain.modules.size(); i++) { - if (std::abs(path.drivetrain.modules.at(i - 1).translation.X() - - path.drivetrain.modules.at(i).translation.X()) != 0) { - minWidth = std::min( - minWidth, std::abs(path.drivetrain.modules.at(i - 1).translation.X() - - path.drivetrain.modules.at(i).translation.X())); + if (std::abs(path.drivetrain.modules.at(i - 1).X() - + path.drivetrain.modules.at(i).X()) != 0) { + minWidth = + std::min(minWidth, std::abs(path.drivetrain.modules.at(i - 1).X() - + path.drivetrain.modules.at(i).X())); } - if (std::abs(path.drivetrain.modules.at(i - 1).translation.Y() - - path.drivetrain.modules.at(i).translation.Y()) != 0) { - minWidth = std::min( - minWidth, std::abs(path.drivetrain.modules.at(i - 1).translation.Y() - - path.drivetrain.modules.at(i).translation.Y())); + if (std::abs(path.drivetrain.modules.at(i - 1).Y() - + path.drivetrain.modules.at(i).Y()) != 0) { + minWidth = + std::min(minWidth, std::abs(path.drivetrain.modules.at(i - 1).Y() - + path.drivetrain.modules.at(i).Y())); } } for (size_t sgmtIndex = 0; sgmtIndex < sgmtCnt; ++sgmtIndex) { dt.emplace_back(problem.DecisionVariable()); - for (auto module : path.drivetrain.modules) { - problem.SubjectTo(dt.at(sgmtIndex) * module.wheelRadius * - module.wheelMaxAngularVelocity <= - minWidth); - } + problem.SubjectTo(dt.at(sgmtIndex) * path.drivetrain.wheelRadius * + path.drivetrain.wheelMaxAngularVelocity <= + minWidth); } // Minimize total time @@ -183,13 +181,16 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator( auto Fy_net = std::accumulate(Fy.at(index).begin(), Fy.at(index).end(), sleipnir::Variable{0.0}); + const auto& wheelRadius = path.drivetrain.wheelRadius; + const auto& wheelMaxAngularVelocity = + path.drivetrain.wheelMaxAngularVelocity; + const auto& wheelMaxTorque = path.drivetrain.wheelMaxTorque; + // Solve for net torque sleipnir::Variable tau_net = 0.0; for (size_t moduleIndex = 0; moduleIndex < path.drivetrain.modules.size(); ++moduleIndex) { - const auto& [translation, wheelRadius, wheelMaxAngularVelocity, - wheelMaxTorque] = path.drivetrain.modules.at(moduleIndex); - + const auto& translation = path.drivetrain.modules.at(moduleIndex); auto r = translation.RotateBy(theta); Translation2v F{Fx.at(index).at(moduleIndex), Fy.at(index).at(moduleIndex)}; @@ -201,8 +202,7 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator( auto vWrtRobot = v.RotateBy(-theta); for (size_t moduleIndex = 0; moduleIndex < path.drivetrain.modules.size(); ++moduleIndex) { - const auto& [translation, wheelRadius, wheelMaxAngularVelocity, - wheelMaxTorque] = path.drivetrain.modules.at(moduleIndex); + const auto& translation = path.drivetrain.modules.at(moduleIndex); Translation2v vWheelWrtRobot{ vWrtRobot.X() - translation.Y() * omega.at(index), diff --git a/trajoptlib/src/lib.rs b/trajoptlib/src/lib.rs index 7f3a9f9ad8..fc7d61df3e 100644 --- a/trajoptlib/src/lib.rs +++ b/trajoptlib/src/lib.rs @@ -1,19 +1,19 @@ #[cxx::bridge(namespace = "trajopt::rsffi")] mod ffi { #[derive(Debug, Deserialize, Serialize)] - struct SwerveModule { + struct Translation2d { x: f64, y: f64, - wheel_radius: f64, - wheel_max_angular_velocity: f64, - wheel_max_torque: f64, } #[derive(Debug, Deserialize, Serialize)] struct SwerveDrivetrain { mass: f64, moi: f64, - modules: Vec, + wheel_radius: f64, + wheel_max_angular_velocity: f64, + wheel_max_torque: f64, + modules: Vec, } #[derive(Debug, Deserialize, Serialize)] @@ -440,4 +440,4 @@ pub use ffi::HolonomicTrajectory; pub use ffi::HolonomicTrajectorySample; pub use ffi::Pose2d; pub use ffi::SwerveDrivetrain; -pub use ffi::SwerveModule; +pub use ffi::Translation2d; diff --git a/trajoptlib/test/src/ObstacleTest.cpp b/trajoptlib/test/src/ObstacleTest.cpp index 24e8e18d09..2571dc348d 100644 --- a/trajoptlib/test/src/ObstacleTest.cpp +++ b/trajoptlib/test/src/ObstacleTest.cpp @@ -11,12 +11,13 @@ TEST_CASE("Obstacle - Linear initial guess", "[Obstacle]") { using namespace trajopt; - SwerveDrivetrain swerveDrivetrain{.mass = 45, - .moi = 6, - .modules = {{{+0.6, +0.6}, 0.04, 70, 2}, - {{+0.6, -0.6}, 0.04, 70, 2}, - {{-0.6, +0.6}, 0.04, 70, 2}, - {{-0.6, -0.6}, 0.04, 70, 2}}}; + SwerveDrivetrain swerveDrivetrain{ + .mass = 45, + .moi = 6, + .wheelRadius = 0.04, + .wheelMaxAngularVelocity = 70, + .wheelMaxTorque = 2, + .modules = {{+0.6, +0.6}, {+0.6, -0.6}, {-0.6, +0.6}, {-0.6, -0.6}}}; trajopt::SwervePathBuilder path; path.PoseWpt(0, 0.0, 0.0, 0.0);