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
|