From 1c2f6e1f1afe972df881f0facb0b94ef2fa8b08a Mon Sep 17 00:00:00 2001 From: camaj Date: Sun, 30 Jun 2024 22:39:57 -0700 Subject: [PATCH] spline refactor --- .../util/GenerateSplineInitialGuess.hpp | 260 +++++++++++------- 1 file changed, 153 insertions(+), 107 deletions(-) diff --git a/include/trajopt/util/GenerateSplineInitialGuess.hpp b/include/trajopt/util/GenerateSplineInitialGuess.hpp index b6c04fed..a9c66473 100644 --- a/include/trajopt/util/GenerateSplineInitialGuess.hpp +++ b/include/trajopt/util/GenerateSplineInitialGuess.hpp @@ -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 GenerateWaypointSplineTrajectories( - const trajopt::SwervePath path, - const std::vector> initialGuessPoints) { - std::vector splines = - CubicPoseControlVectorsFromWaypoints(initialGuessPoints); - // Generate a parameterized spline +// Generate a parameterized spline +std::vector> +ParameterizeSplines( + const std::vector& splines) { std::vector> 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 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(c)) { + const auto& velocityHolonomicConstraint = + std::get(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( + c)) { + const auto& angVelConstraint = + std::get(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 trajs; - trajs.reserve(path.waypoints.size()); +inline std::vector GenerateTrajectories( + const SwervePath& path, + const std::vector>& initialGuessPoints, + const std::vector>& + splinePoints, + const frc::SwerveDriveKinematics<4>& kinematics) { + const auto maxWheelVelocity = CalculateMaxWheelVelocity(path); + std::vector 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(c)) { - const auto& velocityHolonomicConstraint = - std::get(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(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 @@ -118,55 +148,23 @@ std::vector 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> CalculateWaypointStatesWithDt( +inline std::vector GenerateWaypointSplineTrajectories( const trajopt::SwervePath path, - const std::vector> initialGuessPoints, - const double desiredDt) { - const auto trajs = - GenerateWaypointSplineTrajectories(path, initialGuessPoints); - - size_t guessPoints = 0; - for (const auto& guesses : initialGuessPoints) { - guessPoints += guesses.size(); - } - std::vector> waypoint_states; - waypoint_states.reserve(guessPoints); - for (size_t i = 0; i < guessPoints; ++i) { - waypoint_states.push_back(std::vector()); - } - - 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(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> initialGuessPoints) { + std::vector splines = + CubicPoseControlVectorsFromWaypoints(initialGuessPoints); + auto splinePoints = ParameterizeSplines(splines); + const auto kinematics = CreateKinematics(path); + return GenerateTrajectories(path, initialGuessPoints, splinePoints, + kinematics); } std::vector> @@ -219,6 +217,50 @@ CalculateWaypointStatesWithControlIntervals( return waypoint_states; } +std::vector> CalculateWaypointStatesWithDt( + const trajopt::SwervePath path, + const std::vector> initialGuessPoints, + const double desiredDt) { + const auto trajs = + GenerateWaypointSplineTrajectories(path, initialGuessPoints); + + size_t guessPoints = 0; + for (const auto& guesses : initialGuessPoints) { + guessPoints += guesses.size(); + } + std::vector> waypoint_states; + waypoint_states.reserve(guessPoints); + for (size_t i = 0; i < guessPoints; ++i) { + waypoint_states.push_back(std::vector()); + } + + 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(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 CalculateControlIntervalCountsWithDt( const trajopt::SwervePath path, const std::vector> initialGuessPoints, @@ -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(),