Use cubic spline for initial guess

.github/workflows/trajoptlib-cpp.yml

@@ -8,7 +8,7 @@ concurrency:
 
 jobs:
   build:
-    timeout-minutes: 10
+    timeout-minutes: 15
     strategy:
       fail-fast: false
       matrix:

.github/workflows/trajoptlib-sanitizers.yml

@@ -8,7 +8,7 @@ concurrency:
 
 jobs:
   build:
-    timeout-minutes: 10
+    timeout-minutes: 15
     strategy:
       fail-fast: false
       matrix:

src-tauri/src/main.rs

@@ -277,16 +277,14 @@ struct ProgressUpdate {
 
 #[allow(non_snake_case)]
 #[tauri::command]
-async fn generate_trajectory(
+fn configure_path_builder(
     _app_handle: tauri::AppHandle,
     path: Vec<ChoreoWaypoint>,
     config: ChoreoRobotConfig,
     constraints: Vec<Constraints>,
     circleObstacles: Vec<CircleObstacle>,
     polygonObstacles: Vec<PolygonObstacle>,
-    // The handle referring to this path for the solver state callback
-    handle: i64,
-) -> Result<HolonomicTrajectory, String> {
+) -> SwervePathBuilder {
     let mut path_builder = SwervePathBuilder::new();
     let mut wpt_cnt: usize = 0;
     let mut rm: Vec<usize> = Vec::new();
@@ -454,6 +452,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<ChoreoWaypoint>,
+    config: ChoreoRobotConfig,
+    constraints: Vec<Constraints>,
+    circleObstacles: Vec<CircleObstacle>,
+    polygonObstacles: Vec<PolygonObstacle>,
+) -> Vec<usize> {
+    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<ChoreoWaypoint>,
+    config: ChoreoRobotConfig,
+    constraints: Vec<Constraints>,
+    circleObstacles: Vec<CircleObstacle>,
+    polygonObstacles: Vec<PolygonObstacle>,
+    // The handle referring to this path for the solver state callback
+    handle: i64,
+) -> Result<HolonomicTrajectory, String> {
+    let mut path_builder = configure_path_builder(
+        _app_handle,
+        path,
+        config,
+        constraints,
+        circleObstacles,
+        polygonObstacles,
+    );
     path_builder.generate(true, handle)
 }
 
@@ -486,6 +529,7 @@ fn main() {
         })
         .invoke_handler(tauri::generate_handler![
             generate_trajectory,
+            calculate_interval_counts,
             cancel,
             open_file_dialog,
             file_event_payload_from_dir,

src/document/DocumentModel.tsx

@@ -98,19 +98,32 @@ const StateStore = types
               }
             }
           });
-          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());
           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();
+            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

src/document/HolonomicPathStore.ts

@@ -717,6 +717,13 @@ export const HolonomicPathStore = types
         self.eventMarkers.push(marker);
         return marker;
       },
+      setControlIntervalCounts(counts: number[]) {
+        counts.forEach((count, idx) => {
+          if (idx < self.waypoints.length) {
+            self.waypoints[idx]?.setControlIntervalCount(count);
+          }
+        });
+      },
       optimizeControlIntervalCounts(
         robotConfig: IRobotConfigStore
       ): string | undefined {

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-07-10
+    GIT_TAG 27a2e02b52c349d48a6c798d7cd4611d3d6200c8
+    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

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");

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()

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)

trajoptlib/cmake/allwpilib_patch.py

@@ -0,0 +1,145 @@
+#!/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)"
+            and line != "find_program(PROTOC_COMPILER protoc 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 "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/protobuf>" 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,
+    )
+
+    # 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()