[trajoptlib] Simple spline initial guess with wpilib dependency

.github/workflows/cache-cleanup.yml

@@ -16,7 +16,7 @@ jobs:
     steps:
       - name: Clean up caches
         run: |
-          ## Setting this to not fail the workflow while deleting cache keys. 
+          ## Setting this to not fail the workflow while deleting cache keys.
           set +e
           for i in {1..20}
           do

.vscode/settings.json

@@ -2,4 +2,106 @@
     "rust-analyzer.check.overrideCommand": [
         "cargo", "clippy", "--target-dir", "./target/clippy"
     ],
+    "files.associations": {
+        "*.inc": "cpp",
+        "algorithm": "cpp",
+        "any": "cpp",
+        "array": "cpp",
+        "atomic": "cpp",
+        "bit": "cpp",
+        "bitset": "cpp",
+        "cctype": "cpp",
+        "cfenv": "cpp",
+        "charconv": "cpp",
+        "chrono": "cpp",
+        "cinttypes": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "compare": "cpp",
+        "complex": "cpp",
+        "concepts": "cpp",
+        "condition_variable": "cpp",
+        "coroutine": "cpp",
+        "csignal": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdint": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "deque": "cpp",
+        "exception": "cpp",
+        "expected": "cpp",
+        "filesystem": "cpp",
+        "format": "cpp",
+        "forward_list": "cpp",
+        "fstream": "cpp",
+        "functional": "cpp",
+        "future": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "ios": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "iterator": "cpp",
+        "limits": "cpp",
+        "list": "cpp",
+        "locale": "cpp",
+        "map": "cpp",
+        "memory": "cpp",
+        "memory_resource": "cpp",
+        "mutex": "cpp",
+        "new": "cpp",
+        "numbers": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "ostream": "cpp",
+        "print": "cpp",
+        "queue": "cpp",
+        "random": "cpp",
+        "ranges": "cpp",
+        "ratio": "cpp",
+        "regex": "cpp",
+        "scoped_allocator": "cpp",
+        "set": "cpp",
+        "shared_mutex": "cpp",
+        "source_location": "cpp",
+        "span": "cpp",
+        "sstream": "cpp",
+        "stack": "cpp",
+        "stdexcept": "cpp",
+        "stop_token": "cpp",
+        "streambuf": "cpp",
+        "string": "cpp",
+        "system_error": "cpp",
+        "thread": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "typeindex": "cpp",
+        "typeinfo": "cpp",
+        "unordered_map": "cpp",
+        "unordered_set": "cpp",
+        "utility": "cpp",
+        "valarray": "cpp",
+        "variant": "cpp",
+        "vector": "cpp",
+        "xfacet": "cpp",
+        "xhash": "cpp",
+        "xiosbase": "cpp",
+        "xlocale": "cpp",
+        "xlocbuf": "cpp",
+        "xlocinfo": "cpp",
+        "xlocmes": "cpp",
+        "xlocmon": "cpp",
+        "xlocnum": "cpp",
+        "xloctime": "cpp",
+        "xmemory": "cpp",
+        "xstring": "cpp",
+        "xtr1common": "cpp",
+        "xtree": "cpp",
+        "xutility": "cpp"
+    },
 }

src-core/src/generation/transformers/callback.rs

@@ -46,4 +46,4 @@ impl DiffyGenerationTransformer for CallbackSetter {
     fn transform(&self, builder: &mut trajoptlib::DifferentialPathBuilder) {
         builder.add_progress_callback(diff_status_callback);
     }
-}
+}

src-core/src/generation/transformers/constraints.rs

@@ -178,4 +178,4 @@ impl DiffyGenerationTransformer for ConstraintSetter {
             };
         }
     }
-}
+}

src-core/src/generation/transformers/drivetrain_and_bumpers.rs

@@ -67,4 +67,4 @@ impl DiffyGenerationTransformer for DrivetrainAndBumpersSetter {
             config.bumper.left + config.bumper.right,
         );
     }
-}
+}

src-core/src/generation/transformers/interval_count.rs

@@ -77,4 +77,4 @@ impl DiffyGenerationTransformer for IntervalCountSetter {
     fn transform(&self, builder: &mut trajoptlib::DifferentialPathBuilder) {
         self.transform(builder);
     }
-}
+}

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)
+
 # Set default build type to release with debug info (i.e. release mode
 # optimizations are performed, but debug info still exists).
 if(NOT CMAKE_BUILD_TYPE)
@@ -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/0004-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

@@ -12,7 +12,8 @@ fn main() {
     if cfg!(target_os = "windows") {
         cmake_config
             .generator("Visual Studio 17 2022")
-            .cxxflag("/EHsc");
+            .cxxflag("/EHsc")
+            .cxxflag("/utf-8");
     }
 
     let cmake_dest = cmake_config.build();
@@ -24,6 +25,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") {
@@ -48,6 +51,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/0004-allwpilib_patch.py

@@ -0,0 +1,122 @@
+#!/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()
+
+
+if __name__ == "__main__":
+    main()

trajoptlib/include/.styleguide

@@ -14,5 +14,6 @@ modifiableFileExclude {
 
 includeOtherLibs {
   ^Eigen/
+  ^frc/
   ^sleipnir/
 }

trajoptlib/include/trajopt/path/PathBuilder.hpp

@@ -13,6 +13,7 @@
 #include "trajopt/obstacle/Obstacle.hpp"
 #include "trajopt/path/Path.hpp"
 #include "trajopt/util/GenerateLinearInitialGuess.hpp"
+#include "trajopt/util/GenerateSplineInitialGuess.hpp"
 #include "trajopt/util/SymbolExports.hpp"
 
 namespace trajopt {
@@ -325,6 +326,17 @@ class TRAJOPT_DLLEXPORT PathBuilder {
                                                 controlIntervalCounts);
   }
 
+  /**
+   * Calculate a discrete, linear initial guess of the x, y, and heading
+   * of the robot that goes through each segment.
+   *
+   * @return the initial guess, as a solution
+   */
+  Solution CalculateSplineInitialGuess() const {
+    return GenerateSplineInitialGuess<Solution>(initialGuessPoints,
+                                                controlIntervalCounts);
+  }
+
   /**
    * Add a callback to retrieve the state of the solver as a Solution.
    *