From 47f51efb4880e71375558ce4b1a7bfc6dc378ba4 Mon Sep 17 00:00:00 2001 From: Lewy Seiden Date: Sat, 21 Oct 2023 16:02:01 -0700 Subject: [PATCH 01/17] started playing with dt --- CMakeCache.txt | 56 +++++++++++++++++++ CMakeFiles/cmake.check_cache | 1 + src/optimization/SwerveTrajoptUtil.inc | 2 +- .../algorithms/SwerveDiscreteOptimal.inc | 7 +-- 4 files changed, 60 insertions(+), 6 deletions(-) create mode 100644 CMakeCache.txt create mode 100644 CMakeFiles/cmake.check_cache diff --git a/CMakeCache.txt b/CMakeCache.txt new file mode 100644 index 00000000..4e4b3755 --- /dev/null +++ b/CMakeCache.txt @@ -0,0 +1,56 @@ +# This is the CMakeCache file. +# For build in directory: c:/Users/lzzm7/Documents/GitHub/TrajoptLib +# It was generated by CMake: C:/Program Files/CMake/bin/cmake.exe +# You can edit this file to change values found and used by cmake. +# If you do not want to change any of the values, simply exit the editor. +# If you do want to change a value, simply edit, save, and exit the editor. +# The syntax for the file is as follows: +# KEY:TYPE=VALUE +# KEY is the name of a variable in the cache. +# TYPE is a hint to GUIs for the type of VALUE, DO NOT EDIT TYPE!. +# VALUE is the current value for the KEY. + +######################## +# EXTERNAL cache entries +######################## + +//Value Computed by CMake. +CMAKE_FIND_PACKAGE_REDIRECTS_DIR:STATIC=C:/Users/lzzm7/Documents/GitHub/TrajoptLib/CMakeFiles/pkgRedirects + + +######################## +# INTERNAL cache entries +######################## + +//This is the directory where this CMakeCache.txt was created +CMAKE_CACHEFILE_DIR:INTERNAL=c:/Users/lzzm7/Documents/GitHub/TrajoptLib +//Major version of cmake used to create the current loaded cache +CMAKE_CACHE_MAJOR_VERSION:INTERNAL=3 +//Minor version of cmake used to create the current loaded cache +CMAKE_CACHE_MINOR_VERSION:INTERNAL=27 +//Patch version of cmake used to create the current loaded cache +CMAKE_CACHE_PATCH_VERSION:INTERNAL=0 +//Path to CMake executable. +CMAKE_COMMAND:INTERNAL=C:/Program Files/CMake/bin/cmake.exe +//Path to cpack program executable. +CMAKE_CPACK_COMMAND:INTERNAL=C:/Program Files/CMake/bin/cpack.exe +//Path to ctest program executable. +CMAKE_CTEST_COMMAND:INTERNAL=C:/Program Files/CMake/bin/ctest.exe +//Name of external makefile project generator. +CMAKE_EXTRA_GENERATOR:INTERNAL= +//Name of generator. +CMAKE_GENERATOR:INTERNAL=Visual Studio 17 2022 +//Generator instance identifier. +CMAKE_GENERATOR_INSTANCE:INTERNAL= +//Name of generator platform. +CMAKE_GENERATOR_PLATFORM:INTERNAL= +//Name of generator toolset. +CMAKE_GENERATOR_TOOLSET:INTERNAL= +//Source directory with the top level CMakeLists.txt file for this +// project +CMAKE_HOME_DIRECTORY:INTERNAL=C:/Users/lzzm7/Documents/GitHub/TrajoptLib +//number of local generators +CMAKE_NUMBER_OF_MAKEFILES:INTERNAL=1 +//Path to CMake installation. +CMAKE_ROOT:INTERNAL=C:/Program Files/CMake/share/cmake-3.27 + diff --git a/CMakeFiles/cmake.check_cache b/CMakeFiles/cmake.check_cache new file mode 100644 index 00000000..3dccd731 --- /dev/null +++ b/CMakeFiles/cmake.check_cache @@ -0,0 +1 @@ +# This file is generated by cmake for dependency checking of the CMakeCache.txt file diff --git a/src/optimization/SwerveTrajoptUtil.inc b/src/optimization/SwerveTrajoptUtil.inc index 266c7b0a..8d51e2d9 100644 --- a/src/optimization/SwerveTrajoptUtil.inc +++ b/src/optimization/SwerveTrajoptUtil.inc @@ -66,9 +66,9 @@ void ApplyKinematicsConstraints( for (size_t wptIdx = 1; wptIdx < wptCnt; ++wptIdx) { size_t N_sgmt = N.at(wptIdx - 1); - auto dt_sgmt = dt.at(wptIdx - 1); for (size_t sampIdx = 0; sampIdx < N_sgmt; ++sampIdx) { size_t idx = GetIdx(N, wptIdx, sampIdx); + auto dt_sgmt = dt.at(idx - 1); auto x_n = x.at(idx); auto x_n_1 = x.at(idx - 1); auto y_n = y.at(idx); diff --git a/src/optimization/algorithms/SwerveDiscreteOptimal.inc b/src/optimization/algorithms/SwerveDiscreteOptimal.inc index 7fecf038..1f904c98 100644 --- a/src/optimization/algorithms/SwerveDiscreteOptimal.inc +++ b/src/optimization/algorithms/SwerveDiscreteOptimal.inc @@ -60,7 +60,7 @@ SwerveDiscreteOptimal::SwerveDiscreteOptimal( _Fy.reserve(moduleCnt); } - dt.reserve(sgmtCnt); + dt.reserve(sampTot); for (size_t idx = 0; idx < sampTot; ++idx) { x.emplace_back(opti.DecisionVariable()); @@ -72,6 +72,7 @@ SwerveDiscreteOptimal::SwerveDiscreteOptimal( ax.emplace_back(opti.DecisionVariable()); ay.emplace_back(opti.DecisionVariable()); alpha.emplace_back(opti.DecisionVariable()); + dt.emplace_back(opti.DecisionVariable()); for (size_t moduleIdx = 0; moduleIdx < moduleCnt; ++moduleIdx) { Fx.at(idx).emplace_back(opti.DecisionVariable()); @@ -79,10 +80,6 @@ SwerveDiscreteOptimal::SwerveDiscreteOptimal( } } - for (size_t sgmtIdx = 0; sgmtIdx < sgmtCnt; ++sgmtIdx) { - dt.emplace_back(opti.DecisionVariable()); - } - ApplyDiscreteTimeObjective(opti, dt, N); ApplyKinematicsConstraints(opti, x, y, theta, vx, vy, omega, ax, ay, alpha, dt, N); From 991d6d7e4468fe56b121bdb2629ef475bb978b0e Mon Sep 17 00:00:00 2001 From: Lewy Seiden Date: Sat, 21 Oct 2023 16:09:45 -0700 Subject: [PATCH 02/17] removed cmake cache --- CMakeCache.txt | 56 -------------------------------------------------- 1 file changed, 56 deletions(-) delete mode 100644 CMakeCache.txt diff --git a/CMakeCache.txt b/CMakeCache.txt deleted file mode 100644 index 4e4b3755..00000000 --- a/CMakeCache.txt +++ /dev/null @@ -1,56 +0,0 @@ -# This is the CMakeCache file. -# For build in directory: c:/Users/lzzm7/Documents/GitHub/TrajoptLib -# It was generated by CMake: C:/Program Files/CMake/bin/cmake.exe -# You can edit this file to change values found and used by cmake. -# If you do not want to change any of the values, simply exit the editor. -# If you do want to change a value, simply edit, save, and exit the editor. -# The syntax for the file is as follows: -# KEY:TYPE=VALUE -# KEY is the name of a variable in the cache. -# TYPE is a hint to GUIs for the type of VALUE, DO NOT EDIT TYPE!. -# VALUE is the current value for the KEY. - -######################## -# EXTERNAL cache entries -######################## - -//Value Computed by CMake. -CMAKE_FIND_PACKAGE_REDIRECTS_DIR:STATIC=C:/Users/lzzm7/Documents/GitHub/TrajoptLib/CMakeFiles/pkgRedirects - - -######################## -# INTERNAL cache entries -######################## - -//This is the directory where this CMakeCache.txt was created -CMAKE_CACHEFILE_DIR:INTERNAL=c:/Users/lzzm7/Documents/GitHub/TrajoptLib -//Major version of cmake used to create the current loaded cache -CMAKE_CACHE_MAJOR_VERSION:INTERNAL=3 -//Minor version of cmake used to create the current loaded cache -CMAKE_CACHE_MINOR_VERSION:INTERNAL=27 -//Patch version of cmake used to create the current loaded cache -CMAKE_CACHE_PATCH_VERSION:INTERNAL=0 -//Path to CMake executable. -CMAKE_COMMAND:INTERNAL=C:/Program Files/CMake/bin/cmake.exe -//Path to cpack program executable. -CMAKE_CPACK_COMMAND:INTERNAL=C:/Program Files/CMake/bin/cpack.exe -//Path to ctest program executable. -CMAKE_CTEST_COMMAND:INTERNAL=C:/Program Files/CMake/bin/ctest.exe -//Name of external makefile project generator. -CMAKE_EXTRA_GENERATOR:INTERNAL= -//Name of generator. -CMAKE_GENERATOR:INTERNAL=Visual Studio 17 2022 -//Generator instance identifier. -CMAKE_GENERATOR_INSTANCE:INTERNAL= -//Name of generator platform. -CMAKE_GENERATOR_PLATFORM:INTERNAL= -//Name of generator toolset. -CMAKE_GENERATOR_TOOLSET:INTERNAL= -//Source directory with the top level CMakeLists.txt file for this -// project -CMAKE_HOME_DIRECTORY:INTERNAL=C:/Users/lzzm7/Documents/GitHub/TrajoptLib -//number of local generators -CMAKE_NUMBER_OF_MAKEFILES:INTERNAL=1 -//Path to CMake installation. -CMAKE_ROOT:INTERNAL=C:/Program Files/CMake/share/cmake-3.27 - From 470f3fbce3841e93e842ee7b2ddc1451be7c846b Mon Sep 17 00:00:00 2001 From: Lewy Seiden Date: Sat, 21 Oct 2023 18:29:32 -0700 Subject: [PATCH 03/17] made solution return actual dts --- src/optimization/SwerveTrajoptUtil.inc | 27 ++++++++++---------------- 1 file changed, 10 insertions(+), 17 deletions(-) diff --git a/src/optimization/SwerveTrajoptUtil.inc b/src/optimization/SwerveTrajoptUtil.inc index 8d51e2d9..f8c170e4 100644 --- a/src/optimization/SwerveTrajoptUtil.inc +++ b/src/optimization/SwerveTrajoptUtil.inc @@ -68,7 +68,7 @@ void ApplyKinematicsConstraints( size_t N_sgmt = N.at(wptIdx - 1); for (size_t sampIdx = 0; sampIdx < N_sgmt; ++sampIdx) { size_t idx = GetIdx(N, wptIdx, sampIdx); - auto dt_sgmt = dt.at(idx - 1); + auto dt_n = dt.at(idx - 1); auto x_n = x.at(idx); auto x_n_1 = x.at(idx - 1); auto y_n = y.at(idx); @@ -84,12 +84,13 @@ void ApplyKinematicsConstraints( auto ax_n = ax.at(idx); auto ay_n = ay.at(idx); auto alpha_n = alpha.at(idx); - opti.SubjectTo(x_n_1 + vx_n * dt_sgmt == x_n); - opti.SubjectTo(y_n_1 + vy_n * dt_sgmt == y_n); - opti.SubjectTo(theta_n_1 + omega_n * dt_sgmt == theta_n); - opti.SubjectTo(vx_n_1 + ax_n * dt_sgmt == vx_n); - opti.SubjectTo(vy_n_1 + ay_n * dt_sgmt == vy_n); - opti.SubjectTo(omega_n_1 + alpha_n * dt_sgmt == omega_n); + opti.SubjectTo(dt_n >= 0); + opti.SubjectTo(x_n_1 + vx_n * dt_n == x_n); + opti.SubjectTo(y_n_1 + vy_n * dt_n == y_n); + opti.SubjectTo(theta_n_1 + omega_n * dt_n == theta_n); + opti.SubjectTo(vx_n_1 + ax_n * dt_n == vx_n); + opti.SubjectTo(vy_n_1 + ay_n * dt_n == vy_n); + opti.SubjectTo(omega_n_1 + alpha_n * dt_n == omega_n); } } } @@ -154,16 +155,8 @@ SwerveSolution ConstructSwerveSolution( const std::vector& alpha, const std::vector>& Fx, const std::vector>& Fy, const std::vector& dt, const std::vector& N) { - std::vector dtPerSamp; - for (size_t sgmtIdx = 0; sgmtIdx < N.size(); ++sgmtIdx) { - size_t N_sgmt = N.at(sgmtIdx); - Expr dt_sgmt = dt.at(sgmtIdx); - double dt_val = opti.SolutionValue(dt_sgmt); - for (size_t i = 0; i < N_sgmt; ++i) { - dtPerSamp.push_back(dt_val); - } - } - return SwerveSolution{dtPerSamp, + + return SwerveSolution{RowSolutionValue(opti, dt), RowSolutionValue(opti, x), RowSolutionValue(opti, y), RowSolutionValue(opti, theta), From d4f77df3442685a11f915f83c4b3a266b4d501ea Mon Sep 17 00:00:00 2001 From: Lewy Seiden Date: Sat, 21 Oct 2023 19:18:00 -0700 Subject: [PATCH 04/17] unfucked(?) variable dt --- src/optimization/TrajoptUtil.inc | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/optimization/TrajoptUtil.inc b/src/optimization/TrajoptUtil.inc index 0603bd00..f53a9b92 100644 --- a/src/optimization/TrajoptUtil.inc +++ b/src/optimization/TrajoptUtil.inc @@ -60,14 +60,17 @@ template void ApplyDiscreteTimeObjective(Opti& opti, std::vector& dt, const std::vector N) { Expr T_tot = 0; - for (size_t sgmtIdx = 0; sgmtIdx < N.size(); ++sgmtIdx) { - auto& dt_sgmt = dt.at(sgmtIdx); - auto N_sgmt = N.at(sgmtIdx); - auto T_sgmt = dt_sgmt * N_sgmt; - T_tot += T_sgmt; + size_t wptCnt = N.size() + 1; + for (size_t wptIdx = 1; wptIdx < wptCnt; ++wptIdx) { + size_t N_sgmt = N.at(wptIdx - 1); + for (size_t sampIdx = 0; sampIdx < N_sgmt; ++sampIdx) { + size_t idx = GetIdx(N, wptIdx, sampIdx); + auto dt_n = dt.at(idx - 1); + T_tot += dt_n; - opti.SubjectTo(dt_sgmt >= 0); - opti.SetInitial(dt_sgmt, 5.0 / N_sgmt); + opti.SubjectTo(dt_n >= 0); + opti.SetInitial(dt_n, 5.0 / N_sgmt); + } } opti.Minimize(std::move(T_tot)); } From 4b5e34077beb0c93897e7853e342d9afe298427b Mon Sep 17 00:00:00 2001 From: Lewy Seiden Date: Sat, 21 Oct 2023 19:50:42 -0700 Subject: [PATCH 05/17] added min time to dt --- src/optimization/TrajoptUtil.inc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/optimization/TrajoptUtil.inc b/src/optimization/TrajoptUtil.inc index f53a9b92..c5525164 100644 --- a/src/optimization/TrajoptUtil.inc +++ b/src/optimization/TrajoptUtil.inc @@ -68,7 +68,7 @@ void ApplyDiscreteTimeObjective(Opti& opti, std::vector& dt, auto dt_n = dt.at(idx - 1); T_tot += dt_n; - opti.SubjectTo(dt_n >= 0); + opti.SubjectTo(dt_n >= 0.01 / N_sgmt); opti.SetInitial(dt_n, 5.0 / N_sgmt); } } From e2059d1ae4b6bad5cb4f7951f3b51d339a7dd2d3 Mon Sep 17 00:00:00 2001 From: Lewy Seiden Date: Sat, 21 Oct 2023 23:05:52 -0700 Subject: [PATCH 06/17] stopped double constraining dt --- src/optimization/TrajoptUtil.inc | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/optimization/TrajoptUtil.inc b/src/optimization/TrajoptUtil.inc index c5525164..5586ea7a 100644 --- a/src/optimization/TrajoptUtil.inc +++ b/src/optimization/TrajoptUtil.inc @@ -67,8 +67,6 @@ void ApplyDiscreteTimeObjective(Opti& opti, std::vector& dt, size_t idx = GetIdx(N, wptIdx, sampIdx); auto dt_n = dt.at(idx - 1); T_tot += dt_n; - - opti.SubjectTo(dt_n >= 0.01 / N_sgmt); opti.SetInitial(dt_n, 5.0 / N_sgmt); } } From 477b713d5f65fb3e0d0603163eadee75d86dee32 Mon Sep 17 00:00:00 2001 From: Lewy Seiden Date: Sat, 21 Oct 2023 23:28:31 -0700 Subject: [PATCH 07/17] changed kinematics constraint dt sampling to current sample instead of prev --- src/optimization/SwerveTrajoptUtil.inc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/optimization/SwerveTrajoptUtil.inc b/src/optimization/SwerveTrajoptUtil.inc index f8c170e4..c032358a 100644 --- a/src/optimization/SwerveTrajoptUtil.inc +++ b/src/optimization/SwerveTrajoptUtil.inc @@ -68,7 +68,8 @@ void ApplyKinematicsConstraints( size_t N_sgmt = N.at(wptIdx - 1); for (size_t sampIdx = 0; sampIdx < N_sgmt; ++sampIdx) { size_t idx = GetIdx(N, wptIdx, sampIdx); - auto dt_n = dt.at(idx - 1); + auto dt_n = dt.at(idx); + auto dt_n_1 = dt.at(idx - 1); auto x_n = x.at(idx); auto x_n_1 = x.at(idx - 1); auto y_n = y.at(idx); From 2b32b9dc06534ab122e8decdfaacf388ce3597f9 Mon Sep 17 00:00:00 2001 From: Lewy Seiden Date: Mon, 23 Oct 2023 10:10:20 -0700 Subject: [PATCH 08/17] changed dt sample in discrete time goal --- src/optimization/TrajoptUtil.inc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/optimization/TrajoptUtil.inc b/src/optimization/TrajoptUtil.inc index 5586ea7a..dd9bf0a1 100644 --- a/src/optimization/TrajoptUtil.inc +++ b/src/optimization/TrajoptUtil.inc @@ -65,7 +65,7 @@ void ApplyDiscreteTimeObjective(Opti& opti, std::vector& dt, size_t N_sgmt = N.at(wptIdx - 1); for (size_t sampIdx = 0; sampIdx < N_sgmt; ++sampIdx) { size_t idx = GetIdx(N, wptIdx, sampIdx); - auto dt_n = dt.at(idx - 1); + auto dt_n = dt.at(idx); T_tot += dt_n; opti.SetInitial(dt_n, 5.0 / N_sgmt); } From 841e7865e3af4779edca51f1ee9384ef0f43ccc3 Mon Sep 17 00:00:00 2001 From: Lewy Seiden Date: Mon, 23 Oct 2023 11:29:53 -0700 Subject: [PATCH 09/17] added upper bound to dt --- src/optimization/SwerveTrajoptUtil.inc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/optimization/SwerveTrajoptUtil.inc b/src/optimization/SwerveTrajoptUtil.inc index c032358a..63805ece 100644 --- a/src/optimization/SwerveTrajoptUtil.inc +++ b/src/optimization/SwerveTrajoptUtil.inc @@ -86,6 +86,7 @@ void ApplyKinematicsConstraints( auto ay_n = ay.at(idx); auto alpha_n = alpha.at(idx); opti.SubjectTo(dt_n >= 0); + opti.SubjectTo(dt_n <= 0.1); opti.SubjectTo(x_n_1 + vx_n * dt_n == x_n); opti.SubjectTo(y_n_1 + vy_n * dt_n == y_n); opti.SubjectTo(theta_n_1 + omega_n * dt_n == theta_n); From 72e34f23d266d6c50a983bf713da38c6366009fd Mon Sep 17 00:00:00 2001 From: Lewy Seiden Date: Sat, 28 Oct 2023 23:45:31 -0700 Subject: [PATCH 10/17] added control interval count plumbing to rust bindings --- rust/include/trajoptlib.h | 1 + rust/src/lib.rs | 5 +++++ rust/src/trajoptlib.cc | 15 ++++++++++++++- 3 files changed, 20 insertions(+), 1 deletion(-) diff --git a/rust/include/trajoptlib.h b/rust/include/trajoptlib.h index e93a9632..149b108d 100644 --- a/rust/include/trajoptlib.h +++ b/rust/include/trajoptlib.h @@ -18,6 +18,7 @@ class SwervePathBuilderImpl { public: void set_drivetrain(const SwerveDrivetrain& drivetrain); void set_bumpers(double length, double width); + void set_control_interval_counts(const rust::Vec counts); void pose_wpt(size_t idx, double x, double y, double heading); void translation_wpt(size_t idx, double x, double y, double heading_guess); diff --git a/rust/src/lib.rs b/rust/src/lib.rs index 451613a0..f48480da 100644 --- a/rust/src/lib.rs +++ b/rust/src/lib.rs @@ -46,6 +46,7 @@ mod ffi { fn set_drivetrain(self: Pin<&mut SwervePathBuilderImpl>, drivetrain: &SwerveDrivetrain); fn set_bumpers(self: Pin<&mut SwervePathBuilderImpl>, length: f64, width: f64); + fn set_control_interval_counts(self: Pin<&mut SwervePathBuilderImpl>, counts: Vec); fn pose_wpt( self: Pin<&mut SwervePathBuilderImpl>, @@ -168,6 +169,10 @@ impl SwervePathBuilder { crate::ffi::SwervePathBuilderImpl::set_bumpers(self.path.pin_mut(), length, width); } + pub fn set_control_interval_counts(&mut self, counts: Vec) { + crate::ffi::SwervePathBuilderImpl::set_control_interval_counts(self.path.pin_mut(), counts); + } + pub fn pose_wpt(&mut self, idx: usize, x: f64, y: f64, heading: f64) { crate::ffi::SwervePathBuilderImpl::pose_wpt(self.path.pin_mut(), idx, x, y, heading); } diff --git a/rust/src/trajoptlib.cc b/rust/src/trajoptlib.cc index dc2e4a6f..95c9392b 100644 --- a/rust/src/trajoptlib.cc +++ b/rust/src/trajoptlib.cc @@ -83,6 +83,18 @@ void SwervePathBuilderImpl::set_drivetrain(const SwerveDrivetrain& drivetrain) { path.SetDrivetrain(_convert_swerve_drivetrain(drivetrain)); } +size_t _convert_count(const size_t& count) { + return count; +} + +void SwervePathBuilderImpl::set_control_interval_counts( + const rust::Vec counts) { + std::vector converted_counts = + _rust_vec_to_cpp_vector + (counts); + path.ControlIntervalCounts(std::move(converted_counts)); +} + void SwervePathBuilderImpl::set_bumpers(double length, double width) { path.AddBumpers(trajopt::Bumpers{ .safetyDistance = 0.0, @@ -107,7 +119,8 @@ void SwervePathBuilderImpl::empty_wpt(size_t idx, double x_guess, double y_guess path.WptInitialGuessPoint(idx, {x_guess, y_guess, heading_guess}); } -void SwervePathBuilderImpl::sgmt_initial_guess_points(size_t from_idx, const rust::Vec& guess_points) { +void SwervePathBuilderImpl::sgmt_initial_guess_points( + size_t from_idx, const rust::Vec& guess_points) { std::vector convertedGuessPoints = _rust_vec_to_cpp_vector Date: Sun, 29 Oct 2023 13:07:21 -0700 Subject: [PATCH 11/17] increased maximum dt --- src/optimization/SwerveTrajoptUtil.inc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/optimization/SwerveTrajoptUtil.inc b/src/optimization/SwerveTrajoptUtil.inc index 63805ece..d7e4c601 100644 --- a/src/optimization/SwerveTrajoptUtil.inc +++ b/src/optimization/SwerveTrajoptUtil.inc @@ -86,7 +86,7 @@ void ApplyKinematicsConstraints( auto ay_n = ay.at(idx); auto alpha_n = alpha.at(idx); opti.SubjectTo(dt_n >= 0); - opti.SubjectTo(dt_n <= 0.1); + opti.SubjectTo(dt_n <= 0.2); opti.SubjectTo(x_n_1 + vx_n * dt_n == x_n); opti.SubjectTo(y_n_1 + vy_n * dt_n == y_n); opti.SubjectTo(theta_n_1 + omega_n * dt_n == theta_n); From 4df3d19eda0817d5198d58a36dfa767b3176da9f Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Fri, 3 Nov 2023 17:55:36 -0700 Subject: [PATCH 12/17] Run wpiformat --- rust/src/trajoptlib.cc | 2 +- src/optimization/SwerveTrajoptUtil.inc | 20 +++++++------------- src/optimization/TrajoptUtil.inc | 2 +- 3 files changed, 9 insertions(+), 15 deletions(-) diff --git a/rust/src/trajoptlib.cc b/rust/src/trajoptlib.cc index 95c9392b..a2875107 100644 --- a/rust/src/trajoptlib.cc +++ b/rust/src/trajoptlib.cc @@ -89,7 +89,7 @@ size_t _convert_count(const size_t& count) { void SwervePathBuilderImpl::set_control_interval_counts( const rust::Vec counts) { - std::vector converted_counts = + std::vector converted_counts = _rust_vec_to_cpp_vector (counts); path.ControlIntervalCounts(std::move(converted_counts)); diff --git a/src/optimization/SwerveTrajoptUtil.inc b/src/optimization/SwerveTrajoptUtil.inc index d7e4c601..36cf8172 100644 --- a/src/optimization/SwerveTrajoptUtil.inc +++ b/src/optimization/SwerveTrajoptUtil.inc @@ -157,19 +157,13 @@ SwerveSolution ConstructSwerveSolution( const std::vector& alpha, const std::vector>& Fx, const std::vector>& Fy, const std::vector& dt, const std::vector& N) { - - return SwerveSolution{RowSolutionValue(opti, dt), - RowSolutionValue(opti, x), - RowSolutionValue(opti, y), - RowSolutionValue(opti, theta), - RowSolutionValue(opti, vx), - RowSolutionValue(opti, vy), - RowSolutionValue(opti, omega), - RowSolutionValue(opti, ax), - RowSolutionValue(opti, ay), - RowSolutionValue(opti, alpha), - MatrixSolutionValue(opti, Fx), - MatrixSolutionValue(opti, Fy)}; + return SwerveSolution{ + RowSolutionValue(opti, dt), RowSolutionValue(opti, x), + RowSolutionValue(opti, y), RowSolutionValue(opti, theta), + RowSolutionValue(opti, vx), RowSolutionValue(opti, vy), + RowSolutionValue(opti, omega), RowSolutionValue(opti, ax), + RowSolutionValue(opti, ay), RowSolutionValue(opti, alpha), + MatrixSolutionValue(opti, Fx), MatrixSolutionValue(opti, Fy)}; } } // namespace trajopt diff --git a/src/optimization/TrajoptUtil.inc b/src/optimization/TrajoptUtil.inc index dd9bf0a1..7112552d 100644 --- a/src/optimization/TrajoptUtil.inc +++ b/src/optimization/TrajoptUtil.inc @@ -60,7 +60,7 @@ template void ApplyDiscreteTimeObjective(Opti& opti, std::vector& dt, const std::vector N) { Expr T_tot = 0; - size_t wptCnt = N.size() + 1; + size_t wptCnt = N.size() + 1; for (size_t wptIdx = 1; wptIdx < wptCnt; ++wptIdx) { size_t N_sgmt = N.at(wptIdx - 1); for (size_t sampIdx = 0; sampIdx < N_sgmt; ++sampIdx) { From 8ae8b01617a4abcd81da8303009fc841996d506a Mon Sep 17 00:00:00 2001 From: Lewy Seiden Date: Sat, 4 Nov 2023 15:36:40 -0700 Subject: [PATCH 13/17] fixed control interval jumping --- src/optimization/SwerveTrajoptUtil.inc | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/src/optimization/SwerveTrajoptUtil.inc b/src/optimization/SwerveTrajoptUtil.inc index d7e4c601..a7c493d8 100644 --- a/src/optimization/SwerveTrajoptUtil.inc +++ b/src/optimization/SwerveTrajoptUtil.inc @@ -64,6 +64,13 @@ void ApplyKinematicsConstraints( const std::vector N) { size_t wptCnt = N.size() + 1; + for (size_t i = 0; i < dt.size(); i++) + { + opti.SubjectTo(dt.at(i) >= 0); + opti.SubjectTo(dt.at(i) <= 0.2); + } + + for (size_t wptIdx = 1; wptIdx < wptCnt; ++wptIdx) { size_t N_sgmt = N.at(wptIdx - 1); for (size_t sampIdx = 0; sampIdx < N_sgmt; ++sampIdx) { @@ -85,14 +92,12 @@ void ApplyKinematicsConstraints( auto ax_n = ax.at(idx); auto ay_n = ay.at(idx); auto alpha_n = alpha.at(idx); - opti.SubjectTo(dt_n >= 0); - opti.SubjectTo(dt_n <= 0.2); - opti.SubjectTo(x_n_1 + vx_n * dt_n == x_n); - opti.SubjectTo(y_n_1 + vy_n * dt_n == y_n); - opti.SubjectTo(theta_n_1 + omega_n * dt_n == theta_n); - opti.SubjectTo(vx_n_1 + ax_n * dt_n == vx_n); - opti.SubjectTo(vy_n_1 + ay_n * dt_n == vy_n); - opti.SubjectTo(omega_n_1 + alpha_n * dt_n == omega_n); + opti.SubjectTo(x_n_1 + vx_n * dt_n_1 == x_n); + opti.SubjectTo(y_n_1 + vy_n * dt_n_1 == y_n); + opti.SubjectTo(theta_n_1 + omega_n * dt_n_1 == theta_n); + opti.SubjectTo(vx_n_1 + ax_n * dt_n_1 == vx_n); + opti.SubjectTo(vy_n_1 + ay_n * dt_n_1 == vy_n); + opti.SubjectTo(omega_n_1 + alpha_n * dt_n_1 == omega_n); } } } From 47c71f614499c00993b2d5bcca31e405fbb8a30b Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Fri, 10 Nov 2023 19:47:24 -0800 Subject: [PATCH 14/17] Run wpiformat --- src/optimization/SwerveTrajoptUtil.inc | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/optimization/SwerveTrajoptUtil.inc b/src/optimization/SwerveTrajoptUtil.inc index 166e7f5b..626800b4 100644 --- a/src/optimization/SwerveTrajoptUtil.inc +++ b/src/optimization/SwerveTrajoptUtil.inc @@ -64,12 +64,10 @@ void ApplyKinematicsConstraints( const std::vector N) { size_t wptCnt = N.size() + 1; - for (size_t i = 0; i < dt.size(); i++) - { + for (size_t i = 0; i < dt.size(); i++) { opti.SubjectTo(dt.at(i) >= 0); opti.SubjectTo(dt.at(i) <= 0.2); } - for (size_t wptIdx = 1; wptIdx < wptCnt; ++wptIdx) { size_t N_sgmt = N.at(wptIdx - 1); From 6e4f3d5268f78ae96c05e26e3757d4dae086490f Mon Sep 17 00:00:00 2001 From: Lewy Seiden Date: Sat, 11 Nov 2023 14:51:18 -0800 Subject: [PATCH 15/17] reduced max dt --- src/optimization/SwerveTrajoptUtil.inc | 2 +- src/optimization/TrajoptUtil.inc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/optimization/SwerveTrajoptUtil.inc b/src/optimization/SwerveTrajoptUtil.inc index 166e7f5b..8dc64b10 100644 --- a/src/optimization/SwerveTrajoptUtil.inc +++ b/src/optimization/SwerveTrajoptUtil.inc @@ -67,7 +67,7 @@ void ApplyKinematicsConstraints( for (size_t i = 0; i < dt.size(); i++) { opti.SubjectTo(dt.at(i) >= 0); - opti.SubjectTo(dt.at(i) <= 0.2); + opti.SubjectTo(dt.at(i) <= 0.1); } diff --git a/src/optimization/TrajoptUtil.inc b/src/optimization/TrajoptUtil.inc index 7112552d..51ff8a7f 100644 --- a/src/optimization/TrajoptUtil.inc +++ b/src/optimization/TrajoptUtil.inc @@ -67,7 +67,7 @@ void ApplyDiscreteTimeObjective(Opti& opti, std::vector& dt, size_t idx = GetIdx(N, wptIdx, sampIdx); auto dt_n = dt.at(idx); T_tot += dt_n; - opti.SetInitial(dt_n, 5.0 / N_sgmt); + opti.SetInitial(dt_n, 0.1); } } opti.Minimize(std::move(T_tot)); From c85c02b1fe92a7fbb12adb3c262539f8d91ffa6b Mon Sep 17 00:00:00 2001 From: Lewy Seiden Date: Wed, 15 Nov 2023 08:58:46 -0800 Subject: [PATCH 16/17] reverted var-dt changes --- src/optimization/SwerveTrajoptUtil.inc | 48 +++++++++++-------- src/optimization/TrajoptUtil.inc | 17 ++++--- .../algorithms/SwerveDiscreteOptimal.inc | 7 ++- 3 files changed, 41 insertions(+), 31 deletions(-) diff --git a/src/optimization/SwerveTrajoptUtil.inc b/src/optimization/SwerveTrajoptUtil.inc index 4f8d0f43..266c7b0a 100644 --- a/src/optimization/SwerveTrajoptUtil.inc +++ b/src/optimization/SwerveTrajoptUtil.inc @@ -64,17 +64,11 @@ void ApplyKinematicsConstraints( const std::vector N) { size_t wptCnt = N.size() + 1; - for (size_t i = 0; i < dt.size(); i++) { - opti.SubjectTo(dt.at(i) >= 0); - opti.SubjectTo(dt.at(i) <= 0.1); - } - for (size_t wptIdx = 1; wptIdx < wptCnt; ++wptIdx) { size_t N_sgmt = N.at(wptIdx - 1); + auto dt_sgmt = dt.at(wptIdx - 1); for (size_t sampIdx = 0; sampIdx < N_sgmt; ++sampIdx) { size_t idx = GetIdx(N, wptIdx, sampIdx); - auto dt_n = dt.at(idx); - auto dt_n_1 = dt.at(idx - 1); auto x_n = x.at(idx); auto x_n_1 = x.at(idx - 1); auto y_n = y.at(idx); @@ -90,12 +84,12 @@ void ApplyKinematicsConstraints( auto ax_n = ax.at(idx); auto ay_n = ay.at(idx); auto alpha_n = alpha.at(idx); - opti.SubjectTo(x_n_1 + vx_n * dt_n_1 == x_n); - opti.SubjectTo(y_n_1 + vy_n * dt_n_1 == y_n); - opti.SubjectTo(theta_n_1 + omega_n * dt_n_1 == theta_n); - opti.SubjectTo(vx_n_1 + ax_n * dt_n_1 == vx_n); - opti.SubjectTo(vy_n_1 + ay_n * dt_n_1 == vy_n); - opti.SubjectTo(omega_n_1 + alpha_n * dt_n_1 == omega_n); + opti.SubjectTo(x_n_1 + vx_n * dt_sgmt == x_n); + opti.SubjectTo(y_n_1 + vy_n * dt_sgmt == y_n); + opti.SubjectTo(theta_n_1 + omega_n * dt_sgmt == theta_n); + opti.SubjectTo(vx_n_1 + ax_n * dt_sgmt == vx_n); + opti.SubjectTo(vy_n_1 + ay_n * dt_sgmt == vy_n); + opti.SubjectTo(omega_n_1 + alpha_n * dt_sgmt == omega_n); } } } @@ -160,13 +154,27 @@ SwerveSolution ConstructSwerveSolution( const std::vector& alpha, const std::vector>& Fx, const std::vector>& Fy, const std::vector& dt, const std::vector& N) { - return SwerveSolution{ - RowSolutionValue(opti, dt), RowSolutionValue(opti, x), - RowSolutionValue(opti, y), RowSolutionValue(opti, theta), - RowSolutionValue(opti, vx), RowSolutionValue(opti, vy), - RowSolutionValue(opti, omega), RowSolutionValue(opti, ax), - RowSolutionValue(opti, ay), RowSolutionValue(opti, alpha), - MatrixSolutionValue(opti, Fx), MatrixSolutionValue(opti, Fy)}; + std::vector dtPerSamp; + for (size_t sgmtIdx = 0; sgmtIdx < N.size(); ++sgmtIdx) { + size_t N_sgmt = N.at(sgmtIdx); + Expr dt_sgmt = dt.at(sgmtIdx); + double dt_val = opti.SolutionValue(dt_sgmt); + for (size_t i = 0; i < N_sgmt; ++i) { + dtPerSamp.push_back(dt_val); + } + } + return SwerveSolution{dtPerSamp, + RowSolutionValue(opti, x), + RowSolutionValue(opti, y), + RowSolutionValue(opti, theta), + RowSolutionValue(opti, vx), + RowSolutionValue(opti, vy), + RowSolutionValue(opti, omega), + RowSolutionValue(opti, ax), + RowSolutionValue(opti, ay), + RowSolutionValue(opti, alpha), + MatrixSolutionValue(opti, Fx), + MatrixSolutionValue(opti, Fy)}; } } // namespace trajopt diff --git a/src/optimization/TrajoptUtil.inc b/src/optimization/TrajoptUtil.inc index 51ff8a7f..0603bd00 100644 --- a/src/optimization/TrajoptUtil.inc +++ b/src/optimization/TrajoptUtil.inc @@ -60,15 +60,14 @@ template void ApplyDiscreteTimeObjective(Opti& opti, std::vector& dt, const std::vector N) { Expr T_tot = 0; - size_t wptCnt = N.size() + 1; - for (size_t wptIdx = 1; wptIdx < wptCnt; ++wptIdx) { - size_t N_sgmt = N.at(wptIdx - 1); - for (size_t sampIdx = 0; sampIdx < N_sgmt; ++sampIdx) { - size_t idx = GetIdx(N, wptIdx, sampIdx); - auto dt_n = dt.at(idx); - T_tot += dt_n; - opti.SetInitial(dt_n, 0.1); - } + for (size_t sgmtIdx = 0; sgmtIdx < N.size(); ++sgmtIdx) { + auto& dt_sgmt = dt.at(sgmtIdx); + auto N_sgmt = N.at(sgmtIdx); + auto T_sgmt = dt_sgmt * N_sgmt; + T_tot += T_sgmt; + + opti.SubjectTo(dt_sgmt >= 0); + opti.SetInitial(dt_sgmt, 5.0 / N_sgmt); } opti.Minimize(std::move(T_tot)); } diff --git a/src/optimization/algorithms/SwerveDiscreteOptimal.inc b/src/optimization/algorithms/SwerveDiscreteOptimal.inc index 1f904c98..7fecf038 100644 --- a/src/optimization/algorithms/SwerveDiscreteOptimal.inc +++ b/src/optimization/algorithms/SwerveDiscreteOptimal.inc @@ -60,7 +60,7 @@ SwerveDiscreteOptimal::SwerveDiscreteOptimal( _Fy.reserve(moduleCnt); } - dt.reserve(sampTot); + dt.reserve(sgmtCnt); for (size_t idx = 0; idx < sampTot; ++idx) { x.emplace_back(opti.DecisionVariable()); @@ -72,7 +72,6 @@ SwerveDiscreteOptimal::SwerveDiscreteOptimal( ax.emplace_back(opti.DecisionVariable()); ay.emplace_back(opti.DecisionVariable()); alpha.emplace_back(opti.DecisionVariable()); - dt.emplace_back(opti.DecisionVariable()); for (size_t moduleIdx = 0; moduleIdx < moduleCnt; ++moduleIdx) { Fx.at(idx).emplace_back(opti.DecisionVariable()); @@ -80,6 +79,10 @@ SwerveDiscreteOptimal::SwerveDiscreteOptimal( } } + for (size_t sgmtIdx = 0; sgmtIdx < sgmtCnt; ++sgmtIdx) { + dt.emplace_back(opti.DecisionVariable()); + } + ApplyDiscreteTimeObjective(opti, dt, N); ApplyKinematicsConstraints(opti, x, y, theta, vx, vy, omega, ax, ay, alpha, dt, N); From 71ab9514dd0c2ae3451c16a5d6fc7cb6ffb6ebcf Mon Sep 17 00:00:00 2001 From: shueja <32416547+shueja@users.noreply.github.com> Date: Wed, 15 Nov 2023 19:38:10 -0800 Subject: [PATCH 17/17] Delete CMakeFiles folder --- CMakeFiles/cmake.check_cache | 1 - 1 file changed, 1 deletion(-) delete mode 100644 CMakeFiles/cmake.check_cache diff --git a/CMakeFiles/cmake.check_cache b/CMakeFiles/cmake.check_cache deleted file mode 100644 index 3dccd731..00000000 --- a/CMakeFiles/cmake.check_cache +++ /dev/null @@ -1 +0,0 @@ -# This file is generated by cmake for dependency checking of the CMakeCache.txt file