82 lines
2.7 KiB
C++
82 lines
2.7 KiB
C++
#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
|