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/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/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/TrajoptUtil.hpp b/src/optimization/TrajoptUtil.hpp index 2f48ec14..cf35b9a6 100644 --- a/src/optimization/TrajoptUtil.hpp +++ b/src/optimization/TrajoptUtil.hpp @@ -27,16 +27,6 @@ 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 +52,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 +146,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 +153,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(); @@ -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/src/optimization/algorithms/SwerveDiscreteOptimal.hpp b/src/optimization/algorithms/SwerveDiscreteOptimal.hpp index 1bfad996..0fdb8019 100644 --- a/src/optimization/algorithms/SwerveDiscreteOptimal.hpp +++ b/src/optimization/algorithms/SwerveDiscreteOptimal.hpp @@ -5,14 +5,15 @@ #include #include #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" @@ -42,9 +43,7 @@ class SwerveDiscreteOptimal { lastFrameTime = now; - auto soln = - ConstructSwerveSolution(x, y, thetacos, thetasin, vx, vy, omega, ax, - ay, alpha, Fx, Fy, dt, this->N); + auto soln = ConstructSwerveSolution(); for (auto& callback : this->path.callbacks) { callback(soln, handle); } @@ -121,22 +120,97 @@ class SwerveDiscreteOptimal { } } - ApplyDiscreteTimeObjective(problem, dt, N); - ApplyKinematicsConstraints(problem, x, y, thetacos, thetasin, vx, vy, omega, - ax, ay, alpha, dt, N); + // Apply minimum-time objective + 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) { - 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); + 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) { @@ -149,6 +223,7 @@ class SwerveDiscreteOptimal { 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 @@ -167,8 +242,7 @@ class SwerveDiscreteOptimal { } } - ApplyInitialGuess(initialGuess, x, y, thetacos, thetasin, vx, vy, omega, ax, - ay, alpha); + ApplyInitialGuess(initialGuess); } /** @@ -198,8 +272,7 @@ class SwerveDiscreteOptimal { 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); + return ConstructSwerveSolution(); } } @@ -207,7 +280,7 @@ class SwerveDiscreteOptimal { /** * The swerve drivetrain. */ - const SwervePath& path; + SwervePath path; /// State Variables std::vector x; @@ -233,6 +306,77 @@ class SwerveDiscreteOptimal { sleipnir::OptimizationProblem problem; std::vector> callbacks; + + void ApplyInitialGuess(const Solution& 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/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/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 =