bang_bang_trajectory_1d.h

#pragma once
 
#include <array>
 
#include "software/ai/navigator/trajectory/trajectory.hpp"
 
class BangBangTrajectory1D : public Trajectory<double, double, double>
{
   public:
    struct TrajectoryPart
    {
        double end_time_sec = 0;
        double position     = 0;
        double velocity     = 0;
        double acceleration = 0;
    };
 
    BangBangTrajectory1D();
 
    void generate(double initial_pos, double final_pos, double initial_vel,
                  double max_vel, double max_accel, double max_decel);
 
    double getPosition(double t_sec) const override;
 
    double getVelocity(double t_sec) const override;
 
    double getAcceleration(double t_sec) const override;
 
    inline double getTotalTime() const override
    {
        return trajectory_parts[num_trajectory_parts - 1].end_time_sec;
    }
 
    std::pair<double, double> getMinMaxPositions() const;
 
    const TrajectoryPart &getTrajectoryPart(size_t index) const;
 
    size_t getNumTrajectoryParts() const;
 
   private:
    void generateTrapezoidalTrajectory(double initial_pos, double final_pos,
                                       double initial_vel, double max_vel,
                                       double max_accel, double max_decel,
                                       double time_offset_sec = 0.0);
 
    void generateTriangularTrajectory(double initial_pos, double final_pos,
                                      double initial_vel, double max_accel,
                                      double max_decel, double time_offset_sec = 0.0);
 
    inline double closestPositionToStop(double initial_pos, double initial_vel,
                                        double max_decel) const;
 
    inline double triangularProfileStopPosition(double initial_pos, double initial_vel,
                                                double max_vel, double max_accel,
                                                double max_decel, double direction) const;
 
    inline size_t getTrajectoryIndexAtTime(double t_sec) const;
 
    void getTrajPartAndDeltaTime(double t_sec,
                                 BangBangTrajectory1D::TrajectoryPart &out_traj_part,
                                 double &out_t_delta_sec) const;
 
    inline void addTrajectoryPart(const TrajectoryPart &part);
 
    // We use a fixed size array instead of a vector to avoid the overhead
    // of dynamic memory allocation + emplace_back, push_back, etc.
    static constexpr unsigned int MAX_TRAJECTORY_PARTS                = 4;
    size_t num_trajectory_parts                                       = 0;
    std::array<TrajectoryPart, MAX_TRAJECTORY_PARTS> trajectory_parts = {
        TrajectoryPart(), TrajectoryPart(), TrajectoryPart(), TrajectoryPart()};
};