fsb_trajectory_segment.h

#ifndef FSB_TRAJECTORY_SEGMENT_H
#define FSB_TRAJECTORY_SEGMENT_H
 
#include "fsb_motion.h"
#include "fsb_trajectory_types.h"
#include "fsb_types.h"
 
namespace fsb
{
 
class SegmentBase
{
public:
    virtual ~SegmentBase() = default;
 
    [[nodiscard]] virtual real_t get_start_time() const = 0;
 
    [[nodiscard]] virtual real_t get_duration() const = 0;
 
    [[nodiscard]] virtual real_t get_final_time() const = 0;
};
 
class Segment : public SegmentBase
{
public:
    [[nodiscard]] virtual TrajState evaluate(real_t t_eval) const = 0;
 
    [[nodiscard]] virtual TrajState get_final_state() const = 0;
};
 
class Segment3 : public SegmentBase
{
public:
    [[nodiscard]] virtual TrajState3 evaluate(real_t t_eval) const = 0;
 
    [[nodiscard]] virtual TrajState3 get_final_state() const = 0;
};
 
class Segment6 : public SegmentBase
{
public:
    [[nodiscard]] virtual CartesianPva evaluate(real_t t_eval) const = 0;
 
    [[nodiscard]] virtual CartesianPva get_final_state() const = 0;
};
 
class SegmentConstJerk final : public Segment
{
public:
    SegmentConstJerk() = default;
 
    void generate(real_t start_time, real_t duration, const TrajState& initial_state, real_t jerk);
 
    [[nodiscard]] TrajState evaluate(real_t t_eval) const override;
 
    [[nodiscard]] TrajState get_final_state() const override
    {
        return evaluate(m_start_time + m_duration);
    }
 
    [[nodiscard]] real_t get_start_time() const override
    {
        return m_start_time;
    }
 
    [[nodiscard]] real_t get_duration() const override
    {
        return m_duration;
    }
 
    [[nodiscard]] real_t get_final_time() const override
    {
        return m_start_time + m_duration;
    }
 
private:
    real_t m_initial_position = 0.0; 
    real_t m_initial_velocity = 0.0; 
    real_t m_initial_acceleration = 0.0; 
    real_t m_jerk = 0.0; 
    real_t m_start_time = 0.0; 
    real_t m_duration = 0.0; 
};
 
class SegmentConstAcc final : public Segment
{
public:
    SegmentConstAcc() = default;
 
    void generate(real_t start_time, real_t duration, const TrajState& initial_state, real_t acceleration);
 
    [[nodiscard]] TrajState evaluate(real_t t_eval) const override;
 
    [[nodiscard]] TrajState get_final_state() const override
    {
        return evaluate(m_start_time + m_duration);
    }
 
    [[nodiscard]] real_t get_start_time() const override
    {
        return m_start_time;
    }
 
    [[nodiscard]] real_t get_duration() const override
    {
        return m_duration;
    }
 
    [[nodiscard]] real_t get_final_time() const override
    {
        return m_start_time + m_duration;
    }
 
private:
    real_t m_initial_position = 0.0; 
    real_t m_initial_velocity = 0.0; 
    real_t m_acceleration = 0.0; 
    real_t m_start_time = 0.0; 
    real_t m_duration = 0.0; 
};
 
class SegmentConstVel final : public Segment
{
public:
    SegmentConstVel() = default;
 
    void generate(real_t start_time, real_t duration, const TrajState& initial_state, real_t velocity);
 
    [[nodiscard]] TrajState evaluate(real_t t_eval) const override;
 
    [[nodiscard]] TrajState get_final_state() const override
    {
        return evaluate(m_start_time + m_duration);
    }
 
    [[nodiscard]] real_t get_start_time() const override
    {
        return m_start_time;
    }
 
    [[nodiscard]] real_t get_duration() const override
    {
        return m_duration;
    }
 
    [[nodiscard]] real_t get_final_time() const override
    {
        return m_start_time + m_duration;
    }
 
private:
    real_t m_initial_position = 0.0; 
    real_t m_velocity = 0.0; 
    real_t m_start_time = 0.0; 
    real_t m_duration = 0.0; 
};
 
} // namespace fsb
 
#endif