Use cubic spline for initial guess

.github/workflows/build-cpp.yml

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

.github/workflows/build-rust.yml

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

.github/workflows/sanitizers.yml

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

.styleguide

@@ -9,6 +9,7 @@ cppSrcFileInclude {
 }
 
 modifiableFileExclude {
+  \.patch$
   expected$
   \.patch$
 }

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

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/trajoptlibrust.hpp");
     println!("cargo:rerun-if-changed=src/trajoptlibrust.cpp");

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)

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 "$<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,
+    )
+
+
+if __name__ == "__main__":
+    main()

examples/swerve.rs

@@ -1,37 +1,37 @@
-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 {
                 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,
             },
             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));
 }

include/.styleguide

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

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

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