diff --git a/build.rs b/build.rs index 42fd9985..bd8ed72f 100644 --- a/build.rs +++ b/build.rs @@ -23,6 +23,7 @@ fn main() { .file("src/trajoptlibrust.cpp") .include("src") .include(format!("{}/include", cmake_dest.display())) + .include(format!("{}/include/eigen3", cmake_dest.display())) .std("c++20"); bridge_build.compile("trajoptrust"); diff --git a/examples/Swerve/src/Main.cpp b/examples/Swerve/src/Main.cpp index 93074931..7044616b 100644 --- a/examples/Swerve/src/Main.cpp +++ b/examples/Swerve/src/Main.cpp @@ -1,6 +1,6 @@ // Copyright (c) TrajoptLib contributors -#include +#include int main() { trajopt::SwerveDrivetrain swerveDrivetrain{ @@ -23,7 +23,9 @@ int main() { path.WptZeroVelocity(1); path.ControlIntervalCounts({4}); + trajopt::SwerveTrajectoryGenerator generator{path}; + // SOLVE [[maybe_unused]] - auto solution = trajopt::OptimalTrajectoryGenerator::Generate(path, true); + auto solution = generator.Generate(true); } diff --git a/include/trajopt/OptimalTrajectoryGenerator.hpp b/include/trajopt/OptimalTrajectoryGenerator.hpp deleted file mode 100644 index 606191ca..00000000 --- a/include/trajopt/OptimalTrajectoryGenerator.hpp +++ /dev/null @@ -1,38 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include - -#include "trajopt/expected" -#include "trajopt/path/SwervePathBuilder.hpp" -#include "trajopt/solution/SwerveSolution.hpp" -#include "trajopt/util/SymbolExports.hpp" - -namespace trajopt { - -/** - * @brief This trajectory generator class contains functions to generate - * time-optimal trajectories for several drivetrain types. - */ -class TRAJOPT_DLLEXPORT OptimalTrajectoryGenerator { - public: - OptimalTrajectoryGenerator() = delete; - - /** - * Initializes and solves an optimization problem for a swerve drivetrain. - * - * @param path the path - * @param diagnostics Enables diagnostic prints. - * @param handle an identifier for state callbacks - * @return The optimized swerve trajectory solution on success, or a string - * containing the failure reason. - */ - static expected Generate( - const SwervePathBuilder& path, bool diagnostics = false, - int64_t handle = 0); -}; - -} // namespace trajopt diff --git a/include/trajopt/SwerveTrajectoryGenerator.hpp b/include/trajopt/SwerveTrajectoryGenerator.hpp new file mode 100644 index 00000000..599d787c --- /dev/null +++ b/include/trajopt/SwerveTrajectoryGenerator.hpp @@ -0,0 +1,485 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "trajopt/drivetrain/SwerveDrivetrain.hpp" +#include "trajopt/path/Path.hpp" +#include "trajopt/path/SwervePathBuilder.hpp" +#include "trajopt/solution/SwerveSolution.hpp" +#include "trajopt/util/Cancellation.hpp" +#include "trajopt/util/SymbolExports.hpp" +#include "trajopt/util/TrajoptUtil.hpp" +#include "trajopt/util/expected" + +namespace trajopt { + +/** + * This trajectory generator class contains functions to generate + * time-optimal trajectories for several drivetrain types. + */ +class TRAJOPT_DLLEXPORT SwerveTrajectoryGenerator { + public: + /** + * Construct a new swerve trajectory optimization problem. + * + * @param pathBuilder The path builder. + * @param handle An identifier for state callbacks. + */ + explicit SwerveTrajectoryGenerator(const SwervePathBuilder& pathBuilder, + int64_t handle = 0) + : path(pathBuilder.GetPath()), N(pathBuilder.GetControlIntervalCounts()) { + auto initialGuess = pathBuilder.CalculateInitialGuess(); + + callbacks.emplace_back([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(); + 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); + thetacos.reserve(sampTot); + thetasin.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(problem.DecisionVariable()); + y.emplace_back(problem.DecisionVariable()); + thetacos.emplace_back(problem.DecisionVariable()); + thetasin.emplace_back(problem.DecisionVariable()); + vx.emplace_back(problem.DecisionVariable()); + vy.emplace_back(problem.DecisionVariable()); + omega.emplace_back(problem.DecisionVariable()); + ax.emplace_back(problem.DecisionVariable()); + ay.emplace_back(problem.DecisionVariable()); + alpha.emplace_back(problem.DecisionVariable()); + + for (size_t moduleIdx = 0; moduleIdx < moduleCnt; ++moduleIdx) { + Fx.at(idx).emplace_back(problem.DecisionVariable()); + Fy.at(idx).emplace_back(problem.DecisionVariable()); + } + } + + double minWidth = INFINITY; + for (size_t i = 1; i < path.drivetrain.modules.size(); i++) { + if (std::abs(path.drivetrain.modules.at(i - 1).translation.X() - + path.drivetrain.modules.at(i).translation.X()) != 0) { + minWidth = std::min( + minWidth, + std::abs(path.drivetrain.modules.at(i - 1).translation.X() - + path.drivetrain.modules.at(i).translation.X())); + } + if (std::abs(path.drivetrain.modules.at(i - 1).translation.Y() - + path.drivetrain.modules.at(i).translation.Y()) != 0) { + minWidth = std::min( + minWidth, + std::abs(path.drivetrain.modules.at(i - 1).translation.Y() - + path.drivetrain.modules.at(i).translation.Y())); + } + } + + for (size_t sgmtIdx = 0; sgmtIdx < sgmtCnt; ++sgmtIdx) { + dt.emplace_back(problem.DecisionVariable()); + for (auto module : path.drivetrain.modules) { + problem.SubjectTo(dt.at(sgmtIdx) * module.wheelRadius * + module.wheelMaxAngularVelocity <= + minWidth); + } + } + + // Minimize total time + sleipnir::Variable 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 * static_cast(N_sgmt); + T_tot += T_sgmt; + + problem.SubjectTo(dt_sgmt >= 0); + dt_sgmt.SetValue(5.0 / N_sgmt); + } + problem.Minimize(std::move(T_tot)); + + // Apply kinematics constraints + 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); + + Translation2v x_n{x.at(idx), y.at(idx)}; + Translation2v x_n_1{x.at(idx - 1), y.at(idx - 1)}; + + Rotation2v theta_n{thetacos.at(idx), thetasin.at(idx)}; + Rotation2v theta_n_1{thetacos.at(idx - 1), thetasin.at(idx - 1)}; + + Translation2v v_n{vx.at(idx), vy.at(idx)}; + Translation2v v_n_1{vx.at(idx - 1), vy.at(idx - 1)}; + + auto omega_n = omega.at(idx); + auto omega_n_1 = omega.at(idx - 1); + + Translation2v a_n{ax.at(idx), ay.at(idx)}; + auto alpha_n = alpha.at(idx); + + problem.SubjectTo(x_n_1 + v_n * dt_sgmt == x_n); + problem.SubjectTo((theta_n - theta_n_1) == + Rotation2v{omega_n * dt_sgmt}); + problem.SubjectTo(v_n_1 + a_n * dt_sgmt == v_n); + problem.SubjectTo(omega_n_1 + alpha_n * dt_sgmt == omega_n); + } + } + + for (size_t idx = 0; idx < sampTot; ++idx) { + Rotation2v theta{thetacos.at(idx), thetasin.at(idx)}; + Translation2v v{vx.at(idx), vy.at(idx)}; + + // Solve for net force + auto Fx_net = std::accumulate(Fx.at(idx).begin(), Fx.at(idx).end(), + sleipnir::Variable{0.0}); + auto Fy_net = std::accumulate(Fy.at(idx).begin(), Fy.at(idx).end(), + sleipnir::Variable{0.0}); + + // Solve for net torque + sleipnir::Variable tau_net = 0.0; + for (size_t moduleIdx = 0; moduleIdx < path.drivetrain.modules.size(); + ++moduleIdx) { + const auto& [translation, wheelRadius, wheelMaxAngularVelocity, + wheelMaxTorque] = path.drivetrain.modules.at(moduleIdx); + + auto r = translation.RotateBy(theta); + Translation2v F{Fx.at(idx).at(moduleIdx), Fy.at(idx).at(moduleIdx)}; + + tau_net += r.Cross(F); + } + + // Apply module power constraints + auto vWrtRobot = v.RotateBy(-theta); + for (size_t moduleIdx = 0; moduleIdx < path.drivetrain.modules.size(); + ++moduleIdx) { + const auto& [translation, wheelRadius, wheelMaxAngularVelocity, + wheelMaxTorque] = path.drivetrain.modules.at(moduleIdx); + + Translation2v vWheelWrtRobot{ + vWrtRobot.X() - translation.Y() * omega.at(idx), + vWrtRobot.Y() + translation.X() * omega.at(idx)}; + double maxWheelVelocity = wheelRadius * wheelMaxAngularVelocity; + problem.SubjectTo(vWheelWrtRobot.SquaredNorm() <= + maxWheelVelocity * maxWheelVelocity); + + Translation2v moduleF{Fx.at(idx).at(moduleIdx), + Fy.at(idx).at(moduleIdx)}; + double maxForce = wheelMaxTorque / wheelRadius; + problem.SubjectTo(moduleF.SquaredNorm() <= maxForce * maxForce); + } + + // Apply dynamics constraints + problem.SubjectTo(Fx_net == path.drivetrain.mass * ax.at(idx)); + problem.SubjectTo(Fy_net == path.drivetrain.mass * ay.at(idx)); + problem.SubjectTo(tau_net == path.drivetrain.moi * alpha.at(idx)); + } + + 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 + + Pose2v pose{x.at(idx), y.at(idx), {thetacos.at(idx), thetasin.at(idx)}}; + Translation2v v{vx.at(idx), vy.at(idx)}; + + if (std::holds_alternative(constraint)) { + ApplyConstraint(problem, pose, + std::get(constraint)); + } else if (std::holds_alternative(constraint)) { + ApplyConstraint(problem, pose, + std::get(constraint)); + } else if (std::holds_alternative(constraint)) { + ApplyConstraint(problem, pose, + std::get(constraint)); + } else if (std::holds_alternative(constraint)) { + ApplyConstraint(problem, pose, + std::get(constraint)); + } else if (std::holds_alternative(constraint)) { + ApplyConstraint(problem, pose, + std::get(constraint)); + } else if (std::holds_alternative( + constraint)) { + const auto& angularVelocityConstraint = + std::get(constraint); + ApplyIntervalSet1dConstraint( + problem, omega.at(idx), + angularVelocityConstraint.angularVelocityBound); + } else if (std::holds_alternative( + constraint)) { + const auto& velocityHolonomicConstraint = + std::get(constraint); + ApplySet2dConstraint(problem, v, + velocityHolonomicConstraint.velocityBound); + } else if (std::holds_alternative(constraint)) { + auto pointAtConstraint = std::get(constraint); + const auto& [fieldPoint, headingTolerance] = pointAtConstraint; + + // 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 = fieldPoint.X() - pose.X(); + auto dy = fieldPoint.Y() - pose.Y(); + auto dot = pose.Rotation().Cos() * dx + pose.Rotation().Sin() * dy; + problem.SubjectTo(dot >= std::cos(headingTolerance) * + sleipnir::hypot(dx, dy)); + } + } + } + + // 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) { + Pose2v pose{ + x.at(idx), y.at(idx), {thetacos.at(idx), thetasin.at(idx)}}; + Translation2v v{vx.at(idx), vy.at(idx)}; + + if (std::holds_alternative(constraint)) { + ApplyConstraint(problem, pose, + std::get(constraint)); + } else if (std::holds_alternative(constraint)) { + ApplyConstraint(problem, pose, + std::get(constraint)); + } else if (std::holds_alternative(constraint)) { + ApplyConstraint(problem, pose, + std::get(constraint)); + } else if (std::holds_alternative(constraint)) { + ApplyConstraint(problem, pose, + std::get(constraint)); + } else if (std::holds_alternative(constraint)) { + ApplyConstraint(problem, pose, + std::get(constraint)); + } else if (std::holds_alternative( + constraint)) { + const auto& angularVelocityConstraint = + std::get(constraint); + ApplyIntervalSet1dConstraint( + problem, omega.at(idx), + angularVelocityConstraint.angularVelocityBound); + } else if (std::holds_alternative( + constraint)) { + const auto& velocityHolonomicConstraint = + std::get(constraint); + ApplySet2dConstraint(problem, v, + velocityHolonomicConstraint.velocityBound); + } else if (std::holds_alternative(constraint)) { + auto pointAtConstraint = std::get(constraint); + const auto& [fieldPoint, headingTolerance] = pointAtConstraint; + + // 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 = fieldPoint.X() - pose.X(); + auto dy = fieldPoint.Y() - pose.Y(); + auto dot = pose.Rotation().Cos() * dx + pose.Rotation().Sin() * dy; + problem.SubjectTo(dot >= std::cos(headingTolerance) * + sleipnir::hypot(dx, dy)); + } + } + } + } + + ApplyInitialGuess(initialGuess); + } + + /** + * Generates an optimal trajectory. + * + * This function may take a long time to complete. + * + * @param diagnostics Enables diagnostic prints. + * @return Returns a holonomic trajectory on success, or a string containing a + * failure reason. + */ + expected Generate(bool diagnostics = false) { + GetCancellationFlag() = 0; + problem.Callback([this](const sleipnir::SolverIterationInfo&) -> bool { + for (auto& callback : callbacks) { + callback(); + } + return trajopt::GetCancellationFlag(); + }); + + // tolerance of 1e-4 is 0.1 mm + auto status = + problem.Solve({.tolerance = 1e-4, .diagnostics = diagnostics}); + + if (static_cast(status.exitCondition) < 0 || + status.exitCondition == + sleipnir::SolverExitCondition::kCallbackRequestedStop) { + return unexpected{std::string{sleipnir::ToMessage(status.exitCondition)}}; + } else { + return ConstructSwerveSolution(); + } + } + + private: + /** + * The swerve drivetrain. + */ + SwervePath path; + + /// State Variables + std::vector x; + std::vector y; + std::vector thetacos; + std::vector thetasin; + std::vector vx; + std::vector vy; + std::vector omega; + std::vector ax; + std::vector ay; + std::vector alpha; + + /// Input Variables + std::vector> Fx; + std::vector> Fy; + + /// Time Variables + std::vector dt; + + /// Discretization Constants + std::vector N; + + sleipnir::OptimizationProblem problem; + std::vector> callbacks; + + void ApplyInitialGuess(const SwerveSolution& solution) { + size_t sampleTotal = x.size(); + for (size_t sampleIndex = 0; sampleIndex < sampleTotal; sampleIndex++) { + x[sampleIndex].SetValue(solution.x[sampleIndex]); + y[sampleIndex].SetValue(solution.y[sampleIndex]); + thetacos[sampleIndex].SetValue(solution.thetacos[sampleIndex]); + thetasin[sampleIndex].SetValue(solution.thetasin[sampleIndex]); + } + + vx[0].SetValue(0.0); + vy[0].SetValue(0.0); + omega[0].SetValue(0.0); + ax[0].SetValue(0.0); + ay[0].SetValue(0.0); + alpha[0].SetValue(0.0); + + for (size_t sampleIndex = 1; sampleIndex < sampleTotal; sampleIndex++) { + vx[sampleIndex].SetValue( + (solution.x[sampleIndex] - solution.x[sampleIndex - 1]) / + solution.dt[sampleIndex]); + vy[sampleIndex].SetValue( + (solution.y[sampleIndex] - solution.y[sampleIndex - 1]) / + solution.dt[sampleIndex]); + + double thetacos = solution.thetacos[sampleIndex]; + double thetasin = solution.thetasin[sampleIndex]; + double last_thetacos = solution.thetacos[sampleIndex - 1]; + double last_thetasin = solution.thetasin[sampleIndex - 1]; + + omega[sampleIndex].SetValue( + Rotation2d{thetacos, thetasin} + .RotateBy(-Rotation2d{last_thetacos, last_thetasin}) + .Radians() / + solution.dt[sampleIndex]); + + ax[sampleIndex].SetValue( + (vx[sampleIndex].Value() - vx[sampleIndex - 1].Value()) / + solution.dt[sampleIndex]); + ay[sampleIndex].SetValue( + (vy[sampleIndex].Value() - vy[sampleIndex - 1].Value()) / + solution.dt[sampleIndex]); + alpha[sampleIndex].SetValue( + (omega[sampleIndex].Value() - omega[sampleIndex - 1].Value()) / + solution.dt[sampleIndex]); + } + } + + SwerveSolution ConstructSwerveSolution() { + std::vector dtPerSamp; + for (size_t sgmtIdx = 0; sgmtIdx < N.size(); ++sgmtIdx) { + size_t N_sgmt = N.at(sgmtIdx); + sleipnir::Variable dt_sgmt = dt.at(sgmtIdx); + double dt_val = dt_sgmt.Value(); + for (size_t i = 0; i < N_sgmt; ++i) { + dtPerSamp.push_back(dt_val); + } + } + + return SwerveSolution{dtPerSamp, + RowSolutionValue(x), + RowSolutionValue(y), + RowSolutionValue(thetacos), + RowSolutionValue(thetasin), + RowSolutionValue(vx), + RowSolutionValue(vy), + RowSolutionValue(omega), + RowSolutionValue(ax), + RowSolutionValue(ay), + RowSolutionValue(alpha), + MatrixSolutionValue(Fx), + MatrixSolutionValue(Fy)}; + } +}; + +} // namespace trajopt diff --git a/include/trajopt/geometry/Pose2.hpp b/include/trajopt/geometry/Pose2.hpp index 3a91967f..ebf712c9 100644 --- a/include/trajopt/geometry/Pose2.hpp +++ b/include/trajopt/geometry/Pose2.hpp @@ -5,6 +5,7 @@ #include #include +#include #include "trajopt/geometry/Rotation2.hpp" #include "trajopt/geometry/Translation2.hpp" @@ -99,4 +100,16 @@ class Pose2 { using Pose2d = Pose2; using Pose2v = Pose2; +template +sleipnir::EqualityConstraints operator==(const Pose2& lhs, + const Pose2& rhs) { + auto translationConstraints = + (lhs.Translation() == rhs.Translation()).constraints; + auto rotationConstraints = (lhs.Rotation() == rhs.Rotation()).constraints; + + return sleipnir::EqualityConstraints{translationConstraints.insert( + translationConstraints.begin(), rotationConstraints.begin(), + rotationConstraints.end())}; +} + } // namespace trajopt diff --git a/include/trajopt/geometry/Rotation2.hpp b/include/trajopt/geometry/Rotation2.hpp index d6c1f260..ff63d83f 100644 --- a/include/trajopt/geometry/Rotation2.hpp +++ b/include/trajopt/geometry/Rotation2.hpp @@ -7,6 +7,7 @@ #include #include +#include namespace trajopt { @@ -131,4 +132,29 @@ class Rotation2 { using Rotation2d = Rotation2; using Rotation2v = Rotation2; +template +sleipnir::EqualityConstraints operator==(const Rotation2& lhs, + const Rotation2& rhs) { + // Constrain angle equality on manifold. + // + // Let lhs = . NOLINT + // Let rhs = . NOLINT + // + // If the angles are equal, the angle between the unit vectors should be + // zero. + // + // lhs x rhs = ||lhs|| ||rhs|| sin(angleBetween) NOLINT + // = 1 * 1 * 0 + // = 0 + // + // NOTE: angleBetween = π rad would be another solution + sleipnir::VariableMatrix lhsVar{ + {lhs.Cos() * rhs.Sin() - rhs.Cos() * lhs.Sin() * rhs.Cos()}, + // Require that lhs and rhs are unit vectors + {lhs.Cos() * lhs.Cos() + lhs.Sin() * lhs.Sin()}, + {rhs.Cos() * rhs.Cos() + rhs.Sin() * rhs.Sin()}}; + sleipnir::VariableMatrix rhsVar{{0.0}, {1.0}, {1.0}}; + return lhsVar == rhsVar; +} + } // namespace trajopt diff --git a/include/trajopt/geometry/Translation2.hpp b/include/trajopt/geometry/Translation2.hpp index 5c035f99..045e36b4 100644 --- a/include/trajopt/geometry/Translation2.hpp +++ b/include/trajopt/geometry/Translation2.hpp @@ -7,6 +7,7 @@ #include #include +#include #include "trajopt/geometry/Rotation2.hpp" @@ -102,6 +103,32 @@ class Translation2 { */ constexpr Translation2 operator-() const { return {-m_x, -m_y}; } + /** + * Returns the translation multiplied by a scalar. + * + * @param scalar The scalar to multiply by. + * + * @return The scaled translation. + */ + template + constexpr auto operator*(const U& scalar) const { + using R = decltype(std::declval() + std::declval()); + return Translation2{m_x * scalar, m_y * scalar}; + } + + /** + * Returns the translation divided by a scalar. + * + * @param scalar The scalar to divide by. + * + * @return The scaled translation. + */ + template + constexpr auto operator/(const U& scalar) const { + using R = decltype(std::declval() + std::declval()); + return Translation2{m_x / scalar, m_y / scalar}; + } + /** * Applies a rotation to the translation in 2D space. * @@ -192,6 +219,14 @@ constexpr decltype(auto) get(const trajopt::Translation2& translation) { using Translation2d = Translation2; using Translation2v = Translation2; +template +sleipnir::EqualityConstraints operator==(const Translation2& lhs, + const Translation2& rhs) { + sleipnir::VariableMatrix lhsVar{{lhs.X()}, {lhs.Y()}}; + sleipnir::VariableMatrix rhsVar{{rhs.X()}, {rhs.Y()}}; + return lhsVar == rhsVar; +} + } // namespace trajopt namespace std { diff --git a/include/trajopt/path/Path.hpp b/include/trajopt/path/Path.hpp index fb9c4fe5..ce8aa1bc 100644 --- a/include/trajopt/path/Path.hpp +++ b/include/trajopt/path/Path.hpp @@ -17,46 +17,50 @@ namespace trajopt { /** - * A swerve path waypoint + * A swerve path waypoint. */ struct TRAJOPT_DLLEXPORT SwerveWaypoint { - /// instantaneous constraints at the waypoint + /// Instantaneous constraints at the waypoint. std::vector waypointConstraints; - /// continuous constraints along the segment + + /// Continuous constraints along the segment. std::vector segmentConstraints; }; /** - * A differential path waypoint + * A differential path waypoint. */ struct TRAJOPT_DLLEXPORT DifferentialWaypoint { - /// instantaneous constraints at the waypoint + /// Instantaneous constraints at the waypoint. std::vector waypointConstraints; - /// continuous constraints along the segment + + /// Continuous constraints along the segment. std::vector segmentConstraints; }; /** - * Swerve path + * Swerve path. */ struct TRAJOPT_DLLEXPORT SwervePath { - /// waypoints along the path + /// Waypoints along the path. std::vector waypoints; - /// drivetrain of the robot + + /// Drivetrain of the robot. SwerveDrivetrain drivetrain; /// A vector of callbacks to be called with the intermediate SwerveSolution - /// and a user-specified handle at every iteration of the solver + /// and a user-specified handle at every iteration of the solver. std::vector> callbacks; }; /** - * Differential path + * Differential path. */ struct TRAJOPT_DLLEXPORT DifferentialPath { - /// waypoints along the path + /// Waypoints along the path. std::vector waypoints; - /// drivetrain of the robot + + /// Drivetrain of the robot. DifferentialDrivetrain drivetrain; }; diff --git a/include/trajopt/path/SwervePathBuilder.hpp b/include/trajopt/path/SwervePathBuilder.hpp index fd8cbd8b..607bed3e 100644 --- a/include/trajopt/path/SwervePathBuilder.hpp +++ b/include/trajopt/path/SwervePathBuilder.hpp @@ -13,7 +13,6 @@ #include "trajopt/obstacle/Bumpers.hpp" #include "trajopt/obstacle/Obstacle.hpp" #include "trajopt/path/Path.hpp" -#include "trajopt/solution/Solution.hpp" #include "trajopt/solution/SwerveSolution.hpp" namespace trajopt { @@ -21,7 +20,7 @@ namespace trajopt { /** * Builds a swerve path using information about how the robot * must travel through a series of waypoints. This path can be converted - * to a trajectory using OptimalTrajectoryGenerator. + * to a trajectory using SwerveTrajectoryGenerator. */ class TRAJOPT_DLLEXPORT SwervePathBuilder { public: @@ -297,7 +296,7 @@ class TRAJOPT_DLLEXPORT SwervePathBuilder { * * @return the initial guess, as a solution */ - Solution CalculateInitialGuess() const; + SwerveSolution CalculateInitialGuess() const; /** * Add a callback to retrieve the state of the solver as a SwerveSolution. diff --git a/include/trajopt/solution/DifferentialSolution.hpp b/include/trajopt/solution/DifferentialSolution.hpp index 9d49cdbd..f77535e1 100644 --- a/include/trajopt/solution/DifferentialSolution.hpp +++ b/include/trajopt/solution/DifferentialSolution.hpp @@ -4,7 +4,6 @@ #include -#include "trajopt/solution/Solution.hpp" #include "trajopt/util/SymbolExports.hpp" namespace trajopt { @@ -12,14 +11,32 @@ namespace trajopt { /** * The holonomic trajectory optimization solution. */ -struct TRAJOPT_DLLEXPORT DifferentialSolution : public Solution { +struct TRAJOPT_DLLEXPORT DifferentialSolution { + /// Times between samples. + std::vector dt; + + /// X positions. + std::vector x; + + /// Y positions. + std::vector y; + + /// Heading cosine. + std::vector thetacos; + + /// Heading sine. + std::vector thetasin; + /// The x velocities. std::vector vL; + /// The y velocities. std::vector vR; - /// the torque of the left driverail wheels + + /// The torque of the left driverail wheels. std::vector tauL; - /// the torque of the right driverail wheels + + /// The torque of the right driverail wheels. std::vector tauR; }; diff --git a/include/trajopt/solution/HolonomicSolution.hpp b/include/trajopt/solution/HolonomicSolution.hpp deleted file mode 100644 index 994397a6..00000000 --- a/include/trajopt/solution/HolonomicSolution.hpp +++ /dev/null @@ -1,35 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include "trajopt/solution/Solution.hpp" -#include "trajopt/util/SymbolExports.hpp" - -namespace trajopt { - -/** - * The holonomic trajectory optimization solution. - */ -struct TRAJOPT_DLLEXPORT HolonomicSolution : public Solution { - /// The x velocities. - std::vector vx; - - /// The y velocities. - std::vector vy; - - /// The angular velocities. - std::vector omega; - - /// The x accelerations. - std::vector ax; - - /// The y accelerations. - std::vector ay; - - /// The angular accelerations. - std::vector alpha; -}; - -} // namespace trajopt diff --git a/include/trajopt/solution/Solution.hpp b/include/trajopt/solution/Solution.hpp deleted file mode 100644 index 2759390e..00000000 --- a/include/trajopt/solution/Solution.hpp +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include "trajopt/util/SymbolExports.hpp" - -namespace trajopt { - -/** - * The trajectory optimization solution. - */ -struct TRAJOPT_DLLEXPORT Solution { - /// Times between samples. - std::vector dt; - - /// X positions. - std::vector x; - - /// Y positions. - std::vector y; - - /// Heading cosine. - std::vector thetacos; - /// Heading sine. - std::vector thetasin; -}; - -} // namespace trajopt diff --git a/include/trajopt/solution/SwerveSolution.hpp b/include/trajopt/solution/SwerveSolution.hpp index 5c4cfee6..c079a805 100644 --- a/include/trajopt/solution/SwerveSolution.hpp +++ b/include/trajopt/solution/SwerveSolution.hpp @@ -4,7 +4,6 @@ #include -#include "trajopt/solution/HolonomicSolution.hpp" #include "trajopt/util/SymbolExports.hpp" namespace trajopt { @@ -12,7 +11,40 @@ namespace trajopt { /** * The swerve drive trajectory optimization solution. */ -struct TRAJOPT_DLLEXPORT SwerveSolution : HolonomicSolution { +struct TRAJOPT_DLLEXPORT SwerveSolution { + /// Times between samples. + std::vector dt; + + /// X positions. + std::vector x; + + /// Y positions. + std::vector y; + + /// Heading cosine. + std::vector thetacos; + + /// Heading sine. + std::vector thetasin; + + /// The x velocities. + std::vector vx; + + /// The y velocities. + std::vector vy; + + /// The angular velocities. + std::vector omega; + + /// The x accelerations. + std::vector ax; + + /// The y accelerations. + std::vector ay; + + /// The angular accelerations. + std::vector alpha; + /// The x forces for each module. std::vector> moduleFX; diff --git a/include/trajopt/trajectory/HolonomicTrajectory.hpp b/include/trajopt/trajectory/HolonomicTrajectory.hpp index f0203b97..dab90cc9 100644 --- a/include/trajopt/trajectory/HolonomicTrajectory.hpp +++ b/include/trajopt/trajectory/HolonomicTrajectory.hpp @@ -2,10 +2,10 @@ #pragma once +#include #include #include -#include "trajopt/solution/HolonomicSolution.hpp" #include "trajopt/solution/SwerveSolution.hpp" #include "trajopt/trajectory/HolonomicTrajectorySample.hpp" #include "trajopt/util/SymbolExports.hpp" @@ -30,24 +30,6 @@ class TRAJOPT_DLLEXPORT HolonomicTrajectory { explicit HolonomicTrajectory(std::vector samples) : samples{std::move(samples)} {} - /** - * Construct a HolonomicTrajectory from a solution. - * - * @param solution The solution. - */ - explicit HolonomicTrajectory(const HolonomicSolution& solution) { - double ts = 0.0; - for (size_t samp = 0; samp < solution.x.size(); ++samp) { - if (samp != 0) { - ts += solution.dt[samp - 1]; - } - samples.emplace_back( - ts, solution.x[samp], solution.y[samp], - std::atan2(solution.thetasin[samp], solution.thetacos[samp]), - solution.vx[samp], solution.vy[samp], solution.omega[samp]); - } - } - /** * Construct a HolonomicTrajectory from a swerve solution. * diff --git a/src/optimization/Cancellation.hpp b/include/trajopt/util/Cancellation.hpp similarity index 100% rename from src/optimization/Cancellation.hpp rename to include/trajopt/util/Cancellation.hpp diff --git a/src/optimization/TrajoptUtil.hpp b/include/trajopt/util/TrajoptUtil.hpp similarity index 80% rename from src/optimization/TrajoptUtil.hpp rename to include/trajopt/util/TrajoptUtil.hpp index 2f48ec14..deeb67e1 100644 --- a/src/optimization/TrajoptUtil.hpp +++ b/include/trajopt/util/TrajoptUtil.hpp @@ -23,20 +23,9 @@ #include "trajopt/set/LinearSet2d.hpp" #include "trajopt/set/RectangularSet2d.hpp" #include "trajopt/set/Set2d.hpp" -#include "trajopt/solution/Solution.hpp" namespace trajopt { -inline sleipnir::Variable max(const sleipnir::Variable& a, - const sleipnir::Variable& b) { - return +0.5 * (1 + sleipnir::sign(b - a)) * (b - a) + a; -} - -inline sleipnir::Variable min(const sleipnir::Variable& a, - const sleipnir::Variable& b) { - return -0.5 * (1 + sleipnir::sign(b - a)) * (b - a) + b; -} - /** * @brief Get the index of an item in a decision variable array, given the * waypoint and sample indices and whether this array includes an entry @@ -62,22 +51,6 @@ inline size_t GetIdx(const std::vector& N, size_t wptIdx, return idx; } -inline void ApplyDiscreteTimeObjective(sleipnir::OptimizationProblem& problem, - std::vector& dt, - const std::vector N) { - sleipnir::Variable 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 * static_cast(N_sgmt); - T_tot += T_sgmt; - - problem.SubjectTo(dt_sgmt >= 0); - dt_sgmt.SetValue(5.0 / N_sgmt); - } - problem.Minimize(std::move(T_tot)); -} - inline void ApplyIntervalSet1dConstraint(sleipnir::OptimizationProblem& problem, const sleipnir::Variable& scalar, const IntervalSet1d& set1d) { @@ -172,11 +145,6 @@ inline std::vector> MatrixSolutionValue( return valueMatrix; } -template -inline auto Lerp(T a, U b, V t) { - return a + t * (b - a); -} - // https://www.desmos.com/calculator/cqmc1tjtsv template decltype(auto) linePointDist(const Translation2& lineStart, @@ -184,11 +152,20 @@ decltype(auto) linePointDist(const Translation2& lineStart, const Translation2& point) { using R = decltype(std::declval() + std::declval()); + auto max = [](R a, R b) { + return +0.5 * (1 + sleipnir::sign(b - a)) * (b - a) + a; + }; + auto min = [](R a, R b) { + return -0.5 * (1 + sleipnir::sign(b - a)) * (b - a) + b; + }; + auto Lerp = [](R a, R b, R t) { return a + t * (b - a); }; + Translation2 l{lineEnd.X() - lineStart.X(), lineEnd.Y() - lineStart.Y()}; Translation2 v{point.X() - lineStart.X(), point.Y() - lineStart.Y()}; auto t = v.Dot(l) / l.SquaredNorm(); auto tBounded = max(min(t, 1), 0); // NOLINT + // Translation2 i{Lerp(lineStart.X(), lineEnd.X(), tBounded), Lerp(lineStart.Y(), lineEnd.Y(), tBounded)}; return (i - point).SquaredNorm(); @@ -284,6 +261,7 @@ inline void append_vector(std::vector& base, base.insert(base.end(), newItems.begin(), newItems.end()); } +template inline Solution GenerateLinearInitialGuess( const std::vector>& initialGuessPoints, const std::vector controlIntervalCounts) { @@ -380,61 +358,4 @@ inline Solution GenerateLinearInitialGuess( return initialGuess; } -inline void ApplyInitialGuess(const Solution& solution, - std::vector& x, - std::vector& y, - std::vector& thetacos, - std::vector& thetasin, - 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++) { - x[sampleIndex].SetValue(solution.x[sampleIndex]); - y[sampleIndex].SetValue(solution.y[sampleIndex]); - thetacos[sampleIndex].SetValue(solution.thetacos[sampleIndex]); - thetasin[sampleIndex].SetValue(solution.thetasin[sampleIndex]); - } - - vx[0].SetValue(0.0); - vy[0].SetValue(0.0); - omega[0].SetValue(0.0); - ax[0].SetValue(0.0); - ay[0].SetValue(0.0); - alpha[0].SetValue(0.0); - - for (size_t sampleIndex = 1; sampleIndex < sampleTotal; sampleIndex++) { - vx[sampleIndex].SetValue( - (solution.x[sampleIndex] - solution.x[sampleIndex - 1]) / - solution.dt[sampleIndex]); - vy[sampleIndex].SetValue( - (solution.y[sampleIndex] - solution.y[sampleIndex - 1]) / - solution.dt[sampleIndex]); - - double thetacos = solution.thetacos[sampleIndex]; - double thetasin = solution.thetasin[sampleIndex]; - double last_thetacos = solution.thetacos[sampleIndex - 1]; - double last_thetasin = solution.thetasin[sampleIndex - 1]; - - omega[sampleIndex].SetValue( - Rotation2d{thetacos, thetasin} - .RotateBy(-Rotation2d{last_thetacos, last_thetasin}) - .Radians() / - solution.dt[sampleIndex]); - - ax[sampleIndex].SetValue( - (vx[sampleIndex].Value() - vx[sampleIndex - 1].Value()) / - solution.dt[sampleIndex]); - ay[sampleIndex].SetValue( - (vy[sampleIndex].Value() - vy[sampleIndex - 1].Value()) / - solution.dt[sampleIndex]); - alpha[sampleIndex].SetValue( - (omega[sampleIndex].Value() - omega[sampleIndex - 1].Value()) / - solution.dt[sampleIndex]); - } -} - } // namespace trajopt diff --git a/include/trajopt/expected b/include/trajopt/util/expected similarity index 100% rename from include/trajopt/expected rename to include/trajopt/util/expected diff --git a/src/OptimalTrajectoryGenerator.cpp b/src/OptimalTrajectoryGenerator.cpp deleted file mode 100644 index c81dbd7f..00000000 --- a/src/OptimalTrajectoryGenerator.cpp +++ /dev/null @@ -1,16 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#include "trajopt/OptimalTrajectoryGenerator.hpp" - -#include "optimization/algorithms/SwerveDiscreteOptimal.hpp" - -namespace trajopt { - -expected OptimalTrajectoryGenerator::Generate( - const SwervePathBuilder& path, bool diagnostics, int64_t handle) { - SwerveDiscreteOptimal problem(path.GetPath(), path.GetControlIntervalCounts(), - path.CalculateInitialGuess(), handle); - return problem.Generate(diagnostics); -} - -} // namespace trajopt diff --git a/src/optimization/HolonomicTrajoptUtil.hpp b/src/optimization/HolonomicTrajoptUtil.hpp deleted file mode 100644 index 1b689aad..00000000 --- a/src/optimization/HolonomicTrajoptUtil.hpp +++ /dev/null @@ -1,62 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include "optimization/TrajoptUtil.hpp" -#include "trajopt/constraint/HeadingConstraint.hpp" -#include "trajopt/constraint/LinePointConstraint.hpp" -#include "trajopt/constraint/PointAtConstraint.hpp" -#include "trajopt/constraint/PointLineConstraint.hpp" -#include "trajopt/constraint/PointPointConstraint.hpp" -#include "trajopt/constraint/TranslationConstraint.hpp" -#include "trajopt/constraint/holonomic/HolonomicConstraint.hpp" - -namespace trajopt { - -inline void ApplyHolonomicConstraint( - sleipnir::OptimizationProblem& problem, const Pose2v& pose, - const Translation2v v, const sleipnir::Variable& omega, - [[maybe_unused]] const Translation2v& a, - [[maybe_unused]] const sleipnir::Variable& alpha, - const HolonomicConstraint& constraint) { - if (std::holds_alternative(constraint)) { - ApplyConstraint(problem, pose, std::get(constraint)); - } else if (std::holds_alternative(constraint)) { - ApplyConstraint(problem, pose, std::get(constraint)); - } else if (std::holds_alternative(constraint)) { - ApplyConstraint(problem, pose, std::get(constraint)); - } else if (std::holds_alternative(constraint)) { - ApplyConstraint(problem, pose, std::get(constraint)); - } else if (std::holds_alternative(constraint)) { - ApplyConstraint(problem, pose, std::get(constraint)); - } else if (std::holds_alternative(constraint)) { - const auto& angularVelocityConstraint = - std::get(constraint); - ApplyIntervalSet1dConstraint( - problem, omega, angularVelocityConstraint.angularVelocityBound); - } else if (std::holds_alternative(constraint)) { - const auto& velocityHolonomicConstraint = - std::get(constraint); - ApplySet2dConstraint(problem, v, velocityHolonomicConstraint.velocityBound); - } else if (std::holds_alternative(constraint)) { - auto pointAtConstraint = std::get(constraint); - const auto& [fieldPoint, headingTolerance] = pointAtConstraint; - - // 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 = fieldPoint.X() - pose.X(); - auto dy = fieldPoint.Y() - pose.Y(); - auto dot = pose.Rotation().Cos() * dx + pose.Rotation().Sin() * dy; - problem.SubjectTo(dot >= - std::cos(headingTolerance) * sleipnir::hypot(dx, dy)); - } -} - -} // namespace trajopt diff --git a/src/optimization/SwerveTrajoptUtil.hpp b/src/optimization/SwerveTrajoptUtil.hpp deleted file mode 100644 index 2aa04ad7..00000000 --- a/src/optimization/SwerveTrajoptUtil.hpp +++ /dev/null @@ -1,226 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include -#include -#include - -#include -#include - -#include "optimization/TrajoptUtil.hpp" -#include "trajopt/drivetrain/SwerveDrivetrain.hpp" -#include "trajopt/geometry/Translation2.hpp" -#include "trajopt/solution/SwerveSolution.hpp" - -namespace trajopt { - -inline std::pair SolveNetForce( - const std::vector& Fx, - const std::vector& Fy) { - return {std::accumulate(Fx.begin(), Fx.end(), sleipnir::Variable{0.0}), - std::accumulate(Fy.begin(), Fy.end(), sleipnir::Variable{0.0})}; -} - -inline sleipnir::Variable SolveNetTorque( - const Rotation2v& theta, const std::vector& Fx, - const std::vector& Fy, - const std::vector& swerveModules) { - sleipnir::Variable tau_net = 0; - - for (size_t moduleIdx = 0; moduleIdx < swerveModules.size(); ++moduleIdx) { - auto& swerveModule = swerveModules.at(moduleIdx); - - const auto& [x_m, y_m] = swerveModule.translation.RotateBy(theta); - Translation2v r{x_m, y_m}; - - Translation2v F{Fx.at(moduleIdx), Fy.at(moduleIdx)}; - - tau_net += r.Cross(F); - } - - return tau_net; -} - -inline void ApplyKinematicsConstraints( - sleipnir::OptimizationProblem& problem, - const std::vector& x, - const std::vector& y, - const std::vector& thetacos, - const std::vector& thetasin, - 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); - Rotation2v theta_n{thetacos.at(idx), thetasin.at(idx)}; - Rotation2v theta_n_1{thetacos.at(idx - 1), thetasin.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); - - problem.SubjectTo(x_n_1 + vx_n * dt_sgmt == x_n); - problem.SubjectTo(y_n_1 + vy_n * dt_sgmt == y_n); - - auto theta_diff = theta_n - theta_n_1; - - // Constrain angle equality on manifold: theta_diff = omega_n * dt_sgmt. - // - // Let a = . NOLINT - // Let b = . NOLINT - // - // If the angles are equal, the angle between the unit vectors should be - // zero. - // - // a x b = ||a|| ||b|| sin(angleBetween) NOLINT - // = 1 * 1 * 0 - // = 0 - // - // NOTE: angleBetween = π rad would be another solution - Translation2v a{theta_diff.Cos(), theta_diff.Sin()}; - Translation2v b{sleipnir::cos(omega_n * dt_sgmt), - sleipnir::sin(omega_n * dt_sgmt)}; - problem.SubjectTo(a.Cross(b) == 0); - problem.SubjectTo( - Translation2v{theta_n_1.Cos(), theta_n_1.Sin()}.SquaredNorm() == 1); - - problem.SubjectTo(vx_n_1 + ax_n * dt_sgmt == vx_n); - problem.SubjectTo(vy_n_1 + ay_n * dt_sgmt == vy_n); - problem.SubjectTo(omega_n_1 + alpha_n * dt_sgmt == omega_n); - } - size_t lastIdx = GetIdx(N, wptIdx, N_sgmt - 1); - problem.SubjectTo(thetacos.at(lastIdx) * thetacos.at(lastIdx) + - thetasin.at(lastIdx) * thetasin.at(lastIdx) == - 1); - } -} - -/** - * Applies the drivetrain-specific constraints to the optimizer. These - * constraints prevent motors from spinning too fast or with too much power. - * For swerve, this applies constraints that connect the speed and direction - * of each swerve module wheel to the overall kinematics and dynamics of the - * system. There are two parts: the velocity of each wheel is connected to the - * velocity of the robot, and the force generated by each wheel is connected - * to the acceleration of the robot. For both of these, limits are placed on - * the speed and torque of each wheel. This allows the optimizer to generate - * an efficient, smooth path that the robot can follow. - * - * @param problem the current optimization problem to which to apply constraints - * @param theta (controlIntervalTotal + 1) x 1 column vector of the robot's - * heading for each sample point - * @param vx (controlIntervalTotal + 1) x 1 column vector of the x-coordinate - * of the robot's velocity for each sample point - * @param vy (controlIntervalTotal + 1) x 1 column vector of the y-coordinate - * of the robot's velocity for each sample point - * @param omega (controlIntervalTotal + 1) x 1 column vector of the robot's - * angular velocity for each sample point - * @param ax controlIntervalTotal x 1 column vector of the x-coordinate of the - * robot's acceleration for each sample point - * @param ay controlIntervalTotal x 1 column vector of the y-coordinate of the - * robot's acceleration for each sample point - * @param alpha controlIntervalTotal x 1 column vector of the robot's angular - * velocity for each sample point - * @param swerveDrivetrain the swerve drivetrain - */ -inline void ApplyDynamicsConstraints( - sleipnir::OptimizationProblem& problem, const sleipnir::Variable& ax, - const sleipnir::Variable& ay, const sleipnir::Variable& alpha, - const sleipnir::Variable& Fx_net, const sleipnir::Variable& Fy_net, - const sleipnir::Variable& tau_net, double mass, double moi) { - problem.SubjectTo(Fx_net == mass * ax); - problem.SubjectTo(Fy_net == mass * ay); - problem.SubjectTo(tau_net == moi * alpha); -} - -inline void ApplyPowerConstraints(sleipnir::OptimizationProblem& problem, - const Rotation2v& theta, - const Translation2v& v, - const sleipnir::Variable& omega, - const std::vector& Fx, - const std::vector& Fy, - const SwerveDrivetrain& swerveDrivetrain) { - const auto [vx_prime, vy_prime] = v.RotateBy(-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) { - const auto& [x_m, y_m] = swerveDrivetrain.modules.at(moduleIdx).translation; - 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); - problem.SubjectTo(_vx_m * _vx_m + _vy_m * _vy_m <= - maxWheelVelocity * maxWheelVelocity); - - problem.SubjectTo(Fx_m * Fx_m + Fy_m * Fy_m <= maxForce * maxForce); - } -} - -inline SwerveSolution ConstructSwerveSolution( - std::vector& x, std::vector& y, - std::vector& thetacos, - std::vector& thetasin, - std::vector& vx, std::vector& vy, - std::vector& omega, std::vector& ax, - std::vector& ay, std::vector& alpha, - std::vector>& Fx, - std::vector>& Fy, - 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); - sleipnir::Variable dt_sgmt = dt.at(sgmtIdx); - double dt_val = dt_sgmt.Value(); - for (size_t i = 0; i < N_sgmt; ++i) { - dtPerSamp.push_back(dt_val); - } - } - return SwerveSolution{ - {{dtPerSamp, RowSolutionValue(x), RowSolutionValue(y), - RowSolutionValue(thetacos), RowSolutionValue(thetasin)}, - RowSolutionValue(vx), - RowSolutionValue(vy), - RowSolutionValue(omega), - RowSolutionValue(ax), - RowSolutionValue(ay), - RowSolutionValue(alpha)}, - MatrixSolutionValue(Fx), - MatrixSolutionValue(Fy)}; -} - -} // namespace trajopt diff --git a/src/optimization/algorithms/SwerveDiscreteOptimal.hpp b/src/optimization/algorithms/SwerveDiscreteOptimal.hpp deleted file mode 100644 index 1bfad996..00000000 --- a/src/optimization/algorithms/SwerveDiscreteOptimal.hpp +++ /dev/null @@ -1,238 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include -#include -#include -#include -#include - -#include - -#include "optimization/Cancellation.hpp" -#include "optimization/HolonomicTrajoptUtil.hpp" -#include "optimization/SwerveTrajoptUtil.hpp" -#include "optimization/TrajoptUtil.hpp" -#include "trajopt/drivetrain/SwerveDrivetrain.hpp" -#include "trajopt/expected" -#include "trajopt/path/Path.hpp" -#include "trajopt/solution/SwerveSolution.hpp" - -namespace trajopt { - -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) { - callbacks.emplace_back([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(x, y, thetacos, thetasin, 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); - thetacos.reserve(sampTot); - thetasin.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(problem.DecisionVariable()); - y.emplace_back(problem.DecisionVariable()); - thetacos.emplace_back(problem.DecisionVariable()); - thetasin.emplace_back(problem.DecisionVariable()); - vx.emplace_back(problem.DecisionVariable()); - vy.emplace_back(problem.DecisionVariable()); - omega.emplace_back(problem.DecisionVariable()); - ax.emplace_back(problem.DecisionVariable()); - ay.emplace_back(problem.DecisionVariable()); - alpha.emplace_back(problem.DecisionVariable()); - - for (size_t moduleIdx = 0; moduleIdx < moduleCnt; ++moduleIdx) { - Fx.at(idx).emplace_back(problem.DecisionVariable()); - Fy.at(idx).emplace_back(problem.DecisionVariable()); - } - } - - double minWidth = INFINITY; - for (size_t i = 1; i < path.drivetrain.modules.size(); i++) { - if (std::abs(path.drivetrain.modules.at(i - 1).translation.X() - - path.drivetrain.modules.at(i).translation.X()) != 0) { - minWidth = std::min( - minWidth, - std::abs(path.drivetrain.modules.at(i - 1).translation.X() - - path.drivetrain.modules.at(i).translation.X())); - } - if (std::abs(path.drivetrain.modules.at(i - 1).translation.Y() - - path.drivetrain.modules.at(i).translation.Y()) != 0) { - minWidth = std::min( - minWidth, - std::abs(path.drivetrain.modules.at(i - 1).translation.Y() - - path.drivetrain.modules.at(i).translation.Y())); - } - } - - for (size_t sgmtIdx = 0; sgmtIdx < sgmtCnt; ++sgmtIdx) { - dt.emplace_back(problem.DecisionVariable()); - for (auto module : path.drivetrain.modules) { - problem.SubjectTo(dt.at(sgmtIdx) * module.wheelRadius * - module.wheelMaxAngularVelocity <= - minWidth); - } - } - - ApplyDiscreteTimeObjective(problem, dt, N); - ApplyKinematicsConstraints(problem, x, y, thetacos, thetasin, 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( - problem, ax.at(idx), ay.at(idx), alpha.at(idx), Fx_net, Fy_net, - SolveNetTorque({thetacos.at(idx), thetasin.at(idx)}, Fx.at(idx), - Fy.at(idx), path.drivetrain.modules), - path.drivetrain.mass, path.drivetrain.moi); - - ApplyPowerConstraints(problem, - Rotation2v{thetacos.at(idx), thetasin.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( - problem, - {x.at(idx), y.at(idx), {thetacos.at(idx), thetasin.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( - problem, - {x.at(idx), y.at(idx), {thetacos.at(idx), thetasin.at(idx)}}, - {vx.at(idx), vy.at(idx)}, omega.at(idx), {ax.at(idx), ay.at(idx)}, - alpha.at(idx), constraint); - } - } - } - - ApplyInitialGuess(initialGuess, x, y, thetacos, thetasin, vx, vy, omega, ax, - ay, alpha); - } - - /** - * Generates an optimal trajectory. - * - * This function may take a long time to complete. - * - * @param diagnostics Enables diagnostic prints. - * @return Returns a holonomic trajectory on success, or a string containing a - * failure reason. - */ - expected Generate(bool diagnostics = false) { - GetCancellationFlag() = 0; - problem.Callback([=, this](const sleipnir::SolverIterationInfo&) -> bool { - for (auto& callback : callbacks) { - callback(); - } - return trajopt::GetCancellationFlag(); - }); - - // tolerance of 1e-4 is 0.1 mm - auto status = - problem.Solve({.tolerance = 1e-4, .diagnostics = diagnostics}); - - if (static_cast(status.exitCondition) < 0 || - status.exitCondition == - sleipnir::SolverExitCondition::kCallbackRequestedStop) { - return unexpected{std::string{sleipnir::ToMessage(status.exitCondition)}}; - } else { - return ConstructSwerveSolution(x, y, thetacos, thetasin, vx, vy, omega, - ax, ay, alpha, Fx, Fy, dt, N); - } - } - - private: - /** - * The swerve drivetrain. - */ - const SwervePath& path; - - /// State Variables - std::vector x; - std::vector y; - std::vector thetacos; - std::vector thetasin; - std::vector vx; - std::vector vy; - std::vector omega; - std::vector ax; - std::vector ay; - std::vector alpha; - - /// Input Variables - std::vector> Fx; - std::vector> Fy; - - /// Time Variables - std::vector dt; - - /// Discretization Constants - const std::vector& N; - - sleipnir::OptimizationProblem problem; - std::vector> callbacks; -}; - -} // namespace trajopt diff --git a/src/path/SwervePathBuilder.cpp b/src/path/SwervePathBuilder.cpp index a60356bf..76450460 100644 --- a/src/path/SwervePathBuilder.cpp +++ b/src/path/SwervePathBuilder.cpp @@ -6,8 +6,6 @@ #include #include -#include "optimization/Cancellation.hpp" -#include "optimization/TrajoptUtil.hpp" #include "trajopt/constraint/AngularVelocityConstraint.hpp" #include "trajopt/constraint/Constraint.hpp" #include "trajopt/constraint/HeadingConstraint.hpp" @@ -20,6 +18,9 @@ #include "trajopt/set/IntervalSet1d.hpp" #include "trajopt/set/LinearSet2d.hpp" #include "trajopt/set/RectangularSet2d.hpp" +#include "trajopt/solution/SwerveSolution.hpp" +#include "trajopt/util/Cancellation.hpp" +#include "trajopt/util/TrajoptUtil.hpp" namespace trajopt { @@ -201,8 +202,9 @@ const std::vector& SwervePathBuilder::GetControlIntervalCounts() const { return controlIntervalCounts; } -Solution SwervePathBuilder::CalculateInitialGuess() const { - return GenerateLinearInitialGuess(initialGuessPoints, controlIntervalCounts); +SwerveSolution SwervePathBuilder::CalculateInitialGuess() const { + return GenerateLinearInitialGuess(initialGuessPoints, + controlIntervalCounts); } void SwervePathBuilder::NewWpts(size_t finalIndex) { diff --git a/src/trajoptlibrust.cpp b/src/trajoptlibrust.cpp index 12b75de7..e0789a50 100644 --- a/src/trajoptlibrust.cpp +++ b/src/trajoptlibrust.cpp @@ -9,7 +9,7 @@ #include #include -#include "trajopt/OptimalTrajectoryGenerator.hpp" +#include "trajopt/SwerveTrajectoryGenerator.hpp" #include "trajopt/drivetrain/SwerveDrivetrain.hpp" #include "trajopt/geometry/Translation2.hpp" #include "trajopt/trajectory/HolonomicTrajectory.hpp" @@ -305,9 +305,8 @@ void SwervePathBuilderImpl::add_progress_callback( HolonomicTrajectory SwervePathBuilderImpl::generate(bool diagnostics, int64_t handle) const { - if (auto sol = trajopt::OptimalTrajectoryGenerator::Generate( - path, diagnostics, handle); - sol.has_value()) { + trajopt::SwerveTrajectoryGenerator generator{path, handle}; + if (auto sol = generator.Generate(diagnostics); sol.has_value()) { return _convert_holonomic_trajectory( trajopt::HolonomicTrajectory{sol.value()}); } else { diff --git a/src/optimization/Cancellation.cpp b/src/util/Cancellation.cpp similarity index 75% rename from src/optimization/Cancellation.cpp rename to src/util/Cancellation.cpp index 7a9b08b7..fe37f174 100644 --- a/src/optimization/Cancellation.cpp +++ b/src/util/Cancellation.cpp @@ -1,8 +1,6 @@ // Copyright (c) TrajoptLib contributors -#include "optimization/Cancellation.hpp" - -#include +#include "trajopt/util/Cancellation.hpp" namespace trajopt { diff --git a/test/src/ObstacleTest.cpp b/test/src/ObstacleTest.cpp index 599c4099..24e8e18d 100644 --- a/test/src/ObstacleTest.cpp +++ b/test/src/ObstacleTest.cpp @@ -3,7 +3,7 @@ #include #include -#include +#include #include TEST_CASE("Obstacle - Linear initial guess", "[Obstacle]") { @@ -34,5 +34,6 @@ TEST_CASE("Obstacle - Linear initial guess", "[Obstacle]") { 0, 1, trajopt::Obstacle{.safetyDistance = 1.0, .points = {{1.0, 1.0}}}); path.ControlIntervalCounts({10}); - CHECK_NOTHROW(trajopt::OptimalTrajectoryGenerator::Generate(path)); + trajopt::SwerveTrajectoryGenerator generator{path}; + CHECK_NOTHROW(generator.Generate()); } diff --git a/test/src/SwerveTrajoptUtilTest.cpp b/test/src/SwerveTrajoptUtilTest.cpp deleted file mode 100644 index 924b56e0..00000000 --- a/test/src/SwerveTrajoptUtilTest.cpp +++ /dev/null @@ -1,38 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#include - -#include -#include -#include -#include - -#include "optimization/SwerveTrajoptUtil.hpp" - -TEST_CASE("SwerveTrajoptUtil - SolveNetForce()", "[SwerveTrajoptUtil]") { - std::vector Fx{4.0, 1.0, 3.0, 5.0}; - std::vector Fy{0.0, 2.0, -5.0, 2.0}; - - auto [Fx_net, Fy_net] = trajopt::SolveNetForce(Fx, Fy); - - CHECK(Fx_net.Value() == Catch::Approx(13.0).margin(1e-3)); - CHECK(Fy_net.Value() == Catch::Approx(-1.0).margin(1e-3)); -} - -TEST_CASE("SwerveTrajoptUtil - SolveNetTorque()", "[SwerveTrajoptUtil]") { - constexpr double theta = 1.0; - - std::vector Fx{-3.0, -5.0, -4.0, 2.0}; - std::vector Fy{4.0, -5.0, -2.0, -4.0}; - std::vector swerveModules = { - {trajopt::Translation2d{1.0, 1.0}, 0.0, 0.0, 0.0}, - {trajopt::Translation2d{1.0, -1.0}, 0.0, 0.0, 0.0}, - {trajopt::Translation2d{-1.0, 1.0}, 0.0, 0.0, 0.0}, - {trajopt::Translation2d{-1.0, -1.0}, 0.0, 0.0, 0.0}, - }; - - sleipnir::Variable tau_net = trajopt::SolveNetTorque( - {std::cos(theta), std::sin(theta)}, Fx, Fy, swerveModules); - - CHECK(tau_net.Value() == Catch::Approx(0.6553658).margin(1e-3)); -} diff --git a/test/src/TrajoptUtilTest.cpp b/test/src/TrajoptUtilTest.cpp index a6cab965..ef6c4c20 100644 --- a/test/src/TrajoptUtilTest.cpp +++ b/test/src/TrajoptUtilTest.cpp @@ -4,8 +4,8 @@ #include #include - -#include "optimization/TrajoptUtil.hpp" +#include +#include TEST_CASE("TrajoptUtil - GetIdx()", "[TrajoptUtil]") { auto result0 = trajopt::GetIdx({2, 3}, 0, 0); @@ -29,7 +29,7 @@ TEST_CASE("TrajoptUtil - Linear initial guess", "[TrajoptUtil]") { {{1, 0, 0}}, {{2, 0, 0}, {3, 0, 0}}, {{6, 0, 0}}}; std::vector controlIntervalCounts{2, 3}; std::vector expectedX{1, 2, 3, 4, 5, 6}; - auto result = trajopt::GenerateLinearInitialGuess(initialGuessPoints, - controlIntervalCounts); + auto result = trajopt::GenerateLinearInitialGuess( + initialGuessPoints, controlIntervalCounts); CHECK(expectedX == result.x); } diff --git a/test/src/geometry/Translation2dTest.cpp b/test/src/geometry/Translation2dTest.cpp index 57cd3480..d46804fe 100644 --- a/test/src/geometry/Translation2dTest.cpp +++ b/test/src/geometry/Translation2dTest.cpp @@ -45,6 +45,22 @@ TEST_CASE("Translation2d - UnaryMinus", "[Translation2d]") { CHECK(inverted.Y() == -7.0); } +TEST_CASE("Translation2d - Multiplication", "[Translation2d]") { + const trajopt::Translation2d original{3.0, 5.0}; + const auto mult = original * 3; + + CHECK(mult.X() == 9.0); + CHECK(mult.Y() == 15.0); +} + +TEST_CASE("Translation2d - Division", "[Translation2d]") { + const trajopt::Translation2d original{3.0, 5.0}; + const auto div = original / 2; + + CHECK(div.X() == 1.5); + CHECK(div.Y() == 2.5); +} + TEST_CASE("Translation2d - RotateBy", "[Translation2d]") { const trajopt::Translation2d another{3.0, 0.0}; const auto rotated =