Skip to content

Commit 1a1da2b

Browse files
calcmogulcalcmogul
committed
[trajoptlib] Give each sample its own dt
1 parent 0f4b194 commit 1a1da2b

File tree

2 files changed

+47
-66
lines changed

2 files changed

+47
-66
lines changed

‎trajoptlib/src/DifferentialTrajectoryGenerator.cpp‎

Lines changed: 23 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>
@@ -117,7 +116,7 @@ DifferentialTrajectoryGenerator::DifferentialTrajectoryGenerator(
117116
Fl.reserve(sampTot);
118117
Fr.reserve(sampTot);
119118

120-
dts.reserve(sgmtCnt);
119+
dts.reserve(sampTot);
121120

122121
for (size_t index = 0; index < sampTot; ++index) {
123122
x.emplace_back(problem.decision_variable());
@@ -130,14 +129,11 @@ DifferentialTrajectoryGenerator::DifferentialTrajectoryGenerator(
130129

131130
Fl.emplace_back(problem.decision_variable());
132131
Fr.emplace_back(problem.decision_variable());
133-
}
134132

135-
for (size_t sgmtIndex = 0; sgmtIndex < sgmtCnt; ++sgmtIndex) {
136133
dts.emplace_back(problem.decision_variable());
137134
}
138135

139136
// Minimize total time
140-
slp::Variable T_tot = 0;
141137
const double maxForce =
142138
path.drivetrain.wheelMaxTorque * 2 / path.drivetrain.wheelRadius;
143139
const auto maxAccel = maxForce / path.drivetrain.mass;
@@ -146,21 +142,16 @@ DifferentialTrajectoryGenerator::DifferentialTrajectoryGenerator(
146142
const auto maxAngVel = maxDrivetrainVelocity * 2 / path.drivetrain.trackwidth;
147143
const auto maxAngAccel = maxAccel * 2 / path.drivetrain.trackwidth;
148144
for (size_t sgmtIndex = 0; sgmtIndex < Ns.size(); ++sgmtIndex) {
149-
auto& dt = dts.at(sgmtIndex);
150145
auto N_sgmt = Ns.at(sgmtIndex);
151-
auto T_sgmt = dt * static_cast<int>(N_sgmt);
152-
T_tot += T_sgmt;
146+
const auto sgmt_start = GetIndex(Ns, sgmtIndex);
147+
const auto sgmt_end = GetIndex(Ns, sgmtIndex + 1);
153148

154-
problem.subject_to(dt >= 0);
155-
problem.subject_to(dt * path.drivetrain.wheelRadius *
156-
path.drivetrain.wheelMaxAngularVelocity <=
157-
path.drivetrain.trackwidth);
158149
if (N_sgmt == 0) {
159-
dt.set_value(0);
150+
for (size_t index = sgmt_start; index < sgmt_end + 1; ++index) {
151+
dts.at(index).set_value(0.0);
152+
}
160153
} else {
161154
// Use initialGuess and Ns to find the dx, dy, dθ between wpts
162-
const auto sgmt_start = GetIndex(Ns, sgmtIndex);
163-
const auto sgmt_end = GetIndex(Ns, sgmtIndex + 1);
164155
const auto dx =
165156
initialGuess.x.at(sgmt_end) - initialGuess.x.at(sgmt_start);
166157
const auto dy =
@@ -180,15 +171,19 @@ DifferentialTrajectoryGenerator::DifferentialTrajectoryGenerator(
180171
CalculateTrapezoidalTime(dist, maxLinearVel, maxAccel);
181172
const double sgmtTime = angularTime + linearTime;
182173

183-
dt.set_value(sgmtTime / N_sgmt);
174+
for (size_t index = sgmt_start; index < sgmt_end + 1; ++index) {
175+
auto& dt = dts.at(index);
176+
problem.subject_to(dt >= 0);
177+
problem.subject_to(dt <= 3);
178+
dt.set_value(sgmtTime / N_sgmt);
179+
}
184180
}
185181
}
186-
problem.minimize(std::move(T_tot));
182+
problem.minimize(std::accumulate(dts.begin(), dts.end(), slp::Variable{0.0}));
187183

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

193188
for (size_t sampleIndex = 0; sampleIndex < N_sgmt; ++sampleIndex) {
194189
size_t index = GetIndex(Ns, wptIndex, sampleIndex);
@@ -207,13 +202,20 @@ DifferentialTrajectoryGenerator::DifferentialTrajectoryGenerator(
207202
{vr.at(index + 1)}};
208203
slp::VariableMatrix u_k_1{{Fl.at(index + 1)}, {Fr.at(index + 1)}};
209204

205+
auto dt_k = dts.at(index);
206+
if (sampleIndex < N_sgmt - 1) {
207+
auto dt_k_1 = dts.at(index + 1);
208+
problem.subject_to(dt_k_1 == dt_k);
209+
}
210+
210211
// Dynamics constraints - direct collocation
211212
// (https://mec560sbu.github.io/2016/09/30/direct_collocation/)
212213
auto xdot_k = f(x_k, u_k);
213214
auto xdot_k_1 = f(x_k_1, u_k_1);
214-
auto xdot_c = -3 / (2 * dt) * (x_k - x_k_1) - 0.25 * (xdot_k + xdot_k_1);
215+
auto xdot_c =
216+
-3 / (2 * dt_k) * (x_k - x_k_1) - 0.25 * (xdot_k + xdot_k_1);
215217

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

219221
problem.subject_to(xdot_c == f(x_c, u_c));
@@ -351,17 +353,6 @@ void DifferentialTrajectoryGenerator::ApplyInitialGuess(
351353

352354
DifferentialSolution
353355
DifferentialTrajectoryGenerator::ConstructDifferentialSolution() {
354-
std::vector<double> dtPerSample;
355-
for (size_t sgmtIndex = 0; sgmtIndex < Ns.size(); ++sgmtIndex) {
356-
auto N = Ns.at(sgmtIndex);
357-
auto dt = dts.at(sgmtIndex);
358-
359-
double dt_value = dt.value();
360-
for (size_t i = 0; i < N; ++i) {
361-
dtPerSample.push_back(dt_value);
362-
}
363-
}
364-
365356
auto getValue = [](auto& var) { return var.value(); };
366357

367358
auto vectorValue = [&](std::vector<slp::Variable>& row) {
@@ -374,7 +365,7 @@ DifferentialTrajectoryGenerator::ConstructDifferentialSolution() {
374365
ω.push_back((vr.at(sample).value() - vl.at(sample).value()) / trackwidth);
375366
}
376367
return DifferentialSolution{
377-
dtPerSample,
368+
vectorValue(dts),
378369
vectorValue(x),
379370
vectorValue(y),
380371
vectorValue(θ),

‎trajoptlib/src/SwerveTrajectoryGenerator.cpp‎

Lines changed: 24 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/problem.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.decision_variable());
@@ -102,9 +101,7 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator(
102101
Fx.at(index).emplace_back(problem.decision_variable());
103102
Fy.at(index).emplace_back(problem.decision_variable());
104103
}
105-
}
106104

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

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

120117
// Minimize total time
121-
slp::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.subject_to(dt >= 0);
140-
problem.subject_to(dt * path.drivetrain.wheelRadius *
141-
path.drivetrain.wheelMaxAngularVelocity <=
142-
minWidth);
143134
if (N_sgmt == 0) {
144-
dt.set_value(0);
135+
for (size_t index = sgmt_start; index < sgmt_end + 1; ++index) {
136+
dts.at(index).set_value(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,19 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator(
167158
CalculateTrapezoidalTime(dist, maxLinearVel, maxAccel);
168159
const double sgmtTime = angularTime + linearTime;
169160

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

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

180175
for (size_t sampleIndex = 0; sampleIndex < N_sgmt; ++sampleIndex) {
181176
size_t index = GetIndex(Ns, wptIndex, sampleIndex);
@@ -198,14 +193,20 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator(
198193
auto α_k = α.at(index);
199194
auto α_k_1 = α.at(index + 1);
200195

196+
auto dt_k = dts.at(index);
197+
if (sampleIndex < N_sgmt - 1) {
198+
auto dt_k_1 = dts.at(index + 1);
199+
problem.subject_to(dt_k_1 == dt_k);
200+
}
201+
201202
// xₖ₊₁ = xₖ + vₖt + 1/2aₖt²
202203
// θₖ₊₁ = θₖ + ωₖt
203204
// vₖ₊₁ = vₖ + aₖt
204205
// ωₖ₊₁ = ωₖ + αₖt
205-
problem.subject_to(x_k_1 == x_k + v_k * dt + a_k * 0.5 * dt * dt);
206-
problem.subject_to((θ_k_1 - θ_k) == Rotation2v{ω_k * dt});
207-
problem.subject_to(v_k_1 == v_k + a_k * dt);
208-
problem.subject_to(ω_k_1 == ω_k + α_k * dt);
206+
problem.subject_to(x_k_1 == x_k + v_k * dt_k + a_k * 0.5 * dt_k * dt_k);
207+
problem.subject_to((θ_k_1 - θ_k) == Rotation2v{ω_k * dt_k});
208+
problem.subject_to(v_k_1 == v_k + a_k * dt_k);
209+
problem.subject_to(ω_k_1 == ω_k + α_k * dt_k);
209210
}
210211
}
211212

@@ -378,17 +379,6 @@ void SwerveTrajectoryGenerator::ApplyInitialGuess(
378379
}
379380

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

394384
auto vectorValue = [&](std::vector<slp::Variable>& row) {
@@ -404,7 +394,7 @@ SwerveSolution SwerveTrajectoryGenerator::ConstructSwerveSolution() {
404394
std::ranges::to<std::vector>();
405395
};
406396

407-
return SwerveSolution{dtPerSample, vectorValue(x), vectorValue(y),
397+
return SwerveSolution{vectorValue(dts), vectorValue(x), vectorValue(y),
408398
vectorValue(cosθ), vectorValue(sinθ), vectorValue(vx),
409399
vectorValue(vy), vectorValue(ω), vectorValue(ax),
410400
vectorValue(ay), vectorValue(α), matrixValue(Fx),

0 commit comments

Comments
 (0)