Skip to content
This repository has been archived by the owner on Jul 8, 2024. It is now read-only.

Commit

Permalink
Add velocity and acceleration to initial guess (#79)
Browse files Browse the repository at this point in the history
  • Loading branch information
Lewis-Seiden authored Jan 24, 2024
1 parent 95c20be commit 575826e
Show file tree
Hide file tree
Showing 4 changed files with 51 additions and 10 deletions.
12 changes: 7 additions & 5 deletions rust/examples/swerve.rs
Original file line number Diff line number Diff line change
Expand Up @@ -41,10 +41,12 @@ fn main() {
path.set_bumpers(1.3, 1.3);
path.pose_wpt(0, 0.0, 0.0, 0.0);
path.pose_wpt(1, 1.0, 0.0, 0.0);
path.wpt_linear_velocity_polar(0, 0.0, 0.0);
path.wpt_linear_velocity_polar(1, 0.0, 0.0);
path.wpt_angular_velocity(0, 0.0);
path.wpt_angular_velocity(1, 0.0);
path.sgmt_circle_obstacle(0, 1, 0.5, 0.0, 0.2);
// path.wpt_linear_velocity_polar(0, 0.0, 0.0);
// path.wpt_linear_velocity_polar(1, 0.0, 0.0);
// path.wpt_angular_velocity(0, 0.0);
// path.wpt_angular_velocity(1, 0.0);
// path.sgmt_circle_obstacle(0, 1, 0.5, 0.1, 0.2);
path.set_control_interval_counts(vec![40]);
println!("setup complete");
println!("{:?}", path.generate());
}
9 changes: 6 additions & 3 deletions src/optimization/TrajoptUtil.h
Original file line number Diff line number Diff line change
Expand Up @@ -97,9 +97,12 @@ inline Solution GenerateLinearInitialGuess(

template <typename Expr, typename Opti>
requires OptiSys<Expr, Opti>
static void ApplyInitialGuess(Opti& opti, const Solution& solution,
std::vector<Expr>& x, std::vector<Expr>& y,
std::vector<Expr>& theta);
void ApplyInitialGuess(Opti& opti, const Solution& solution,
std::vector<Expr>& x, std::vector<Expr>& y,
std::vector<Expr>& theta, std::vector<Expr>& vx,
std::vector<Expr>& vy, std::vector<Expr>& omega,
std::vector<Expr>& ax, std::vector<Expr>& ay,
std::vector<Expr>& alpha);
} // namespace trajopt

#include "optimization/TrajoptUtil.inc"
37 changes: 36 additions & 1 deletion src/optimization/TrajoptUtil.inc
Original file line number Diff line number Diff line change
Expand Up @@ -280,9 +280,13 @@ Solution GenerateLinearInitialGuess(
initialGuess.x.reserve(sampTot);
initialGuess.y.reserve(sampTot);
initialGuess.theta.reserve(sampTot);
initialGuess.dt.reserve(sampTot);
initialGuess.x.push_back(initialGuessPoints.front().front().x);
initialGuess.y.push_back(initialGuessPoints.front().front().y);
initialGuess.theta.push_back(initialGuessPoints.front().front().heading);
for (size_t i = 0; i < sampTot; i++) {
initialGuess.dt.push_back((wptCnt * 5.0) / sampTot);
}
for (size_t wptIdx = 1; wptIdx < wptCnt; wptIdx++) {
size_t N_sgmt = controlIntervalCounts.at(wptIdx - 1);
size_t guessPointCount = initialGuessPoints.at(wptIdx).size();
Expand Down Expand Up @@ -341,13 +345,44 @@ template <typename Expr, typename Opti>
requires OptiSys<Expr, Opti>
void ApplyInitialGuess(Opti& opti, const Solution& solution,
std::vector<Expr>& x, std::vector<Expr>& y,
std::vector<Expr>& theta) {
std::vector<Expr>& theta, std::vector<Expr>& vx,
std::vector<Expr>& vy, std::vector<Expr>& omega,
std::vector<Expr>& ax, std::vector<Expr>& ay,
std::vector<Expr>& alpha) {
size_t sampleTotal = x.size();
for (size_t sampleIndex = 0; sampleIndex < sampleTotal; sampleIndex++) {
opti.SetInitial(x[sampleIndex], solution.x[sampleIndex]);
opti.SetInitial(y[sampleIndex], solution.y[sampleIndex]);
opti.SetInitial(theta[sampleIndex], solution.theta[sampleIndex]);
}
opti.SetInitial(vx[0], 0.0);
opti.SetInitial(vy[0], 0.0);
opti.SetInitial(omega[0], 0.0);
opti.SetInitial(ax[0], 0.0);
opti.SetInitial(ay[0], 0.0);
opti.SetInitial(alpha[0], 0.0);
for (size_t sampleIndex = 1; sampleIndex < sampleTotal; sampleIndex++) {
opti.SetInitial(vx[sampleIndex],
(solution.x[sampleIndex] - solution.x[sampleIndex - 1]) /
solution.dt[sampleIndex]);
opti.SetInitial(vy[sampleIndex],
(solution.y[sampleIndex] - solution.y[sampleIndex - 1]) /
solution.dt[sampleIndex]);
opti.SetInitial(omega[sampleIndex], (solution.theta[sampleIndex] -
solution.theta[sampleIndex - 1]) /
solution.dt[sampleIndex]);

opti.SetInitial(ax[sampleIndex], (opti.SolutionValue(vx[sampleIndex]) -
opti.SolutionValue(vx[sampleIndex - 1])) /
solution.dt[sampleIndex]);
opti.SetInitial(ay[sampleIndex], (opti.SolutionValue(vy[sampleIndex]) -
opti.SolutionValue(vy[sampleIndex - 1])) /
solution.dt[sampleIndex]);
opti.SetInitial(alpha[sampleIndex],
(opti.SolutionValue(omega[sampleIndex]) -
opti.SolutionValue(omega[sampleIndex - 1])) /
solution.dt[sampleIndex]);
}
}

} // namespace trajopt
3 changes: 2 additions & 1 deletion src/optimization/algorithms/SwerveDiscreteOptimal.inc
Original file line number Diff line number Diff line change
Expand Up @@ -145,6 +145,7 @@ SwerveDiscreteOptimal<Expr, Opti>::SwerveDiscreteOptimal(
}
}

ApplyInitialGuess(opti, initialGuess, x, y, theta);
ApplyInitialGuess(opti, initialGuess, x, y, theta, vx, vy, omega, ax, ay,
alpha);
}
} // namespace trajopt

0 comments on commit 575826e

Please sign in to comment.