diff --git a/.github/workflows/build-cpp.yml b/.github/workflows/build-cpp.yml index 2a7f70db..03cd296c 100644 --- a/.github/workflows/build-cpp.yml +++ b/.github/workflows/build-cpp.yml @@ -8,7 +8,7 @@ concurrency: jobs: build: - timeout-minutes: 10 + timeout-minutes: 15 strategy: fail-fast: false matrix: diff --git a/.github/workflows/build-rust.yml b/.github/workflows/build-rust.yml index f42fce92..2d1ed871 100644 --- a/.github/workflows/build-rust.yml +++ b/.github/workflows/build-rust.yml @@ -8,7 +8,7 @@ concurrency: jobs: build: - timeout-minutes: 10 + timeout-minutes: 15 strategy: fail-fast: false matrix: diff --git a/.github/workflows/sanitizers.yml b/.github/workflows/sanitizers.yml index 17f6a3ea..fa5006b2 100644 --- a/.github/workflows/sanitizers.yml +++ b/.github/workflows/sanitizers.yml @@ -8,7 +8,7 @@ concurrency: jobs: build: - timeout-minutes: 10 + timeout-minutes: 15 strategy: fail-fast: false matrix: diff --git a/.styleguide b/.styleguide index bbe0ea3e..eb3bc54e 100644 --- a/.styleguide +++ b/.styleguide @@ -9,6 +9,7 @@ cppSrcFileInclude { } modifiableFileExclude { + \.patch$ expected$ \.patch$ } diff --git a/CMakeLists.txt b/CMakeLists.txt index 50989cce..26a9c6b8 100644 --- a/CMakeLists.txt +++ b/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/build.rs b/build.rs index 73b66a2e..7803658e 100644 --- a/build.rs +++ b/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"); diff --git a/cmake/allwpilib-disable-psabi-warning.patch b/cmake/allwpilib-disable-psabi-warning.patch new file mode 100644 index 00000000..1ba8b8e3 --- /dev/null +++ b/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/cmake/allwpilib_patch.py b/cmake/allwpilib_patch.py new file mode 100755 index 00000000..618f08e9 --- /dev/null +++ b/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/examples/swerve.rs b/examples/swerve.rs index 23aaa4e1..42d439ff 100644 --- a/examples/swerve.rs +++ b/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/include/.styleguide b/include/.styleguide index 63357f53..9be0a2b8 100644 --- a/include/.styleguide +++ b/include/.styleguide @@ -14,5 +14,6 @@ modifiableFileExclude { includeOtherLibs { ^Eigen/ + ^frc/ ^sleipnir/ } diff --git a/include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp b/include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp index 6b76d927..74103377 100644 --- a/include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp +++ b/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/include/trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp b/include/trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp index 9909d662..fb811ce9 100644 --- a/include/trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp +++ b/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/include/trajopt/path/SwervePathBuilder.hpp b/include/trajopt/path/SwervePathBuilder.hpp index abd6dd24..bb3ddfe9 100644 --- a/include/trajopt/path/SwervePathBuilder.hpp +++ b/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/include/trajopt/util/GenerateSplineInitialGuess.hpp b/include/trajopt/util/GenerateSplineInitialGuess.hpp new file mode 100644 index 00000000..59cbf843 --- /dev/null +++ b/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/src/.styleguide b/src/.styleguide index 9fff8133..56fe6c35 100644 --- a/src/.styleguide +++ b/src/.styleguide @@ -14,6 +14,9 @@ modifiableFileExclude { includeOtherLibs { ^Eigen/ + ^frc/ ^rust/ ^sleipnir/ + ^units/ + ^wpi/ } diff --git a/src/lib.rs b/src/lib.rs index a9e0ad13..c5725e12 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -171,6 +171,12 @@ mod ffi { fn cancel_all(self: Pin<&mut SwervePathBuilderImpl>); fn new_swerve_path_builder_impl() -> UniquePtr; + + fn calculate_linear_initial_guess(self: &SwervePathBuilderImpl) -> HolonomicTrajectory; + + fn calculate_spline_initial_guess(self: &SwervePathBuilderImpl) -> HolonomicTrajectory; + + fn calculate_control_interval_counts(self: &SwervePathBuilderImpl) -> Vec; } } @@ -429,6 +435,18 @@ impl SwervePathBuilder { pub fn cancel_all(&mut self) { crate::ffi::SwervePathBuilderImpl::cancel_all(self.path.pin_mut()); } + + pub fn calculate_linear_initial_guess(&self) -> HolonomicTrajectory { + self.path.calculate_linear_initial_guess() + } + + pub fn calculate_spline_initial_guess(&self) -> HolonomicTrajectory { + self.path.calculate_spline_initial_guess() + } + + pub fn calculate_control_interval_counts(&self) -> Vec { + self.path.calculate_control_interval_counts() + } } impl Default for SwervePathBuilder { diff --git a/src/path/SwervePathBuilder.cpp b/src/path/SwervePathBuilder.cpp index f9404981..bde69d4b 100644 --- a/src/path/SwervePathBuilder.cpp +++ b/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/src/spline/CubicHermitePoseSplineHolonomic.hpp b/src/spline/CubicHermitePoseSplineHolonomic.hpp new file mode 100644 index 00000000..320139a4 --- /dev/null +++ b/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/src/spline/CubicHermiteSpline1d.hpp b/src/spline/CubicHermiteSpline1d.hpp new file mode 100644 index 00000000..aa5e3a40 --- /dev/null +++ b/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/src/spline/Spline1d.hpp b/src/spline/Spline1d.hpp new file mode 100644 index 00000000..3d36675f --- /dev/null +++ b/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/src/spline/SplineParameterizer.cpp b/src/spline/SplineParameterizer.cpp new file mode 100644 index 00000000..c8905491 --- /dev/null +++ b/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/src/spline/SplineParameterizer.hpp b/src/spline/SplineParameterizer.hpp new file mode 100644 index 00000000..f652acba --- /dev/null +++ b/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/src/spline/SplineUtil.cpp b/src/spline/SplineUtil.cpp new file mode 100644 index 00000000..ff605776 --- /dev/null +++ b/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/src/spline/SplineUtil.hpp b/src/spline/SplineUtil.hpp new file mode 100644 index 00000000..5e875e2f --- /dev/null +++ b/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 diff --git a/src/trajoptlibrust.cpp b/src/trajoptlibrust.cpp index 6b05d1e7..5a34f5f1 100644 --- a/src/trajoptlibrust.cpp +++ b/src/trajoptlibrust.cpp @@ -239,6 +239,49 @@ void SwervePathBuilderImpl::cancel_all() { path.CancelAll(); } +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 SwervePathBuilderImpl::calculate_linear_initial_guess() + const { + return _convert_sol_to_holonomic_trajectory(path.CalculateInitialGuess()); +} + +HolonomicTrajectory SwervePathBuilderImpl::calculate_spline_initial_guess() + const { + return _convert_sol_to_holonomic_trajectory( + path.CalculateSplineInitialGuess()); +} + +rust::Vec +SwervePathBuilderImpl::calculate_control_interval_counts() const { + auto cppCounts = path.CalculateControlIntervalCounts(); + rust::Vec rustCounts; + for (const auto count : cppCounts) { + rustCounts.emplace_back(count); + } + return rustCounts; +} + std::unique_ptr new_swerve_path_builder_impl() { return std::make_unique(SwervePathBuilderImpl()); } diff --git a/src/trajoptlibrust.hpp b/src/trajoptlibrust.hpp index 8fc64cdd..bf2adfd6 100644 --- a/src/trajoptlibrust.hpp +++ b/src/trajoptlibrust.hpp @@ -65,6 +65,10 @@ class SwervePathBuilderImpl { 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; + void cancel_all(); private: