cmvr-es/src/planner/joint_space_planner/include/toppra_bspline.h

#pragma once
#include <memory>
#include <vector>
#include <Eigen/Dense>

#include "planner/joint_space_planner/include/joint_space_planner.h"
#include <toppra/geometric_path/piecewise_poly_path.hpp>
#include <toppra/parametrizer/const_accel.hpp>
#include <toppra/parametrizer/spline.hpp>

namespace cmvr {


    // 适配器：ConstAccel
    class ConstAccelTraj : public ITrajectory {
    public:
        explicit ConstAccelTraj(std::shared_ptr<toppra::parametrizer::ConstAccel> p);

        toppra::Bound timeInterval() const override;

        Eigen::VectorXd q(double t) const override;

        Eigen::VectorXd qd(double t) const override;

        Eigen::VectorXd qdd(double t) const override;

    private:
        std::shared_ptr<toppra::parametrizer::ConstAccel> impl_;
    };

    // 适配器：Spline
    class SplineTraj : public ITrajectory {
    public:
        SplineTraj(const std::shared_ptr<toppra::PiecewisePolyPath> &path,
                   const toppra::Vector &grid,
                   const toppra::Vector &vsq);

        toppra::Bound timeInterval() const override;

        Eigen::VectorXd q(double t) const override;

        Eigen::VectorXd qd(double t) const override;

        Eigen::VectorXd qdd(double t) const override;

    private:
        toppra::parametrizer::Spline impl_;
    };

    // 具体规划器：一次/三次/五次可切换；ConstAccel 校验失败自动回退 Spline
    class ToppraBSpline : public JointSpacePlanner {
    public:
        explicit ToppraBSpline(PathType type = PathType::Quintic);

        // 主入口：尽量保证返回轨迹（失败回退到 Spline）
        bool plan(const std::vector<double> &start_joints,
                  const std::vector<double> &goal_joints,
                  TrajPtr &traj_out) override;

        std::vector<TrajSample> sampleTrajectory(const TrajPtr& traj, double dt) override;
        bool writeTrajectoryCsv(const std::string& filename,const std::vector<TrajSample>& samples) override;

    private:
        // 几何路径构造
        static std::shared_ptr<toppra::PiecewisePolyPath>
        buildLinear(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);

        static std::shared_ptr<toppra::PiecewisePolyPath>
        buildCubicHermiteRest(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);

        static std::shared_ptr<toppra::PiecewisePolyPath>
        buildQuinticRestToRest(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);

        static std::shared_ptr<toppra::PiecewisePolyPath>
        buildPath(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1, PathType t);

        static void sanitizeVsq(toppra::Vector &vsq);

        bool ensureLimitsSized(std::size_t DoF);
    };
} // namespace cmvr