diff --git a/extra/compensator/CMakeLists.txt b/extra/compensator/CMakeLists.txt index e58fda60..d8fba16e 100755 --- a/extra/compensator/CMakeLists.txt +++ b/extra/compensator/CMakeLists.txt @@ -1,5 +1,5 @@ rmvl_add_module( - compensator + compensator INTERFACE DEPENDS group ) @@ -26,3 +26,12 @@ rmvl_add_module( ) rmvl_generate_module_para(compensator) + +# Tests +if(BUILD_TESTS) + rmvl_add_test( + compensator Unit + DEPENDS gravity_compensator + DEPEND_TESTS GTest::gtest_main + ) +endif() diff --git a/extra/compensator/include/rmvl/compensator/compensator.h b/extra/compensator/include/rmvl/compensator/compensator.h index ffa5b44c..0d7a1b70 100755 --- a/extra/compensator/include/rmvl/compensator/compensator.h +++ b/extra/compensator/include/rmvl/compensator/compensator.h @@ -63,34 +63,7 @@ class compensator * @param[in] com_flag 手动调节补偿标志 * @return 补偿模块信息 */ - virtual CompensateInfo compensate(const std::vector &groups, uint8_t shoot_speed, - CompensateType com_flag) = 0; - - /** - * @brief 获得补偿角度 - * @note - * - 使用不动点迭代法进行补偿量的求解 \cite icra2019 - * - 需要严格满足相机对水平方向的夹角等于 `gyro_angle.y` - * - * @param[in] x 目标离相机的水平宽度 - * @param[in] y 目标离相机的铅垂高度 - * @param[in] velocity 枪口射速 - * - * @return 补偿角度 - */ - static double getPitch(double x, double y, double velocity); - -private: - /** - * @brief 计算真实的 y 坐标 - * - * @param[in] distance 相机到装甲板的水平距离 (m) - * @param[in] velocity 枪口射速 (m/s) - * @param[in] angle 枪口与水平方向的夹角 (rad) - * - * @return 世界坐标系下真实的 y 坐标 - */ - static double bulletModel(double distance, double velocity, double angle); + virtual CompensateInfo compensate(const std::vector &groups, float shoot_speed, CompensateType com_flag) = 0; }; //! @} compensator diff --git a/extra/compensator/include/rmvl/compensator/gravity_compensator.h b/extra/compensator/include/rmvl/compensator/gravity_compensator.h index 503d5d68..8d80b04f 100755 --- a/extra/compensator/include/rmvl/compensator/gravity_compensator.h +++ b/extra/compensator/include/rmvl/compensator/gravity_compensator.h @@ -9,6 +9,8 @@ * */ +#pragma once + #include "compensator.h" namespace rm @@ -21,32 +23,25 @@ namespace rm class GravityCompensator final : public compensator { public: - GravityCompensator(); + GravityCompensator() noexcept; + ~GravityCompensator(); //! 构造 GravityCompensator - static inline std::unique_ptr make_compensator() - { - return std::make_unique(); - } + static inline auto make_compensator() { return std::make_unique(); } /** - * @brief 补偿核心函数 + * @brief 补偿函数，考虑空气阻力，使用 2 阶龙格库塔方法（中点公式）计算弹道 * * @param[in] groups 所有序列组 * @param[in] shoot_speed 子弹射速 (m/s) * @param[in] com_flag 手动调节补偿标志 * @return 补偿模块信息 */ - CompensateInfo compensate(const std::vector &groups, - uint8_t shoot_speed, CompensateType com_flag) override; + CompensateInfo compensate(const std::vector &groups, float shoot_speed, CompensateType com_flag) override; private: - /** - * @brief 更新静态补偿量 - * - * @param[in] com_flag 补偿类型 - */ - void updateStaticCom(CompensateType com_flag); + class Impl; + Impl *_impl; }; //! @} compensator diff --git a/extra/compensator/include/rmvl/compensator/gyro_compensator.h b/extra/compensator/include/rmvl/compensator/gyro_compensator.h index 4689c54b..3b9056fb 100755 --- a/extra/compensator/include/rmvl/compensator/gyro_compensator.h +++ b/extra/compensator/include/rmvl/compensator/gyro_compensator.h @@ -9,6 +9,8 @@ * */ +#pragma once + #include "compensator.h" namespace rm @@ -21,32 +23,21 @@ namespace rm class GyroCompensator final : public compensator { public: + //! 创建 GyroCompensator 对象 GyroCompensator(); - //! 构造 GyroCompensator - static inline std::unique_ptr make_compensator() - { - return std::make_unique(); - } + //! 使用静态工厂函数创建 GyroCompensator 对象 + static inline auto make_compensator() { return std::make_unique(); } /** - * @brief 补偿核心函数 + * @brief 补偿函数，未考虑空气阻力，仅使用抛物线模型 \cite icra2019 * * @param[in] groups 所有序列组 * @param[in] shoot_speed 子弹射速 (m/s) * @param[in] com_flag 手动调节补偿标志 * @return 补偿模块信息 */ - CompensateInfo compensate(const std::vector &groups, - uint8_t shoot_speed, CompensateType com_flag) override; - -private: - /** - * @brief 更新静态补偿量 - * - * @param[in] com_flag 补偿类型 - */ - void updateStaticCom(CompensateType com_flag); + CompensateInfo compensate(const std::vector &groups, float shoot_speed, CompensateType com_flag) override; }; //! @} compensator diff --git a/extra/compensator/param/gravity_compensator.para b/extra/compensator/param/gravity_compensator.para index e686e8f3..ba031e9c 100644 --- a/extra/compensator/param/gravity_compensator.para +++ b/extra/compensator/param/gravity_compensator.para @@ -1,3 +1,15 @@ -float YAW_COMPENSATE = 0.f # yaw 静态补偿 (相机比测速模块高出的角度) -float PITCH_COMPENSATE = 0.f # pitch 静态补偿 (相机比测速模块高出的角度) -float MINIMUM_COM = 0.5f # 手动补偿最小步进 +#################### 物理参数 #################### +double g = 9.788 # 重力加速度，单位 m/s^2 +double m = 0.041 # 质量，单位 kg +double rho = 1.29 # 空气密度，单位 kg/m^3 +double A = 1.4186e-3 # 参考面积，单位 m^2 +double V = 4.0194e-5 # 体积，单位 m^3 +double Cd = 0.26 # 阻力系数 +double Cl = 0.2 # 升力系数 + +################## 补偿调节参数 ################## +double h = 0.02 # 龙格库塔迭代步长 +size_t MAX_COM = 100 # 最大补偿步进 +float YAW_COMPENSATE = 0 # yaw 静态补偿 (相机比测速模块高出的角度) +float PITCH_COMPENSATE = 0 # pitch 静态补偿 (相机比测速模块高出的角度) +float MINIMUM_COM = 0.5 # 手动补偿最小步进 diff --git a/extra/compensator/param/gyro_compensator.para b/extra/compensator/param/gyro_compensator.para index e686e8f3..e053771f 100644 --- a/extra/compensator/param/gyro_compensator.para +++ b/extra/compensator/param/gyro_compensator.para @@ -1,3 +1,7 @@ +#################### 物理参数 #################### +double g = 9.788 # 重力加速度，单位 m/s^2 + +################## 补偿调节参数 ################## float YAW_COMPENSATE = 0.f # yaw 静态补偿 (相机比测速模块高出的角度) float PITCH_COMPENSATE = 0.f # pitch 静态补偿 (相机比测速模块高出的角度) float MINIMUM_COM = 0.5f # 手动补偿最小步进 diff --git a/extra/compensator/src/compensator.cpp b/extra/compensator/src/compensator.cpp deleted file mode 100644 index 59fc04b9..00000000 --- a/extra/compensator/src/compensator.cpp +++ /dev/null @@ -1,36 +0,0 @@ -/** - * @file compensator.cpp - * @author RoboMaster Vision Community - * @brief 补偿模块 - * @version 1.0 - * @date 2023-07-24 - * - * @copyright Copyright 2023 (c), RoboMaster Vision Community - * - */ - -#include "rmvl/compensator/compensator.h" - -double rm::compensator::bulletModel(double distance, double velocity, double angle) -{ - // x(m), v(m/s), angle(rad) - // 忽略空气阻力影响 - double time = distance / (velocity * cos(angle)); - return velocity * sin(angle) * time - g * time * time / 2.0; -} - -double rm::compensator::getPitch(double x, double y, double velocity) -{ - double y_temp = y; - double angle = 0.f; - // 使用迭代法求得补偿角度 - for (int i = 0; i < 50; i++) - { - angle = atan2(y_temp, x); - double dy = y - bulletModel(x, velocity, angle); - y_temp += dy; - if (abs(dy) < 0.001) - break; - } - return angle; -} \ No newline at end of file diff --git a/extra/compensator/src/gravity_compensator/gravity_compensator.cpp b/extra/compensator/src/gravity_compensator/gravity_compensator.cpp index edf1744e..654d5859 100755 --- a/extra/compensator/src/gravity_compensator/gravity_compensator.cpp +++ b/extra/compensator/src/gravity_compensator/gravity_compensator.cpp @@ -9,75 +9,141 @@ * */ -#include "rmvl/compensator/gravity_compensator.h" - #include "rmvlpara/compensator/gravity_compensator.h" -using namespace rm; -using namespace para; -using namespace std; -using namespace cv; +#include "gravity_impl.h" + +namespace rm +{ -GravityCompensator::GravityCompensator() +GravityCompensator::GravityCompensator() noexcept : _impl(new Impl) { - _pitch_static_com = gravity_compensator_param.PITCH_COMPENSATE; - _yaw_static_com = gravity_compensator_param.YAW_COMPENSATE; + _pitch_static_com = para::gravity_compensator_param.PITCH_COMPENSATE; + _yaw_static_com = para::gravity_compensator_param.YAW_COMPENSATE; } -CompensateInfo GravityCompensator::compensate(const vector &groups, uint8_t shoot_speed, - CompensateType com_flag) +GravityCompensator::~GravityCompensator() { delete _impl; } + +CompensateInfo GravityCompensator::compensate(const std::vector &groups, float shoot_speed, CompensateType com_flag) { - CompensateInfo info{}; - // 补偿手动调节 - updateStaticCom(com_flag); - // 对每个序列组的每个追踪器按照一种方式进行补偿计算 - for (auto &p_group : groups) + return _impl->compensate(groups, shoot_speed, com_flag, _yaw_static_com, _pitch_static_com); +} + +GravityCompensator::Impl::Impl() +{ + using namespace para; + double tmp = gravity_compensator_param.rho * gravity_compensator_param.A * gravity_compensator_param.V / (2 * gravity_compensator_param.m); + double a22 = -tmp * gravity_compensator_param.Cd; + double a24 = -tmp * gravity_compensator_param.Cl; + double a42 = -a24, a44 = a22; + + Odes fs(4); + fs[0] = [](double, const std::vector &x) { return x[1]; }; + fs[1] = [=](double, const std::vector &x) { return a22 * x[1] + a24 * x[3]; }; + fs[2] = [](double, const std::vector &x) { return x[3]; }; + fs[3] = [=](double, const std::vector &x) { return a42 * x[1] + a44 * x[3] - para::gravity_compensator_param.g; }; + _rk = std::make_unique(fs); +} + +std::pair GravityCompensator::Impl::bulletModel(double x, double v, double angle) +{ + // 预估子弹飞行时间，并延长 15% + double t_pre{x / (v * cos(angle)) * 1.15}; + // 使用 RK2 方法计算较长一段时间的弹道轨迹 + _rk->init(0, {0, v * cos(angle), 0, v * sin(angle)}); + auto res = _rk->solve(para::gravity_compensator_param.h, static_cast(std::floor(t_pre / para::gravity_compensator_param.h))); + // 在计算出的结果中根据 res[0] 二分查找最接近 x 的解 + size_t l{res.size() >> 1}, r{res.size() - 1}; + for (int i = 0; i < 2 * log2(res.size()); i++) { - for (auto &p_tracker : p_group->data()) - { - // 单位换算 - double dis = p_tracker->getExtrinsics().distance() / 1000.; - // 提取当前陀螺仪角度 - auto gyro_angle = Point2f(p_tracker->front()->getGyroData().rotation.yaw, - p_tracker->front()->getGyroData().rotation.pitch); - // 目标与云台转轴的连线与水平方向的夹角 - double angle = gyro_angle.y + p_tracker->getRelativeAngle().y; - double gp = rad2deg(-getPitch(dis * cos(deg2rad(-angle)), // 模型中角度要求向上为正，这里需取反 - dis * sin(deg2rad(-angle)), static_cast(shoot_speed))); + if (l == r) + break; + size_t m = (l + r) >> 1; + res[m][0] < x ? (l = m + 1) : (r = m); + } + // 线性插值，获取 x 对应的落点高度 y 以及对应的子弹飞行时间 t + size_t i1 = res[l][0] < x ? l : l - 1; + size_t i2 = res[l][0] < x ? l + 1 : l; + double retval_y = res[i1][2] + (res[i2][2] - res[i1][2]) / (res[i2][0] - res[i1][0]) * (x - res[i1][0]); + double retval_t = (i1 + (x - res[i1][0]) / (res[i2][0] - res[i1][0])) * para::gravity_compensator_param.h; + return {retval_y, retval_t}; +} - double x_com = _yaw_static_com; - double y_com = gp - angle + _pitch_static_com; - double tf = dis / (static_cast(shoot_speed) * cos(deg2rad(y_com + angle))); // 子弹飞行时间计算 - // 更新 - info.compensation.emplace(p_tracker, Point2f(x_com, y_com)); - info.tof.emplace(p_tracker, tf); - } +std::pair GravityCompensator::Impl::calc(double x, double y, double velocity) +{ + double y_temp{y}; + double angle{}; + double t{}; + // 使用迭代法求得补偿角度，并获取对应的子弹飞行时间 + for (int i = 0; i < 50; i++) + { + angle = atan2(y_temp, x); + // 通过子弹模型计算落点以及子弹飞行时间 + auto [cur_y, cur_t] = bulletModel(x, velocity, angle); + t = cur_t; + double dy = y - cur_y; + y_temp += dy; + if (abs(dy) < 0.001) + break; } - return info; + return {angle, t}; } -void GravityCompensator::updateStaticCom(CompensateType com_flag) +void GravityCompensator::Impl::updateStaticCom(CompensateType com_flag, float &x_st, float &y_st) { - float com_step = gravity_compensator_param.MINIMUM_COM; + float com_step = para::gravity_compensator_param.MINIMUM_COM; switch (com_flag) { case CompensateType::UP: - _pitch_static_com += com_step; - gravity_compensator_param.PITCH_COMPENSATE += com_step; + y_st += com_step; + para::gravity_compensator_param.PITCH_COMPENSATE += com_step; break; case CompensateType::DOWN: - _pitch_static_com -= com_step; - gravity_compensator_param.PITCH_COMPENSATE -= com_step; + y_st -= com_step; + para::gravity_compensator_param.PITCH_COMPENSATE -= com_step; break; case CompensateType::LEFT: - _yaw_static_com += com_step; - gravity_compensator_param.YAW_COMPENSATE += com_step; + x_st += com_step; + para::gravity_compensator_param.YAW_COMPENSATE += com_step; break; case CompensateType::RIGHT: - _yaw_static_com -= com_step; - gravity_compensator_param.YAW_COMPENSATE -= com_step; + x_st -= com_step; + para::gravity_compensator_param.YAW_COMPENSATE -= com_step; break; default: break; } } + +CompensateInfo GravityCompensator::Impl::compensate(const std::vector &groups, float shoot_speed, + CompensateType com_flag, float &yaw_static_com, float &pitch_static_com) +{ + CompensateInfo info{}; + // 补偿手动调节 + updateStaticCom(com_flag, yaw_static_com, pitch_static_com); + // 对每个序列组的每个追踪器按照一种方式进行补偿计算 + for (auto &p_group : groups) + { + for (auto &p_tracker : p_group->data()) + { + // 单位换算 + double dis = p_tracker->getExtrinsics().distance() / 1000.; + // 提取当前陀螺仪角度 + auto gyro_angle = cv::Point2f(p_tracker->front()->getGyroData().rotation.yaw, + p_tracker->front()->getGyroData().rotation.pitch); + // 目标与云台转轴的连线与水平方向的夹角 + double angle = gyro_angle.y + p_tracker->getRelativeAngle().y; + // 计算补偿角度和对应的子弹飞行时间（模型中角度要求向上为正，这里需取反） + auto [angle_com, t_com] = calc(dis * cos(deg2rad(-angle)), dis * sin(deg2rad(-angle)), shoot_speed); + double gp = rad2deg(-angle_com); + double x_com = yaw_static_com; + double y_com = gp - angle + pitch_static_com; + // 更新 + info.compensation.emplace(p_tracker, cv::Point2f(x_com, y_com)); + info.tof.emplace(p_tracker, t_com); + } + } + return info; +} + +} // namespace rm \ No newline at end of file diff --git a/extra/compensator/src/gravity_compensator/gravity_impl.h b/extra/compensator/src/gravity_compensator/gravity_impl.h new file mode 100644 index 00000000..0608ac26 --- /dev/null +++ b/extra/compensator/src/gravity_compensator/gravity_impl.h @@ -0,0 +1,67 @@ +/** + * @file gravity_impl.h + * @author zhaoxi (535394140@qq.com) + * @brief + * @version 1.0 + * @date 2024-03-03 + * + * @copyright Copyright 2023 (c), zhaoxi + * + */ + +#pragma once + +#include "rmvl/compensator/gravity_compensator.h" + +#include "rmvl/core/numcal.hpp" + +namespace rm +{ + +class GravityCompensator::Impl +{ +public: + Impl(); + + //! 补偿函数，考虑空气阻力，使用 2 阶龙格库塔方法（中点公式）计算弹道 + CompensateInfo compensate(const std::vector &groups, float shoot_speed, + CompensateType com_flag, float &yaw_static_com, float &pitch_static_com); + +private: + /** + * @brief 弹道模型 + * + * @param[in] x 目标离相机的水平距离，单位 `m` + * @param[in] v 枪口射速，单位 `m/s` + * @param[in] angle 当前枪口仰角、俯角，`+` 表示仰角，`-` 表示俯角，单位 `rad` + * @return 弹丸飞行至水平距离`x`时，落点的垂直距离即对应的子弹飞行时间 + * @note 返回值第一项是落点高度，为正表示落点在枪口上方，为负表示落点在枪口下方，单位 `m` + */ + std::pair bulletModel(double x, double v, double angle); + + /** + * @brief 更新静态补偿 + * + * @param[in] com_flag 手动调节补偿标志 + * @param[out] x_st 水平补偿 + * @param[out] y_st 垂直补偿 + */ + void updateStaticCom(CompensateType com_flag, float &x_st, float &y_st); + + /** + * @brief 计算补偿角度以及子弹飞行时间 + * @note + * - 需要严格满足相机对水平方向的夹角等于 `gyro_angle.y` + * + * @param[in] x 目标离相机的水平宽度 + * @param[in] y 目标离相机的铅垂高度 + * @param[in] velocity 枪口射速 + * + * @return 补偿角度、子弹飞行时间的二元组 + */ + std::pair calc(double x, double y, double velocity); + + std::unique_ptr _rk; //!< 2 阶龙格库塔方法 +}; + +} // namespace rm diff --git a/extra/compensator/src/gyro_compensator/gyro_compensator.cpp b/extra/compensator/src/gyro_compensator/gyro_compensator.cpp index 02e82289..8b768917 100755 --- a/extra/compensator/src/gyro_compensator/gyro_compensator.cpp +++ b/extra/compensator/src/gyro_compensator/gyro_compensator.cpp @@ -16,23 +16,90 @@ #include "rmvlpara/camera/camera.h" #include "rmvlpara/compensator/gyro_compensator.h" -using namespace cv; -using namespace std; -using namespace para; -using namespace rm; +namespace rm +{ + +/** + * @brief 弹道模型 + * + * @param[in] x 目标离相机的水平距离，单位 `m` + * @param[in] v 枪口射速，单位 `m/s` + * @param[in] angle 当前枪口仰角、俯角，`+` 表示仰角，`-` 表示俯角，单位 `rad` + * @return 弹丸飞行至水平距离`x`时，落点的垂直距离 + * @note 返回值为正表示落点在枪口上方，为负表示落点在枪口下方，单位 `m` + */ +static double bulletModel(double x, double v, double angle) +{ + // 计算飞行 x 距离的子弹飞行时间 + double t = x / (v * cos(angle)); + // 计算子弹落点高度 + return v * sin(angle) * t - para::gyro_compensator_param.g * t * t / 2.0; +} + +static void updateStaticCom(CompensateType com_flag, float &x_st, float &y_st) +{ + float com_step = para::gyro_compensator_param.MINIMUM_COM; + switch (com_flag) + { + case CompensateType::UP: + y_st += com_step; + para::gyro_compensator_param.PITCH_COMPENSATE += com_step; + break; + case CompensateType::DOWN: + y_st -= com_step; + para::gyro_compensator_param.PITCH_COMPENSATE -= com_step; + break; + case CompensateType::LEFT: + x_st += com_step; + para::gyro_compensator_param.YAW_COMPENSATE += com_step; + break; + case CompensateType::RIGHT: + x_st -= com_step; + para::gyro_compensator_param.YAW_COMPENSATE -= com_step; + break; + default: + break; + } +} + +/** + * @brief 获得补偿角度 + * @note + * - 需要严格满足相机对水平方向的夹角等于 `gyro_angle.y` + * + * @param[in] x 目标离相机的水平宽度 + * @param[in] y 目标离相机的铅垂高度 + * @param[in] velocity 枪口射速 + * + * @return 补偿角度 + */ +static double getPitch(double x, double y, double velocity) +{ + double y_temp = y; + double angle = 0.f; + // 使用迭代法求得补偿角度 + for (int i = 0; i < 50; i++) + { + angle = atan2(y_temp, x); + double dy = y - bulletModel(x, velocity, angle); + y_temp += dy; + if (abs(dy) < 0.001) + break; + } + return angle; +} GyroCompensator::GyroCompensator() { - _pitch_static_com = gyro_compensator_param.PITCH_COMPENSATE; - _yaw_static_com = gyro_compensator_param.YAW_COMPENSATE; + _pitch_static_com = para::gyro_compensator_param.PITCH_COMPENSATE; + _yaw_static_com = para::gyro_compensator_param.YAW_COMPENSATE; } -CompensateInfo GyroCompensator::compensate(const vector &groups, - uint8_t shoot_speed, CompensateType com_flag) +CompensateInfo GyroCompensator::compensate(const std::vector &groups, float shoot_speed, CompensateType com_flag) { CompensateInfo info; // 补偿手动调节 - updateStaticCom(com_flag); + updateStaticCom(com_flag, _yaw_static_com, _pitch_static_com); // 对每个序列组的中心点计算补偿 for (auto p_group : groups) { @@ -40,11 +107,11 @@ CompensateInfo GyroCompensator::compensate(const vector &groups, if (p_gyro_group == nullptr) RMVL_Error(RMVL_BadDynamicType, "Fail to cast the type of \"p_group\" to \"GyroGroup::ptr\""); // 直线距离 - auto dis = getDistance(p_gyro_group->getCenter3D(), Vec3f{}) / 1000.; + auto dis = getDistance(p_gyro_group->getCenter3D(), cv::Vec3f{}) / 1000.; // 目标转角 - auto relative_angle = calculateRelativeAngle(camera_param.cameraMatrix, p_group->getCenter()); + auto relative_angle = calculateRelativeAngle(para::camera_param.cameraMatrix, p_group->getCenter()); // 提取当前陀螺仪角度 - auto gyro_angle = Point2f(p_gyro_group->getGyroData().rotation.yaw, + auto gyro_angle = cv::Point2f(p_gyro_group->getGyroData().rotation.yaw, p_gyro_group->getGyroData().rotation.pitch); // 目标与云台转轴的连线与水平方向的夹角 double angle = gyro_angle.y + relative_angle.y; @@ -56,35 +123,11 @@ CompensateInfo GyroCompensator::compensate(const vector &groups, // 更新至每个 tracker for (auto p_tracker : p_group->data()) { - info.compensation.emplace(p_tracker, Point2f(x_com, y_com)); + info.compensation.emplace(p_tracker, cv::Point2f(x_com, y_com)); info.tof.emplace(p_tracker, tf); } } return info; } -void GyroCompensator::updateStaticCom(CompensateType com_flag) -{ - float com_step = gyro_compensator_param.MINIMUM_COM; - switch (com_flag) - { - case CompensateType::UP: - _pitch_static_com += com_step; - gyro_compensator_param.PITCH_COMPENSATE += com_step; - break; - case CompensateType::DOWN: - _pitch_static_com -= com_step; - gyro_compensator_param.PITCH_COMPENSATE -= com_step; - break; - case CompensateType::LEFT: - _yaw_static_com += com_step; - gyro_compensator_param.YAW_COMPENSATE += com_step; - break; - case CompensateType::RIGHT: - _yaw_static_com -= com_step; - gyro_compensator_param.YAW_COMPENSATE -= com_step; - break; - default: - break; - } -} +} // namespace rm \ No newline at end of file diff --git a/extra/compensator/test/test_gravity.cpp b/extra/compensator/test/test_gravity.cpp new file mode 100644 index 00000000..99c6e17c --- /dev/null +++ b/extra/compensator/test/test_gravity.cpp @@ -0,0 +1,41 @@ +/** + * @file test_gravity.cpp + * @author zhaoxi (535394140@qq.com) + * @brief 重力补偿单元测试 + * @version 1.0 + * @date 2024-03-03 + * + * @copyright Copyright 2023 (c), zhaoxi + * + */ + +#include + +#define private public +#define protected public + +#include "../src/gravity_compensator/gravity_impl.h" + +#undef private +#undef protected + +#include "rmvl/rmath/uty_math.hpp" +#include "rmvlpara/compensator/gravity_compensator.h" + +using namespace rm::numeric_literals; + +namespace rm_test +{ + +TEST(GravityCompensator, bulletModel) +{ + rm::GravityCompensator::Impl impl; + auto [y_fric, t_fric] = impl.bulletModel(10, 16, 45_to_rad); + // 无阻力情况下 + double t = 10 / (16 * cos(rm::PI_4)); + double y = 16 * sin(rm::PI_4) * t - 0.5 * para::gravity_compensator_param.g * t * t; + EXPECT_LE(std::abs(y - y_fric), 5e-2); + EXPECT_LE(std::abs(t - t_fric), 5e-2); +} + +} // namespace rm_test diff --git a/extra/detector/test/rune_detector_test.cpp b/extra/detector/test/rune_detector_test.cpp index 8700a26b..ee7df8ed 100644 --- a/extra/detector/test/rune_detector_test.cpp +++ b/extra/detector/test/rune_detector_test.cpp @@ -19,6 +19,7 @@ using namespace std; using namespace cv; using namespace rm; +using namespace rm::numeric_literals; namespace rm_test { diff --git a/extra/group/src/gyro_group/calc.cpp b/extra/group/src/gyro_group/calc.cpp index d0d2767f..ca09f9f3 100644 --- a/extra/group/src/gyro_group/calc.cpp +++ b/extra/group/src/gyro_group/calc.cpp @@ -23,6 +23,7 @@ using namespace cv; using namespace rm; using namespace std; using namespace para; +using namespace rm::numeric_literals; int GyroGroup::calcArmorNum(const vector &ref_combos) { diff --git a/extra/group/src/gyro_group/sync.cpp b/extra/group/src/gyro_group/sync.cpp index 2bf9af5c..48f474b3 100644 --- a/extra/group/src/gyro_group/sync.cpp +++ b/extra/group/src/gyro_group/sync.cpp @@ -20,6 +20,7 @@ using namespace cv; using namespace std; using namespace para; using namespace rm; +using namespace rm::numeric_literals; void GyroGroup::sync(const GyroData &gyro_data, double tick) { diff --git a/modules/camera/include/rmvl/camera/hik_camera.h b/modules/camera/include/rmvl/camera/hik_camera.h index a32b607a..b39de445 100644 --- a/modules/camera/include/rmvl/camera/hik_camera.h +++ b/modules/camera/include/rmvl/camera/hik_camera.h @@ -58,7 +58,7 @@ class HikCamera final * @param[in] init_mode 相机初始化配置模式，需要配置 GrabMode 和 RetrieveMode * @param[in] serial 相机唯一序列号 */ - static std::unique_ptr make_capture(CameraConfig init_mode, std::string_view serial = "") { return std::unique_ptr(new HikCamera(init_mode, serial)); } + static inline std::unique_ptr make_capture(CameraConfig init_mode, std::string_view serial = "") { return std::make_unique(init_mode, serial); } /** * @brief 设置相机参数/事件 diff --git a/modules/core/include/rmvl/core/numcal.hpp b/modules/core/include/rmvl/core/numcal.hpp index 91e0b01d..7a309808 100644 --- a/modules/core/include/rmvl/core/numcal.hpp +++ b/modules/core/include/rmvl/core/numcal.hpp @@ -189,40 +189,19 @@ class NonlinearSolver ////////////// 常微分方程（组）数值解 ////////////// -//! Runge-Kutta 阶数类型 -enum class RkType -{ - Butcher, //!< 指定 `Butcher` 表的 Runge-Kutta 法 - RK2, //!< 2 阶 2 级 Runge-Kutta 方法（中点公式） - RK3, //!< 3 阶 3 级 Runge-Kutta 方法 - RK4, //!< 4 阶 4 级 Runge-Kutta 方法（经典 Runge-Kutta 公式） -}; - //! 常微分方程 using Ode = std::function &)>; //! 常微分方程组 using Odes = std::vector; /** - * @brief 常微分方程（组）数值求解器 + * @brief Butcher 表形式的常微分方程（组）数值求解器 * @brief * - 使用 Runge-Kutta 法求解常微分方程（组），算法介绍见 @ref tutorial_modules_runge_kutta * @brief - * - 包含 4 个特化模板类，分别对应 4 阶 4 级、3 阶 3 级、2 阶 2 级、Butcher 表 Runge-Kutta 求解器，其中通用求解器为 - * `rm::RungeKutta`，其他 3 个模板类均继承自该模板类，并提供了固定的 Butcher 表参数 - * - * @tparam OrderType Runge-Kutta 阶数类型 - */ -template -class RungeKutta; - -/** - * @brief Butcher 表 Runge-Kutta 求解器 - * @brief - * - 算法介绍见 @ref tutorial_modules_runge_kutta + * - 该 Runge-Kutta 数值求解器为通用求解器，提供了一般的 Runge-Kutta 法求解常微分方程（组）的接口 */ -template <> -class RungeKutta +class RungeKutta { // 加权系数，`k[i][j]`: `i` 表示第 `i` 个加权系数组，`j` 表示来自第 `j` 条方程 std::vector> _ks; @@ -252,7 +231,7 @@ class RungeKutta /** * @brief 设置常微分方程（组）的初值 * - * @param[in] t0 初始位置的自变量 \f$t\f$ + * @param[in] t0 初始位置的自变量 \f$t_0\f$ * @param[in] x0 初始位置的因变量 \f$\pmb x(t_0)\f$ */ inline void init(double t0, const std::vector &x0) { _t0 = t0, _x0 = x0; } @@ -260,7 +239,7 @@ class RungeKutta /** * @brief 设置常微分方程（组）的初值 * - * @param[in] t0 初始位置的自变量 \f$t\f$ + * @param[in] t0 初始位置的自变量 \f$t_0\f$ * @param[in] x0 初始位置的因变量 \f$\pmb x(t_0)\f$ */ inline void init(double t0, std::vector &&x0) { _t0 = t0, _x0 = std::move(x0); } @@ -287,17 +266,8 @@ class RungeKutta #endif }; -//! @} core_numcal - -RungeKutta(const Odes &, const std::vector &, const std::vector &, - const std::vector> &) -> RungeKutta; - -//! @addtogroup core_numcal -//! @{ - //! 2 阶 2 级 Runge-Kutta 求解器 -template <> -class RungeKutta : public RungeKutta +class RungeKutta2 : public RungeKutta { public: /** @@ -305,12 +275,11 @@ class RungeKutta : public RungeKutta * * @param[in] fs 常微分方程（组）\f$\pmb x'=\pmb F(t,\pmb x)\f$ 的函数对象 \f$\pmb F(t,\pmb x)\f$ */ - RungeKutta(const Odes &fs); + RungeKutta2(const Odes &fs); }; //! 3 阶 3 级 Runge-Kutta 求解器 -template <> -class RungeKutta : public RungeKutta +class RungeKutta3 : public RungeKutta { public: /** @@ -318,12 +287,11 @@ class RungeKutta : public RungeKutta * * @param[in] fs 常微分方程（组）\f$\pmb x'=\pmb F(t,\pmb x)\f$ 的函数对象 \f$\pmb F(t,\pmb x)\f$ */ - RungeKutta(const Odes &fs); + RungeKutta3(const Odes &fs); }; //! 4 阶 4 级 Runge-Kutta 求解器 -template <> -class RungeKutta : public RungeKutta +class RungeKutta4 : public RungeKutta { public: /** @@ -331,7 +299,7 @@ class RungeKutta : public RungeKutta * * @param[in] fs 常微分方程（组）\f$\pmb x'=\pmb F(t,\pmb x)\f$ 的函数对象 \f$\pmb F(t,\pmb x)\f$ */ - RungeKutta(const Odes &fs); + RungeKutta4(const Odes &fs); }; //! @} core_numcal diff --git a/modules/core/src/numcal.cpp b/modules/core/src/numcal.cpp index 681552de..0e1ed842 100644 --- a/modules/core/src/numcal.cpp +++ b/modules/core/src/numcal.cpp @@ -120,7 +120,7 @@ double NonlinearSolver::operator()(double x0, double eps, std::size_t max_iter) return xk; } -RungeKutta::RungeKutta( +RungeKutta::RungeKutta( const Odes &fs, const std::vector &p, const std::vector &lambda, const std::vector> &r) : _ks(p.size()), _fs(fs), _p(p), _lambda(lambda), _r(r) @@ -203,7 +203,7 @@ static inline void calcRK(const std::vector> &r, const std:: } } -std::vector> RungeKutta::solve(double h, std::size_t n) +std::vector> RungeKutta::solve(double h, std::size_t n) { if (_x0.empty()) RMVL_Error(RMVL_StsBadArg, "The initial value must be set."); @@ -235,22 +235,19 @@ std::generator> RungeKutta::generate(double } #endif -RungeKutta::RungeKutta(const Odes &fs) - : RungeKutta(fs, {0.0, 0.5}, {0.0, 1.0}, - {{0.0, 0.0}, - {0.5, 0.0}}) {} +RungeKutta2::RungeKutta2(const Odes &fs) : RungeKutta(fs, {0.0, 0.5}, {0.0, 1.0}, + {{0.0, 0.0}, + {0.5, 0.0}}) {} -RungeKutta::RungeKutta(const Odes &fs) - : RungeKutta(fs, {0.0, 0.5, 1.0}, {1.0 / 6.0, 2.0 / 3.0, 1.0 / 6.0}, - {{0.0, 0.0, 0.0}, - {0.5, 0.0, 0.0}, - {-1.0, 2.0, 0.0}}) {} +RungeKutta3::RungeKutta3(const Odes &fs) : RungeKutta(fs, {0.0, 0.5, 1.0}, {1.0 / 6.0, 2.0 / 3.0, 1.0 / 6.0}, + {{0.0, 0.0, 0.0}, + {0.5, 0.0, 0.0}, + {-1.0, 2.0, 0.0}}) {} -RungeKutta::RungeKutta(const Odes &fs) - : RungeKutta(fs, {0.0, 0.5, 0.5, 1.0}, {1.0 / 6.0, 1.0 / 3.0, 1.0 / 3.0, 1.0 / 6.0}, - {{0.0, 0.0, 0.0, 0.0}, - {0.5, 0.0, 0.0, 0.0}, - {0.0, 0.5, 0.0, 0.0}, - {0.0, 0.0, 1.0, 0.0}}) {} +RungeKutta4::RungeKutta4(const Odes &fs) : RungeKutta(fs, {0.0, 0.5, 0.5, 1.0}, {1.0 / 6.0, 1.0 / 3.0, 1.0 / 3.0, 1.0 / 6.0}, + {{0.0, 0.0, 0.0, 0.0}, + {0.5, 0.0, 0.0, 0.0}, + {0.0, 0.5, 0.0, 0.0}, + {0.0, 0.0, 1.0, 0.0}}) {} } // namespace rm diff --git a/modules/core/test/test_numcal.cpp b/modules/core/test/test_numcal.cpp index ab0559cf..83cc113f 100644 --- a/modules/core/test/test_numcal.cpp +++ b/modules/core/test/test_numcal.cpp @@ -61,17 +61,17 @@ TEST(NumberCalculation, runge_kutta_ode) auto resb = rkb.solve(0.01, 100).back(); EXPECT_LE(std::abs(resb.front() - std::expm1(-2)), 1e-4); - rm::RungeKutta rk2(fs); + rm::RungeKutta2 rk2(fs); rk2.init(0, {0}); auto res2 = rk2.solve(0.01, 100).back(); EXPECT_LE(std::abs(res2.front() - std::expm1(-2)), 1e-4); - rm::RungeKutta rk3(fs); + rm::RungeKutta3 rk3(fs); rk3.init(0, {0}); auto res3 = rk3.solve(0.01, 100).back(); EXPECT_LE(std::abs(res3.front() - std::expm1(-2)), 1e-5); - rm::RungeKutta rk4(fs); + rm::RungeKutta4 rk4(fs); rk4.init(0, {0}); auto res4 = rk4.solve(0.01, 100).back(); EXPECT_LE(std::abs(res4.front() - std::expm1(-2)), 1e-6); @@ -88,14 +88,14 @@ TEST(NumberCalculation, runge_kutta_odes) double real_x1 = 3.0 / 4.0 * std::exp(-2) + 2 * std::exp(-1) + 3.0 / 2.0 - 7.0 / 4.0; double real_x2 = -3.0 / 4.0 * std::exp(-2) - std::exp(-1) - 1.0 / 2.0 + 3.0 / 4.0; - rm::RungeKutta rk2(fs); + rm::RungeKutta2 rk2(fs); rk2.init(0, {1, -1}); auto res2 = rk2.solve(0.01, 100).back(); EXPECT_EQ(res2.size(), 2); EXPECT_LE(std::abs(res2[0] - real_x1), 1e-4); EXPECT_LE(std::abs(res2[1] - real_x2), 1e-4); - rm::RungeKutta rk4(fs); + rm::RungeKutta4 rk4(fs); rk4.init(0, {1, -1}); auto res4 = rk4.solve(0.01, 100).back(); EXPECT_LE(std::abs(res4[0] - real_x1), 1e-6); @@ -108,7 +108,7 @@ TEST(NumberCalculation, runge_kutta_ode_generator) auto f = [](double, const std::vector &xs) { return 1; }; // x + 1 std::vector fs = {f}; - rm::RungeKutta rk2(fs); + rm::RungeKutta2 rk2(fs); rk2.init(0, {1}); std::vector> all_res; all_res.reserve(10); diff --git a/modules/light/include/rmvl/light/opt_light_control.h b/modules/light/include/rmvl/light/opt_light_control.h index 6218eb41..1678cb5a 100644 --- a/modules/light/include/rmvl/light/opt_light_control.h +++ b/modules/light/include/rmvl/light/opt_light_control.h @@ -21,7 +21,7 @@ namespace rm //! @{ //! OPT 光源控制器 IP 配置信息 -struct DevIpConfig +struct LightIpConfig { std::string ip; //!< IP 地址 std::string subnet_mask; //!< 子网掩码 @@ -53,7 +53,7 @@ class OPTLightController * @param[in] ip_config IP 配置信息 * @return 连接是否成功建立？ */ - bool connect(const DevIpConfig &ip_config); + bool connect(const LightIpConfig &ip_config); /** * @brief 使用设备序列号创建 EtherNet 以太网连接 @@ -61,7 +61,7 @@ class OPTLightController * @param[in] SN 设备序列号 * @return 连接是否成功建立？ */ - bool connect(const std::string &SN); + bool connect(std::string_view SN); /** * @brief 断开已存在网口的连接 @@ -71,8 +71,7 @@ class OPTLightController /** * @brief 打开指定通道 * - * @param[in] channels 要打开的通道的索引组成的 `std::vector` ，范围:[1 ~ 32]（十进制格式） - * + * @param[in] channels 要打开的通道的索引组成的 `std::vector` ，范围: [1 ~ 32]（十进制格式） * @return 指定通道是否打开成功？ */ bool openChannels(const std::vector &channels); diff --git a/modules/light/src/opt_light_control/light_control.cpp b/modules/light/src/opt_light_control/light_control.cpp index 261f3a91..2dcfd96b 100644 --- a/modules/light/src/opt_light_control/light_control.cpp +++ b/modules/light/src/opt_light_control/light_control.cpp @@ -24,7 +24,7 @@ rm::OPTLightController::OPTLightController(rm::OPTLightController &&obj) obj._init = false; } -bool rm::OPTLightController::connect(const DevIpConfig &ip_config) +bool rm::OPTLightController::connect(const LightIpConfig &ip_config) { if (_init) disconnect(); @@ -32,12 +32,12 @@ bool rm::OPTLightController::connect(const DevIpConfig &ip_config) return OPTController_CreateEtheConnectionByIP(const_cast(ip_config.ip.c_str()), &_handle) == OPT_SUCCEED; } -bool rm::OPTLightController::connect(const std::string &SN) +bool rm::OPTLightController::connect(std::string_view SN) { if (_init) disconnect(); _init = true; - if (OPTController_CreateEtheConnectionBySN(const_cast(SN.c_str()), &_handle) == OPT_SUCCEED) + if (OPTController_CreateEtheConnectionBySN(const_cast(SN.data()), &_handle) == OPT_SUCCEED) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); return true; diff --git a/modules/rmath/include/rmvl/rmath/uty_math.hpp b/modules/rmath/include/rmvl/rmath/uty_math.hpp index 676dfc26..2af978f6 100644 --- a/modules/rmath/include/rmvl/rmath/uty_math.hpp +++ b/modules/rmath/include/rmvl/rmath/uty_math.hpp @@ -36,9 +36,9 @@ constexpr double FLOAT_MAX{HUGE}; #endif #if __cpp_lib_math_constants >= 201907L -constexpr double PI = std::numbers::pi; //!< 圆周率: \f$\pi\f$ -constexpr double e = std::numbers::e; //!< 自然对数底数: \f$e\f$ -constexpr double SQRT_2 = std::numbers::sqrt2; //!< 根号 2: \f$\sqrt2\f$ +constexpr double PI = ::std::numbers::pi; //!< 圆周率: \f$\pi\f$ +constexpr double e = ::std::numbers::e; //!< 自然对数底数: \f$e\f$ +constexpr double SQRT_2 = ::std::numbers::sqrt2; //!< 根号 2: \f$\sqrt2\f$ #else constexpr double PI = 3.14159265358979323; //!< 圆周率: \f$\pi\f$ constexpr double e = 2.7182818459045; //!< 自然对数底数: \f$e\f$ @@ -47,7 +47,9 @@ constexpr double SQRT_2 = 1.4142135623731; //!< 根号 2: \f$\sqrt2\f$ constexpr double PI_2 = PI / 2.; //!< PI / 2: \f$\frac\pi2\f$ constexpr double PI_4 = PI / 4.; //!< PI / 4: \f$\frac\pi4\f$ -constexpr double g = 9.788; //!< 重力加速度: \f$g\f$ + +namespace numeric_literals +{ constexpr double operator""_PI(long double num) { return num * PI; } constexpr double operator""_PI(long long unsigned num) { return num * PI; } @@ -58,6 +60,8 @@ constexpr double operator""_to_rad(long long unsigned num) { return num * PI / 1 constexpr double operator""_to_deg(long double num) { return num * 180. / PI; } constexpr double operator""_to_deg(long long unsigned num) { return num * 180. / PI; } +} // namespace numeric_literals + } // namespace rm // 定义部分 Matx