227 lines
9.2 KiB
C++
227 lines
9.2 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);
|
||
|
||
|
||
// 统一入口：两点/多点皆可
|
||
bool plan(const std::vector<std::vector<double> > &waypoints, TrajPtr &traj_out) override;
|
||
|
||
// 兼容旧 API（可选）：转发为两点的统一入口
|
||
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:
|
||
// —— 几何路径统一分发 ——
|
||
std::shared_ptr<toppra::PiecewisePolyPath>
|
||
buildPathUnified(const std::vector<Eigen::VectorXd> &q,
|
||
const std::vector<toppra::value_type> &S);
|
||
|
||
// 二点专用
|
||
std::shared_ptr<toppra::PiecewisePolyPath>
|
||
buildTwoPointPath(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);
|
||
|
||
std::shared_ptr<toppra::PiecewisePolyPath>
|
||
buildLinearTwo(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);
|
||
|
||
std::shared_ptr<toppra::PiecewisePolyPath>
|
||
buildCubicHermiteTwo(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);
|
||
|
||
std::shared_ptr<toppra::PiecewisePolyPath>
|
||
buildQuinticRestToRestTwo(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);
|
||
|
||
std::shared_ptr<toppra::PiecewisePolyPath>
|
||
buildNaturalTwo(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);
|
||
|
||
// 多点
|
||
std::shared_ptr<toppra::PiecewisePolyPath>
|
||
buildLinearMulti(const std::vector<Eigen::VectorXd> &q,
|
||
const std::vector<toppra::value_type> &S);
|
||
|
||
std::shared_ptr<toppra::PiecewisePolyPath>
|
||
buildCubicHermiteMulti(const std::vector<Eigen::VectorXd> &q,
|
||
const std::vector<toppra::value_type> &S);
|
||
|
||
std::shared_ptr<toppra::PiecewisePolyPath>
|
||
buildQuinticC2Multi(const std::vector<Eigen::VectorXd> &q,
|
||
const std::vector<toppra::value_type> &S);
|
||
|
||
std::shared_ptr<toppra::PiecewisePolyPath>
|
||
buildNaturalMulti(const std::vector<Eigen::VectorXd> &q,
|
||
const std::vector<toppra::value_type> &S);
|
||
|
||
// —— 工具：限幅/参数/估计 ——
|
||
bool ensureLimitsSized(std::size_t DoF);
|
||
|
||
static void sanitizeVsq(toppra::Vector &v);
|
||
|
||
|
||
// centripetal 弦长（alpha=0.5），生成严格递增 S
|
||
static std::vector<toppra::value_type>
|
||
makeS_centripetal(const std::vector<Eigen::VectorXd> &q) {
|
||
const size_t M = q.size();
|
||
std::vector<toppra::value_type> S(M, 0.0);
|
||
auto chord = [](const Eigen::VectorXd &a, const Eigen::VectorXd &b) {
|
||
double d = (a - b).norm();
|
||
return std::pow(std::max(d, 1e-16), 0.5);
|
||
};
|
||
for (size_t i = 1; i < M; ++i) {
|
||
S[i] = S[i - 1] + chord(q[i], q[i - 1]);
|
||
if (S[i] <= S[i - 1]) S[i] = S[i - 1] + 1e-12;
|
||
}
|
||
return S;
|
||
}
|
||
|
||
// 等距参数（简单稳妥）
|
||
static inline std::vector<toppra::value_type> makeS_equal(size_t M) {
|
||
std::vector<toppra::value_type> S(M);
|
||
for (size_t i = 0; i < M; ++i) S[i] = static_cast<toppra::value_type>(i);
|
||
return S;
|
||
}
|
||
|
||
// 或：先用centripetal，再整体归一化到跨度≈(M-1)，并设置每段最小ds
|
||
static inline void normalize_and_floor_S(std::vector<toppra::value_type> &S, double ds_min = 0.2) {
|
||
for (size_t i = 1; i < S.size(); ++i) S[i] -= S[0];
|
||
double L = S.back();
|
||
if (L > 0) for (auto &x: S) x *= (S.size() - 1) / L;
|
||
for (size_t i = 1; i < S.size(); ++i) if (S[i] - S[i - 1] < ds_min) S[i] = S[i - 1] + ds_min;
|
||
}
|
||
|
||
// Catmull–Rom（centripetal）估计结点几何速度 v（端点=0）
|
||
static std::vector<Eigen::VectorXd>
|
||
estimateVelsCatmull(const std::vector<Eigen::VectorXd> &q,
|
||
const std::vector<toppra::value_type> &S) {
|
||
const size_t M = q.size();
|
||
const int DoF = static_cast<int>(q[0].size());
|
||
std::vector<Eigen::VectorXd> v(M, Eigen::VectorXd::Zero(DoF));
|
||
if (M <= 2) return v;
|
||
for (size_t i = 1; i + 1 < M; ++i) {
|
||
double ds0 = std::max<double>(S[i] - S[i - 1], 1e-12);
|
||
double ds1 = std::max<double>(S[i + 1] - S[i], 1e-12);
|
||
v[i] = ((q[i + 1] - q[i]) / ds1 * ds0 + (q[i] - q[i - 1]) / ds0 * ds1) / (ds0 + ds1);
|
||
}
|
||
return v;
|
||
}
|
||
|
||
// 对内点几何速度限幅，抑制过冲（k∈[0.5,1.0]）
|
||
static void clampNodeVels(std::vector<Eigen::VectorXd> &v,
|
||
const std::vector<Eigen::VectorXd> &q,
|
||
double k = 1.0) {
|
||
const size_t M = q.size();
|
||
if (M <= 2) return;
|
||
for (size_t i = 1; i + 1 < M; ++i) {
|
||
double d0 = (q[i] - q[i - 1]).norm();
|
||
double d1 = (q[i + 1] - q[i]).norm();
|
||
double d = std::max(std::min(d0, d1), 1e-12);
|
||
double vmax = k * d;
|
||
double n = v[i].norm();
|
||
if (n > vmax) v[i] *= (vmax / n);
|
||
}
|
||
}
|
||
|
||
// 估计结点几何加速度 a（端点=0；中点二阶差分，按 s 尺度）
|
||
static std::vector<Eigen::VectorXd>
|
||
estimateAccelsSecondDiff(const std::vector<Eigen::VectorXd> &q,
|
||
const std::vector<toppra::value_type> &S) {
|
||
const size_t M = q.size();
|
||
const int DoF = static_cast<int>(q[0].size());
|
||
std::vector<Eigen::VectorXd> a(M, Eigen::VectorXd::Zero(DoF));
|
||
if (M <= 2) return a;
|
||
for (size_t i = 1; i + 1 < M; ++i) {
|
||
double h0 = std::max<double>(S[i] - S[i - 1], 1e-12); // 左间距
|
||
double h1 = std::max<double>(S[i + 1] - S[i], 1e-12); // 右间距
|
||
double denom = 0.5 * (h0 + h1); // 局部尺度
|
||
// 非均匀中心二阶差分（更精确）：
|
||
// a ≈ 2 * [ (q_{i+1}-q_i)/h1 - (q_i - q_{i-1})/h0 ] / (h0 + h1)
|
||
a[i] = 2.0 * ((q[i + 1] - q[i]) / h1 - (q[i] - q[i - 1]) / h0) / (h0 + h1);
|
||
}
|
||
return a;
|
||
}
|
||
|
||
|
||
// τ→s 变元：把局部 Quintic(τ) 的系数 c_tau[0..5]（τ^0..τ^5）
|
||
// 变成全局 s 的系数 alpha[0..5]（s^0..s^5），其中 τ = (s - S_k) / ds
|
||
static inline void localQuinticToGlobalCoeffs(
|
||
const std::array<Eigen::VectorXd, 6> &c_tau, // c0..c5（DoF维向量）
|
||
double Sk, double ds,
|
||
std::array<Eigen::VectorXd, 6> &alpha // α0..α5（DoF维向量）
|
||
) {
|
||
static const double C[6][6] = {
|
||
// binomial(n,m)
|
||
{1, 0, 0, 0, 0, 0},
|
||
{1, 1, 0, 0, 0, 0},
|
||
{1, 2, 1, 0, 0, 0},
|
||
{1, 3, 3, 1, 0, 0},
|
||
{1, 4, 6, 4, 1, 0},
|
||
{1, 5, 10, 10, 5, 1}
|
||
};
|
||
const double eps = 1e-12;
|
||
ds = std::max(ds, eps);
|
||
|
||
for (int m = 0; m <= 5; ++m) alpha[m].setZero(c_tau[0].size());
|
||
|
||
// α_m = Σ_{n=m..5} c_n * C(n,m) * (-S_k)^{n-m} / ds^{n}
|
||
for (int n = 0; n <= 5; ++n) {
|
||
double invdsn = std::pow(ds, -n);
|
||
for (int m = 0; m <= n; ++m) {
|
||
double factor = C[n][m] * std::pow(-Sk, n - m) * invdsn;
|
||
alpha[m].noalias() += factor * c_tau[n];
|
||
}
|
||
}
|
||
}
|
||
};
|
||
} // namespace cmvr
|