75 lines
2.3 KiB
C++
75 lines
2.3 KiB
C++
//
|
||
// Created by lgv on 11/7/25.
|
||
//
|
||
|
||
#pragma once
|
||
|
||
#include <vector>
|
||
#include "common/consts/constant.h"
|
||
#include <toppra/geometric_path/piecewise_poly_path.hpp>
|
||
#include <toppra/parametrizer/const_accel.hpp>
|
||
#include <toppra/parametrizer/spline.hpp>
|
||
|
||
namespace cmvr {
|
||
// 统一轨迹接口
|
||
struct ITrajectory {
|
||
virtual ~ITrajectory() = default;
|
||
virtual toppra::Bound timeInterval() const = 0;
|
||
virtual Eigen::VectorXd q(double t) const = 0;
|
||
virtual Eigen::VectorXd qd(double t) const = 0;
|
||
virtual Eigen::VectorXd qdd(double t) const = 0;
|
||
};
|
||
using TrajPtr = std::shared_ptr<ITrajectory>;
|
||
|
||
struct TrajSample {
|
||
double t{};
|
||
Eigen::VectorXd q; // DoF x 1
|
||
Eigen::VectorXd qd; // DoF x 1
|
||
Eigen::VectorXd qdd; // DoF x 1
|
||
};
|
||
|
||
// 三种几何路径
|
||
enum class PathType { Linear, CubicHermite, Quintic };
|
||
class JointSpacePlanner {
|
||
public:
|
||
|
||
explicit JointSpacePlanner(PathType p):path_type_(p){};
|
||
JointSpacePlanner()=default;
|
||
virtual ~JointSpacePlanner() = default;
|
||
|
||
virtual bool plan(const std::vector<double>& start_joints,
|
||
const std::vector<double>& goal_joints,
|
||
TrajPtr& traj) {
|
||
UNUSED_VARIABLE(start_joints, goal_joints,traj);
|
||
return false;
|
||
}
|
||
|
||
// 采样函数：从 ITrajectory 生成采样序列 dt(s)
|
||
virtual std::vector<TrajSample> sampleTrajectory(const TrajPtr& traj, double dt) {
|
||
UNUSED_VARIABLE(traj,dt);
|
||
return {};
|
||
}
|
||
|
||
virtual bool writeTrajectoryCsv(const std::string& filename,const std::vector<TrajSample>& samples) {
|
||
UNUSED_VARIABLE(filename,samples);
|
||
return false;
|
||
}
|
||
|
||
// 对称限：[-v_max, v_max]、[-a_max, a_max]
|
||
virtual void setSymmetricLimits(const std::vector<double> &v_max,
|
||
const std::vector<double> &a_max);
|
||
|
||
// 可调网格密度（默认 150/300）
|
||
virtual void setGridSizes(int N, int N_high);
|
||
|
||
// 切换几何路径类型
|
||
virtual void setPathType(PathType p);
|
||
protected:
|
||
PathType path_type_{PathType::Quintic};
|
||
int N_grid_ = 150;
|
||
int N_grid_high_ = 300;
|
||
std::vector<double> v_max_, a_max_;
|
||
|
||
};
|
||
}
|