From c54d877fc6b1238487b7f07216d3f8e2fb60f18d Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 18 Jun 2024 22:58:16 -0700 Subject: [PATCH] Merge .inc files into .h files (#181) This fixes clangd's header parsing and makes the function implementations easier to find. --- .styleguide | 1 - include/.styleguide | 1 - src/.styleguide | 1 - src/optimization/HolonomicTrajoptUtil.h | 66 ++- src/optimization/HolonomicTrajoptUtil.inc | 84 ---- src/optimization/SwerveTrajoptUtil.h | 130 +++++- src/optimization/SwerveTrajoptUtil.inc | 177 -------- src/optimization/TrajoptUtil.h | 333 ++++++++++++++- src/optimization/TrajoptUtil.inc | 386 ------------------ .../algorithms/SwerveDiscreteOptimal.h | 166 +++++++- .../algorithms/SwerveDiscreteOptimal.inc | 167 -------- 11 files changed, 643 insertions(+), 869 deletions(-) delete mode 100644 src/optimization/HolonomicTrajoptUtil.inc delete mode 100644 src/optimization/SwerveTrajoptUtil.inc delete mode 100644 src/optimization/TrajoptUtil.inc delete mode 100644 src/optimization/algorithms/SwerveDiscreteOptimal.inc diff --git a/.styleguide b/.styleguide index 7f0c8ce7..ad0c2a15 100644 --- a/.styleguide +++ b/.styleguide @@ -1,7 +1,6 @@ cppHeaderFileInclude { \.h$ \.hpp$ - \.inc$ expected$ } diff --git a/include/.styleguide b/include/.styleguide index ea4cffe5..018272ee 100644 --- a/include/.styleguide +++ b/include/.styleguide @@ -1,7 +1,6 @@ cppHeaderFileInclude { \.h$ \.hpp$ - \.inc$ expected$ } diff --git a/src/.styleguide b/src/.styleguide index 73205c88..fb8114cd 100644 --- a/src/.styleguide +++ b/src/.styleguide @@ -1,7 +1,6 @@ cppHeaderFileInclude { \.h$ \.hpp$ - \.inc$ expected$ } diff --git a/src/optimization/HolonomicTrajoptUtil.h b/src/optimization/HolonomicTrajoptUtil.h index f3ff6436..6e455b39 100644 --- a/src/optimization/HolonomicTrajoptUtil.h +++ b/src/optimization/HolonomicTrajoptUtil.h @@ -2,12 +2,13 @@ #pragma once -#include -#include - #include "optimization/TrajoptUtil.h" -#include "trajopt/obstacle/Obstacle.h" -#include "trajopt/solution/HolonomicSolution.h" +#include "trajopt/constraint/HeadingConstraint.h" +#include "trajopt/constraint/LinePointConstraint.h" +#include "trajopt/constraint/PointAtConstraint.h" +#include "trajopt/constraint/PointLineConstraint.h" +#include "trajopt/constraint/PointPointConstraint.h" +#include "trajopt/constraint/TranslationConstraint.h" namespace trajopt { @@ -17,7 +18,56 @@ void ApplyHolonomicConstraint(Opti& opti, const Expr& x, const Expr& y, const Expr& theta, const Expr& vx, const Expr& vy, const Expr& omega, const Expr& ax, const Expr& ay, const Expr& alpha, - const HolonomicConstraint& constraint); -} // namespace trajopt + const HolonomicConstraint& constraint) { + if (std::holds_alternative(constraint)) { + const auto& velocityHolonomicConstraint = + std::get(constraint); + ApplySet2dConstraint(opti, vx, vy, + velocityHolonomicConstraint.velocityBound); + } else if (std::holds_alternative(constraint)) { + const auto& angularVelocityConstraint = + std::get(constraint); + ApplyIntervalSet1dConstraint( + opti, omega, angularVelocityConstraint.angularVelocityBound); + } else if (std::holds_alternative(constraint)) { + ApplyConstraint(opti, x, y, theta, + std::get(constraint)); + } else if (std::holds_alternative(constraint)) { + ApplyConstraint(opti, x, y, theta, std::get(constraint)); + } else if (std::holds_alternative(constraint)) { + ApplyConstraint(opti, x, y, theta, + std::get(constraint)); + } else if (std::holds_alternative(constraint)) { + auto pointAtConstraint = std::get(constraint); + double fieldPointX = pointAtConstraint.fieldPointX; + double fieldPointY = pointAtConstraint.fieldPointY; + double headingTolerance = pointAtConstraint.headingTolerance; + /** + * dx,dy = desired heading + * ux,uy = unit vector of desired heading + * hx,hy = heading + * dot = dot product of ux,uy and hx,hy + * + * constrain dot to cos(1.0), which is colinear + * and cos(thetaTolerance) + */ + auto dx = fieldPointX - x; + auto dy = fieldPointY - y; + auto ux = dx / hypot(dx, dy); // NOLINT + auto uy = dy / hypot(dx, dy); // NOLINT + auto hx = cos(theta); // NOLINT + auto hy = sin(theta); // NOLINT + auto dot = hx * ux + hy * uy; -#include "optimization/HolonomicTrajoptUtil.inc" + ApplyIntervalSet1dConstraint( + opti, dot, IntervalSet1d(std::cos(headingTolerance), 1.0)); + } else if (std::holds_alternative(constraint)) { + ApplyConstraint(opti, x, y, theta, + std::get(constraint)); + } else if (std::holds_alternative(constraint)) { + ApplyConstraint(opti, x, y, theta, + std::get(constraint)); + } // TODO: Investigate a way to condense the code above +} + +} // namespace trajopt diff --git a/src/optimization/HolonomicTrajoptUtil.inc b/src/optimization/HolonomicTrajoptUtil.inc deleted file mode 100644 index 9400b7e4..00000000 --- a/src/optimization/HolonomicTrajoptUtil.inc +++ /dev/null @@ -1,84 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include "optimization/HolonomicTrajoptUtil.h" -#include "optimization/TrajoptUtil.h" -#include "trajopt/constraint/Constraint.h" -#include "trajopt/constraint/HeadingConstraint.h" -#include "trajopt/constraint/LinePointConstraint.h" -#include "trajopt/constraint/PointAtConstraint.h" -#include "trajopt/constraint/PointLineConstraint.h" -#include "trajopt/constraint/PointPointConstraint.h" -#include "trajopt/constraint/TranslationConstraint.h" -#include "trajopt/obstacle/Obstacle.h" -#include "trajopt/path/Path.h" -#include "trajopt/solution/HolonomicSolution.h" - -namespace trajopt { - -template - requires OptiSys -void ApplyHolonomicConstraint(Opti& opti, const Expr& x, const Expr& y, - const Expr& theta, const Expr& vx, const Expr& vy, - const Expr& omega, const Expr& ax, const Expr& ay, - const Expr& alpha, - const HolonomicConstraint& holonomicConstraint) { - if (std::holds_alternative( - holonomicConstraint)) { - const auto& velocityHolonomicConstraint = - std::get(holonomicConstraint); - ApplySet2dConstraint(opti, vx, vy, - velocityHolonomicConstraint.velocityBound); - } else if (std::holds_alternative( - holonomicConstraint)) { - const auto& angularVelocityConstraint = - std::get(holonomicConstraint); - ApplyIntervalSet1dConstraint( - opti, omega, angularVelocityConstraint.angularVelocityBound); - } else if (std::holds_alternative( - holonomicConstraint)) { - ApplyConstraint(opti, x, y, theta, - std::get(holonomicConstraint)); - } else if (std::holds_alternative(holonomicConstraint)) { - ApplyConstraint(opti, x, y, theta, - std::get(holonomicConstraint)); - } else if (std::holds_alternative(holonomicConstraint)) { - ApplyConstraint(opti, x, y, theta, - std::get(holonomicConstraint)); - } else if (std::holds_alternative(holonomicConstraint)) { - auto pointAtConstraint = std::get(holonomicConstraint); - double fieldPointX = pointAtConstraint.fieldPointX; - double fieldPointY = pointAtConstraint.fieldPointY; - double headingTolerance = pointAtConstraint.headingTolerance; - /** - * dx,dy = desired heading - * ux,uy = unit vector of desired heading - * hx,hy = heading - * dot = dot product of ux,uy and hx,hy - * - * constrain dot to cos(1.0), which is colinear - * and cos(thetaTolerance) - */ - auto dx = fieldPointX - x; - auto dy = fieldPointY - y; - auto ux = dx / hypot(dx, dy); // NOLINT - auto uy = dy / hypot(dx, dy); // NOLINT - auto hx = cos(theta); // NOLINT - auto hy = sin(theta); // NOLINT - auto dot = hx * ux + hy * uy; - - ApplyIntervalSet1dConstraint( - opti, dot, IntervalSet1d(std::cos(headingTolerance), 1.0)); - } else if (std::holds_alternative(holonomicConstraint)) { - ApplyConstraint(opti, x, y, theta, - std::get(holonomicConstraint)); - } else if (std::holds_alternative( - holonomicConstraint)) { - ApplyConstraint(opti, x, y, theta, - std::get(holonomicConstraint)); - } // TODO: Investigate a way to condense the code above -} -} // namespace trajopt diff --git a/src/optimization/SwerveTrajoptUtil.h b/src/optimization/SwerveTrajoptUtil.h index 9f355e8a..e2901b32 100644 --- a/src/optimization/SwerveTrajoptUtil.h +++ b/src/optimization/SwerveTrajoptUtil.h @@ -5,11 +5,8 @@ #include #include -#include "optimization/HolonomicTrajoptUtil.h" -#include "optimization/OptiSys.h" #include "optimization/TrajoptUtil.h" #include "trajopt/drivetrain/SwerveDrivetrain.h" -#include "trajopt/path/Path.h" #include "trajopt/solution/SwerveSolution.h" namespace trajopt { @@ -17,13 +14,38 @@ namespace trajopt { template requires ExprSys std::pair SolveNetForce(const std::vector& Fx, - const std::vector& Fy); + const std::vector& Fy) { + Expr Fx_net = 0; + Expr Fy_net = 0; + + for (auto& _Fx : Fx) { + Fx_net += _Fx; + } + for (auto& _Fy : Fy) { + Fy_net += _Fy; + } + + return {Fx_net, Fy_net}; +} template requires ExprSys Expr SolveNetTorque(const Expr& theta, const std::vector& Fx, const std::vector& Fy, - const std::vector& swerveModules); + const std::vector& swerveModules) { + Expr tau_net = 0; + + for (size_t moduleIdx = 0; moduleIdx < swerveModules.size(); ++moduleIdx) { + auto& swerveModule = swerveModules.at(moduleIdx); + auto [x_m, y_m] = + RotateConstantVector(swerveModule.x, swerveModule.y, theta); + auto& Fx_m = Fx.at(moduleIdx); + auto& Fy_m = Fy.at(moduleIdx); + tau_net += x_m * Fy_m - y_m * Fx_m; + } + + return tau_net; +} template requires OptiSys @@ -33,7 +55,38 @@ void ApplyKinematicsConstraints( const std::vector& vy, const std::vector& omega, const std::vector& ax, const std::vector& ay, const std::vector& alpha, const std::vector& dt, - const std::vector& N); + const std::vector& N) { + size_t wptCnt = N.size() + 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 x_n = x.at(idx); + auto x_n_1 = x.at(idx - 1); + auto y_n = y.at(idx); + auto y_n_1 = y.at(idx - 1); + auto theta_n = theta.at(idx); + auto theta_n_1 = theta.at(idx - 1); + auto vx_n = vx.at(idx); + auto vx_n_1 = vx.at(idx - 1); + auto vy_n = vy.at(idx); + auto vy_n_1 = vy.at(idx - 1); + auto omega_n = omega.at(idx); + auto omega_n_1 = omega.at(idx - 1); + 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); + } + } +} /** * Applies the drivetrain-specific constraints to the optimizer. These @@ -68,7 +121,11 @@ template void ApplyDynamicsConstraints(Opti& opti, const Expr& ax, const Expr& ay, const Expr& alpha, const Expr& Fx_net, const Expr& Fy_net, const Expr& tau_net, - double mass, double moi); + double mass, double moi) { + opti.SubjectTo(Fx_net == mass * ax); + opti.SubjectTo(Fy_net == mass * ay); + opti.SubjectTo(tau_net == moi * alpha); +} template requires OptiSys @@ -76,7 +133,38 @@ void ApplyPowerConstraints(Opti& opti, const Expr& theta, const Expr& vx, const Expr& vy, const Expr& omega, const std::vector& Fx, const std::vector& Fy, - const SwerveDrivetrain& swerveDrivetrain); + const SwerveDrivetrain& swerveDrivetrain) { + auto [vx_prime, vy_prime] = RotateVector(vx, vy, -theta); + + size_t moduleCount = swerveDrivetrain.modules.size(); + + std::vector vx_m; + std::vector vy_m; + vx_m.reserve(moduleCount); + vy_m.reserve(moduleCount); + + for (size_t moduleIdx = 0; moduleIdx < moduleCount; ++moduleIdx) { + auto x_m = swerveDrivetrain.modules.at(moduleIdx).x; + auto y_m = swerveDrivetrain.modules.at(moduleIdx).y; + vx_m.emplace_back(vx_prime - y_m * omega); + vy_m.emplace_back(vy_prime + x_m * omega); + } + + for (size_t moduleIdx = 0; moduleIdx < moduleCount; ++moduleIdx) { + auto& _module = swerveDrivetrain.modules.at(moduleIdx); + double maxWheelVelocity = + _module.wheelRadius * _module.wheelMaxAngularVelocity; + double maxForce = _module.wheelMaxTorque / _module.wheelRadius; + auto _vx_m = vx_m.at(moduleIdx); + auto _vy_m = vy_m.at(moduleIdx); + auto Fx_m = Fx.at(moduleIdx); + auto Fy_m = Fy.at(moduleIdx); + opti.SubjectTo(_vx_m * _vx_m + _vy_m * _vy_m <= + maxWheelVelocity * maxWheelVelocity); + + opti.SubjectTo(Fx_m * Fx_m + Fy_m * Fy_m <= maxForce * maxForce); + } +} template requires OptiSys @@ -87,7 +175,27 @@ SwerveSolution ConstructSwerveSolution( const std::vector& ax, const std::vector& ay, const std::vector& alpha, const std::vector>& Fx, const std::vector>& Fy, const std::vector& dt, - const std::vector& N); -} // namespace trajopt + 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, 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)}; +} -#include "optimization/SwerveTrajoptUtil.inc" +} // namespace trajopt diff --git a/src/optimization/SwerveTrajoptUtil.inc b/src/optimization/SwerveTrajoptUtil.inc deleted file mode 100644 index 2d0f3c91..00000000 --- a/src/optimization/SwerveTrajoptUtil.inc +++ /dev/null @@ -1,177 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include -#include -#include -#include - -#include "optimization/SwerveTrajoptUtil.h" -#include "optimization/TrajoptUtil.h" -#include "trajopt/drivetrain/SwerveDrivetrain.h" -#include "trajopt/path/Path.h" -#include "trajopt/solution/SwerveSolution.h" - -namespace trajopt { - -template - requires ExprSys -std::pair SolveNetForce(const std::vector& Fx, - const std::vector& Fy) { - Expr Fx_net = 0; - Expr Fy_net = 0; - - for (auto& _Fx : Fx) { - Fx_net += _Fx; - } - for (auto& _Fy : Fy) { - Fy_net += _Fy; - } - - return {Fx_net, Fy_net}; -} - -template - requires ExprSys -Expr SolveNetTorque(const Expr& theta, const std::vector& Fx, - const std::vector& Fy, - const std::vector& swerveModules) { - Expr tau_net = 0; - - for (size_t moduleIdx = 0; moduleIdx < swerveModules.size(); ++moduleIdx) { - auto& swerveModule = swerveModules.at(moduleIdx); - auto [x_m, y_m] = - RotateConstantVector(swerveModule.x, swerveModule.y, theta); - auto& Fx_m = Fx.at(moduleIdx); - auto& Fy_m = Fy.at(moduleIdx); - tau_net += x_m * Fy_m - y_m * Fx_m; - } - - return tau_net; -} - -template - requires OptiSys -void ApplyKinematicsConstraints( - Opti& opti, const std::vector& x, const std::vector& y, - const std::vector& theta, const std::vector& vx, - const std::vector& vy, const std::vector& omega, - const std::vector& ax, const std::vector& ay, - const std::vector& alpha, const std::vector& dt, - const std::vector& N) { - size_t wptCnt = N.size() + 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 x_n = x.at(idx); - auto x_n_1 = x.at(idx - 1); - auto y_n = y.at(idx); - auto y_n_1 = y.at(idx - 1); - auto theta_n = theta.at(idx); - auto theta_n_1 = theta.at(idx - 1); - auto vx_n = vx.at(idx); - auto vx_n_1 = vx.at(idx - 1); - auto vy_n = vy.at(idx); - auto vy_n_1 = vy.at(idx - 1); - auto omega_n = omega.at(idx); - auto omega_n_1 = omega.at(idx - 1); - 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); - } - } -} - -template - requires OptiSys -void ApplyDynamicsConstraints(Opti& opti, const Expr& ax, const Expr& ay, - const Expr& alpha, const Expr& Fx_net, - const Expr& Fy_net, const Expr& tau_net, - double mass, double moi) { - opti.SubjectTo(Fx_net == mass * ax); - opti.SubjectTo(Fy_net == mass * ay); - opti.SubjectTo(tau_net == moi * alpha); -} - -template - requires OptiSys -void ApplyPowerConstraints(Opti& opti, const Expr& theta, const Expr& vx, - const Expr& vy, const Expr& omega, - const std::vector& Fx, - const std::vector& Fy, - const SwerveDrivetrain& swerveDrivetrain) { - auto [vx_prime, vy_prime] = RotateVector(vx, vy, -theta); - - size_t moduleCount = swerveDrivetrain.modules.size(); - - std::vector vx_m; - std::vector vy_m; - vx_m.reserve(moduleCount); - vy_m.reserve(moduleCount); - - for (size_t moduleIdx = 0; moduleIdx < moduleCount; ++moduleIdx) { - auto x_m = swerveDrivetrain.modules.at(moduleIdx).x; - auto y_m = swerveDrivetrain.modules.at(moduleIdx).y; - vx_m.emplace_back(vx_prime - y_m * omega); - vy_m.emplace_back(vy_prime + x_m * omega); - } - - for (size_t moduleIdx = 0; moduleIdx < moduleCount; ++moduleIdx) { - auto& _module = swerveDrivetrain.modules.at(moduleIdx); - double maxWheelVelocity = - _module.wheelRadius * _module.wheelMaxAngularVelocity; - double maxForce = _module.wheelMaxTorque / _module.wheelRadius; - auto _vx_m = vx_m.at(moduleIdx); - auto _vy_m = vy_m.at(moduleIdx); - auto Fx_m = Fx.at(moduleIdx); - auto Fy_m = Fy.at(moduleIdx); - opti.SubjectTo(_vx_m * _vx_m + _vy_m * _vy_m <= - maxWheelVelocity * maxWheelVelocity); - - opti.SubjectTo(Fx_m * Fx_m + Fy_m * Fy_m <= maxForce * maxForce); - } -} - -template - requires OptiSys -SwerveSolution ConstructSwerveSolution( - const Opti& opti, const std::vector& x, const std::vector& y, - const std::vector& theta, const std::vector& vx, - const std::vector& vy, const std::vector& omega, - const std::vector& ax, const std::vector& ay, - 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, 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.h b/src/optimization/TrajoptUtil.h index 07091a41..556f712b 100644 --- a/src/optimization/TrajoptUtil.h +++ b/src/optimization/TrajoptUtil.h @@ -2,15 +2,23 @@ #pragma once +#include #include +#include #include #include "optimization/OptiSys.h" #include "trajopt/constraint/Constraint.h" -#include "trajopt/obstacle/Obstacle.h" +#include "trajopt/constraint/LinePointConstraint.h" +#include "trajopt/constraint/PointLineConstraint.h" +#include "trajopt/constraint/PointPointConstraint.h" +#include "trajopt/constraint/TranslationConstraint.h" #include "trajopt/path/InitialGuessPoint.h" -#include "trajopt/path/Path.h" +#include "trajopt/set/ConeSet2d.h" +#include "trajopt/set/EllipticalSet2d.h" #include "trajopt/set/IntervalSet1d.h" +#include "trajopt/set/LinearSet2d.h" +#include "trajopt/set/RectangularSet2d.h" #include "trajopt/set/Set2d.h" #include "trajopt/solution/Solution.h" @@ -18,11 +26,17 @@ namespace trajopt { template std::pair RotateVector(const Expr& x, const Expr& y, - const Expr& theta); + const Expr& theta) { + return {x * cos(theta) - y * sin(theta), // NOLINT + x * sin(theta) + y * cos(theta)}; // NOLINT +} template std::pair RotateConstantVector(double x, double y, - const Expr& theta); + const Expr& theta) { + return {x * cos(theta) - y * sin(theta), // NOLINT + x * sin(theta) + y * cos(theta)}; // NOLINT +} /** * @brief Get the index of an item in a decision variable array, given the @@ -37,32 +51,115 @@ std::pair RotateConstantVector(double x, double y, * @return the index in the array */ inline size_t GetIdx(const std::vector& N, size_t wptIdx, - size_t sampIdx = 0); + size_t sampIdx = 0) { + size_t idx = 0; + if (wptIdx > 0) { + ++idx; + } + for (size_t _wptIdx = 1; _wptIdx < wptIdx; ++_wptIdx) { + idx += N.at(_wptIdx - 1); + } + idx += sampIdx; + return idx; +} template requires OptiSys void ApplyDiscreteTimeObjective(Opti& opti, std::vector& dt, - const std::vector N); + 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; + + opti.SubjectTo(dt_sgmt >= 0); + opti.SetInitial(dt_sgmt, 5.0 / N_sgmt); + } + opti.Minimize(std::move(T_tot)); +} template requires OptiSys void ApplyIntervalSet1dConstraint(Opti& opti, const Expr& scalar, - const IntervalSet1d& set1d); + const IntervalSet1d& set1d) { + if (set1d.IsExact()) { + opti.SubjectTo(scalar == set1d.lower); + } else { + if (set1d.IsLowerBounded()) { + opti.SubjectTo(scalar >= set1d.lower); + } + if (set1d.IsUpperBounded()) { + opti.SubjectTo(scalar <= set1d.upper); + } + } +} template requires OptiSys void ApplySet2dConstraint(Opti& opti, const Expr& vectorX, const Expr& vectorY, - const Set2d& set2d); + const Set2d& set2d) { + if (std::holds_alternative(set2d)) { + auto& rectangularSet2d = std::get(set2d); + ApplyIntervalSet1dConstraint(opti, vectorX, rectangularSet2d.xBound); + ApplyIntervalSet1dConstraint(opti, vectorY, rectangularSet2d.yBound); + } else if (std::holds_alternative(set2d)) { + auto& linearSet2d = std::get(set2d); + double sinTheta = std::sin(linearSet2d.theta); + double cosTheta = std::cos(linearSet2d.theta); + opti.SubjectTo(vectorX * sinTheta == vectorY * cosTheta); + } else if (std::holds_alternative(set2d)) { + auto& ellipticalSet2d = std::get(set2d); + auto scaledVectorXSquared = (vectorX * vectorX) / (ellipticalSet2d.xRadius * + ellipticalSet2d.xRadius); + auto scaledVectorYSquared = (vectorY * vectorY) / (ellipticalSet2d.yRadius * + ellipticalSet2d.yRadius); + auto lhs = scaledVectorXSquared + scaledVectorYSquared; + using enum EllipticalSet2d::Direction; + switch (ellipticalSet2d.direction) { + case kInside: + opti.SubjectTo(lhs <= 1.0); + break; + case kCentered: + opti.SubjectTo(lhs == 1.0); + break; + case kOutside: + opti.SubjectTo(lhs >= 1.0); + break; + } + } else if (std::holds_alternative(set2d)) { + auto& coneSet2d = std::get(set2d); + opti.SubjectTo(vectorX * sin(coneSet2d.thetaBound.upper) >= // NOLINT + vectorY * cos(coneSet2d.thetaBound.upper)); // NOLINT + opti.SubjectTo(vectorX * sin(coneSet2d.thetaBound.lower) <= // NOLINT + vectorY * cos(coneSet2d.thetaBound.lower)); // NOLINT + } +} template requires OptiSys std::vector RowSolutionValue(const Opti& opti, - const std::vector& rowVector); + const std::vector& rowVector) { + std::vector valueRowVector; + valueRowVector.reserve(rowVector.size()); + for (auto& expression : rowVector) { + valueRowVector.push_back(opti.SolutionValue(expression)); + } + return valueRowVector; +} template requires OptiSys std::vector> MatrixSolutionValue( - const Opti& opti, const std::vector>& matrix); + const Opti& opti, const std::vector>& matrix) { + std::vector> valueMatrix; + valueMatrix.reserve(matrix.size()); + for (auto& row : matrix) { + valueMatrix.push_back(RowSolutionValue(opti, row)); + } + return valueMatrix; +} /** * @brief Get an expression for the position of a bumper corner relative @@ -82,19 +179,187 @@ static const std::pair SolveRobotPointPosition(const Expr& x, const Expr& y, const Expr& theta, double robotPointX, - double robotPointY); + double robotPointY) { + std::pair position{0.0, 0.0}; + if (robotPointX == 0.0 && robotPointY == 0.0) { + position.first = x; + position.second = y; + } else { + double cornerDiagonal = std::hypot(robotPointX, robotPointY); + double cornerAngle = std::atan2(robotPointY, robotPointX); + position.first = x + cornerDiagonal * cos(cornerAngle + theta); // NOLINT + position.second = y + cornerDiagonal * sin(cornerAngle + theta); // NOLINT + } + return position; +} + +// https://www.desmos.com/calculator/cqmc1tjtsv +template +decltype(LineNumberType() + PointNumberType()) linePointDist( + LineNumberType lineStartX, LineNumberType lineStartY, + LineNumberType lineEndX, LineNumberType lineEndY, PointNumberType pointX, + PointNumberType pointY) { + auto lX = lineEndX - lineStartX; + auto lY = lineEndY - lineStartY; + auto vX = pointX - lineStartX; + auto vY = pointY - lineStartY; + auto dot = vX * lX + vY * lY; + auto lNormSquared = lX * lX + lY * lY; + auto t = dot / lNormSquared; + auto tBounded = fmax(fmin(t, 1), 0); // NOLINT + auto iX = (1 - tBounded) * lineStartX + tBounded * lineEndX; + auto iY = (1 - tBounded) * lineStartY + tBounded * lineEndY; + auto distSquared = + (iX - pointX) * (iX - pointX) + (iY - pointY) * (iY - pointY); + return distSquared; +} template requires OptiSys void ApplyConstraint(Opti& opti, const Expr& x, const Expr& y, - const Expr& theta, const Constraint& constraint); + const Expr& theta, const Constraint& constraint) { + if (std::holds_alternative(constraint)) { + auto& translationConstraint = std::get(constraint); + ApplySet2dConstraint(opti, x, y, translationConstraint.translationBound); + } else if (std::holds_alternative(constraint)) { + auto& headingConstraint = std::get(constraint); + ApplyIntervalSet1dConstraint(opti, theta, headingConstraint.headingBound); + } else if (std::holds_alternative(constraint)) { + auto linePointConstraint = std::get(constraint); + auto [lineStartX, lineStartY] = SolveRobotPointPosition( + x, y, theta, linePointConstraint.robotLineStartX, + linePointConstraint.robotLineStartY); + auto [lineEndX, lineEndY] = + SolveRobotPointPosition(x, y, theta, linePointConstraint.robotLineEndX, + linePointConstraint.robotLineEndY); + double pointX = linePointConstraint.fieldPointX; + double pointY = linePointConstraint.fieldPointY; + auto dist = linePointDist(lineStartX, lineStartY, lineEndX, lineEndY, + pointX, pointY); + auto distSquared = dist * dist; + auto& distInterval = linePointConstraint.distance; + auto distIntervalSquared = IntervalSet1d(std::pow(distInterval.lower, 2), + std::pow(distInterval.upper, 2)); + ApplyIntervalSet1dConstraint(opti, distSquared, distIntervalSquared); + } else if (std::holds_alternative(constraint)) { + auto pointLineConstraint = std::get(constraint); + double lineStartX = pointLineConstraint.fieldLineStartX; + double lineStartY = pointLineConstraint.fieldLineStartY; + double lineEndX = pointLineConstraint.fieldLineEndX; + double lineEndY = pointLineConstraint.fieldLineEndY; + auto [pointX, pointY] = + SolveRobotPointPosition(x, y, theta, pointLineConstraint.robotPointX, + pointLineConstraint.robotPointY); + auto dist = linePointDist(lineStartX, lineStartY, lineEndX, lineEndY, + pointX, pointY); + auto distSquared = dist * dist; + auto& distInterval = pointLineConstraint.distance; + auto distIntervalSquared = IntervalSet1d(std::pow(distInterval.lower, 2), + std::pow(distInterval.upper, 2)); + ApplyIntervalSet1dConstraint(opti, distSquared, distIntervalSquared); + } else if (std::holds_alternative(constraint)) { + auto pointPointConstraint = std::get(constraint); + double robotPointX = pointPointConstraint.robotPointX; + double robotPointY = pointPointConstraint.robotPointY; + double fieldPointX = pointPointConstraint.fieldPointX; + double fieldPointY = pointPointConstraint.fieldPointY; + auto [bumperCornerX, bumperCornerY] = + SolveRobotPointPosition(x, y, theta, robotPointX, robotPointY); + auto dx = fieldPointX - bumperCornerX; + auto dy = fieldPointY - bumperCornerY; + auto pointDistSquared = dx * dx + dy * dy; + IntervalSet1d distSquared = pointPointConstraint.distance; + distSquared.lower *= distSquared.lower; + distSquared.upper *= distSquared.upper; + ApplyIntervalSet1dConstraint(opti, pointDistSquared, distSquared); + } +} inline std::vector Linspace(double startValue, double endValue, - size_t numSamples); + size_t numSamples) { + std::vector result; + double delta = (endValue - startValue) / numSamples; + for (size_t index = 1; index <= numSamples; index++) { + result.push_back(startValue + index * delta); + } + return result; +} + +template +inline void append_vector(std::vector& base, + const std::vector& newItems) { + base.insert(base.end(), newItems.begin(), newItems.end()); +} inline Solution GenerateLinearInitialGuess( const std::vector>& initialGuessPoints, - const std::vector controlIntervalCounts); + const std::vector controlIntervalCounts) { + size_t wptCnt = controlIntervalCounts.size() + 1; + size_t sampTot = GetIdx(controlIntervalCounts, wptCnt, 0); + Solution initialGuess{}; + initialGuess.x.reserve(sampTot); + initialGuess.y.reserve(sampTot); + initialGuess.theta.reserve(sampTot); + initialGuess.dt.reserve(sampTot); + initialGuess.x.push_back(initialGuessPoints.front().front().x); + initialGuess.y.push_back(initialGuessPoints.front().front().y); + initialGuess.theta.push_back(initialGuessPoints.front().front().heading); + for (size_t i = 0; i < sampTot; i++) { + initialGuess.dt.push_back((wptCnt * 5.0) / sampTot); + } + for (size_t wptIdx = 1; wptIdx < wptCnt; wptIdx++) { + size_t N_sgmt = controlIntervalCounts.at(wptIdx - 1); + size_t guessPointCount = initialGuessPoints.at(wptIdx).size(); + size_t N_guessSgmt = N_sgmt / guessPointCount; + append_vector( + initialGuess.x, + Linspace(initialGuessPoints.at(wptIdx - 1).back().x, + initialGuessPoints.at(wptIdx).front().x, N_guessSgmt)); + append_vector( + initialGuess.y, + Linspace(initialGuessPoints.at(wptIdx - 1).back().y, + initialGuessPoints.at(wptIdx).front().y, N_guessSgmt)); + append_vector( + initialGuess.theta, + Linspace(initialGuessPoints.at(wptIdx - 1).back().heading, + initialGuessPoints.at(wptIdx).front().heading, N_guessSgmt)); + for (size_t guessPointIdx = 1; guessPointIdx < guessPointCount - 1; + guessPointIdx++) { // if three or more guess points + append_vector( + initialGuess.x, + Linspace(initialGuessPoints.at(wptIdx).at(guessPointIdx - 1).x, + initialGuessPoints.at(wptIdx).at(guessPointIdx).x, + N_guessSgmt)); + append_vector( + initialGuess.y, + Linspace(initialGuessPoints.at(wptIdx).at(guessPointIdx - 1).y, + initialGuessPoints.at(wptIdx).at(guessPointIdx).y, + N_guessSgmt)); + append_vector( + initialGuess.theta, + Linspace(initialGuessPoints.at(wptIdx).at(guessPointIdx - 1).heading, + initialGuessPoints.at(wptIdx).at(guessPointIdx).heading, + N_guessSgmt)); + } + if (guessPointCount > 1) { // if two or more guess points + size_t N_lastGuessSgmt = N_sgmt - (guessPointCount - 1) * N_guessSgmt; + append_vector( + initialGuess.x, + Linspace(initialGuessPoints.at(wptIdx).at(guessPointCount - 2).x, + initialGuessPoints.at(wptIdx).back().x, N_lastGuessSgmt)); + append_vector( + initialGuess.y, + Linspace(initialGuessPoints.at(wptIdx).at(guessPointCount - 2).y, + initialGuessPoints.at(wptIdx).back().y, N_lastGuessSgmt)); + append_vector( + initialGuess.theta, + Linspace( + initialGuessPoints.at(wptIdx).at(guessPointCount - 2).heading, + initialGuessPoints.at(wptIdx).back().heading, N_lastGuessSgmt)); + } + } + return initialGuess; +} template requires OptiSys @@ -103,7 +368,41 @@ void ApplyInitialGuess(Opti& opti, const Solution& solution, std::vector& theta, std::vector& vx, std::vector& vy, std::vector& omega, std::vector& ax, std::vector& ay, - std::vector& alpha); -} // namespace trajopt + std::vector& alpha) { + size_t sampleTotal = x.size(); + for (size_t sampleIndex = 0; sampleIndex < sampleTotal; sampleIndex++) { + opti.SetInitial(x[sampleIndex], solution.x[sampleIndex]); + opti.SetInitial(y[sampleIndex], solution.y[sampleIndex]); + opti.SetInitial(theta[sampleIndex], solution.theta[sampleIndex]); + } + opti.SetInitial(vx[0], 0.0); + opti.SetInitial(vy[0], 0.0); + opti.SetInitial(omega[0], 0.0); + opti.SetInitial(ax[0], 0.0); + opti.SetInitial(ay[0], 0.0); + opti.SetInitial(alpha[0], 0.0); + for (size_t sampleIndex = 1; sampleIndex < sampleTotal; sampleIndex++) { + opti.SetInitial(vx[sampleIndex], + (solution.x[sampleIndex] - solution.x[sampleIndex - 1]) / + solution.dt[sampleIndex]); + opti.SetInitial(vy[sampleIndex], + (solution.y[sampleIndex] - solution.y[sampleIndex - 1]) / + solution.dt[sampleIndex]); + opti.SetInitial(omega[sampleIndex], (solution.theta[sampleIndex] - + solution.theta[sampleIndex - 1]) / + solution.dt[sampleIndex]); + + opti.SetInitial(ax[sampleIndex], (opti.SolutionValue(vx[sampleIndex]) - + opti.SolutionValue(vx[sampleIndex - 1])) / + solution.dt[sampleIndex]); + opti.SetInitial(ay[sampleIndex], (opti.SolutionValue(vy[sampleIndex]) - + opti.SolutionValue(vy[sampleIndex - 1])) / + solution.dt[sampleIndex]); + opti.SetInitial(alpha[sampleIndex], + (opti.SolutionValue(omega[sampleIndex]) - + opti.SolutionValue(omega[sampleIndex - 1])) / + solution.dt[sampleIndex]); + } +} -#include "optimization/TrajoptUtil.inc" +} // namespace trajopt diff --git a/src/optimization/TrajoptUtil.inc b/src/optimization/TrajoptUtil.inc deleted file mode 100644 index f93c40d2..00000000 --- a/src/optimization/TrajoptUtil.inc +++ /dev/null @@ -1,386 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "optimization/TrajoptUtil.h" -#include "trajopt/constraint/LinePointConstraint.h" -#include "trajopt/constraint/PointLineConstraint.h" -#include "trajopt/constraint/PointPointConstraint.h" -#include "trajopt/constraint/TranslationConstraint.h" -#include "trajopt/obstacle/Obstacle.h" -#include "trajopt/path/Path.h" -#include "trajopt/set/ConeSet2d.h" -#include "trajopt/set/EllipticalSet2d.h" -#include "trajopt/set/IntervalSet1d.h" -#include "trajopt/set/LinearSet2d.h" -#include "trajopt/set/RectangularSet2d.h" -#include "trajopt/set/Set2d.h" - -namespace trajopt { - -template -std::pair RotateVector(const Expr& x, const Expr& y, - const Expr& theta) { - return {x * cos(theta) - y * sin(theta), // NOLINT - x * sin(theta) + y * cos(theta)}; // NOLINT -} - -template -std::pair RotateConstantVector(double x, double y, - const Expr& theta) { - return {x * cos(theta) - y * sin(theta), // NOLINT - x * sin(theta) + y * cos(theta)}; // NOLINT -} - -size_t GetIdx(const std::vector& N, size_t wptIdx, size_t sampIdx) { - size_t idx = 0; - if (wptIdx > 0) { - ++idx; - } - for (size_t _wptIdx = 1; _wptIdx < wptIdx; ++_wptIdx) { - idx += N.at(_wptIdx - 1); - } - idx += sampIdx; - return idx; -} - -template - requires OptiSys -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; - - opti.SubjectTo(dt_sgmt >= 0); - opti.SetInitial(dt_sgmt, 5.0 / N_sgmt); - } - opti.Minimize(std::move(T_tot)); -} - -template - requires OptiSys -void ApplyIntervalSet1dConstraint(Opti& opti, const Expr& scalar, - const IntervalSet1d& set1d) { - if (set1d.IsExact()) { - opti.SubjectTo(scalar == set1d.lower); - } else { - if (set1d.IsLowerBounded()) { - opti.SubjectTo(scalar >= set1d.lower); - } - if (set1d.IsUpperBounded()) { - opti.SubjectTo(scalar <= set1d.upper); - } - } -} - -template - requires OptiSys -void ApplySet2dConstraint(Opti& opti, const Expr& vectorX, const Expr& vectorY, - const Set2d& set2d) { - if (std::holds_alternative(set2d)) { - auto& rectangularSet2d = std::get(set2d); - ApplyIntervalSet1dConstraint(opti, vectorX, rectangularSet2d.xBound); - ApplyIntervalSet1dConstraint(opti, vectorY, rectangularSet2d.yBound); - } else if (std::holds_alternative(set2d)) { - auto& linearSet2d = std::get(set2d); - double sinTheta = std::sin(linearSet2d.theta); - double cosTheta = std::cos(linearSet2d.theta); - opti.SubjectTo(vectorX * sinTheta == vectorY * cosTheta); - } else if (std::holds_alternative(set2d)) { - auto& ellipticalSet2d = std::get(set2d); - auto scaledVectorXSquared = (vectorX * vectorX) / (ellipticalSet2d.xRadius * - ellipticalSet2d.xRadius); - auto scaledVectorYSquared = (vectorY * vectorY) / (ellipticalSet2d.yRadius * - ellipticalSet2d.yRadius); - auto lhs = scaledVectorXSquared + scaledVectorYSquared; - using enum EllipticalSet2d::Direction; - switch (ellipticalSet2d.direction) { - case kInside: - opti.SubjectTo(lhs <= 1.0); - break; - case kCentered: - opti.SubjectTo(lhs == 1.0); - break; - case kOutside: - opti.SubjectTo(lhs >= 1.0); - break; - } - } else if (std::holds_alternative(set2d)) { - auto& coneSet2d = std::get(set2d); - opti.SubjectTo(vectorX * sin(coneSet2d.thetaBound.upper) >= // NOLINT - vectorY * cos(coneSet2d.thetaBound.upper)); // NOLINT - opti.SubjectTo(vectorX * sin(coneSet2d.thetaBound.lower) <= // NOLINT - vectorY * cos(coneSet2d.thetaBound.lower)); // NOLINT - } -} - -template - requires OptiSys -std::vector RowSolutionValue(const Opti& opti, - const std::vector& rowVector) { - std::vector valueRowVector; - valueRowVector.reserve(rowVector.size()); - for (auto& expression : rowVector) { - valueRowVector.push_back(opti.SolutionValue(expression)); - } - return valueRowVector; -} - -template - requires OptiSys -std::vector> MatrixSolutionValue( - const Opti& opti, const std::vector>& matrix) { - std::vector> valueMatrix; - valueMatrix.reserve(matrix.size()); - for (auto& row : matrix) { - valueMatrix.push_back(RowSolutionValue(opti, row)); - } - return valueMatrix; -} - -template - requires ExprSys -const std::pair SolveRobotPointPosition(const Expr& x, - const Expr& y, - const Expr& theta, - double robotPointX, - double robotPointY) { - std::pair position{0.0, 0.0}; - if (robotPointX == 0.0 && robotPointY == 0.0) { - position.first = x; - position.second = y; - } else { - double cornerDiagonal = std::hypot(robotPointX, robotPointY); - double cornerAngle = std::atan2(robotPointY, robotPointX); - position.first = x + cornerDiagonal * cos(cornerAngle + theta); // NOLINT - position.second = y + cornerDiagonal * sin(cornerAngle + theta); // NOLINT - } - return position; -} - -// https://www.desmos.com/calculator/cqmc1tjtsv -template -decltype(LineNumberType() + PointNumberType()) linePointDist( - LineNumberType lineStartX, LineNumberType lineStartY, - LineNumberType lineEndX, LineNumberType lineEndY, PointNumberType pointX, - PointNumberType pointY) { - auto lX = lineEndX - lineStartX; - auto lY = lineEndY - lineStartY; - auto vX = pointX - lineStartX; - auto vY = pointY - lineStartY; - auto dot = vX * lX + vY * lY; - auto lNormSquared = lX * lX + lY * lY; - auto t = dot / lNormSquared; - auto tBounded = fmax(fmin(t, 1), 0); // NOLINT - auto iX = (1 - tBounded) * lineStartX + tBounded * lineEndX; - auto iY = (1 - tBounded) * lineStartY + tBounded * lineEndY; - auto distSquared = - (iX - pointX) * (iX - pointX) + (iY - pointY) * (iY - pointY); - return distSquared; -} - -template - requires OptiSys -void ApplyConstraint(Opti& opti, const Expr& x, const Expr& y, - const Expr& theta, const Constraint& constraint) { - if (std::holds_alternative(constraint)) { - auto& translationConstraint = std::get(constraint); - ApplySet2dConstraint(opti, x, y, translationConstraint.translationBound); - } else if (std::holds_alternative(constraint)) { - auto& headingConstraint = std::get(constraint); - ApplyIntervalSet1dConstraint(opti, theta, headingConstraint.headingBound); - } else if (std::holds_alternative(constraint)) { - auto linePointConstraint = std::get(constraint); - auto [lineStartX, lineStartY] = SolveRobotPointPosition( - x, y, theta, linePointConstraint.robotLineStartX, - linePointConstraint.robotLineStartY); - auto [lineEndX, lineEndY] = - SolveRobotPointPosition(x, y, theta, linePointConstraint.robotLineEndX, - linePointConstraint.robotLineEndY); - double pointX = linePointConstraint.fieldPointX; - double pointY = linePointConstraint.fieldPointY; - auto dist = linePointDist(lineStartX, lineStartY, lineEndX, lineEndY, - pointX, pointY); - auto distSquared = dist * dist; - auto& distInterval = linePointConstraint.distance; - auto distIntervalSquared = IntervalSet1d(std::pow(distInterval.lower, 2), - std::pow(distInterval.upper, 2)); - ApplyIntervalSet1dConstraint(opti, distSquared, distIntervalSquared); - } else if (std::holds_alternative(constraint)) { - auto pointLineConstraint = std::get(constraint); - double lineStartX = pointLineConstraint.fieldLineStartX; - double lineStartY = pointLineConstraint.fieldLineStartY; - double lineEndX = pointLineConstraint.fieldLineEndX; - double lineEndY = pointLineConstraint.fieldLineEndY; - auto [pointX, pointY] = - SolveRobotPointPosition(x, y, theta, pointLineConstraint.robotPointX, - pointLineConstraint.robotPointY); - auto dist = linePointDist(lineStartX, lineStartY, lineEndX, lineEndY, - pointX, pointY); - auto distSquared = dist * dist; - auto& distInterval = pointLineConstraint.distance; - auto distIntervalSquared = IntervalSet1d(std::pow(distInterval.lower, 2), - std::pow(distInterval.upper, 2)); - ApplyIntervalSet1dConstraint(opti, distSquared, distIntervalSquared); - } else if (std::holds_alternative(constraint)) { - auto pointPointConstraint = std::get(constraint); - double robotPointX = pointPointConstraint.robotPointX; - double robotPointY = pointPointConstraint.robotPointY; - double fieldPointX = pointPointConstraint.fieldPointX; - double fieldPointY = pointPointConstraint.fieldPointY; - auto [bumperCornerX, bumperCornerY] = - SolveRobotPointPosition(x, y, theta, robotPointX, robotPointY); - auto dx = fieldPointX - bumperCornerX; - auto dy = fieldPointY - bumperCornerY; - auto pointDistSquared = dx * dx + dy * dy; - IntervalSet1d distSquared = pointPointConstraint.distance; - distSquared.lower *= distSquared.lower; - distSquared.upper *= distSquared.upper; - ApplyIntervalSet1dConstraint(opti, pointDistSquared, distSquared); - } -} - -std::vector Linspace(double startValue, double endValue, - size_t numSamples) { - std::vector result; - double delta = (endValue - startValue) / numSamples; - for (size_t index = 1; index <= numSamples; index++) { - result.push_back(startValue + index * delta); - } - return result; -} - -template -inline void append_vector(std::vector& base, - const std::vector& newItems) { - base.insert(base.end(), newItems.begin(), newItems.end()); -} - -Solution GenerateLinearInitialGuess( - const std::vector>& initialGuessPoints, - const std::vector controlIntervalCounts) { - size_t wptCnt = controlIntervalCounts.size() + 1; - size_t sampTot = GetIdx(controlIntervalCounts, wptCnt, 0); - Solution initialGuess{}; - initialGuess.x.reserve(sampTot); - initialGuess.y.reserve(sampTot); - initialGuess.theta.reserve(sampTot); - initialGuess.dt.reserve(sampTot); - initialGuess.x.push_back(initialGuessPoints.front().front().x); - initialGuess.y.push_back(initialGuessPoints.front().front().y); - initialGuess.theta.push_back(initialGuessPoints.front().front().heading); - for (size_t i = 0; i < sampTot; i++) { - initialGuess.dt.push_back((wptCnt * 5.0) / sampTot); - } - for (size_t wptIdx = 1; wptIdx < wptCnt; wptIdx++) { - size_t N_sgmt = controlIntervalCounts.at(wptIdx - 1); - size_t guessPointCount = initialGuessPoints.at(wptIdx).size(); - size_t N_guessSgmt = N_sgmt / guessPointCount; - append_vector( - initialGuess.x, - Linspace(initialGuessPoints.at(wptIdx - 1).back().x, - initialGuessPoints.at(wptIdx).front().x, N_guessSgmt)); - append_vector( - initialGuess.y, - Linspace(initialGuessPoints.at(wptIdx - 1).back().y, - initialGuessPoints.at(wptIdx).front().y, N_guessSgmt)); - append_vector( - initialGuess.theta, - Linspace(initialGuessPoints.at(wptIdx - 1).back().heading, - initialGuessPoints.at(wptIdx).front().heading, N_guessSgmt)); - for (size_t guessPointIdx = 1; guessPointIdx < guessPointCount - 1; - guessPointIdx++) { // if three or more guess points - append_vector( - initialGuess.x, - Linspace(initialGuessPoints.at(wptIdx).at(guessPointIdx - 1).x, - initialGuessPoints.at(wptIdx).at(guessPointIdx).x, - N_guessSgmt)); - append_vector( - initialGuess.y, - Linspace(initialGuessPoints.at(wptIdx).at(guessPointIdx - 1).y, - initialGuessPoints.at(wptIdx).at(guessPointIdx).y, - N_guessSgmt)); - append_vector( - initialGuess.theta, - Linspace(initialGuessPoints.at(wptIdx).at(guessPointIdx - 1).heading, - initialGuessPoints.at(wptIdx).at(guessPointIdx).heading, - N_guessSgmt)); - } - if (guessPointCount > 1) { // if two or more guess points - size_t N_lastGuessSgmt = N_sgmt - (guessPointCount - 1) * N_guessSgmt; - append_vector( - initialGuess.x, - Linspace(initialGuessPoints.at(wptIdx).at(guessPointCount - 2).x, - initialGuessPoints.at(wptIdx).back().x, N_lastGuessSgmt)); - append_vector( - initialGuess.y, - Linspace(initialGuessPoints.at(wptIdx).at(guessPointCount - 2).y, - initialGuessPoints.at(wptIdx).back().y, N_lastGuessSgmt)); - append_vector( - initialGuess.theta, - Linspace( - initialGuessPoints.at(wptIdx).at(guessPointCount - 2).heading, - initialGuessPoints.at(wptIdx).back().heading, N_lastGuessSgmt)); - } - } - return initialGuess; -} - -template - requires OptiSys -void ApplyInitialGuess(Opti& opti, const Solution& solution, - std::vector& x, std::vector& y, - std::vector& theta, std::vector& vx, - std::vector& vy, std::vector& omega, - std::vector& ax, std::vector& ay, - std::vector& alpha) { - size_t sampleTotal = x.size(); - for (size_t sampleIndex = 0; sampleIndex < sampleTotal; sampleIndex++) { - opti.SetInitial(x[sampleIndex], solution.x[sampleIndex]); - opti.SetInitial(y[sampleIndex], solution.y[sampleIndex]); - opti.SetInitial(theta[sampleIndex], solution.theta[sampleIndex]); - } - opti.SetInitial(vx[0], 0.0); - opti.SetInitial(vy[0], 0.0); - opti.SetInitial(omega[0], 0.0); - opti.SetInitial(ax[0], 0.0); - opti.SetInitial(ay[0], 0.0); - opti.SetInitial(alpha[0], 0.0); - for (size_t sampleIndex = 1; sampleIndex < sampleTotal; sampleIndex++) { - opti.SetInitial(vx[sampleIndex], - (solution.x[sampleIndex] - solution.x[sampleIndex - 1]) / - solution.dt[sampleIndex]); - opti.SetInitial(vy[sampleIndex], - (solution.y[sampleIndex] - solution.y[sampleIndex - 1]) / - solution.dt[sampleIndex]); - opti.SetInitial(omega[sampleIndex], (solution.theta[sampleIndex] - - solution.theta[sampleIndex - 1]) / - solution.dt[sampleIndex]); - - opti.SetInitial(ax[sampleIndex], (opti.SolutionValue(vx[sampleIndex]) - - opti.SolutionValue(vx[sampleIndex - 1])) / - solution.dt[sampleIndex]); - opti.SetInitial(ay[sampleIndex], (opti.SolutionValue(vy[sampleIndex]) - - opti.SolutionValue(vy[sampleIndex - 1])) / - solution.dt[sampleIndex]); - opti.SetInitial(alpha[sampleIndex], - (opti.SolutionValue(omega[sampleIndex]) - - opti.SolutionValue(omega[sampleIndex - 1])) / - solution.dt[sampleIndex]); - } -} - -} // namespace trajopt diff --git a/src/optimization/algorithms/SwerveDiscreteOptimal.h b/src/optimization/algorithms/SwerveDiscreteOptimal.h index 5f17b323..33143ab0 100644 --- a/src/optimization/algorithms/SwerveDiscreteOptimal.h +++ b/src/optimization/algorithms/SwerveDiscreteOptimal.h @@ -2,11 +2,14 @@ #pragma once +#include +#include #include #include #include "optimization/HolonomicTrajoptUtil.h" #include "optimization/OptiSys.h" +#include "optimization/SwerveTrajoptUtil.h" #include "optimization/TrajoptUtil.h" #include "trajopt/drivetrain/SwerveDrivetrain.h" #include "trajopt/expected" @@ -19,6 +22,144 @@ template requires OptiSys class SwerveDiscreteOptimal { public: + /** + * Construct a new swerve trajectory optimization problem. + */ + SwerveDiscreteOptimal(const SwervePath& path, const std::vector& N, + const Solution& initialGuess, int64_t handle = 0) + : path(path), N(N) { + opti.AddIntermediateCallback([this, handle = handle] { + constexpr int fps = 60; + constexpr std::chrono::duration timePerFrame{1.0 / fps}; + + // FPS limit on sending updates + static auto lastFrameTime = std::chrono::steady_clock::now(); + auto now = std::chrono::steady_clock::now(); + if (now - lastFrameTime < timePerFrame) { + return; + } + + lastFrameTime = now; + + auto soln = ConstructSwerveSolution(opti, x, y, theta, vx, vy, omega, ax, + ay, alpha, Fx, Fy, dt, this->N); + for (auto& callback : this->path.callbacks) { + callback(soln, handle); + } + }); + size_t wptCnt = 1 + N.size(); + size_t sgmtCnt = N.size(); + size_t sampTot = GetIdx(N, wptCnt, 0); + size_t moduleCnt = path.drivetrain.modules.size(); + + x.reserve(sampTot); + y.reserve(sampTot); + theta.reserve(sampTot); + vx.reserve(sampTot); + vy.reserve(sampTot); + omega.reserve(sampTot); + ax.reserve(sampTot); + ay.reserve(sampTot); + alpha.reserve(sampTot); + + Fx.reserve(sampTot); + Fy.reserve(sampTot); + for (size_t sampIdx = 0; sampIdx < sampTot; ++sampIdx) { + auto& _Fx = Fx.emplace_back(); + auto& _Fy = Fy.emplace_back(); + _Fx.reserve(moduleCnt); + _Fy.reserve(moduleCnt); + } + + dt.reserve(sgmtCnt); + + for (size_t idx = 0; idx < sampTot; ++idx) { + x.emplace_back(opti.DecisionVariable()); + y.emplace_back(opti.DecisionVariable()); + theta.emplace_back(opti.DecisionVariable()); + vx.emplace_back(opti.DecisionVariable()); + vy.emplace_back(opti.DecisionVariable()); + omega.emplace_back(opti.DecisionVariable()); + ax.emplace_back(opti.DecisionVariable()); + ay.emplace_back(opti.DecisionVariable()); + alpha.emplace_back(opti.DecisionVariable()); + + for (size_t moduleIdx = 0; moduleIdx < moduleCnt; ++moduleIdx) { + Fx.at(idx).emplace_back(opti.DecisionVariable()); + Fy.at(idx).emplace_back(opti.DecisionVariable()); + } + } + + double minWidth = INFINITY; + for (size_t i = 1; i < path.drivetrain.modules.size(); i++) { + if (std::abs(path.drivetrain.modules.at(i - 1).x - + path.drivetrain.modules.at(i).x) != 0) { + minWidth = + std::min(minWidth, std::abs(path.drivetrain.modules.at(i - 1).x - + path.drivetrain.modules.at(i).x)); + } + if (std::abs(path.drivetrain.modules.at(i - 1).y - + path.drivetrain.modules.at(i).y) != 0) { + minWidth = + std::min(minWidth, std::abs(path.drivetrain.modules.at(i - 1).y - + path.drivetrain.modules.at(i).y)); + } + } + + for (size_t sgmtIdx = 0; sgmtIdx < sgmtCnt; ++sgmtIdx) { + dt.emplace_back(opti.DecisionVariable()); + for (auto module : path.drivetrain.modules) { + opti.SubjectTo(dt.at(sgmtIdx) * module.wheelRadius * + module.wheelMaxAngularVelocity <= + minWidth); + } + } + + ApplyDiscreteTimeObjective(opti, dt, N); + ApplyKinematicsConstraints(opti, x, y, theta, vx, vy, omega, ax, ay, alpha, + dt, N); + + for (size_t idx = 0; idx < sampTot; ++idx) { + auto [Fx_net, Fy_net] = SolveNetForce(Fx.at(idx), Fy.at(idx)); + ApplyDynamicsConstraints( + opti, ax.at(idx), ay.at(idx), alpha.at(idx), Fx_net, Fy_net, + SolveNetTorque(theta.at(idx), Fx.at(idx), Fy.at(idx), + path.drivetrain.modules), + path.drivetrain.mass, path.drivetrain.moi); + + ApplyPowerConstraints(opti, theta.at(idx), vx.at(idx), vy.at(idx), + omega.at(idx), Fx.at(idx), Fy.at(idx), + path.drivetrain); + } + + for (size_t wptIdx = 0; wptIdx < wptCnt; ++wptIdx) { + for (auto& constraint : path.waypoints.at(wptIdx).waypointConstraints) { + size_t idx = GetIdx(N, wptIdx + 1, 0) - 1; // first idx of next wpt - 1 + ApplyHolonomicConstraint( + opti, x.at(idx), y.at(idx), theta.at(idx), vx.at(idx), vy.at(idx), + omega.at(idx), ax.at(idx), ay.at(idx), alpha.at(idx), constraint); + } + } // TODO: try changing the path struct so instead of having waypoint + // objects + // it's just two vectors of waypoint constraints and segment + // constraints, the waypoint one would be one larger by size + for (size_t sgmtIdx = 0; sgmtIdx < sgmtCnt; ++sgmtIdx) { + for (auto& constraint : + path.waypoints.at(sgmtIdx + 1).segmentConstraints) { + size_t startIdx = GetIdx(N, sgmtIdx + 1, 0); + size_t endIdx = GetIdx(N, sgmtIdx + 2, 0); + for (size_t idx = startIdx; idx < endIdx; ++idx) { + ApplyHolonomicConstraint( + opti, x.at(idx), y.at(idx), theta.at(idx), vx.at(idx), vy.at(idx), + omega.at(idx), ax.at(idx), ay.at(idx), alpha.at(idx), constraint); + } + } + } + + ApplyInitialGuess(opti, initialGuess, x, y, theta, vx, vy, omega, ax, ay, + alpha); + } + /** * Generates an optimal trajectory. * @@ -28,7 +169,14 @@ class SwerveDiscreteOptimal { * @return Returns a holonomic trajectory on success, or a string containing a * failure reason. */ - expected Generate(bool diagnostics = false); + expected Generate(bool diagnostics = false) { + if (auto sol = opti.Solve(diagnostics); sol.has_value()) { + return ConstructSwerveSolution(opti, x, y, theta, vx, vy, omega, ax, ay, + alpha, Fx, Fy, dt, N); + } else { + return unexpected{sol.error()}; + } + } private: /** @@ -58,20 +206,6 @@ class SwerveDiscreteOptimal { const std::vector& N; Opti opti; - - public: - /** - * Construct a new CasADi Swerve Trajectory Optimization Problem - * with a swerve drivetrain and holonomic path. - * - * @param swerveDrivetrain the swerve drivetrain - * @param holonomicPath the holonomic path - */ - explicit SwerveDiscreteOptimal(const SwervePath& path, - const std::vector& N, - const Solution& initialGuess, - int64_t handle = 0); }; -} // namespace trajopt -#include "optimization/algorithms/SwerveDiscreteOptimal.inc" +} // namespace trajopt diff --git a/src/optimization/algorithms/SwerveDiscreteOptimal.inc b/src/optimization/algorithms/SwerveDiscreteOptimal.inc deleted file mode 100644 index 2511b475..00000000 --- a/src/optimization/algorithms/SwerveDiscreteOptimal.inc +++ /dev/null @@ -1,167 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include -#include -#include -#include - -#include "optimization/SwerveTrajoptUtil.h" -#include "optimization/TrajoptUtil.h" -#include "optimization/algorithms/SwerveDiscreteOptimal.h" -#include "trajopt/drivetrain/SwerveDrivetrain.h" -#include "trajopt/expected" -#include "trajopt/path/Path.h" -#include "trajopt/solution/SwerveSolution.h" - -namespace trajopt { - -template - requires OptiSys -expected -SwerveDiscreteOptimal::Generate(bool diagnostics) { - if (auto sol = opti.Solve(diagnostics); sol.has_value()) { - return ConstructSwerveSolution(opti, x, y, theta, vx, vy, omega, ax, ay, - alpha, Fx, Fy, dt, N); - } else { - return unexpected{sol.error()}; - } -} - -template - requires OptiSys -SwerveDiscreteOptimal::SwerveDiscreteOptimal( - const SwervePath& path, const std::vector& N, - const Solution& initialGuess, int64_t handle) - : path(path), N(N) { - opti.AddIntermediateCallback([this, handle = handle] { - constexpr int fps = 60; - constexpr std::chrono::duration timePerFrame{1.0 / fps}; - - // FPS limit on sending updates - static auto lastFrameTime = std::chrono::steady_clock::now(); - auto now = std::chrono::steady_clock::now(); - if (now - lastFrameTime < timePerFrame) { - return; - } - - lastFrameTime = now; - - auto soln = ConstructSwerveSolution(opti, x, y, theta, vx, vy, omega, ax, - ay, alpha, Fx, Fy, dt, this->N); - for (auto& callback : this->path.callbacks) { - callback(soln, handle); - } - }); - size_t wptCnt = 1 + N.size(); - size_t sgmtCnt = N.size(); - size_t sampTot = GetIdx(N, wptCnt, 0); - size_t moduleCnt = path.drivetrain.modules.size(); - - x.reserve(sampTot); - y.reserve(sampTot); - theta.reserve(sampTot); - vx.reserve(sampTot); - vy.reserve(sampTot); - omega.reserve(sampTot); - ax.reserve(sampTot); - ay.reserve(sampTot); - alpha.reserve(sampTot); - - Fx.reserve(sampTot); - Fy.reserve(sampTot); - for (size_t sampIdx = 0; sampIdx < sampTot; ++sampIdx) { - auto& _Fx = Fx.emplace_back(); - auto& _Fy = Fy.emplace_back(); - _Fx.reserve(moduleCnt); - _Fy.reserve(moduleCnt); - } - - dt.reserve(sgmtCnt); - - for (size_t idx = 0; idx < sampTot; ++idx) { - x.emplace_back(opti.DecisionVariable()); - y.emplace_back(opti.DecisionVariable()); - theta.emplace_back(opti.DecisionVariable()); - vx.emplace_back(opti.DecisionVariable()); - vy.emplace_back(opti.DecisionVariable()); - omega.emplace_back(opti.DecisionVariable()); - ax.emplace_back(opti.DecisionVariable()); - ay.emplace_back(opti.DecisionVariable()); - alpha.emplace_back(opti.DecisionVariable()); - - for (size_t moduleIdx = 0; moduleIdx < moduleCnt; ++moduleIdx) { - Fx.at(idx).emplace_back(opti.DecisionVariable()); - Fy.at(idx).emplace_back(opti.DecisionVariable()); - } - } - - double minWidth = INFINITY; - for (size_t i = 1; i < path.drivetrain.modules.size(); i++) { - if (std::abs(path.drivetrain.modules.at(i - 1).x - - path.drivetrain.modules.at(i).x) != 0) { - minWidth = - std::min(minWidth, std::abs(path.drivetrain.modules.at(i - 1).x - - path.drivetrain.modules.at(i).x)); - } - if (std::abs(path.drivetrain.modules.at(i - 1).y - - path.drivetrain.modules.at(i).y) != 0) { - minWidth = - std::min(minWidth, std::abs(path.drivetrain.modules.at(i - 1).y - - path.drivetrain.modules.at(i).y)); - } - } - - for (size_t sgmtIdx = 0; sgmtIdx < sgmtCnt; ++sgmtIdx) { - dt.emplace_back(opti.DecisionVariable()); - for (auto module : path.drivetrain.modules) { - opti.SubjectTo(dt.at(sgmtIdx) * module.wheelRadius * - module.wheelMaxAngularVelocity <= - minWidth); - } - } - - ApplyDiscreteTimeObjective(opti, dt, N); - ApplyKinematicsConstraints(opti, x, y, theta, vx, vy, omega, ax, ay, alpha, - dt, N); - - for (size_t idx = 0; idx < sampTot; ++idx) { - auto [Fx_net, Fy_net] = SolveNetForce(Fx.at(idx), Fy.at(idx)); - ApplyDynamicsConstraints( - opti, ax.at(idx), ay.at(idx), alpha.at(idx), Fx_net, Fy_net, - SolveNetTorque(theta.at(idx), Fx.at(idx), Fy.at(idx), - path.drivetrain.modules), - path.drivetrain.mass, path.drivetrain.moi); - - ApplyPowerConstraints(opti, theta.at(idx), vx.at(idx), vy.at(idx), - omega.at(idx), Fx.at(idx), Fy.at(idx), - path.drivetrain); - } - - for (size_t wptIdx = 0; wptIdx < wptCnt; ++wptIdx) { - for (auto& constraint : path.waypoints.at(wptIdx).waypointConstraints) { - size_t idx = GetIdx(N, wptIdx + 1, 0) - 1; // first idx of next wpt - 1 - ApplyHolonomicConstraint( - opti, x.at(idx), y.at(idx), theta.at(idx), vx.at(idx), vy.at(idx), - omega.at(idx), ax.at(idx), ay.at(idx), alpha.at(idx), constraint); - } - } // TODO: try changing the path struct so instead of having waypoint objects - // it's just two vectors of waypoint constraints and segment - // constraints, the waypoint one would be one larger by size - for (size_t sgmtIdx = 0; sgmtIdx < sgmtCnt; ++sgmtIdx) { - for (auto& constraint : path.waypoints.at(sgmtIdx + 1).segmentConstraints) { - size_t startIdx = GetIdx(N, sgmtIdx + 1, 0); - size_t endIdx = GetIdx(N, sgmtIdx + 2, 0); - for (size_t idx = startIdx; idx < endIdx; ++idx) { - ApplyHolonomicConstraint( - opti, x.at(idx), y.at(idx), theta.at(idx), vx.at(idx), vy.at(idx), - omega.at(idx), ax.at(idx), ay.at(idx), alpha.at(idx), constraint); - } - } - } - - ApplyInitialGuess(opti, initialGuess, x, y, theta, vx, vy, omega, ax, ay, - alpha); -} -} // namespace trajopt