Skip to content

Commit 824fcb1

Browse files
calcmogulcalcmogul
committed
[trajoptlib] Give each sample its own dt
1 parent 2b0465c commit 824fcb1

File tree

2 files changed

+49
-66
lines changed

2 files changed

+49
-66
lines changed

‎trajoptlib/src/DifferentialTrajectoryGenerator.cpp‎

Lines changed: 24 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,6 @@
55
#include <algorithm>
66
#include <cmath>
77
#include <ranges>
8-
#include <utility>
98
#include <vector>
109

1110
#include <sleipnir/autodiff/Variable.hpp>
@@ -120,7 +119,7 @@ DifferentialTrajectoryGenerator::DifferentialTrajectoryGenerator(
120119
Fl.reserve(sampTot);
121120
Fr.reserve(sampTot);
122121

123-
dts.reserve(sgmtCnt);
122+
dts.reserve(sampTot);
124123

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

134133
Fl.emplace_back(problem.DecisionVariable());
135134
Fr.emplace_back(problem.DecisionVariable());
136-
}
137135

138-
for (size_t sgmtIndex = 0; sgmtIndex < sgmtCnt; ++sgmtIndex) {
139136
dts.emplace_back(problem.DecisionVariable());
140137
}
141138

142139
// Minimize total time
143-
sleipnir::Variable T_tot = 0;
144140
const double maxForce =
145141
path.drivetrain.wheelMaxTorque * 2 / path.drivetrain.wheelRadius;
146142
const auto maxAccel = maxForce / path.drivetrain.mass;
@@ -149,21 +145,16 @@ DifferentialTrajectoryGenerator::DifferentialTrajectoryGenerator(
149145
const auto maxAngVel = maxDrivetrainVelocity * 2 / path.drivetrain.trackwidth;
150146
const auto maxAngAccel = maxAccel * 2 / path.drivetrain.trackwidth;
151147
for (size_t sgmtIndex = 0; sgmtIndex < Ns.size(); ++sgmtIndex) {
152-
auto& dt = dts.at(sgmtIndex);
153148
auto N_sgmt = Ns.at(sgmtIndex);
154-
auto T_sgmt = dt * static_cast<int>(N_sgmt);
155-
T_tot += T_sgmt;
149+
const auto sgmt_start = GetIndex(Ns, sgmtIndex);
150+
const auto sgmt_end = GetIndex(Ns, sgmtIndex + 1);
156151

157-
problem.SubjectTo(dt >= 0);
158-
problem.SubjectTo(dt * path.drivetrain.wheelRadius *
159-
path.drivetrain.wheelMaxAngularVelocity <=
160-
path.drivetrain.trackwidth);
161152
if (N_sgmt == 0) {
162-
dt.SetValue(0);
153+
for (size_t index = sgmt_start; index < sgmt_end + 1; ++index) {
154+
dts.at(index).SetValue(0.0);
155+
}
163156
} else {
164157
// Use initialGuess and Ns to find the dx, dy, dθ between wpts
165-
const auto sgmt_start = GetIndex(Ns, sgmtIndex);
166-
const auto sgmt_end = GetIndex(Ns, sgmtIndex + 1);
167158
const auto dx =
168159
initialGuess.x.at(sgmt_end) - initialGuess.x.at(sgmt_start);
169160
const auto dy =
@@ -183,15 +174,20 @@ DifferentialTrajectoryGenerator::DifferentialTrajectoryGenerator(
183174
CalculateTrapezoidalTime(dist, maxLinearVel, maxAccel);
184175
const double sgmtTime = angularTime + linearTime;
185176

186-
dt.SetValue(sgmtTime / N_sgmt);
177+
for (size_t index = sgmt_start; index < sgmt_end + 1; ++index) {
178+
auto& dt = dts.at(index);
179+
problem.SubjectTo(dt >= 0);
180+
problem.SubjectTo(dt <= 3);
181+
dt.SetValue(sgmtTime / N_sgmt);
182+
}
187183
}
188184
}
189-
problem.Minimize(std::move(T_tot));
185+
problem.Minimize(
186+
std::accumulate(dts.begin(), dts.end(), sleipnir::Variable{0.0}));
190187

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

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

209+
auto dt_k = dts.at(index);
210+
if (sampleIndex < N_sgmt - 1) {
211+
auto dt_k_1 = dts.at(index + 1);
212+
problem.SubjectTo(dt_k_1 == dt_k);
213+
}
214+
213215
// Dynamics constraints - direct collocation
214216
// (https://mec560sbu.github.io/2016/09/30/direct_collocation/)
215217
auto xdot_k = f(x_k, u_k);
216218
auto xdot_k_1 = f(x_k_1, u_k_1);
217-
auto xdot_c = -3 / (2 * dt) * (x_k - x_k_1) - 0.25 * (xdot_k + xdot_k_1);
219+
auto xdot_c =
220+
-3 / (2 * dt_k) * (x_k - x_k_1) - 0.25 * (xdot_k + xdot_k_1);
218221

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

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

356359
DifferentialSolution
357360
DifferentialTrajectoryGenerator::ConstructDifferentialSolution() {
358-
std::vector<double> dtPerSample;
359-
for (size_t sgmtIndex = 0; sgmtIndex < Ns.size(); ++sgmtIndex) {
360-
auto N = Ns.at(sgmtIndex);
361-
auto dt = dts.at(sgmtIndex);
362-
363-
double dt_value = dt.Value();
364-
for (size_t i = 0; i < N; ++i) {
365-
dtPerSample.push_back(dt_value);
366-
}
367-
}
368-
369361
auto getValue = [](auto& var) { return var.Value(); };
370362

371363
// TODO: Use std::ranges::to() from C++23
@@ -379,7 +371,7 @@ DifferentialTrajectoryGenerator::ConstructDifferentialSolution() {
379371
ω.push_back((vr.at(sample).Value() - vl.at(sample).Value()) / trackwidth);
380372
}
381373
return DifferentialSolution{
382-
dtPerSample,
374+
vectorValue(dts),
383375
vectorValue(x),
384376
vectorValue(y),
385377
vectorValue(θ),

‎trajoptlib/src/SwerveTrajectoryGenerator.cpp‎

Lines changed: 25 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,6 @@
77
#include <algorithm>
88
#include <chrono>
99
#include <ranges>
10-
#include <utility>
1110
#include <vector>
1211

1312
#include <sleipnir/optimization/OptimizationProblem.hpp>
@@ -84,7 +83,7 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator(
8483
_Fy.reserve(moduleCnt);
8584
}
8685

87-
dts.reserve(sgmtCnt);
86+
dts.reserve(sampTot);
8887

8988
for (size_t index = 0; index < sampTot; ++index) {
9089
x.emplace_back(problem.DecisionVariable());
@@ -102,9 +101,7 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator(
102101
Fx.at(index).emplace_back(problem.DecisionVariable());
103102
Fy.at(index).emplace_back(problem.DecisionVariable());
104103
}
105-
}
106104

107-
for (size_t sgmtIndex = 0; sgmtIndex < sgmtCnt; ++sgmtIndex) {
108105
dts.emplace_back(problem.DecisionVariable());
109106
}
110107

@@ -118,7 +115,6 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator(
118115
}
119116

120117
// Minimize total time
121-
sleipnir::Variable T_tot = 0;
122118
const double maxForce =
123119
path.drivetrain.wheelMaxTorque * 4 / path.drivetrain.wheelRadius;
124120
const auto maxAccel = maxForce / path.drivetrain.mass;
@@ -131,21 +127,16 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator(
131127
const auto maxAngVel = maxDrivetrainVelocity / maxWheelPositionRadius;
132128
const auto maxAngAccel = maxAccel / maxWheelPositionRadius;
133129
for (size_t sgmtIndex = 0; sgmtIndex < Ns.size(); ++sgmtIndex) {
134-
auto& dt = dts.at(sgmtIndex);
135130
auto N_sgmt = Ns.at(sgmtIndex);
136-
auto T_sgmt = dt * static_cast<int>(N_sgmt);
137-
T_tot += T_sgmt;
131+
const auto sgmt_start = GetIndex(Ns, sgmtIndex);
132+
const auto sgmt_end = GetIndex(Ns, sgmtIndex + 1);
138133

139-
problem.SubjectTo(dt >= 0);
140-
problem.SubjectTo(dt * path.drivetrain.wheelRadius *
141-
path.drivetrain.wheelMaxAngularVelocity <=
142-
minWidth);
143134
if (N_sgmt == 0) {
144-
dt.SetValue(0);
135+
for (size_t index = sgmt_start; index < sgmt_end + 1; ++index) {
136+
dts.at(index).SetValue(0.0);
137+
}
145138
} else {
146139
// Use initialGuess and Ns to find the dx, dy, dθ between wpts
147-
const auto sgmt_start = GetIndex(Ns, sgmtIndex);
148-
const auto sgmt_end = GetIndex(Ns, sgmtIndex + 1);
149140
const auto dx =
150141
initialGuess.x.at(sgmt_end) - initialGuess.x.at(sgmt_start);
151142
const auto dy =
@@ -167,15 +158,20 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator(
167158
CalculateTrapezoidalTime(dist, maxLinearVel, maxAccel);
168159
const double sgmtTime = angularTime + linearTime;
169160

170-
dt.SetValue(sgmtTime / N_sgmt);
161+
for (size_t index = sgmt_start; index < sgmt_end + 1; ++index) {
162+
auto& dt = dts.at(index);
163+
problem.SubjectTo(dt >= 0);
164+
problem.SubjectTo(dt <= 3);
165+
dt.SetValue(sgmtTime / N_sgmt);
166+
}
171167
}
172168
}
173-
problem.Minimize(std::move(T_tot));
169+
problem.Minimize(
170+
std::accumulate(dts.begin(), dts.end(), sleipnir::Variable{0.0}));
174171

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

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

197+
auto dt_k = dts.at(index);
198+
if (sampleIndex < N_sgmt - 1) {
199+
auto dt_k_1 = dts.at(index + 1);
200+
problem.SubjectTo(dt_k_1 == dt_k);
201+
}
202+
201203
// xₖ₊₁ = xₖ + vₖt + 1/2aₖt²
202204
// θₖ₊₁ = θₖ + ωₖt
203205
// vₖ₊₁ = vₖ + aₖt
204206
// ωₖ₊₁ = ωₖ + αₖt
205-
problem.SubjectTo(x_k_1 == x_k + v_k * dt + a_k * 0.5 * dt * dt);
206-
problem.SubjectTo((θ_k_1 - θ_k) == Rotation2v{ω_k * dt});
207-
problem.SubjectTo(v_k_1 == v_k + a_k * dt);
208-
problem.SubjectTo(ω_k_1 == ω_k + α_k * dt);
207+
problem.SubjectTo(x_k_1 == x_k + v_k * dt_k + a_k * 0.5 * dt_k * dt_k);
208+
problem.SubjectTo((θ_k_1 - θ_k) == Rotation2v{ω_k * dt_k});
209+
problem.SubjectTo(v_k_1 == v_k + a_k * dt_k);
210+
problem.SubjectTo(ω_k_1 == ω_k + α_k * dt_k);
209211
}
210212
}
211213

@@ -379,17 +381,6 @@ void SwerveTrajectoryGenerator::ApplyInitialGuess(
379381
}
380382

381383
SwerveSolution SwerveTrajectoryGenerator::ConstructSwerveSolution() {
382-
std::vector<double> dtPerSample;
383-
for (size_t sgmtIndex = 0; sgmtIndex < Ns.size(); ++sgmtIndex) {
384-
auto N = Ns.at(sgmtIndex);
385-
auto dt = dts.at(sgmtIndex);
386-
387-
double dt_value = dt.Value();
388-
for (size_t i = 0; i < N; ++i) {
389-
dtPerSample.push_back(dt_value);
390-
}
391-
}
392-
393384
auto getValue = [](auto& var) { return var.Value(); };
394385

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

411-
return SwerveSolution{dtPerSample, vectorValue(x), vectorValue(y),
402+
return SwerveSolution{vectorValue(dts), vectorValue(x), vectorValue(y),
412403
vectorValue(cosθ), vectorValue(sinθ), vectorValue(vx),
413404
vectorValue(vy), vectorValue(ω), vectorValue(ax),
414405
vectorValue(ay), vectorValue(α), matrixValue(Fx),

0 commit comments

Comments
 (0)