Skip to content

Commit

Permalink
[trajoptlib] Give each sample its own dt
Browse files Browse the repository at this point in the history
  • Loading branch information
@calcmogul
calcmogul committed Jan 29, 2025
1 parent cb3e342 commit f0bcbff
Show file tree
Hide file tree
Showing 2 changed files with 49 additions and 66 deletions.
56 changes: 24 additions & 32 deletions trajoptlib/src/DifferentialTrajectoryGenerator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,6 @@
#include <algorithm>
#include <cmath>
#include <ranges>
#include <utility>
#include <vector>

#include <sleipnir/autodiff/Variable.hpp>
Expand Down Expand Up @@ -120,7 +119,7 @@ DifferentialTrajectoryGenerator::DifferentialTrajectoryGenerator(
Fl.reserve(sampTot);
Fr.reserve(sampTot);

dts.reserve(sgmtCnt);
dts.reserve(sampTot);

for (size_t index = 0; index < sampTot; ++index) {
x.emplace_back(problem.DecisionVariable());
Expand All @@ -133,14 +132,11 @@ DifferentialTrajectoryGenerator::DifferentialTrajectoryGenerator(

Fl.emplace_back(problem.DecisionVariable());
Fr.emplace_back(problem.DecisionVariable());
}

for (size_t sgmtIndex = 0; sgmtIndex < sgmtCnt; ++sgmtIndex) {
dts.emplace_back(problem.DecisionVariable());
}

// Minimize total time
sleipnir::Variable T_tot = 0;
const double maxForce =
path.drivetrain.wheelMaxTorque * 2 / path.drivetrain.wheelRadius;
const auto maxAccel = maxForce / path.drivetrain.mass;
Expand All @@ -149,21 +145,16 @@ DifferentialTrajectoryGenerator::DifferentialTrajectoryGenerator(
const auto maxAngVel = maxDrivetrainVelocity * 2 / path.drivetrain.trackwidth;
const auto maxAngAccel = maxAccel * 2 / path.drivetrain.trackwidth;
for (size_t sgmtIndex = 0; sgmtIndex < Ns.size(); ++sgmtIndex) {
auto& dt = dts.at(sgmtIndex);
auto N_sgmt = Ns.at(sgmtIndex);
auto T_sgmt = dt * static_cast<int>(N_sgmt);
T_tot += T_sgmt;
const auto sgmt_start = GetIndex(Ns, sgmtIndex);
const auto sgmt_end = GetIndex(Ns, sgmtIndex + 1);

problem.SubjectTo(dt >= 0);
problem.SubjectTo(dt * path.drivetrain.wheelRadius *
path.drivetrain.wheelMaxAngularVelocity <=
path.drivetrain.trackwidth);
if (N_sgmt == 0) {
dt.SetValue(0);
for (size_t index = sgmt_start; index < sgmt_end + 1; ++index) {
dts.at(index).SetValue(0.0);
}
} else {
// Use initialGuess and Ns to find the dx, dy, dθ between wpts
const auto sgmt_start = GetIndex(Ns, sgmtIndex);
const auto sgmt_end = GetIndex(Ns, sgmtIndex + 1);
const auto dx =
initialGuess.x.at(sgmt_end) - initialGuess.x.at(sgmt_start);
const auto dy =
Expand All @@ -183,15 +174,20 @@ DifferentialTrajectoryGenerator::DifferentialTrajectoryGenerator(
CalculateTrapezoidalTime(dist, maxLinearVel, maxAccel);
const double sgmtTime = angularTime + linearTime;

dt.SetValue(sgmtTime / N_sgmt);
for (size_t index = sgmt_start; index < sgmt_end + 1; ++index) {
auto& dt = dts.at(index);
problem.SubjectTo(dt >= 0);
problem.SubjectTo(dt <= 3);
dt.SetValue(sgmtTime / N_sgmt);
}
}
}
problem.Minimize(std::move(T_tot));
problem.Minimize(
std::accumulate(dts.begin(), dts.end(), sleipnir::Variable{0.0}));

// Apply dynamics constraints
for (size_t wptIndex = 0; wptIndex < wptCnt - 1; ++wptIndex) {
size_t N_sgmt = Ns.at(wptIndex);
auto dt = dts.at(wptIndex);

for (size_t sampleIndex = 0; sampleIndex < N_sgmt; ++sampleIndex) {
size_t index = GetIndex(Ns, wptIndex, sampleIndex);
Expand All @@ -210,13 +206,20 @@ DifferentialTrajectoryGenerator::DifferentialTrajectoryGenerator(
{vr.at(index + 1)}};
slp::VariableMatrix u_k_1{{Fl.at(index + 1)}, {Fr.at(index + 1)}};

auto dt_k = dts.at(index);
if (sampleIndex < N_sgmt - 1) {
auto dt_k_1 = dts.at(index + 1);
problem.SubjectTo(dt_k_1 == dt_k);
}

// Dynamics constraints - direct collocation
// (https://mec560sbu.github.io/2016/09/30/direct_collocation/)
auto xdot_k = f(x_k, u_k);
auto xdot_k_1 = f(x_k_1, u_k_1);
auto xdot_c = -3 / (2 * dt) * (x_k - x_k_1) - 0.25 * (xdot_k + xdot_k_1);
auto xdot_c =
-3 / (2 * dt_k) * (x_k - x_k_1) - 0.25 * (xdot_k + xdot_k_1);

auto x_c = 0.5 * (x_k + x_k_1) + dt / 8 * (xdot_k - xdot_k_1);
auto x_c = 0.5 * (x_k + x_k_1) + dt_k / 8 * (xdot_k - xdot_k_1);
auto u_c = 0.5 * (u_k + u_k_1);

problem.SubjectTo(xdot_c == f(x_c, u_c));
Expand Down Expand Up @@ -355,17 +358,6 @@ void DifferentialTrajectoryGenerator::ApplyInitialGuess(

DifferentialSolution
DifferentialTrajectoryGenerator::ConstructDifferentialSolution() {
std::vector<double> dtPerSample;
for (size_t sgmtIndex = 0; sgmtIndex < Ns.size(); ++sgmtIndex) {
auto N = Ns.at(sgmtIndex);
auto dt = dts.at(sgmtIndex);

double dt_value = dt.Value();
for (size_t i = 0; i < N; ++i) {
dtPerSample.push_back(dt_value);
}
}

auto getValue = [](auto& var) { return var.Value(); };

// TODO: Use std::ranges::to() from C++23
Expand All @@ -379,7 +371,7 @@ DifferentialTrajectoryGenerator::ConstructDifferentialSolution() {
ω.push_back((vr.at(sample).Value() - vl.at(sample).Value()) / trackwidth);
}
return DifferentialSolution{
dtPerSample,
vectorValue(dts),
vectorValue(x),
vectorValue(y),
vectorValue(θ),
Expand Down
59 changes: 25 additions & 34 deletions trajoptlib/src/SwerveTrajectoryGenerator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,6 @@
#include <algorithm>
#include <chrono>
#include <ranges>
#include <utility>
#include <vector>

#include <sleipnir/optimization/OptimizationProblem.hpp>
Expand Down Expand Up @@ -84,7 +83,7 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator(
_Fy.reserve(moduleCnt);
}

dts.reserve(sgmtCnt);
dts.reserve(sampTot);

for (size_t index = 0; index < sampTot; ++index) {
x.emplace_back(problem.DecisionVariable());
Expand All @@ -102,9 +101,7 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator(
Fx.at(index).emplace_back(problem.DecisionVariable());
Fy.at(index).emplace_back(problem.DecisionVariable());
}
}

for (size_t sgmtIndex = 0; sgmtIndex < sgmtCnt; ++sgmtIndex) {
dts.emplace_back(problem.DecisionVariable());
}

Expand All @@ -118,7 +115,6 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator(
}

// Minimize total time
sleipnir::Variable T_tot = 0;
const double maxForce =
path.drivetrain.wheelMaxTorque * 4 / path.drivetrain.wheelRadius;
const auto maxAccel = maxForce / path.drivetrain.mass;
Expand All @@ -131,21 +127,16 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator(
const auto maxAngVel = maxDrivetrainVelocity / maxWheelPositionRadius;
const auto maxAngAccel = maxAccel / maxWheelPositionRadius;
for (size_t sgmtIndex = 0; sgmtIndex < Ns.size(); ++sgmtIndex) {
auto& dt = dts.at(sgmtIndex);
auto N_sgmt = Ns.at(sgmtIndex);
auto T_sgmt = dt * static_cast<int>(N_sgmt);
T_tot += T_sgmt;
const auto sgmt_start = GetIndex(Ns, sgmtIndex);
const auto sgmt_end = GetIndex(Ns, sgmtIndex + 1);

problem.SubjectTo(dt >= 0);
problem.SubjectTo(dt * path.drivetrain.wheelRadius *
path.drivetrain.wheelMaxAngularVelocity <=
minWidth);
if (N_sgmt == 0) {
dt.SetValue(0);
for (size_t index = sgmt_start; index < sgmt_end + 1; ++index) {
dts.at(index).SetValue(0.0);
}
} else {
// Use initialGuess and Ns to find the dx, dy, dθ between wpts
const auto sgmt_start = GetIndex(Ns, sgmtIndex);
const auto sgmt_end = GetIndex(Ns, sgmtIndex + 1);
const auto dx =
initialGuess.x.at(sgmt_end) - initialGuess.x.at(sgmt_start);
const auto dy =
Expand All @@ -167,15 +158,20 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator(
CalculateTrapezoidalTime(dist, maxLinearVel, maxAccel);
const double sgmtTime = angularTime + linearTime;

dt.SetValue(sgmtTime / N_sgmt);
for (size_t index = sgmt_start; index < sgmt_end + 1; ++index) {
auto& dt = dts.at(index);
problem.SubjectTo(dt >= 0);
problem.SubjectTo(dt <= 3);
dt.SetValue(sgmtTime / N_sgmt);
}
}
}
problem.Minimize(std::move(T_tot));
problem.Minimize(
std::accumulate(dts.begin(), dts.end(), sleipnir::Variable{0.0}));

// Apply kinematics constraints
for (size_t wptIndex = 0; wptIndex < wptCnt - 1; ++wptIndex) {
size_t N_sgmt = Ns.at(wptIndex);
auto dt = dts.at(wptIndex);

for (size_t sampleIndex = 0; sampleIndex < N_sgmt; ++sampleIndex) {
size_t index = GetIndex(Ns, wptIndex, sampleIndex);
Expand All @@ -198,14 +194,20 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator(
auto α_k = α.at(index);
auto α_k_1 = α.at(index + 1);

auto dt_k = dts.at(index);
if (sampleIndex < N_sgmt - 1) {
auto dt_k_1 = dts.at(index + 1);
problem.SubjectTo(dt_k_1 == dt_k);
}

// xₖ₊₁ = xₖ + vₖt + 1/2aₖt²
// θₖ₊₁ = θₖ + ωₖt
// vₖ₊₁ = vₖ + aₖt
// ωₖ₊₁ = ωₖ + αₖt
problem.SubjectTo(x_k_1 == x_k + v_k * dt + a_k * 0.5 * dt * dt);
problem.SubjectTo((θ_k_1 - θ_k) == Rotation2v{ω_k * dt});
problem.SubjectTo(v_k_1 == v_k + a_k * dt);
problem.SubjectTo(ω_k_1 == ω_k + α_k * dt);
problem.SubjectTo(x_k_1 == x_k + v_k * dt_k + a_k * 0.5 * dt_k * dt_k);
problem.SubjectTo((θ_k_1 - θ_k) == Rotation2v{ω_k * dt_k});
problem.SubjectTo(v_k_1 == v_k + a_k * dt_k);
problem.SubjectTo(ω_k_1 == ω_k + α_k * dt_k);
}
}

Expand Down Expand Up @@ -379,17 +381,6 @@ void SwerveTrajectoryGenerator::ApplyInitialGuess(
}

SwerveSolution SwerveTrajectoryGenerator::ConstructSwerveSolution() {
std::vector<double> dtPerSample;
for (size_t sgmtIndex = 0; sgmtIndex < Ns.size(); ++sgmtIndex) {
auto N = Ns.at(sgmtIndex);
auto dt = dts.at(sgmtIndex);

double dt_value = dt.Value();
for (size_t i = 0; i < N; ++i) {
dtPerSample.push_back(dt_value);
}
}

auto getValue = [](auto& var) { return var.Value(); };

// TODO: Use std::ranges::to() from C++23
Expand All @@ -408,7 +399,7 @@ SwerveSolution SwerveTrajectoryGenerator::ConstructSwerveSolution() {
return std::vector<std::vector<double>>{std::begin(view), std::end(view)};
};

return SwerveSolution{dtPerSample, vectorValue(x), vectorValue(y),
return SwerveSolution{vectorValue(dts), vectorValue(x), vectorValue(y),
vectorValue(cosθ), vectorValue(sinθ), vectorValue(vx),
vectorValue(vy), vectorValue(ω), vectorValue(ax),
vectorValue(ay), vectorValue(α), matrixValue(Fx),
Expand Down

0 comments on commit f0bcbff

Please sign in to comment.