73 lines
2.1 KiB
C++
73 lines
2.1 KiB
C++
#ifndef INTERPOLATION_BASE_H
|
||
#define INTERPOLATION_BASE_H
|
||
|
||
#include "utils/math/geometry.h"
|
||
#include <vector>
|
||
#include <stdexcept>
|
||
|
||
namespace cmvr::math {
|
||
|
||
/**
|
||
* 插值基类，用于机械臂位姿插值
|
||
* 支持三维位姿(Pose3d)和二维位姿(Pose2d)的插值
|
||
*/
|
||
class AbstractInterpolation {
|
||
public:
|
||
// 构造函数
|
||
AbstractInterpolation(const std::vector<Pose3d>& waypoints);
|
||
AbstractInterpolation(const std::vector<Pose2d>& waypoints);
|
||
|
||
// 析构函数
|
||
virtual ~AbstractInterpolation() = default;
|
||
|
||
/**
|
||
* 插值计算接口
|
||
* @param t 插值参数，范围[0, 1]，0表示起点，1表示终点
|
||
* @return 插值得到的位姿
|
||
*/
|
||
virtual Pose3d interpolate_3d(double t) const = 0;
|
||
virtual Pose2d interpolate_2d(double t) const = 0;
|
||
|
||
/**
|
||
* 生成轨迹点序列
|
||
* @param num_points 生成的轨迹点数量，包括起点和终点
|
||
* @return 轨迹点序列
|
||
*/
|
||
std::vector<Pose3d> generate_trajectory_3d(size_t num_points) const;
|
||
std::vector<Pose2d> generate_trajectory_2d(size_t num_points) const;
|
||
|
||
protected:
|
||
// 路径点存储
|
||
std::vector<Pose3d> waypoints_3d_;
|
||
std::vector<Pose2d> waypoints_2d_;
|
||
|
||
/**
|
||
* 辅助函数：线性插值单个数值
|
||
* @param a 起始值
|
||
* @param b 目标值
|
||
* @param t 插值参数[0,1]
|
||
* @return 插值结果
|
||
*/
|
||
static double linear_interpolate(double a, double b, double t);
|
||
|
||
/**
|
||
* 辅助函数：四元数球面线性插值(slerp)
|
||
* @param q1 起始四元数
|
||
* @param q2 目标四元数
|
||
* @param t 插值参数[0,1]
|
||
* @return 插值结果
|
||
*/
|
||
static Quat slerp(const Quat& q1, const Quat& q2, double t);
|
||
|
||
/**
|
||
* 检查t的范围并进行约束
|
||
* @param t 原始参数
|
||
* @return 约束在[0,1]范围内的参数
|
||
*/
|
||
static double clamp_t(double t);
|
||
};
|
||
|
||
} // namespace cmvr::math
|
||
|
||
#endif // INTERPOLATION_BASE_H
|