PathPlannerTrajectory.h

#pragma once
 
#include <units/time.h>
#include <units/length.h>
#include <units/velocity.h>
#include <units/acceleration.h>
#include <units/angular_velocity.h>
#include <units/curvature.h>
#include <frc/geometry/Translation2d.h>
#include <frc/geometry/Rotation2d.h>
#include <frc/geometry/Pose2d.h>
#include <frc/kinematics/ChassisSpeeds.h>
#include <frc/kinematics/SwerveModuleState.h>
#include <frc/MathUtil.h>
#include <frc2/command/Command.h>
#include <vector>
#include <utility>
#include <memory>
#include "pathplanner/lib/trajectory/PathPlannerTrajectoryState.h"
#include "pathplanner/lib/path/PathConstraints.h"
#include "pathplanner/lib/util/GeometryUtil.h"
#include "pathplanner/lib/config/RobotConfig.h"
#include "pathplanner/lib/events/Event.h"
 
namespace pathplanner {
 
class PathPlannerPath;
 
class PathPlannerTrajectory {
public:
    PathPlannerTrajectory() {
    }
 
    PathPlannerTrajectory(std::vector<PathPlannerTrajectoryState> states,
            std::vector<std::shared_ptr<Event>> events) : m_states(states), m_events(
            events) {
    }
 
    PathPlannerTrajectory(std::vector<PathPlannerTrajectoryState> states) : m_states(
            states) {
    }
 
    PathPlannerTrajectory(std::shared_ptr<PathPlannerPath> path,
            const frc::ChassisSpeeds &startingSpeeds,
            const frc::Rotation2d &startingRotation, const RobotConfig &config);
 
    inline std::vector<std::shared_ptr<Event>> getEvents() {
        return m_events;
    }
 
    constexpr std::vector<PathPlannerTrajectoryState>& getStates() {
        return m_states;
    }
 
    inline PathPlannerTrajectoryState getState(size_t index) {
        return m_states[index];
    }
 
    inline PathPlannerTrajectoryState getInitialState() {
        return m_states[0];
    }
 
    inline PathPlannerTrajectoryState getEndState() {
        return m_states[m_states.size() - 1];
    }
 
    inline units::second_t getTotalTime() {
        return getEndState().time;
    }
 
    inline frc::Pose2d getInitialPose() {
        return getInitialState().pose;
    }
 
    PathPlannerTrajectoryState sample(const units::second_t time);
 
    inline PathPlannerTrajectory flip() {
        std::vector < PathPlannerTrajectoryState > mirroredStates;
        for (auto state : m_states) {
            mirroredStates.emplace_back(state.flip());
        }
        return PathPlannerTrajectory(mirroredStates, getEvents());
    }
private:
    std::vector<PathPlannerTrajectoryState> m_states;
    std::vector<std::shared_ptr<Event>> m_events;
 
    static void generateStates(std::vector<PathPlannerTrajectoryState> &states,
            std::shared_ptr<PathPlannerPath> path,
            const frc::Rotation2d &startingRotation, const RobotConfig &config);
 
    static void forwardAccelPass(
            std::vector<PathPlannerTrajectoryState> &states,
            const RobotConfig &config);
 
    static void reverseAccelPass(
            std::vector<PathPlannerTrajectoryState> &states,
            const RobotConfig &config);
 
    static void desaturateWheelSpeeds(
            std::vector<SwerveModuleTrajectoryState> &moduleStates,
            const frc::ChassisSpeeds &desiredSpeeds,
            units::meters_per_second_t maxModuleSpeed,
            units::meters_per_second_t maxTranslationSpeed,
            units::radians_per_second_t maxRotationSpeed);
 
    static size_t getNextRotationTargetIdx(
            std::shared_ptr<PathPlannerPath> path, const size_t startingIndex);
 
    static inline frc::Rotation2d cosineInterpolate(const frc::Rotation2d start,
            const frc::Rotation2d end, const double t) {
        double t2 = (1.0 - std::cos(t * M_PI)) / 2.0;
        return GeometryUtil::rotationLerp(start, end, t2);
    }
};
}