diff --git a/trajoptlib/examples/Differential/src/Main.cpp b/trajoptlib/examples/Differential/src/Main.cpp new file mode 100644 index 0000000000..b9dcdd1871 --- /dev/null +++ b/trajoptlib/examples/Differential/src/Main.cpp @@ -0,0 +1,180 @@ +// Copyright (c) TrajoptLib contributors + +#include + +#include + +// 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); + } +} diff --git a/trajoptlib/src/DifferentialTrajectoryGenerator.cpp b/trajoptlib/src/DifferentialTrajectoryGenerator.cpp index c76017e7c0..c4f32779f0 100644 --- a/trajoptlib/src/DifferentialTrajectoryGenerator.cpp +++ b/trajoptlib/src/DifferentialTrajectoryGenerator.cpp @@ -2,6 +2,10 @@ #include "trajopt/DifferentialTrajectoryGenerator.hpp" +#include + +#include + #include "trajopt/geometry/Rotation2.hpp" #include "trajopt/geometry/Translation2.hpp" #include "trajopt/util/TrajoptUtil.hpp" @@ -25,7 +29,6 @@ DifferentialTrajectoryGenerator::DifferentialTrajectoryGenerator( vR.reserve(sampTot); tauL.reserve(sampTot); tauR.reserve(sampTot); - dt.reserve(sgmtCnt); for (size_t index = 0; index < sampTot; ++index) { @@ -37,6 +40,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) { @@ -44,6 +48,8 @@ DifferentialTrajectoryGenerator::DifferentialTrajectoryGenerator( path.waypoints.at(sgmtIndex + 1).segmentConstraints) { size_t startIndex = GetIndex(N, sgmtIndex + 1, 0); size_t endIndex = GetIndex(N, sgmtIndex + 2, 0); + assert(dt.size() == endIndex); + assert(y.size() == endIndex); for (size_t index = startIndex; index < endIndex; ++index) { Pose2v pose{ @@ -51,15 +57,17 @@ 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) { @@ -138,13 +146,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) { @@ -214,10 +225,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()); } }