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

Commit

Permalink
spline refactor
Browse files Browse the repository at this point in the history
  • Loading branch information
@bruingineer
bruingineer committed Jul 1, 2024
1 parent b9be9ab commit 1c2f6e1
Showing 1 changed file with 153 additions and 107 deletions.
260 changes: 153 additions & 107 deletions include/trajopt/util/GenerateSplineInitialGuess.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,88 +21,118 @@ namespace trajopt {

/// TODO: make a better way to pass control intervals to choreo
/// TODO: refactor this file. add inline? possible to template?
std::vector<frc::Trajectory> GenerateWaypointSplineTrajectories(
const trajopt::SwervePath path,
const std::vector<std::vector<Pose2d>> initialGuessPoints) {
std::vector<trajopt::CubicHermitePoseSplineHolonomic> splines =
CubicPoseControlVectorsFromWaypoints(initialGuessPoints);

// Generate a parameterized spline
// Generate a parameterized spline
std::vector<std::vector<frc::TrajectoryGenerator::PoseWithCurvature>>
ParameterizeSplines(
const std::vector<CubicHermitePoseSplineHolonomic>& splines) {
std::vector<std::vector<frc::TrajectoryGenerator::PoseWithCurvature>>
splinePoints;

// Iterate through the vector and parameterize each spline
for (auto&& spline : splines) {
auto points = trajopt::SplineParameterizer::Parameterize(spline);
splinePoints.push_back(points);
for (const auto& spline : splines) {
splinePoints.push_back(trajopt::SplineParameterizer::Parameterize(spline));
}
return splinePoints;
}

const auto maxWheelVelocity = units::meters_per_second_t(
path.drivetrain.modules.front().wheelMaxAngularVelocity *
path.drivetrain.modules.front().wheelRadius);

inline frc::SwerveDriveKinematics<4> CreateKinematics(const SwervePath& path) {
wpi::array<frc::Translation2d, 4> moduleTranslations{wpi::empty_array};
for (size_t i = 0; i < path.drivetrain.modules.size(); ++i) {
const auto mod = path.drivetrain.modules.at(0);
moduleTranslations.at(0) =
const auto& mod = path.drivetrain.modules.at(i);
moduleTranslations.at(i) =
frc::Translation2d{units::meter_t(mod.translation.X()),
units::meter_t(mod.translation.Y())};
}
const frc::SwerveDriveKinematics kinematics{
moduleTranslations.at(0), moduleTranslations.at(1),
moduleTranslations.at(2), moduleTranslations.at(3)};
const frc::SwerveDriveKinematics kinematics{moduleTranslations};
return kinematics;
}

inline units::meters_per_second_t CalculateMaxWheelVelocity(
const SwervePath& path) {
return units::meters_per_second_t(
path.drivetrain.modules.front().wheelMaxAngularVelocity *
path.drivetrain.modules.front().wheelRadius);
}

inline units::meters_per_second_t CalculateSegmentVelocity(
const SwervePath& path, size_t sgmtIdx, units::meters_per_second_t sgmtVel,
const Pose2d& start, const Pose2d& end) {
auto dtheta = std::abs(
frc::AngleModulus(units::radian_t(std::abs(start.Rotation().Radians() -
end.Rotation().Radians())))
.value());
frc::Translation2d sgmtStart{units::meter_t(start.Translation().X()),
units::meter_t(start.Translation().Y())};
frc::Translation2d sgmtEnd{units::meter_t(end.Translation().X()),
units::meter_t(end.Translation().Y())};

for (auto& c : path.waypoints.at(sgmtIdx).segmentConstraints) {
// assuming HolonomicVelocityConstraint with CircularSet2d
if (std::holds_alternative<LinearVelocityMaxMagnitudeConstraint>(c)) {
const auto& velocityHolonomicConstraint =
std::get<LinearVelocityMaxMagnitudeConstraint>(c);
auto vel = units::meters_per_second_t(
velocityHolonomicConstraint.m_maxMagnitude);
std::printf("max lin vel: %.2f - ", vel.value());
if (vel < sgmtVel) {
sgmtVel = vel;
}
} else if (std::holds_alternative<AngularVelocityMaxMagnitudeConstraint>(
c)) {
const auto& angVelConstraint =
std::get<AngularVelocityMaxMagnitudeConstraint>(c);
auto maxAngVel = angVelConstraint.m_maxMagnitude;
std::printf("max ang vel: %.2f - ", maxAngVel);

/*
* Proof for T = 1.5 * θ / ω:
*
* - The velocity function derived from the cubic Hermite spline is:
* v(t) = -6*θ*t^2 + 6*θ*t.
*
* - The peak velocity occurs at t = 0.5, where t∈[0, 1] :
* v(0.5) = 1.5*θ, which is the max angular velocity during the motion.
* - To ensure this peak velocity does not exceed ω, we set:
* 1.5 * θ = ω.
* - The total time T needed to reach the final θ and
* not exceed ω is thus derived as:
* T = θ / (ω / 1.5) = 1.5 * θ / ω.
*
* This calculation ensures the peak velocity meets but does not exceed ω,
* extending the time proportionally to meet this
* requirement.
*/
auto time = 1.5 * dtheta / maxAngVel;
// estimating velocity for a straight line path
/// TODO: use the spine path distance
auto vel = sgmtStart.Distance(sgmtEnd) / units::second_t(time);
if (vel < sgmtVel) {
sgmtVel = vel;
}
}
}
return sgmtVel;
}

std::vector<frc::Trajectory> trajs;
trajs.reserve(path.waypoints.size());
inline std::vector<frc::Trajectory> GenerateTrajectories(
const SwervePath& path,
const std::vector<std::vector<trajopt::Pose2d>>& initialGuessPoints,
const std::vector<std::vector<frc::TrajectoryGenerator::PoseWithCurvature>>&
splinePoints,
const frc::SwerveDriveKinematics<4>& kinematics) {
const auto maxWheelVelocity = CalculateMaxWheelVelocity(path);
std::vector<frc::Trajectory> trajectories;
trajectories.reserve(path.waypoints.size());
size_t splineIdx = 0;
for (size_t sgmtIdx = 1; sgmtIdx < initialGuessPoints.size(); ++sgmtIdx) {
auto sgmtVel = maxWheelVelocity;
const auto& sgmtGuessPoints = initialGuessPoints.at(sgmtIdx);
for (size_t guessIdx = 0; guessIdx < sgmtGuessPoints.size(); ++guessIdx) {
Pose2d start;
Pose2d start = (guessIdx == 0) ? initialGuessPoints.at(sgmtIdx - 1).back()
: sgmtGuessPoints.at(guessIdx - 1);
Pose2d end = sgmtGuessPoints.at(guessIdx);
if (guessIdx == 0) {
start = initialGuessPoints.at(sgmtIdx - 1).back();
} else {
start = sgmtGuessPoints.at(guessIdx - 1);
}
auto dtheta =
std::abs(frc::AngleModulus(
units::radian_t(std::abs(start.Rotation().Radians() -
end.Rotation().Radians())))
.value());
frc::Translation2d sgmtStart{units::meter_t(start.Translation().X()),
units::meter_t(start.Translation().Y())};
frc::Translation2d sgmtEnd{units::meter_t(end.Translation().X()),
units::meter_t(end.Translation().Y())};

for (auto& c : path.waypoints.at(sgmtIdx).segmentConstraints) {
// assuming HolonomicVelocityConstraint with CircularSet2d
if (std::holds_alternative<LinearVelocityMaxMagnitudeConstraint>(c)) {
const auto& velocityHolonomicConstraint =
std::get<LinearVelocityMaxMagnitudeConstraint>(c);
auto vel = units::meters_per_second_t(
velocityHolonomicConstraint.m_maxMagnitude);
std::printf("max lin vel: %.2f - ", vel.value());
if (vel < sgmtVel) {
sgmtVel = vel;
}
} else if (std::holds_alternative<
AngularVelocityMaxMagnitudeConstraint>(c)) {
const auto& angVelConstraint =
std::get<AngularVelocityMaxMagnitudeConstraint>(c);
auto maxAngVel = angVelConstraint.m_maxMagnitude;
std::printf("max ang vel: %.2f - ", maxAngVel);
// TODO add how the 1.5 is determined
auto time = 1.5 * dtheta / maxAngVel;
// estimating velocity for a straight line path
// TODO use the spine path distance
auto vel = sgmtStart.Distance(sgmtEnd) / units::second_t(time);
if (vel < sgmtVel) {
sgmtVel = vel;
}
}
}
sgmtVel = CalculateSegmentVelocity(path, sgmtIdx, sgmtVel, start, end);
std::printf("sgmtVel: %.2f\n", sgmtVel.value());
frc::TrajectoryConfig sgmtConfig{sgmtVel, sgmtVel / units::second_t{1.0}};
// uses each non-init guess waypoint as a stop point for first guess
Expand All @@ -118,55 +148,23 @@ std::vector<frc::Trajectory> GenerateWaypointSplineTrajectories(
sgmtConfig.StartVelocity(), sgmtConfig.EndVelocity(),
sgmtConfig.MaxVelocity(), sgmtConfig.MaxAcceleration(),
sgmtConfig.IsReversed());
trajs.push_back(sgmtTraj);
trajectories.push_back(sgmtTraj);
}
}
std::printf("path.wpt size: %zd\n", path.waypoints.size());
std::printf("trajs size: %zd\n", trajs.size());
return trajs;
std::printf("trajs size: %zd\n", trajectories.size());
return trajectories;
}

std::vector<std::vector<frc::Trajectory::State>> CalculateWaypointStatesWithDt(
inline std::vector<frc::Trajectory> GenerateWaypointSplineTrajectories(
const trajopt::SwervePath path,
const std::vector<std::vector<Pose2d>> initialGuessPoints,
const double desiredDt) {
const auto trajs =
GenerateWaypointSplineTrajectories(path, initialGuessPoints);

size_t guessPoints = 0;
for (const auto& guesses : initialGuessPoints) {
guessPoints += guesses.size();
}
std::vector<std::vector<frc::Trajectory::State>> waypoint_states;
waypoint_states.reserve(guessPoints);
for (size_t i = 0; i < guessPoints; ++i) {
waypoint_states.push_back(std::vector<frc::Trajectory::State>());
}

for (size_t sgmtIdx = 1; sgmtIdx < guessPoints; ++sgmtIdx) {
// specify parameterized spline points to use for trajectory
const auto sgmtTraj = trajs.at(sgmtIdx - 1);

const auto wholeSgmtDt = sgmtTraj.TotalTime();
const size_t samplesForSgmtNew = std::ceil(wholeSgmtDt.value() / desiredDt);
const auto dt = wholeSgmtDt / samplesForSgmtNew;
std::printf("dt for sgmt%zd with %zd samples: %.5f\n", sgmtIdx,
samplesForSgmtNew, dt.value());

if (sgmtIdx == 1) {
std::printf("sgmt1\n");
waypoint_states.at(sgmtIdx - 1).push_back(sgmtTraj.States().front());
}

for (size_t sampleIdx = 1; sampleIdx <= samplesForSgmtNew; ++sampleIdx) {
auto t = static_cast<double>(sampleIdx) * dt;
const auto point = sgmtTraj.Sample(t);
waypoint_states.at(sgmtIdx).push_back(point);
std::printf("%zd,", sampleIdx);
}
std::printf(" size: %zd\n", waypoint_states.at(sgmtIdx).size());
}
return waypoint_states;
const std::vector<std::vector<Pose2d>> initialGuessPoints) {
std::vector<trajopt::CubicHermitePoseSplineHolonomic> splines =
CubicPoseControlVectorsFromWaypoints(initialGuessPoints);
auto splinePoints = ParameterizeSplines(splines);
const auto kinematics = CreateKinematics(path);
return GenerateTrajectories(path, initialGuessPoints, splinePoints,
kinematics);
}

std::vector<std::vector<frc::Trajectory::State>>
Expand Down Expand Up @@ -219,6 +217,50 @@ CalculateWaypointStatesWithControlIntervals(
return waypoint_states;
}

std::vector<std::vector<frc::Trajectory::State>> CalculateWaypointStatesWithDt(
const trajopt::SwervePath path,
const std::vector<std::vector<Pose2d>> initialGuessPoints,
const double desiredDt) {
const auto trajs =
GenerateWaypointSplineTrajectories(path, initialGuessPoints);

size_t guessPoints = 0;
for (const auto& guesses : initialGuessPoints) {
guessPoints += guesses.size();
}
std::vector<std::vector<frc::Trajectory::State>> waypoint_states;
waypoint_states.reserve(guessPoints);
for (size_t i = 0; i < guessPoints; ++i) {
waypoint_states.push_back(std::vector<frc::Trajectory::State>());
}

for (size_t sgmtIdx = 1; sgmtIdx < guessPoints; ++sgmtIdx) {
// specify parameterized spline points to use for trajectory
const auto sgmtTraj = trajs.at(sgmtIdx - 1);

const auto wholeSgmtDt = sgmtTraj.TotalTime();
const size_t samplesForSgmtNew = std::ceil(wholeSgmtDt.value() / desiredDt);
const auto dt = wholeSgmtDt / samplesForSgmtNew;
std::printf("dt for sgmt%zd with %zd samples: %.5f\n", sgmtIdx,
samplesForSgmtNew, dt.value());

if (sgmtIdx == 1) {
std::printf("sgmt1\n");
waypoint_states.at(sgmtIdx - 1).push_back(sgmtTraj.States().front());
}

for (size_t sampleIdx = 1; sampleIdx <= samplesForSgmtNew; ++sampleIdx) {
auto t = static_cast<double>(sampleIdx) * dt;
const auto point = sgmtTraj.Sample(t);
waypoint_states.at(sgmtIdx).push_back(point);
std::printf("%zd,", sampleIdx);
}
std::printf(" size: %zd\n", waypoint_states.at(sgmtIdx).size());
}
return waypoint_states;
}

/// TODO: this probably should move to swervepathbuilder
std::vector<size_t> CalculateControlIntervalCountsWithDt(
const trajopt::SwervePath path,
const std::vector<std::vector<Pose2d>> initialGuessPoints,
Expand All @@ -242,13 +284,17 @@ inline SwerveSolution CalculateSplineInitialGuessWithKinematicsAndConstraints(
path, initialGuessPoints, controlIntervalCounts);

SwerveSolution initialGuess;
for (const auto& traj : trajectoriesSamples) {
for (auto i = 0; i < trajectoriesSamples.size(); ++i) {
const auto& traj = trajectoriesSamples.at(i);
/// FIXME: first segment is always 1 point long so always 0.1s to second
/// sample
auto dt = 0.1_s;
if (traj.size() > 1) {
dt = traj.at(1).t - traj.front().t;
} else if (traj.size() == 1 && i + 1 < trajectoriesSamples.size()) {
dt = trajectoriesSamples.at(i + 1).front().t - traj.front().t;
}

for (const auto& point : traj) {
// std::printf("point{%f, %f, %f, %f, %f}\n",
// point.pose.X().value(),
Expand Down

0 comments on commit 1c2f6e1

Please sign in to comment.