Skip to content

Commit

Permalink
add C++ diffy example
Browse files Browse the repository at this point in the history
  • Loading branch information
@spacey-sooty
spacey-sooty committed Jul 12, 2024
1 parent 2133a6d commit 47d56e6
Show file tree
Hide file tree
Showing 2 changed files with 217 additions and 13 deletions.
180 changes: 180 additions & 0 deletions trajoptlib/examples/Differential/src/Main.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,180 @@
// Copyright (c) TrajoptLib contributors

#include <numbers>

#include <trajopt/DifferentialTrajectoryGenerator.hpp>

// SwervePathBuilder is used to build paths that are optimized into full
// trajectories.
//
// "Wpt" stands for waypoint, an instantaneous moment in the path where certain
// constrains on the robot's state are applied.
//
// "Sgmt" is the abbreviation for segments, the continuum of state between
// waypoints where constraints can also be applied.

int main() {
trajopt::DifferentialDrivetrain differentialDrivetrain{.mass = 45,
.moi = 6,
.trackwidth = 0.6,
.left =
{
0.08,
70,
5,
},
.right = {
0.08,
70,
5,
}};

trajopt::LinearVelocityMaxMagnitudeConstraint zeroLinearVelocity{0.0};
trajopt::AngularVelocityMaxMagnitudeConstraint zeroAngularVelocity{0.0};

// Example 1: Differential, one meter forward motion profile
{
trajopt::DifferentialPathBuilder path;
path.SetDrivetrain(differentialDrivetrain);
path.PoseWpt(0, 0.0, 0.0, 0.0);
path.PoseWpt(1, 1.0, 0.0, 0.0);
path.WptConstraint(0, zeroLinearVelocity);
path.WptConstraint(1, zeroLinearVelocity);
path.ControlIntervalCounts({40});

trajopt::DifferentialTrajectoryGenerator generator{path};
[[maybe_unused]]
auto solution = generator.Generate(true);
}

// Example 2: Differential, basic curve
{
trajopt::DifferentialPathBuilder path;
path.SetDrivetrain(differentialDrivetrain);
path.PoseWpt(0, 1.0, 1.0, -std::numbers::pi / 2);
path.PoseWpt(1, 2.0, 0.0, 0.0);
path.WptConstraint(0, zeroLinearVelocity);
path.WptConstraint(1, zeroLinearVelocity);
path.ControlIntervalCounts({40});

trajopt::DifferentialTrajectoryGenerator generator{path};
[[maybe_unused]]
auto solution = generator.Generate(true);
}

// Example 3: Differential, three waypoints
{
trajopt::DifferentialPathBuilder path;
path.SetDrivetrain(differentialDrivetrain);
path.PoseWpt(0, 0.0, 0.0, std::numbers::pi / 2);
path.PoseWpt(1, 1.0, 1.0, 0.0);
path.PoseWpt(2, 2.0, 0.0, std::numbers::pi / 2);
path.WptConstraint(0, zeroLinearVelocity);
path.WptConstraint(1, zeroLinearVelocity);
path.ControlIntervalCounts({40, 40});

trajopt::DifferentialTrajectoryGenerator generator{path};
[[maybe_unused]]
auto solution = generator.Generate(true);
}

// Example 4: Differential, ending velocity
{
trajopt::DifferentialPathBuilder path;
path.SetDrivetrain(differentialDrivetrain);
path.PoseWpt(0, 0.0, 0.0, 0.0);
path.PoseWpt(1, 0.0, 1.0, 0.0);
path.WptConstraint(0, zeroLinearVelocity);
path.ControlIntervalCounts({40});

trajopt::DifferentialTrajectoryGenerator generator{path};
[[maybe_unused]]
auto solution = generator.Generate(true);
}

// Example 5: Differential, circle obstacle
{
trajopt::DifferentialPathBuilder path;
path.SetDrivetrain(differentialDrivetrain);
path.PoseWpt(0, 0.0, 0.0, 0.0);
trajopt::Obstacle obstacle{// Radius of 0.1
.safetyDistance = 0.1,
.points = {{0.5, 0.5}}};
path.SgmtObstacle(0, 1, obstacle);
path.PoseWpt(1, 1.0, 0.0, 0.0);
path.WptConstraint(0, zeroLinearVelocity);
path.WptConstraint(1, zeroLinearVelocity);
path.ControlIntervalCounts({40});

trajopt::DifferentialTrajectoryGenerator generator{path};
[[maybe_unused]]
auto solution = generator.Generate(true);
}

// Example 6: Approach a pick up station at a certain direction
{
trajopt::DifferentialPathBuilder path;
path.SetDrivetrain(differentialDrivetrain);

// Starting position
path.PoseWpt(0, 0.0, 0.0, 0.0);

// Align towards the station one meter behind
path.PoseWpt(1, 1.0, 1.0, std::numbers::pi / 2);
path.WptConstraint(1, zeroAngularVelocity);
path.WptConstraint(
1, trajopt::LinearVelocityDirectionConstraint{std::numbers::pi / 2});

// Go up to the station. In practice, the optimizer will still end up
// aligning the heading without the pose constraint since it's most optimal.
path.TranslationWpt(2, 1.0, 2.0);

// Realign behind the station
path.PoseWpt(3, 1.0, 1.0, std::numbers::pi / 2);
path.WptConstraint(3, zeroAngularVelocity);
path.WptConstraint(
3, trajopt::LinearVelocityDirectionConstraint{std::numbers::pi / 2});

// Ending position
path.PoseWpt(4, 2.0, 0.0, std::numbers::pi);

path.WptConstraint(0, zeroLinearVelocity);
path.WptConstraint(4, zeroLinearVelocity);
path.ControlIntervalCounts({40, 30, 30, 40});

trajopt::DifferentialTrajectoryGenerator generator{path};
[[maybe_unused]]
auto solution = generator.Generate(true);
}

// Example 7: Circular path with a point-point constraint
{
// Note that forcing a circular path is not a common problem in FRC. This
// example is only here to demonstrate how various constraints work.
trajopt::DifferentialPathBuilder path;
path.SetDrivetrain(differentialDrivetrain);

path.PoseWpt(0, 0.0, 0.0, 0.0);
path.SgmtConstraint(
0, 1,
trajopt::PointPointConstraint{// Robot point -- center of robot
{0.0, 0.0},
// Field point around which to orbit
{1.0, 0.0},
// Stay 1 m away to force circular motion
1.0});

// Tell optimizer to go in +y direction rather than -y
path.WptInitialGuessPoint(0, {0.0, 0.0, 0.0});

path.PoseWpt(1, 2.0, 0.0, 0.0);

path.WptConstraint(0, zeroLinearVelocity);
path.WptConstraint(1, zeroLinearVelocity);
path.ControlIntervalCounts({30});

trajopt::DifferentialTrajectoryGenerator generator{path};
[[maybe_unused]]
auto solution = generator.Generate(true);
}
}
50 changes: 37 additions & 13 deletions trajoptlib/src/DifferentialTrajectoryGenerator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,6 @@ DifferentialTrajectoryGenerator::DifferentialTrajectoryGenerator(
vR.reserve(sampTot);
tauL.reserve(sampTot);
tauR.reserve(sampTot);

dt.reserve(sgmtCnt);

for (size_t index = 0; index < sampTot; ++index) {
Expand All @@ -37,6 +36,7 @@ DifferentialTrajectoryGenerator::DifferentialTrajectoryGenerator(
vR.emplace_back(problem.DecisionVariable());
tauL.emplace_back(problem.DecisionVariable());
tauR.emplace_back(problem.DecisionVariable());
dt.emplace_back(problem.DecisionVariable());
}

for (size_t sgmtIndex = 0; sgmtIndex < sgmtCnt; ++sgmtIndex) {
Expand All @@ -51,15 +51,18 @@ DifferentialTrajectoryGenerator::DifferentialTrajectoryGenerator(
Translation2v linearVelocity{(vL.at(index) + vR.at(index)) / 2, 0.0};
auto angularVelocity =
(vR.at(index) - vL.at(index)) / path.drivetrain.trackwidth;
auto y_n1 = (y.size() > 0) ? y.at(index - 1) : 0;
auto vL_n1 = ((vL.size() > 0) ? vL.at(index - 1) : 0);
auto vR_n1 = ((vR.size() > 0) ? vR.at(index - 1) : 0);
Translation2v linearAcceleration{
((vL.at(index) + vR.at(index)) / 2 -
(vL.at(index - 1) + vR.at(index - 1)) / 2) /
((vL.at(index) + vR.at(index)) / 2 - (vL_n1 + vR_n1) / 2) /
dt.at(index),
0.0};
(y.at(index) - y_n1 / dt.at(index)) - y_n1 -
((y.size() > 1 && index >= 2) ? y.at(index - 2) : 0) /
dt.at(index)};
auto angularAcceleration =
(vR.at(index) - vL.at(index)) / path.drivetrain.trackwidth -
(vR.at(index - 1) - vL.at(index - 1)) / path.drivetrain.trackwidth /
dt.at(index);
(vR_n1 - vL_n1) / path.drivetrain.trackwidth / dt.at(index);

std::visit(
[&](auto&& arg) {
Expand Down Expand Up @@ -138,13 +141,16 @@ DifferentialTrajectoryGenerator::DifferentialTrajectoryGenerator(
(vR.at(index) - vL.at(index)) / path.drivetrain.trackwidth;
Translation2v linearAcceleration{
((vL.at(index) + vR.at(index)) / 2 -
(vL.at(index - 1) + vR.at(index - 1)) / 2) /
(((index > 0) ? vL.at(index - 1) : 0.0) +
((index > 0) ? vR.at(index - 1) : 0.0)) /
2) /
dt.at(index),
0.0};
auto angularAcceleration =
(vR.at(index) - vL.at(index)) / path.drivetrain.trackwidth -
(vR.at(index - 1) - vL.at(index - 1)) / path.drivetrain.trackwidth /
dt.at(index);
(((index > 0) ? vR.at(index - 1) : 0.0) -
((index > 0) ? vL.at(index - 1) : 0.0)) /
path.drivetrain.trackwidth / dt.at(index);

std::visit(
[&](auto&& arg) {
Expand Down Expand Up @@ -214,10 +220,28 @@ void DifferentialTrajectoryGenerator::ApplyInitialGuess(
y[sampleIndex].SetValue(solution.y[sampleIndex]);
thetacos[sampleIndex].SetValue(solution.thetacos[sampleIndex]);
thetasin[sampleIndex].SetValue(solution.thetasin[sampleIndex]);
vL[sampleIndex].SetValue(solution.vL[sampleIndex]);
vR[sampleIndex].SetValue(solution.vR[sampleIndex]);
tauL[sampleIndex].SetValue(solution.tauL[sampleIndex]);
tauR[sampleIndex].SetValue(solution.tauR[sampleIndex]);

Translation2v linearVelocity{
(x.at(sampleIndex) -
((sampleIndex > 0) ? x.at(sampleIndex - 1) : 0.0)) /
dt.at(sampleIndex),
(y.at(sampleIndex) -
((sampleIndex > 0) ? y.at(sampleIndex - 1) : 0.0)) /
dt.at(sampleIndex)};
Rotation2v angularVelocity{thetasin.at(sampleIndex),
thetacos.at(sampleIndex)};
vL[sampleIndex].SetValue(
(linearVelocity.X() +
path.drivetrain.trackwidth / 2 * angularVelocity.Radians())
.Value());
vR[sampleIndex].SetValue(
(linearVelocity.X() -
path.drivetrain.trackwidth / 2 * angularVelocity.Radians())
.Value());
tauL[sampleIndex].SetValue(path.drivetrain.moi *
vL.at(sampleIndex).Value());
tauR[sampleIndex].SetValue(path.drivetrain.moi *
vL.at(sampleIndex).Value());
}
}

Expand Down

0 comments on commit 47d56e6

Please sign in to comment.