From b38299cc7d2800dbfe146f5e490733ef1e0646c3 Mon Sep 17 00:00:00 2001 From: camaj Date: Thu, 16 May 2024 21:13:54 -0700 Subject: [PATCH 01/10] get control intervals from trajoptlib --- src-tauri/src/main.rs | 54 +++++++++++++++++++++++++++--- src/document/DocumentModel.tsx | 18 +++++++++- src/document/HolonomicPathStore.ts | 8 +++++ 3 files changed, 75 insertions(+), 5 deletions(-) diff --git a/src-tauri/src/main.rs b/src-tauri/src/main.rs index 893cc796b1..dacc19881b 100644 --- a/src-tauri/src/main.rs +++ b/src-tauri/src/main.rs @@ -36,6 +36,7 @@ async fn contains_build_gradle(dir: Option<&Path>) -> Result }, ) } + #[tauri::command] async fn open_file_dialog(app_handle: tauri::AppHandle) { let file_path = FileDialogBuilder::new() @@ -285,16 +286,14 @@ struct ProgressUpdate { #[allow(non_snake_case)] #[tauri::command] -async fn generate_trajectory( +fn configure_path_builder( _app_handle: tauri::AppHandle, path: Vec, config: ChoreoRobotConfig, constraints: Vec, circleObstacles: Vec, polygonObstacles: Vec, - // The handle referring to this path for the solver state callback - handle: i64, -) -> Result { +) -> SwervePathBuilder { let mut path_builder = SwervePathBuilder::new(); let mut wpt_cnt: usize = 0; let mut rm: Vec = Vec::new(); @@ -477,6 +476,51 @@ async fn generate_trajectory( path_builder.sgmt_polygon_obstacle(0, wpt_cnt - 1, o.x, o.y, o.radius); } path_builder.set_drivetrain(&drivetrain); + + path_builder +} + +#[allow(non_snake_case)] +#[tauri::command] +fn calculate_interval_counts( + _app_handle: tauri::AppHandle, + path: Vec, + config: ChoreoRobotConfig, + constraints: Vec, + circleObstacles: Vec, + polygonObstacles: Vec, +) -> Vec { + let path_builder = configure_path_builder( + _app_handle, + path, + config, + constraints, + circleObstacles, + polygonObstacles, + ); + path_builder.calculate_control_interval_counts() +} + +#[allow(non_snake_case)] +#[tauri::command] +async fn generate_trajectory( + _app_handle: tauri::AppHandle, + path: Vec, + config: ChoreoRobotConfig, + constraints: Vec, + circleObstacles: Vec, + polygonObstacles: Vec, + // The handle referring to this path for the solver state callback + handle: i64, +) -> Result { + let mut path_builder = configure_path_builder( + _app_handle, + path, + config, + constraints, + circleObstacles, + polygonObstacles, + ); path_builder.generate(true, handle) } @@ -492,6 +536,7 @@ fn solver_status_callback(traj: HolonomicTrajectory, handle: i64) { let _ = tx.send(ProgressUpdate { traj, handle }); }; } + fn main() { let (tx, rx) = channel::(); PROGRESS_SENDER_LOCK.get_or_init(move || tx); @@ -508,6 +553,7 @@ fn main() { }) .invoke_handler(tauri::generate_handler![ generate_trajectory, + calculate_interval_counts, cancel, open_file_dialog, file_event_payload_from_dir, diff --git a/src/document/DocumentModel.tsx b/src/document/DocumentModel.tsx index 2862a2b8f5..43239f5b96 100644 --- a/src/document/DocumentModel.tsx +++ b/src/document/DocumentModel.tsx @@ -99,6 +99,23 @@ const StateStore = types } } }); + resolve(pathStore); + }) + .then(() => { + return invoke("calculate_interval_counts", { + path: pathStore.waypoints, + config: self.document.robotConfig.asSolverRobotConfig(), + constraints: pathStore.asSolverPath().constraints, + circleObstacles: pathStore.asSolverPath().circleObstacles, + polygonObstacles: [] + }) + .then( + (result: unknown | number[]) => { + pathStore.setControlIntervalCounts(result as number[]); + } + ); + }) + .then(() => { pathStore.setGenerating(true); // Capture the timestamps of the waypoints that were actually sent to the solver const waypointTimestamps = pathStore.waypointTimestamps(); @@ -110,7 +127,6 @@ const StateStore = types ...point.asSavedWaypoint() })); pathStore.eventMarkers.forEach((m) => m.updateTargetIndex()); - resolve(pathStore); }) .then( () => { diff --git a/src/document/HolonomicPathStore.ts b/src/document/HolonomicPathStore.ts index 0a64ce8470..da2d5a050e 100644 --- a/src/document/HolonomicPathStore.ts +++ b/src/document/HolonomicPathStore.ts @@ -742,6 +742,14 @@ export const HolonomicPathStore = types self.eventMarkers.push(marker); return marker; }, + setControlIntervalCounts(counts: number[]) { + console.log("wpts: [" + self.waypoints.concat() + "] counts: [" + counts.concat() + "]\n"); + counts.forEach((count, idx) => { + if (idx < self.waypoints.length) { + self.waypoints[idx]?.setControlIntervalCount(count); + } + }); + }, optimizeControlIntervalCounts( robotConfig: IRobotConfigStore ): string | undefined { From 10de36cd794b4d8107ccc77d3755c7932d814719 Mon Sep 17 00:00:00 2001 From: camaj Date: Mon, 20 May 2024 18:51:57 -0700 Subject: [PATCH 02/10] trajopt dependency and remove print goes along with https://github.com/SleipnirGroup/TrajoptLib/pull/148 --- src-tauri/Cargo.lock | 2 +- src-tauri/Cargo.toml | 2 +- src/document/HolonomicPathStore.ts | 1 - 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src-tauri/Cargo.lock b/src-tauri/Cargo.lock index 414a0e0696..a4d774bb2d 100644 --- a/src-tauri/Cargo.lock +++ b/src-tauri/Cargo.lock @@ -3212,7 +3212,7 @@ dependencies = [ [[package]] name = "trajoptlib" version = "0.1.0" -source = "git+https://github.com/SleipnirGroup/TrajoptLib.git?rev=af1fa1f5d2ff69bba8d6e0071ed1ccc964fd3398#af1fa1f5d2ff69bba8d6e0071ed1ccc964fd3398" +source = "git+https://github.com/SleipnirGroup/TrajoptLib.git?rev=58ef24ee18957813ac44fa9bd2cac0d9a489af15#58ef24ee18957813ac44fa9bd2cac0d9a489af15" dependencies = [ "cmake", "cxx", diff --git a/src-tauri/Cargo.toml b/src-tauri/Cargo.toml index c42321b229..1a7224a961 100644 --- a/src-tauri/Cargo.toml +++ b/src-tauri/Cargo.toml @@ -18,7 +18,7 @@ tauri-build = { version = "1.5.2", features = [] } tauri = { version = "1.6.5", features = [ "os-all", "window-close", "window-set-title", "path-all", "dialog", "dialog-confirm", "dialog-save", "dialog-open", "dialog-ask", "fs-all", "shell-open", "devtools"] } serde = { version = "1.0", features = ["derive"] } serde_json = "1.0" -trajoptlib = { git = "https://github.com/SleipnirGroup/TrajoptLib.git", rev = "af1fa1f5d2ff69bba8d6e0071ed1ccc964fd3398", features = ["sleipnir"] } + trajoptlib = { git = "https://github.com/SleipnirGroup/TrajoptLib.git", rev = "58ef24ee18957813ac44fa9bd2cac0d9a489af15", features = ["sleipnir"] } open = "3" lazy_static = "1.4.0" diff --git a/src/document/HolonomicPathStore.ts b/src/document/HolonomicPathStore.ts index da2d5a050e..78d367e629 100644 --- a/src/document/HolonomicPathStore.ts +++ b/src/document/HolonomicPathStore.ts @@ -743,7 +743,6 @@ export const HolonomicPathStore = types return marker; }, setControlIntervalCounts(counts: number[]) { - console.log("wpts: [" + self.waypoints.concat() + "] counts: [" + counts.concat() + "]\n"); counts.forEach((count, idx) => { if (idx < self.waypoints.length) { self.waypoints[idx]?.setControlIntervalCount(count); From 67830a691755e4c56601cf0483400dc741798da4 Mon Sep 17 00:00:00 2001 From: camaj Date: Mon, 20 May 2024 20:11:45 -0700 Subject: [PATCH 03/10] fmtJs --- src/document/DocumentModel.tsx | 49 ++++++++++++++++------------------ 1 file changed, 23 insertions(+), 26 deletions(-) diff --git a/src/document/DocumentModel.tsx b/src/document/DocumentModel.tsx index 43239f5b96..6d0c76b8cd 100644 --- a/src/document/DocumentModel.tsx +++ b/src/document/DocumentModel.tsx @@ -101,33 +101,30 @@ const StateStore = types }); resolve(pathStore); }) - .then(() => { - return invoke("calculate_interval_counts", { - path: pathStore.waypoints, - config: self.document.robotConfig.asSolverRobotConfig(), - constraints: pathStore.asSolverPath().constraints, - circleObstacles: pathStore.asSolverPath().circleObstacles, - polygonObstacles: [] + .then(() => { + return invoke("calculate_interval_counts", { + path: pathStore.waypoints, + config: self.document.robotConfig.asSolverRobotConfig(), + constraints: pathStore.asSolverPath().constraints, + circleObstacles: pathStore.asSolverPath().circleObstacles, + polygonObstacles: [] + }).then((result: unknown | number[]) => { + pathStore.setControlIntervalCounts(result as number[]); + }); + }) + .then(() => { + pathStore.setGenerating(true); + // Capture the timestamps of the waypoints that were actually sent to the solver + const waypointTimestamps = pathStore.waypointTimestamps(); + console.log(waypointTimestamps); + const stopPoints = pathStore.stopPoints(); + generatedWaypoints = pathStore.waypoints.map((point, idx) => ({ + timestamp: 0, + isStopPoint: stopPoints.includes(idx), + ...point.asSavedWaypoint() + })); + pathStore.eventMarkers.forEach((m) => m.updateTargetIndex()); }) - .then( - (result: unknown | number[]) => { - pathStore.setControlIntervalCounts(result as number[]); - } - ); - }) - .then(() => { - pathStore.setGenerating(true); - // Capture the timestamps of the waypoints that were actually sent to the solver - const waypointTimestamps = pathStore.waypointTimestamps(); - console.log(waypointTimestamps); - const stopPoints = pathStore.stopPoints(); - generatedWaypoints = pathStore.waypoints.map((point, idx) => ({ - timestamp: 0, - isStopPoint: stopPoints.includes(idx), - ...point.asSavedWaypoint() - })); - pathStore.eventMarkers.forEach((m) => m.updateTargetIndex()); - }) .then( () => { const handle = pathStore.uuid From 52c8ff4cc5a6c6c448d88097082dcced060782ea Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Thu, 20 Jun 2024 21:28:46 -0700 Subject: [PATCH 04/10] Update TrajoptLib --- src-tauri/Cargo.lock | 2 +- src-tauri/Cargo.toml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src-tauri/Cargo.lock b/src-tauri/Cargo.lock index 7af881e674..dbf70746a9 100644 --- a/src-tauri/Cargo.lock +++ b/src-tauri/Cargo.lock @@ -3212,7 +3212,7 @@ dependencies = [ [[package]] name = "trajoptlib" version = "0.1.0" -source = "git+https://github.com/bruingineer/TrajoptLib.git?rev=748dd18e4eec74ce3a0f2b8ee4fbf7c23aa10924#748dd18e4eec74ce3a0f2b8ee4fbf7c23aa10924" +source = "git+https://github.com/bruingineer/TrajoptLib.git?branch=better-initial-guess-wpi-v2#40325b8761e7d636c4924035d4b5563c33f56125" dependencies = [ "cmake", "cxx", diff --git a/src-tauri/Cargo.toml b/src-tauri/Cargo.toml index 5865badfd3..cf5593c091 100644 --- a/src-tauri/Cargo.toml +++ b/src-tauri/Cargo.toml @@ -18,7 +18,7 @@ tauri-build = { version = "1.5.2", features = [] } tauri = { version = "1.6.6", features = [ "os-all", "window-close", "window-set-title", "path-all", "dialog", "dialog-confirm", "dialog-save", "dialog-open", "dialog-ask", "fs-all", "shell-open", "devtools"] } serde = { version = "1.0", features = ["derive"] } serde_json = "1.0" -trajoptlib = { git = "https://github.com/bruingineer/TrajoptLib.git", rev = "748dd18e4eec74ce3a0f2b8ee4fbf7c23aa10924", features = ["sleipnir"] } +trajoptlib = { git = "https://github.com/bruingineer/TrajoptLib.git", branch = "better-initial-guess-wpi-v2", features = ["sleipnir"] } open = "3" lazy_static = "1.4.0" From 41af0cb34274ba8cfbd883b35a80fd123435bbd4 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 9 Jul 2024 18:58:17 -0700 Subject: [PATCH 05/10] Migrate TrajoptLib changes from SleipnirGroup/TrajoptLib/pull#148 --- .github/workflows/trajoptlib-sanitizers.yml | 2 +- trajoptlib/CMakeLists.txt | 27 ++ trajoptlib/build.rs | 4 + .../allwpilib-disable-psabi-warning.patch | 12 + trajoptlib/cmake/allwpilib_patch.py | 131 +++++++ trajoptlib/examples/swerve.rs | 55 ++- trajoptlib/include/.styleguide | 1 + .../AngularVelocityMaxMagnitudeConstraint.hpp | 6 +- .../LinearVelocityMaxMagnitudeConstraint.hpp | 6 +- .../trajopt/path/SwervePathBuilder.hpp | 18 + .../util/GenerateSplineInitialGuess.hpp | 328 ++++++++++++++++++ trajoptlib/src/.styleguide | 3 + trajoptlib/src/RustFFI.cpp | 42 +++ trajoptlib/src/RustFFI.hpp | 4 + trajoptlib/src/lib.rs | 18 + trajoptlib/src/path/SwervePathBuilder.cpp | 33 ++ .../CubicHermitePoseSplineHolonomic.hpp | 67 ++++ .../src/spline/CubicHermiteSpline1d.hpp | 36 ++ trajoptlib/src/spline/Spline1d.hpp | 24 ++ trajoptlib/src/spline/SplineParameterizer.cpp | 7 + trajoptlib/src/spline/SplineParameterizer.hpp | 147 ++++++++ trajoptlib/src/spline/SplineUtil.cpp | 67 ++++ trajoptlib/src/spline/SplineUtil.hpp | 16 + 23 files changed, 1036 insertions(+), 18 deletions(-) create mode 100644 trajoptlib/cmake/allwpilib-disable-psabi-warning.patch create mode 100755 trajoptlib/cmake/allwpilib_patch.py create mode 100644 trajoptlib/include/trajopt/util/GenerateSplineInitialGuess.hpp create mode 100644 trajoptlib/src/spline/CubicHermitePoseSplineHolonomic.hpp create mode 100644 trajoptlib/src/spline/CubicHermiteSpline1d.hpp create mode 100644 trajoptlib/src/spline/Spline1d.hpp create mode 100644 trajoptlib/src/spline/SplineParameterizer.cpp create mode 100644 trajoptlib/src/spline/SplineParameterizer.hpp create mode 100644 trajoptlib/src/spline/SplineUtil.cpp create mode 100644 trajoptlib/src/spline/SplineUtil.hpp diff --git a/.github/workflows/trajoptlib-sanitizers.yml b/.github/workflows/trajoptlib-sanitizers.yml index 4e45f74119..e8074c1936 100644 --- a/.github/workflows/trajoptlib-sanitizers.yml +++ b/.github/workflows/trajoptlib-sanitizers.yml @@ -8,7 +8,7 @@ concurrency: jobs: build: - timeout-minutes: 10 + timeout-minutes: 15 strategy: fail-fast: false matrix: diff --git a/trajoptlib/CMakeLists.txt b/trajoptlib/CMakeLists.txt index b86647eed0..82e906defb 100644 --- a/trajoptlib/CMakeLists.txt +++ b/trajoptlib/CMakeLists.txt @@ -15,6 +15,9 @@ set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake/modules" ) +# Allow overriding options in parent projects +cmake_policy(SET CMP0077 NEW) + project(TrajoptLib LANGUAGES CXX) set_property(GLOBAL PROPERTY USE_FOLDERS ON) @@ -116,6 +119,30 @@ set(BUILD_EXAMPLES ${BUILD_EXAMPLES_SAVE}) target_link_libraries(TrajoptLib PUBLIC Sleipnir) +# wpimath dependency +set(WITH_CSCORE OFF CACHE INTERNAL "With CSCore") +set(WITH_GUI OFF CACHE INTERNAL "With GUI") +set(WITH_JAVA OFF CACHE INTERNAL "With Java") +set(WITH_NTCORE OFF CACHE INTERNAL "With NTCore") +set(WITH_SIMULATION_MODULES OFF CACHE INTERNAL "With Simulation Modules") +set(WITH_TESTS OFF CACHE INTERNAL "With Tests") +set(WITH_WPIMATH ON CACHE INTERNAL "With WPIMath") +set(WITH_WPILIB OFF CACHE INTERNAL "With WPILib") +find_package(Python REQUIRED COMPONENTS Interpreter) +fetchcontent_declare( + wpilib + GIT_REPOSITORY https://github.com/wpilibsuite/allwpilib.git + # main on 2024-06-20 + GIT_TAG e2893fc1a36720b9c3986f2fc6c9607cea35c8fd + PATCH_COMMAND + ${Python_EXECUTABLE} + ${CMAKE_CURRENT_SOURCE_DIR}/cmake/allwpilib_patch.py + ${CMAKE_CURRENT_SOURCE_DIR} + UPDATE_DISCONNECTED 1 +) +fetchcontent_makeavailable(wpilib) +target_link_libraries(TrajoptLib PUBLIC wpimath Eigen3::Eigen) + target_include_directories( TrajoptLib PUBLIC diff --git a/trajoptlib/build.rs b/trajoptlib/build.rs index b952041af8..19c3dcda24 100644 --- a/trajoptlib/build.rs +++ b/trajoptlib/build.rs @@ -24,6 +24,8 @@ fn main() { .include("src") .include(format!("{}/include", cmake_dest.display())) .include(format!("{}/include/eigen3", cmake_dest.display())) + .include(format!("{}/include/wpimath", cmake_dest.display())) + .include(format!("{}/include/wpiutil", cmake_dest.display())) .std("c++20"); if cfg!(target_os = "windows") { @@ -44,6 +46,8 @@ fn main() { println!("cargo:rustc-link-lib=TrajoptLib"); println!("cargo:rustc-link-lib=Sleipnir"); println!("cargo:rustc-link-lib=fmt"); + println!("cargo:rustc-link-lib=wpimath"); + println!("cargo:rustc-link-lib=wpiutil"); println!("cargo:rerun-if-changed=src/RustFFI.hpp"); println!("cargo:rerun-if-changed=src/RustFFI.cpp"); diff --git a/trajoptlib/cmake/allwpilib-disable-psabi-warning.patch b/trajoptlib/cmake/allwpilib-disable-psabi-warning.patch new file mode 100644 index 0000000000..1ba8b8e3c4 --- /dev/null +++ b/trajoptlib/cmake/allwpilib-disable-psabi-warning.patch @@ -0,0 +1,12 @@ +diff --git a/cmake/modules/CompileWarnings.cmake b/cmake/modules/CompileWarnings.cmake +index 5de103201..78a5c3009 100644 +--- a/cmake/modules/CompileWarnings.cmake ++++ b/cmake/modules/CompileWarnings.cmake +@@ -5,6 +5,7 @@ macro(wpilib_target_warnings target) + -pedantic + -Wextra + -Wno-unused-parameter ++ -Wno-psabi + ${WPILIB_TARGET_WARNINGS} + ) + if(NOT NO_WERROR) diff --git a/trajoptlib/cmake/allwpilib_patch.py b/trajoptlib/cmake/allwpilib_patch.py new file mode 100755 index 0000000000..618f08e912 --- /dev/null +++ b/trajoptlib/cmake/allwpilib_patch.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python3 + +import os +import re +import shutil +import subprocess +import sys +import typing + + +def get_linesep(contents): + """Returns string containing autodetected line separator for file. + + Keyword arguments: + contents -- file contents string + """ + # Find potential line separator + pos = contents.find("\n") + + # If a newline character was found and the character preceding it is a + # carriage return, assume CRLF line endings. LF line endings are assumed + # for empty files. + if pos > 0 and contents[pos - 1] == "\r": + return "\r\n" + else: + return "\n" + + +def modify_file(filename, func: typing.Callable[[list[str]], list[str]]): + """ + Reads a file, modifies the contents with func, then writes the file. + """ + with open(filename, encoding="utf-8") as f: + contents = f.read() + + sep = get_linesep(contents) + lines = contents.split(sep) + lines = func(lines) + + with open(filename, "w", encoding="utf-8") as f: + f.write(sep.join(lines)) + + +def remove_protobuf_support(): + shutil.rmtree("wpimath/src/main/native/cpp/controller/proto", ignore_errors=True) + shutil.rmtree("wpimath/src/main/native/cpp/geometry/proto", ignore_errors=True) + shutil.rmtree("wpimath/src/main/native/cpp/kinematics/proto", ignore_errors=True) + shutil.rmtree("wpimath/src/main/native/cpp/system/plant/proto", ignore_errors=True) + shutil.rmtree("wpimath/src/main/native/cpp/trajectory/proto", ignore_errors=True) + shutil.rmtree("wpiutil/src/main/native/cpp/protobuf", ignore_errors=True) + shutil.rmtree( + "wpimath/src/main/native/include/frc/controller/proto", ignore_errors=True + ) + shutil.rmtree( + "wpimath/src/main/native/include/frc/geometry/proto", ignore_errors=True + ) + shutil.rmtree( + "wpimath/src/main/native/include/frc/kinematics/proto", ignore_errors=True + ) + shutil.rmtree( + "wpimath/src/main/native/include/frc/system/plant/proto", ignore_errors=True + ) + shutil.rmtree( + "wpimath/src/main/native/include/frc/trajectory/proto", ignore_errors=True + ) + shutil.rmtree("wpiutil/src/main/native/thirdparty/protobuf", ignore_errors=True) + shutil.rmtree("wpiutil/src/main/native/cpp/DataLog.cpp", ignore_errors=True) + shutil.rmtree("wpiutil/src/main/native/include/wpi/DataLog.h", ignore_errors=True) + + modify_file( + "CMakeLists.txt", + lambda lines: [ + line for line in lines if line != "find_package(Protobuf REQUIRED)" + ], + ) + modify_file( + "wpiutil/CMakeLists.txt", + lambda lines: [line.replace("protobuf::libprotobuf", "") for line in lines], + ) + modify_file( + "wpiutil/wpiutil-config.cmake.in", + lambda lines: [line for line in lines if line != "find_dependency(Protobuf)"], + ) + + filenames = [os.path.join(dp, f) for dp, dn, fn in os.walk("wpimath") for f in fn] + + def fix(lines): + # Remove lines mentioning protobuf + lines = [ + line + for line in lines + if "$" not in line + and not re.search(r"#include \"\w+\.pb\.h\"", line) + and not re.search(r"#include \"frc/.*?Proto\.h\"", line) + ] + + # Remove protobuf_generate_cpp() call + filtered_lines = [] + found = False + for i in range(len(lines)): + if lines[i].startswith("# workaround for makefiles"): + found = True + if not found: + filtered_lines.append(lines[i]) + if found and lines[i].startswith(")"): + found = False + lines = filtered_lines + + return lines + + for filename in filenames: + modify_file(filename, fix) + + +def main(): + remove_protobuf_support() + + # Disable psabi warning + subprocess.run( + [ + "git", + "apply", + "--ignore-whitespace", + os.path.join(sys.argv[1], "cmake/allwpilib-disable-psabi-warning.patch"), + ], + check=True, + ) + + +if __name__ == "__main__": + main() diff --git a/trajoptlib/examples/swerve.rs b/trajoptlib/examples/swerve.rs index 23aaa4e128..42d439ffd8 100644 --- a/trajoptlib/examples/swerve.rs +++ b/trajoptlib/examples/swerve.rs @@ -1,8 +1,8 @@ -use trajoptlib::{SwerveDrivetrain, SwerveModule, SwervePathBuilder}; +use trajoptlib::{Pose2d, SwerveDrivetrain, SwerveModule, SwervePathBuilder}; fn main() { let drivetrain = SwerveDrivetrain { - mass: 45.0, + mass: 80.0, moi: 6.0, modules: vec![ SwerveModule { @@ -10,28 +10,28 @@ fn main() { y: 0.6, wheel_radius: 0.04, wheel_max_angular_velocity: 70.0, - wheel_max_torque: 2.0, + wheel_max_torque: 0.9, }, SwerveModule { x: 0.6, y: -0.6, wheel_radius: 0.04, wheel_max_angular_velocity: 70.0, - wheel_max_torque: 2.0, + wheel_max_torque: 0.9, }, SwerveModule { x: -0.6, y: 0.6, wheel_radius: 0.04, wheel_max_angular_velocity: 70.0, - wheel_max_torque: 2.0, + wheel_max_torque: 0.9, }, SwerveModule { x: -0.6, y: -0.6, wheel_radius: 0.04, wheel_max_angular_velocity: 70.0, - wheel_max_torque: 2.0, + wheel_max_torque: 0.9, }, ], }; @@ -41,11 +41,44 @@ fn main() { path.set_drivetrain(&drivetrain); path.set_bumpers(1.3, 1.3); path.pose_wpt(0, 0.0, 0.0, 0.0); - path.pose_wpt(1, 1.0, 0.0, 0.0); + // path.sgmt_initial_guess_points( + // 0, + // &vec![ + // Pose2d { + // x: 1., + // y: 0.5, + // heading: -0.5, + // }, + // Pose2d { + // x: 2., + // y: -0.5, + // heading: -0.5, + // }, + // ], + // ); + // path.pose_wpt(1, 0.25, 0.0, 0.0); + path.sgmt_initial_guess_points( + 0, + &vec![Pose2d { + x: 2.0, + y: 0.0, + heading: 0.0, + }], + ); + let end_idx = 1; + path.pose_wpt(end_idx, 4.0, 0., 0.0); + path.wpt_linear_velocity_max_magnitude(0, 0.0); + path.wpt_linear_velocity_max_magnitude(end_idx, 0.0); path.wpt_angular_velocity_max_magnitude(0, 0.0); - path.wpt_angular_velocity_max_magnitude(1, 0.0); - path.sgmt_circle_obstacle(0, 1, 0.5, 0.1, 0.2); - path.set_control_interval_counts(vec![40]); + path.wpt_angular_velocity_max_magnitude(end_idx, 0.0); + // path.sgmt_circle_obstacle(0, 1, 0.5, 0.1, 0.2); + let counts = path.calculate_control_interval_counts(); + println!("counts; {:#?}", &counts); + path.set_control_interval_counts(vec![4, 3, 2]); + println!("setup complete"); - println!("{:?}", path.generate(true, 0)); + // path.calculate_control_interval_counts(); + // println!("linear: {:#?}", path.calculate_linear_initial_guess()); + println!("spline: {:#?}", path.calculate_spline_initial_guess()); + // println!("{:#?}", path.generate(true, 0)); } diff --git a/trajoptlib/include/.styleguide b/trajoptlib/include/.styleguide index 63357f53ff..9be0a2b8ed 100644 --- a/trajoptlib/include/.styleguide +++ b/trajoptlib/include/.styleguide @@ -14,5 +14,6 @@ modifiableFileExclude { includeOtherLibs { ^Eigen/ + ^frc/ ^sleipnir/ } diff --git a/trajoptlib/include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp b/trajoptlib/include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp index 6b76d9274d..7410337721 100644 --- a/trajoptlib/include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp +++ b/trajoptlib/include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp @@ -18,6 +18,9 @@ namespace trajopt { */ class TRAJOPT_DLLEXPORT AngularVelocityMaxMagnitudeConstraint { public: + /// The maximum angular velocity magnitude. + double m_maxMagnitude; + /** * Constructs an AngularVelocityMaxMagnitudeConstraint. * @@ -52,9 +55,6 @@ class TRAJOPT_DLLEXPORT AngularVelocityMaxMagnitudeConstraint { problem.SubjectTo(angularVelocity <= m_maxMagnitude); } } - - private: - double m_maxMagnitude; }; } // namespace trajopt diff --git a/trajoptlib/include/trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp b/trajoptlib/include/trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp index 9909d66222..fb811ce972 100644 --- a/trajoptlib/include/trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp +++ b/trajoptlib/include/trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp @@ -18,6 +18,9 @@ namespace trajopt { */ class TRAJOPT_DLLEXPORT LinearVelocityMaxMagnitudeConstraint { public: + /// The maximum linear velocity magnitude. + double m_maxMagnitude; + /** * Constructs a LinearVelocityMaxMagnitudeConstraint. * @@ -53,9 +56,6 @@ class TRAJOPT_DLLEXPORT LinearVelocityMaxMagnitudeConstraint { m_maxMagnitude * m_maxMagnitude); } } - - private: - double m_maxMagnitude; }; } // namespace trajopt diff --git a/trajoptlib/include/trajopt/path/SwervePathBuilder.hpp b/trajoptlib/include/trajopt/path/SwervePathBuilder.hpp index 421c701902..e229a70cd7 100644 --- a/trajoptlib/include/trajopt/path/SwervePathBuilder.hpp +++ b/trajoptlib/include/trajopt/path/SwervePathBuilder.hpp @@ -9,6 +9,8 @@ #include #include +#include + #include "trajopt/constraint/Constraint.hpp" #include "trajopt/drivetrain/SwerveDrivetrain.hpp" #include "trajopt/geometry/Pose2.hpp" @@ -162,6 +164,13 @@ class TRAJOPT_DLLEXPORT SwervePathBuilder { */ const std::vector& GetControlIntervalCounts() const; + /** + * Calculate a best guess of control interval counts + * + * @return the vector of best guess control interval counts + */ + const std::vector CalculateControlIntervalCounts() const; + /** * Calculate a discrete, linear initial guess of the x, y, and heading * of the robot that goes through each segment. @@ -170,6 +179,15 @@ class TRAJOPT_DLLEXPORT SwervePathBuilder { */ SwerveSolution CalculateInitialGuess() const; + /** + * Calculate a discrete, spline initial guess of the x, y, and heading + * of the robot that goes through each segment with some kinematics + * and considering some path constraints. + * + * @return the initial guess, as a solution + */ + SwerveSolution CalculateSplineInitialGuess() const; + /** * Add a callback to retrieve the state of the solver as a SwerveSolution. * This callback will run on every iteration of the solver. diff --git a/trajoptlib/include/trajopt/util/GenerateSplineInitialGuess.hpp b/trajoptlib/include/trajopt/util/GenerateSplineInitialGuess.hpp new file mode 100644 index 0000000000..59cbf8439a --- /dev/null +++ b/trajoptlib/include/trajopt/util/GenerateSplineInitialGuess.hpp @@ -0,0 +1,328 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +#include "spline/CubicHermitePoseSplineHolonomic.hpp" +#include "spline/SplineParameterizer.hpp" +#include "spline/SplineUtil.hpp" +#include "trajopt/geometry/Pose2.hpp" +#include "trajopt/path/Path.hpp" +#include "trajopt/util/TrajoptUtil.hpp" + +namespace trajopt { + +/// TODO: make a better way to pass control intervals to choreo +/// TODO: refactor this file. add inline? possible to template? + +// Generate a parameterized spline +std::vector> +ParameterizeSplines( + const std::vector& splines) { + std::vector> + splinePoints; + + // Iterate through the vector and parameterize each spline + for (const auto& spline : splines) { + splinePoints.push_back(trajopt::SplineParameterizer::Parameterize(spline)); + } + return splinePoints; +} + +inline frc::SwerveDriveKinematics<4> CreateKinematics(const SwervePath& path) { + wpi::array moduleTranslations{wpi::empty_array}; + for (size_t i = 0; i < path.drivetrain.modules.size(); ++i) { + const auto& mod = path.drivetrain.modules.at(i); + moduleTranslations.at(i) = + frc::Translation2d{units::meter_t(mod.translation.X()), + units::meter_t(mod.translation.Y())}; + } + const frc::SwerveDriveKinematics kinematics{moduleTranslations}; + return kinematics; +} + +inline units::meters_per_second_t CalculateMaxWheelVelocity( + const SwervePath& path) { + return units::meters_per_second_t( + path.drivetrain.modules.front().wheelMaxAngularVelocity * + path.drivetrain.modules.front().wheelRadius); +} + +inline units::meters_per_second_t CalculateSegmentVelocity( + const SwervePath& path, size_t sgmtIdx, units::meters_per_second_t sgmtVel, + const Pose2d& start, const Pose2d& end) { + auto dtheta = std::abs( + frc::AngleModulus(units::radian_t(std::abs(start.Rotation().Radians() - + end.Rotation().Radians()))) + .value()); + frc::Translation2d sgmtStart{units::meter_t(start.Translation().X()), + units::meter_t(start.Translation().Y())}; + frc::Translation2d sgmtEnd{units::meter_t(end.Translation().X()), + units::meter_t(end.Translation().Y())}; + + for (auto& c : path.waypoints.at(sgmtIdx).segmentConstraints) { + // assuming HolonomicVelocityConstraint with CircularSet2d + if (std::holds_alternative(c)) { + const auto& velocityHolonomicConstraint = + std::get(c); + auto vel = units::meters_per_second_t( + velocityHolonomicConstraint.m_maxMagnitude); + std::printf("max lin vel: %.2f - ", vel.value()); + if (vel < sgmtVel) { + sgmtVel = vel; + } + } else if (std::holds_alternative( + c)) { + const auto& angVelConstraint = + std::get(c); + auto maxAngVel = angVelConstraint.m_maxMagnitude; + std::printf("max ang vel: %.2f - ", maxAngVel); + + /* + * Proof for T = 1.5 * θ / ω: + * + * - The velocity function derived from the cubic Hermite spline is: + * v(t) = -6*θ*t^2 + 6*θ*t. + * + * - The peak velocity occurs at t = 0.5, where t∈[0, 1] : + * v(0.5) = 1.5*θ, which is the max angular velocity during the motion. + * - To ensure this peak velocity does not exceed ω, we set: + * 1.5 * θ = ω. + * - The total time T needed to reach the final θ and + * not exceed ω is thus derived as: + * T = θ / (ω / 1.5) = 1.5 * θ / ω. + * + * This calculation ensures the peak velocity meets but does not exceed ω, + * extending the time proportionally to meet this + * requirement. + */ + auto time = 1.5 * dtheta / maxAngVel; + // estimating velocity for a straight line path + /// TODO: use the spine path distance + auto vel = sgmtStart.Distance(sgmtEnd) / units::second_t(time); + if (vel < sgmtVel) { + sgmtVel = vel; + } + } + } + return sgmtVel; +} + +inline std::vector GenerateTrajectories( + const SwervePath& path, + const std::vector>& initialGuessPoints, + const std::vector>& + splinePoints, + const frc::SwerveDriveKinematics<4>& kinematics) { + const auto maxWheelVelocity = CalculateMaxWheelVelocity(path); + std::vector trajectories; + trajectories.reserve(path.waypoints.size()); + size_t splineIdx = 0; + for (size_t sgmtIdx = 1; sgmtIdx < initialGuessPoints.size(); ++sgmtIdx) { + auto sgmtVel = maxWheelVelocity; + const auto& sgmtGuessPoints = initialGuessPoints.at(sgmtIdx); + for (size_t guessIdx = 0; guessIdx < sgmtGuessPoints.size(); ++guessIdx) { + Pose2d start = (guessIdx == 0) ? initialGuessPoints.at(sgmtIdx - 1).back() + : sgmtGuessPoints.at(guessIdx - 1); + Pose2d end = sgmtGuessPoints.at(guessIdx); + sgmtVel = CalculateSegmentVelocity(path, sgmtIdx, sgmtVel, start, end); + std::printf("sgmtVel: %.2f\n", sgmtVel.value()); + frc::TrajectoryConfig sgmtConfig{sgmtVel, sgmtVel / units::second_t{1.0}}; + // uses each non-init guess waypoint as a stop point for first guess + sgmtConfig.SetStartVelocity(0_mps); + sgmtConfig.SetEndVelocity(0_mps); + sgmtConfig.AddConstraint( + frc::SwerveDriveKinematicsConstraint{kinematics, maxWheelVelocity}); + + // specify parameterized spline points to use for trajectory + const auto sgmtTraj = frc::TrajectoryParameterizer:: + TrajectoryParameterizer::TimeParameterizeTrajectory( + splinePoints.at(++splineIdx - 1), sgmtConfig.Constraints(), + sgmtConfig.StartVelocity(), sgmtConfig.EndVelocity(), + sgmtConfig.MaxVelocity(), sgmtConfig.MaxAcceleration(), + sgmtConfig.IsReversed()); + trajectories.push_back(sgmtTraj); + } + } + std::printf("path.wpt size: %zd\n", path.waypoints.size()); + std::printf("trajs size: %zd\n", trajectories.size()); + return trajectories; +} + +inline std::vector GenerateWaypointSplineTrajectories( + const trajopt::SwervePath path, + const std::vector> initialGuessPoints) { + std::vector splines = + CubicPoseControlVectorsFromWaypoints(initialGuessPoints); + auto splinePoints = ParameterizeSplines(splines); + const auto kinematics = CreateKinematics(path); + return GenerateTrajectories(path, initialGuessPoints, splinePoints, + kinematics); +} + +std::vector> +CalculateWaypointStatesWithControlIntervals( + const trajopt::SwervePath path, + const std::vector> initialGuessPoints, + std::vector controlIntervalCounts) { + const auto trajs = + GenerateWaypointSplineTrajectories(path, initialGuessPoints); + + size_t guessPoints = 0; + for (const auto& guesses : initialGuessPoints) { + guessPoints += guesses.size(); + } + std::vector> waypoint_states; + waypoint_states.reserve(guessPoints); + for (size_t i = 0; i < guessPoints; ++i) { + waypoint_states.push_back(std::vector()); + } + + size_t trajIdx = 0; + std::printf("sgmt1\n"); + waypoint_states.at(0).push_back(trajs.at(trajIdx).States().front()); + std::printf("ctrlCount: ["); + for (auto count : controlIntervalCounts) { + std::printf("%zd,", count); + } + std::printf("]\n"); + for (size_t sgmtIdx = 1; sgmtIdx < initialGuessPoints.size(); ++sgmtIdx) { + auto guessPointsSize = initialGuessPoints.at(sgmtIdx).size(); + auto samplesForSgmt = controlIntervalCounts.at(sgmtIdx - 1); + size_t samples = samplesForSgmt / guessPointsSize; + for (size_t guessIdx = 0; guessIdx < guessPointsSize; ++guessIdx) { + if (guessIdx == (guessPointsSize - 1)) { + samples += (samplesForSgmt % guessPointsSize); + } + for (size_t sampleIdx = 1; sampleIdx < samples + 1; ++sampleIdx) { + auto t = trajs.at(trajIdx).TotalTime() * + static_cast(sampleIdx) / samples; + const auto state = trajs.at(trajIdx).Sample(t); + waypoint_states.at(trajIdx + 1).push_back(state); + // std::printf("%zd, x: %f, y: %f, t: %f\n", + // sampleIdx, state.pose.X().value(), + // state.pose.Y().value(), t.value()); + } + std::printf(" size: %zd\n", waypoint_states.at(trajIdx + 1).size()); + ++trajIdx; + } + } + return waypoint_states; +} + +/** + * This is used to calculated the suggested number of states in order to get + * a path with a dt of around desiredDt. + */ +std::vector> CalculateWaypointStatesWithDt( + const trajopt::SwervePath path, + const std::vector> initialGuessPoints, + const double desiredDt) { + const auto trajs = + GenerateWaypointSplineTrajectories(path, initialGuessPoints); + + size_t guessPoints = 0; + for (const auto& guesses : initialGuessPoints) { + guessPoints += guesses.size(); + } + std::vector> waypoint_states; + waypoint_states.reserve(guessPoints); + for (size_t i = 0; i < guessPoints; ++i) { + waypoint_states.push_back(std::vector()); + } + + for (size_t sgmtIdx = 1; sgmtIdx < guessPoints; ++sgmtIdx) { + // specify parameterized spline points to use for trajectory + const auto sgmtTraj = trajs.at(sgmtIdx - 1); + + const auto wholeSgmtDt = sgmtTraj.TotalTime(); + const size_t samplesForSgmtNew = std::ceil(wholeSgmtDt.value() / desiredDt); + const auto dt = wholeSgmtDt / samplesForSgmtNew; + std::printf("dt for sgmt%zd with %zd samples: %.5f\n", sgmtIdx, + samplesForSgmtNew, dt.value()); + + if (sgmtIdx == 1) { + std::printf("sgmt1\n"); + waypoint_states.at(sgmtIdx - 1).push_back(sgmtTraj.States().front()); + } + + for (size_t sampleIdx = 1; sampleIdx <= samplesForSgmtNew; ++sampleIdx) { + auto t = static_cast(sampleIdx) * dt; + const auto point = sgmtTraj.Sample(t); + waypoint_states.at(sgmtIdx).push_back(point); + std::printf("%zd,", sampleIdx); + } + std::printf(" size: %zd\n", waypoint_states.at(sgmtIdx).size()); + } + return waypoint_states; +} + +/// TODO: this probably should move to swervepathbuilder +std::vector CalculateControlIntervalCountsWithDt( + const trajopt::SwervePath path, + const std::vector> initialGuessPoints, + const double desiredDt) { + const auto trajectoriesSamples = + CalculateWaypointStatesWithDt(path, initialGuessPoints, desiredDt); + std::vector counts; + counts.reserve(path.waypoints.size()); + for (size_t i = 1; i < trajectoriesSamples.size(); ++i) { + counts.push_back(trajectoriesSamples.at(i).size()); + } + counts.push_back(1); + return counts; +} + +inline SwerveSolution CalculateSplineInitialGuessWithKinematicsAndConstraints( + const trajopt::SwervePath path, + const std::vector> initialGuessPoints, + std::vector controlIntervalCounts) { + const auto trajectoriesSamples = CalculateWaypointStatesWithControlIntervals( + path, initialGuessPoints, controlIntervalCounts); + SwerveSolution initialGuess; + for (size_t i = 0; i < trajectoriesSamples.size(); ++i) { + const auto& traj = trajectoriesSamples.at(i); + /// FIXME: first segment is always 1 point long so always 0.1s to second + /// sample + auto dt = 0.1_s; + if (traj.size() > 1) { + dt = traj.at(1).t - traj.front().t; + } else if (traj.size() == 1 && (i + 1) < trajectoriesSamples.size()) { + dt = trajectoriesSamples.at(i + 1).front().t - traj.front().t; + } + + for (const auto& point : traj) { + // std::printf("point{%f, %f, %f, %f, %f}\n", + // point.pose.X().value(), + // point.pose.Y().value(), + // point.pose.Rotation().Cos(), + // point.pose.Rotation().Sin(), + // dt.value()); + initialGuess.dt.push_back(dt.value()); + + initialGuess.x.push_back(point.pose.X().value()); + initialGuess.y.push_back(point.pose.Y().value()); + initialGuess.thetacos.push_back(point.pose.Rotation().Cos()); + initialGuess.thetasin.push_back(point.pose.Rotation().Sin()); + initialGuess.vx.push_back(0.0); + initialGuess.vy.push_back(0.0); + initialGuess.omega.push_back(0); + initialGuess.ax.push_back(0); + initialGuess.ay.push_back(0); + initialGuess.alpha.push_back(0); + initialGuess.moduleFX.push_back(std::vector{0, 0, 0, 0}); + initialGuess.moduleFY.push_back(std::vector{0, 0, 0, 0}); + } + } + return initialGuess; +} + +} // namespace trajopt diff --git a/trajoptlib/src/.styleguide b/trajoptlib/src/.styleguide index 9fff813382..56fe6c3556 100644 --- a/trajoptlib/src/.styleguide +++ b/trajoptlib/src/.styleguide @@ -14,6 +14,9 @@ modifiableFileExclude { includeOtherLibs { ^Eigen/ + ^frc/ ^rust/ ^sleipnir/ + ^units/ + ^wpi/ } diff --git a/trajoptlib/src/RustFFI.cpp b/trajoptlib/src/RustFFI.cpp index b8d3fcafba..19f6c79eac 100644 --- a/trajoptlib/src/RustFFI.cpp +++ b/trajoptlib/src/RustFFI.cpp @@ -241,6 +241,48 @@ void SwervePathBuilder::add_progress_callback( }); } +HolonomicTrajectory _convert_sol_to_holonomic_trajectory( + const trajopt::SwerveSolution& solution) { + trajopt::HolonomicTrajectory cppTrajectory{solution}; + + rust::Vec rustSamples; + for (const auto& cppSample : cppTrajectory.samples) { + rust::Vec fx; + std::copy(cppSample.moduleForcesX.begin(), cppSample.moduleForcesX.end(), + std::back_inserter(fx)); + + rust::Vec fy; + std::copy(cppSample.moduleForcesY.begin(), cppSample.moduleForcesY.end(), + std::back_inserter(fy)); + + rustSamples.push_back(HolonomicTrajectorySample{ + cppSample.timestamp, cppSample.x, cppSample.y, cppSample.heading, + cppSample.velocityX, cppSample.velocityY, cppSample.angularVelocity, + std::move(fx), std::move(fy)}); + } + return HolonomicTrajectory{rustSamples}; +} + +HolonomicTrajectory SwervePathBuilder::calculate_linear_initial_guess() const { + return _convert_sol_to_holonomic_trajectory( + path_builder.CalculateInitialGuess()); +} + +HolonomicTrajectory SwervePathBuilder::calculate_spline_initial_guess() const { + return _convert_sol_to_holonomic_trajectory( + path_builder.CalculateSplineInitialGuess()); +} + +rust::Vec SwervePathBuilder::calculate_control_interval_counts() + const { + auto cppCounts = path_builder.CalculateControlIntervalCounts(); + rust::Vec rustCounts; + for (const auto count : cppCounts) { + rustCounts.emplace_back(count); + } + return rustCounts; +} + std::unique_ptr swerve_path_builder_new() { return std::make_unique(); } diff --git a/trajoptlib/src/RustFFI.hpp b/trajoptlib/src/RustFFI.hpp index 723af32374..110ff58f94 100644 --- a/trajoptlib/src/RustFFI.hpp +++ b/trajoptlib/src/RustFFI.hpp @@ -65,6 +65,10 @@ class SwervePathBuilder { void add_progress_callback( rust::Fn callback); + HolonomicTrajectory calculate_linear_initial_guess() const; + HolonomicTrajectory calculate_spline_initial_guess() const; + rust::Vec calculate_control_interval_counts() const; + private: trajopt::SwervePathBuilder path_builder; }; diff --git a/trajoptlib/src/lib.rs b/trajoptlib/src/lib.rs index 7f3a9f9ad8..88a184fd5d 100644 --- a/trajoptlib/src/lib.rs +++ b/trajoptlib/src/lib.rs @@ -164,6 +164,12 @@ mod ffi { fn swerve_path_builder_new() -> UniquePtr; + fn calculate_linear_initial_guess(self: &SwervePathBuilder) -> HolonomicTrajectory; + + fn calculate_spline_initial_guess(self: &SwervePathBuilder) -> HolonomicTrajectory; + + fn calculate_control_interval_counts(self: &SwervePathBuilder) -> Vec; + fn cancel_all(); } } @@ -424,6 +430,18 @@ impl SwervePathBuilder { pub fn add_progress_callback(&mut self, callback: fn(HolonomicTrajectory, i64)) { crate::ffi::SwervePathBuilder::add_progress_callback(self.path_builder.pin_mut(), callback); } + + pub fn calculate_linear_initial_guess(&self) -> HolonomicTrajectory { + self.path_builder.calculate_linear_initial_guess() + } + + pub fn calculate_spline_initial_guess(&self) -> HolonomicTrajectory { + self.path_builder.calculate_spline_initial_guess() + } + + pub fn calculate_control_interval_counts(&self) -> Vec { + self.path_builder.calculate_control_interval_counts() + } } impl Default for SwervePathBuilder { diff --git a/trajoptlib/src/path/SwervePathBuilder.cpp b/trajoptlib/src/path/SwervePathBuilder.cpp index 73afbc4a43..883779c1e7 100644 --- a/trajoptlib/src/path/SwervePathBuilder.cpp +++ b/trajoptlib/src/path/SwervePathBuilder.cpp @@ -1,9 +1,30 @@ // Copyright (c) TrajoptLib contributors +#if __GNUC__ +#pragma GCC diagnostic ignored "-Wunused-parameter" +#endif + #include "trajopt/path/SwervePathBuilder.hpp" +#include + +#include +#include #include +#include +#include +#include +#include +#include +#include +#include + +#include "spline/CubicHermitePoseSplineHolonomic.hpp" +#include "spline/SplineParameterizer.hpp" +#include "spline/SplineUtil.hpp" +#include "trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp" +#include "trajopt/constraint/Constraint.hpp" #include "trajopt/constraint/LinePointConstraint.hpp" #include "trajopt/constraint/PointLineConstraint.hpp" #include "trajopt/constraint/PointPointConstraint.hpp" @@ -13,6 +34,8 @@ #include "trajopt/solution/SwerveSolution.hpp" #include "trajopt/util/Cancellation.hpp" #include "trajopt/util/GenerateLinearInitialGuess.hpp" +#include "trajopt/util/GenerateSplineInitialGuess.hpp" +#include "trajopt/util/TrajoptUtil.hpp" namespace trajopt { @@ -188,11 +211,21 @@ const std::vector& SwervePathBuilder::GetControlIntervalCounts() const { return controlIntervalCounts; } +const std::vector SwervePathBuilder::CalculateControlIntervalCounts() + const { + return CalculateControlIntervalCountsWithDt(path, initialGuessPoints, 0.1); +} + SwerveSolution SwervePathBuilder::CalculateInitialGuess() const { return GenerateLinearInitialGuess(initialGuessPoints, controlIntervalCounts); } +SwerveSolution SwervePathBuilder::CalculateSplineInitialGuess() const { + return CalculateSplineInitialGuessWithKinematicsAndConstraints( + path, initialGuessPoints, controlIntervalCounts); +} + void SwervePathBuilder::AddIntermediateCallback( const std::function callback) { path.callbacks.push_back(callback); diff --git a/trajoptlib/src/spline/CubicHermitePoseSplineHolonomic.hpp b/trajoptlib/src/spline/CubicHermitePoseSplineHolonomic.hpp new file mode 100644 index 0000000000..320139a4d4 --- /dev/null +++ b/trajoptlib/src/spline/CubicHermitePoseSplineHolonomic.hpp @@ -0,0 +1,67 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include + +#include + +#include "spline/CubicHermiteSpline1d.hpp" +#include "trajopt/util/SymbolExports.hpp" + +namespace trajopt { +/** + * Represents a cubic pose spline, which is a specific implementation of a cubic + * hermite spline. + */ +class TRAJOPT_DLLEXPORT CubicHermitePoseSplineHolonomic { + public: + using PoseWithCurvature = std::pair; + CubicHermitePoseSplineHolonomic(wpi::array xInitialControlVector, + wpi::array xFinalControlVector, + wpi::array yInitialControlVector, + wpi::array yFinalControlVector, + frc::Rotation2d r0, frc::Rotation2d r1) + : r0(r0), + theta(0.0, (-r0).RotateBy(r1).Radians().value(), 0, 0), + spline(xInitialControlVector, xFinalControlVector, + yInitialControlVector, yFinalControlVector) {} + + CubicHermitePoseSplineHolonomic(frc::CubicHermiteSpline spline, + frc::Rotation2d r0, frc::Rotation2d r1) + : r0(r0), + theta(0.0, (-r0).RotateBy(r1).Radians().value(), 0, 0), + spline(spline.GetInitialControlVector().x, + spline.GetFinalControlVector().x, + spline.GetInitialControlVector().y, + spline.GetFinalControlVector().y) {} + + frc::Rotation2d getCourse(double t) const { + const auto splinePoint = spline.GetPoint(t); + return splinePoint.first.Rotation(); + } + + frc::Rotation2d getHeading(double t) const { + return r0.RotateBy(frc::Rotation2d(units::radian_t(theta.getPosition(t)))); + } + + double getDHeading(double t) const { return theta.getVelocity(t); } + + /** + * Gets the pose and curvature at some point t on the spline. + * + * @param t The point t + * @return The pose and curvature at that point. + */ + PoseWithCurvature GetPoint(double t) const { + const auto splinePoint = spline.GetPoint(t); + return {{splinePoint.first.Translation(), getHeading(t)}, + splinePoint.second}; + } + + private: + frc::Rotation2d r0; + CubicHermiteSpline1d theta; + frc::CubicHermiteSpline spline; +}; +} // namespace trajopt diff --git a/trajoptlib/src/spline/CubicHermiteSpline1d.hpp b/trajoptlib/src/spline/CubicHermiteSpline1d.hpp new file mode 100644 index 0000000000..aa5e3a40d3 --- /dev/null +++ b/trajoptlib/src/spline/CubicHermiteSpline1d.hpp @@ -0,0 +1,36 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include + +#include "spline/Spline1d.hpp" +#include "trajopt/util/SymbolExports.hpp" + +namespace trajopt { + +class TRAJOPT_DLLEXPORT CubicHermiteSpline1d : public Spline1d { + public: + // Coefficients of the cubic polynomial + const double a, b, c, d; + + CubicHermiteSpline1d(double p0, double p1, double v0, double v1) + : a(v0 + v1 + 2 * p0 - 2 * p1), + b(-2 * v0 - v1 - 3 * p0 + 3 * p1), + c(v0), + d(p0) {} + + double getPosition(double t) const override { + return a * std::pow(t, 3) + b * std::pow(t, 2) + c * t + d; + } + + double getVelocity(double t) const override { + return 3 * a * std::pow(t, 2) + 2 * b * t + c; + } + + double getAcceleration(double t) const override { return 6 * a * t + 2 * b; } + + double getJerk([[maybe_unused]] double t) const override { return 6 * a; } +}; + +} // namespace trajopt diff --git a/trajoptlib/src/spline/Spline1d.hpp b/trajoptlib/src/spline/Spline1d.hpp new file mode 100644 index 0000000000..3d36675f18 --- /dev/null +++ b/trajoptlib/src/spline/Spline1d.hpp @@ -0,0 +1,24 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include "trajopt/util/SymbolExports.hpp" + +namespace trajopt { + +class TRAJOPT_DLLEXPORT Spline1d { + public: + virtual ~Spline1d() {} + + virtual double getPosition(double t) const = 0; + + // ds/dt + virtual double getVelocity(double t) const = 0; + + // ds²/dt + virtual double getAcceleration(double t) const = 0; + + // ds³/dt + virtual double getJerk(double t) const = 0; +}; +} // namespace trajopt diff --git a/trajoptlib/src/spline/SplineParameterizer.cpp b/trajoptlib/src/spline/SplineParameterizer.cpp new file mode 100644 index 0000000000..c890549105 --- /dev/null +++ b/trajoptlib/src/spline/SplineParameterizer.cpp @@ -0,0 +1,7 @@ +// Copyright (c) TrajoptLib contributors + +#include + +constexpr units::meter_t frc::SplineParameterizer::kMaxDx; +constexpr units::meter_t frc::SplineParameterizer::kMaxDy; +constexpr units::radian_t frc::SplineParameterizer::kMaxDtheta; diff --git a/trajoptlib/src/spline/SplineParameterizer.hpp b/trajoptlib/src/spline/SplineParameterizer.hpp new file mode 100644 index 0000000000..f652acba79 --- /dev/null +++ b/trajoptlib/src/spline/SplineParameterizer.hpp @@ -0,0 +1,147 @@ +// Copyright (c) TrajoptLib contributors + +/* + * MIT License + * + * Copyright (c) 2018 Team 254 + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "spline/CubicHermitePoseSplineHolonomic.hpp" +#include "trajopt/util/SymbolExports.hpp" + +namespace trajopt { + +/** + * Class used to parameterize a spline by its arc length. + */ +class TRAJOPT_DLLEXPORT SplineParameterizer { + public: + // using PoseWithCurvature = std::pair; + + struct MalformedSplineException : public std::runtime_error { + explicit MalformedSplineException(const char* what_arg) + : runtime_error(what_arg) {} + }; + + /** + * Parametrizes the spline. This method breaks up the spline into various + * arcs until their dx, dy, and dtheta are within specific tolerances. + * + * @param spline The spline to parameterize. + * @param t0 Starting internal spline parameter. It is recommended to leave + * this as default. + * @param t1 Ending internal spline parameter. It is recommended to leave this + * as default. + * + * @return A vector of poses and curvatures that represents various points on + * the spline. + */ + static std::vector Parameterize( + const CubicHermitePoseSplineHolonomic& spline, double t0 = 0.0, + double t1 = 1.0) { + std::vector splinePoints; + + // The parameterization does not add the initial point. Let's add that. + splinePoints.push_back(spline.GetPoint(t0)); + + // We use an "explicit stack" to simulate recursion, instead of a recursive + // function call This give us greater control, instead of a stack overflow + std::stack stack; + stack.emplace(StackContents{t0, t1}); + + StackContents current; + frc::SplineParameterizer::PoseWithCurvature start; + frc::SplineParameterizer::PoseWithCurvature end; + int iterations = 0; + + while (!stack.empty()) { + current = stack.top(); + stack.pop(); + start = spline.GetPoint(current.t0); + end = spline.GetPoint(current.t1); + + const auto twist = start.first.Log(end.first); + + const auto dcourse = + spline.getCourse(current.t1) - spline.getCourse(current.t0); + + if (units::math::abs(twist.dy) > kMaxDy || + units::math::abs(twist.dx) > kMaxDx || + units::math::abs(twist.dtheta) > kMaxDtheta || + units::math::abs(dcourse.Radians()) > kMaxDtheta) { + stack.emplace(StackContents{(current.t0 + current.t1) / 2, current.t1}); + stack.emplace(StackContents{current.t0, (current.t0 + current.t1) / 2}); + } else { + splinePoints.push_back(spline.GetPoint(current.t1)); + } + + if (iterations++ >= kMaxIterations) { + std::printf("spline parameterization iterations: %d\n", iterations); + throw MalformedSplineException( + "Could not parameterize a malformed spline. " + "This means that you probably had two or more adjacent " + "waypoints that were very close together with headings " + "in opposing directions."); + } + } + std::printf("spline parameterization iterations: %d\n", iterations); + + return splinePoints; + } + + private: + // Constraints for spline parameterization. + static constexpr units::meter_t kMaxDx = 5_in; + static constexpr units::meter_t kMaxDy = 0.05_in; + static constexpr units::radian_t kMaxDtheta = 0.0872_rad; + + struct StackContents { + double t0; + double t1; + }; + + /** + * A malformed spline does not actually explode the LIFO stack size. Instead, + * the stack size stays at a relatively small number (e.g. 30) and never + * decreases. Because of this, we must count iterations. Even long, complex + * paths don't usually go over 300 iterations, so hitting this maximum should + * definitely indicate something has gone wrong. + */ + static constexpr int kMaxIterations = 15000; + + friend class CubicHermiteSplineTest; + friend class QuinticHermiteSplineTest; +}; +} // namespace trajopt diff --git a/trajoptlib/src/spline/SplineUtil.cpp b/trajoptlib/src/spline/SplineUtil.cpp new file mode 100644 index 0000000000..ff60577654 --- /dev/null +++ b/trajoptlib/src/spline/SplineUtil.cpp @@ -0,0 +1,67 @@ +// Copyright (c) TrajoptLib contributors + +#include "spline/SplineUtil.hpp" + +#include + +#include +#include +#include + +#include "spline/CubicHermitePoseSplineHolonomic.hpp" + +namespace trajopt { + +std::vector +CubicPoseControlVectorsFromWaypoints( + const std::vector> initialGuessPoints) { + size_t totalGuessPoints = 0; + for (const auto& points : initialGuessPoints) { + totalGuessPoints += points.size(); + } + std::vector flatTranslationPoints; + std::vector flatHeadings; + flatTranslationPoints.reserve(totalGuessPoints); + flatHeadings.reserve(totalGuessPoints); + + // populate translation and heading vectors + for (const auto& guessPoints : initialGuessPoints) { + for (const auto& guessPoint : guessPoints) { + flatTranslationPoints.emplace_back( + units::meter_t(guessPoint.Translation().X()), + units::meter_t(guessPoint.Translation().Y())); + flatHeadings.emplace_back(guessPoint.Rotation().Cos(), + guessPoint.Rotation().Sin()); + } + } + + // calculate angles and pose for start and end of path spline + const auto startSplineAngle = + (flatTranslationPoints.at(1) - flatTranslationPoints.at(0)).Angle(); + const auto endSplineAngle = + (flatTranslationPoints.back() - + flatTranslationPoints.at(flatTranslationPoints.size() - 2)) + .Angle(); + const frc::Pose2d start{flatTranslationPoints.front(), startSplineAngle}; + const frc::Pose2d end{flatTranslationPoints.back(), endSplineAngle}; + + // use all interior points to create the path spline + std::vector interiorPoints{ + flatTranslationPoints.begin() + 1, flatTranslationPoints.end() - 1}; + + const auto splineControlVectors = + frc::SplineHelper::CubicControlVectorsFromWaypoints(start, interiorPoints, + end); + const auto splines_temp = frc::SplineHelper::CubicSplinesFromControlVectors( + splineControlVectors.front(), interiorPoints, + splineControlVectors.back()); + + std::vector splines; + splines.reserve(splines_temp.size()); + for (size_t i = 1; i <= splines_temp.size(); ++i) { + splines.emplace_back(splines_temp.at(i - 1), flatHeadings.at(i - 1), + flatHeadings.at(i)); + } + return splines; +} +} // namespace trajopt diff --git a/trajoptlib/src/spline/SplineUtil.hpp b/trajoptlib/src/spline/SplineUtil.hpp new file mode 100644 index 0000000000..5e875e2fb7 --- /dev/null +++ b/trajoptlib/src/spline/SplineUtil.hpp @@ -0,0 +1,16 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include + +#include "spline/CubicHermitePoseSplineHolonomic.hpp" +#include "trajopt/geometry/Pose2.hpp" + +namespace trajopt { + +std::vector +CubicPoseControlVectorsFromWaypoints( + const std::vector> initialGuessPoints); + +} // namespace trajopt From 64d96326c959747838e489059c45b631c9bbf8e9 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 9 Jul 2024 19:11:14 -0700 Subject: [PATCH 06/10] Upgrade allwpilib --- trajoptlib/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/trajoptlib/CMakeLists.txt b/trajoptlib/CMakeLists.txt index 82e906defb..6e5e60ee22 100644 --- a/trajoptlib/CMakeLists.txt +++ b/trajoptlib/CMakeLists.txt @@ -132,8 +132,8 @@ find_package(Python REQUIRED COMPONENTS Interpreter) fetchcontent_declare( wpilib GIT_REPOSITORY https://github.com/wpilibsuite/allwpilib.git - # main on 2024-06-20 - GIT_TAG e2893fc1a36720b9c3986f2fc6c9607cea35c8fd + # main on 2024-07-09 + GIT_TAG c62863cf749ebff054e66e965e82e996b17e7773 PATCH_COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/cmake/allwpilib_patch.py From 99b2179355ac7c27edbdff336fe260de926cd0ca Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 9 Jul 2024 19:16:16 -0700 Subject: [PATCH 07/10] Remove protoc compiler check from allwpilib --- trajoptlib/cmake/allwpilib_patch.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/trajoptlib/cmake/allwpilib_patch.py b/trajoptlib/cmake/allwpilib_patch.py index 618f08e912..d27b608bed 100755 --- a/trajoptlib/cmake/allwpilib_patch.py +++ b/trajoptlib/cmake/allwpilib_patch.py @@ -70,7 +70,10 @@ def remove_protobuf_support(): modify_file( "CMakeLists.txt", lambda lines: [ - line for line in lines if line != "find_package(Protobuf REQUIRED)" + line + for line in lines + if line != "find_package(Protobuf REQUIRED)" + and line != "find_program(PROTOC_COMPILER protoc REQUIRED)" ], ) modify_file( From d4ddab56578c99170012e44775eaebf87b4d85d5 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 9 Jul 2024 19:58:43 -0700 Subject: [PATCH 08/10] Compile wpiutil as UTF-8 --- .../cmake/allwpilib-compile-wpiutil-as-utf-8.patch | 13 +++++++++++++ trajoptlib/cmake/allwpilib_patch.py | 11 +++++++++++ 2 files changed, 24 insertions(+) create mode 100644 trajoptlib/cmake/allwpilib-compile-wpiutil-as-utf-8.patch diff --git a/trajoptlib/cmake/allwpilib-compile-wpiutil-as-utf-8.patch b/trajoptlib/cmake/allwpilib-compile-wpiutil-as-utf-8.patch new file mode 100644 index 0000000000..17426d8475 --- /dev/null +++ b/trajoptlib/cmake/allwpilib-compile-wpiutil-as-utf-8.patch @@ -0,0 +1,13 @@ +diff --git a/wpiutil/CMakeLists.txt b/wpiutil/CMakeLists.txt +index eb4a0a8f7..4905f33d6 100644 +--- a/wpiutil/CMakeLists.txt ++++ b/wpiutil/CMakeLists.txt +@@ -155,7 +155,7 @@ target_compile_features(wpiutil PUBLIC cxx_std_20) + if(MSVC) + target_compile_options( + wpiutil +- PUBLIC /permissive- /Zc:preprocessor /Zc:throwingNew /MP /bigobj ++ PUBLIC /permissive- /Zc:preprocessor /Zc:throwingNew /MP /utf-8 /bigobj + ) + target_compile_definitions(wpiutil PRIVATE -D_CRT_SECURE_NO_WARNINGS) + endif() diff --git a/trajoptlib/cmake/allwpilib_patch.py b/trajoptlib/cmake/allwpilib_patch.py index d27b608bed..51e6f0f66a 100755 --- a/trajoptlib/cmake/allwpilib_patch.py +++ b/trajoptlib/cmake/allwpilib_patch.py @@ -129,6 +129,17 @@ def main(): check=True, ) + # Compile wpiutil as utf-8 + subprocess.run( + [ + "git", + "apply", + "--ignore-whitespace", + os.path.join(sys.argv[1], "cmake/allwpilib-compile-wpiutil-as-utf-8.patch"), + ], + check=True, + ) + if __name__ == "__main__": main() From ade6856d55eef0660339c7db39439cd049c045a5 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 9 Jul 2024 20:14:27 -0700 Subject: [PATCH 09/10] Increase TrajoptLib C++ CI timeout --- .github/workflows/trajoptlib-cpp.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/trajoptlib-cpp.yml b/.github/workflows/trajoptlib-cpp.yml index 885f7ed248..482d35b15c 100644 --- a/.github/workflows/trajoptlib-cpp.yml +++ b/.github/workflows/trajoptlib-cpp.yml @@ -8,7 +8,7 @@ concurrency: jobs: build: - timeout-minutes: 10 + timeout-minutes: 15 strategy: fail-fast: false matrix: From d67c33afdfa440f18a319df298d1b99a1e32673f Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 10 Jul 2024 08:34:27 -0700 Subject: [PATCH 10/10] Upgrade allwpilib --- trajoptlib/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/trajoptlib/CMakeLists.txt b/trajoptlib/CMakeLists.txt index 562c558f02..7c0ada9c45 100644 --- a/trajoptlib/CMakeLists.txt +++ b/trajoptlib/CMakeLists.txt @@ -132,8 +132,8 @@ find_package(Python REQUIRED COMPONENTS Interpreter) fetchcontent_declare( wpilib GIT_REPOSITORY https://github.com/wpilibsuite/allwpilib.git - # main on 2024-07-09 - GIT_TAG c62863cf749ebff054e66e965e82e996b17e7773 + # main on 2024-07-10 + GIT_TAG 27a2e02b52c349d48a6c798d7cd4611d3d6200c8 PATCH_COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/cmake/allwpilib_patch.py