Initial commit

2026-06-01 15:55:59 +08:00
commit 40f9bdb590
1799 changed files with 362227 additions and 0 deletions
--- a/inc/QuadrupedRobotSimulation/KinematicsHelper.h
+++ b/inc/QuadrupedRobotSimulation/KinematicsHelper.h
@@ -0,0 +1,144 @@
+// KinematicsHelper.h
+#ifndef KINEMATICS_HELPER_H
+#define KINEMATICS_HELPER_H
+
+#include "BaseClass.h"
+#include "KinematicsSimulation.h"
+#include "KinematicsReverse.h"
+#include "CompleteJsonExporter.h"
+#include <string>
+#include <memory>
+#include <chrono>
+#include <thread>
+
+/**
+ * @brief 工具类：提供便捷的正逆解计算方法
+ */
+class KinematicsHelper
+{
+public:
+    /**
+     * @brief 模拟机器人完整运动
+     */
+    static void SimRobot();
+
+    /**
+     * @brief 输入三个电机的值，返回各个连杆的姿态
+     * @param jsonInput JSON输入字符串（可选）
+     * @return JSON格式的所有点计算结果
+     */
+    static json QuadrupedRobot_CalculateAllPointsFromMotorAngles(const std::string &jsonInput = "");
+    /**
+     * @brief 执行正向运动学计算：计算所有轨迹
+     * @param jsonInput JSON输入字符串（可选）
+     * @return JSON格式的正向运动学计算结果
+     */
+    static json QuadrupedRobot_PerformForwardKinematics(const std::string &jsonInput = "");
+    /**
+     * @brief 单腿运动计算
+     * @param thigh_angle_deg 大腿电机角度（度）
+     * @param shank_angle_deg 小腿电机角度（度）
+     * @param ankle_angle_deg 脚踝电机角度（度）
+     * @param leg_type 腿类型："LF"、"LH"、"RF"、"RH"
+     * @param json JSON输入字符串（可选）
+     * @return JSON格式的单腿计算结果
+     */
+    static std::string QuadrupedRobot_CalculateAllPointsOnlyOneLegFromMotorAngles(
+        double thigh_angle_deg, double shank_angle_deg, double ankle_angle_deg,
+        const std::string &leg_type = "LF", const std::string &json = "");
+
+    /**
+     * @brief 获得RobotGaitDataManager机器人管理句柄
+     * @param json JSON输入字符串（可选）
+     * @return 共享指针指向RobotGaitDataManager对象
+     */
+    static std::shared_ptr<RobotGaitDataManager> RobotGaitDataManagerFromJson(const std::string &jsonInput = "", const std::string &robotID = "0");
+
+    /**
+     * @brief 从JSON字符串创建配置对象
+     * @param jsonInput JSON输入字符串（可选）
+     * @return QuadrupedRobotConfiguration对象
+     */
+    static QuadrupedRobotConfiguration CreateConfigFromJson(const std::string &jsonInput = "");
+
+    /**
+     * @brief 根据机器人的机构数据和机构类型，计算机器人四个腿的所有轨迹数据
+     * @param config 机器人配置
+     * @return JSON格式的正向运动学计算结果
+     */
+    static std::string PerformForwardKinematics(const QuadrupedRobotConfiguration &config);
+
+    /**
+     * @brief 导出完整数据为JSON字符串
+     * @param config 机器人配置
+     * @return JSON格式的完整导出数据
+     */
+    static std::string ExportCompleteData(const QuadrupedRobotConfiguration &config);
+
+    /**
+     * @brief 批量逆向计算
+     * @param motor_data 电机数据映射
+     * @param leg_type 腿类型
+     * @param max_frames 最大帧数（可选）
+     * @param jsonInput 配置JSON（可选）
+     * @return 逆向计算结果向量
+     */
+    static std::vector<ReverseCalculationResult> BatchReverseCalculation(
+        const std::map<std::string, std::vector<double>> &motor_data,
+        const std::string &leg_type = "LF",
+        std::optional<int> max_frames = std::nullopt,
+        const std::string &jsonInput = "");
+
+    /**
+     * @brief 计算并验证支撑点
+     * @param pointsDict 点字典
+     * @param groundHeight 地面高度
+     * @param tolerance 容差
+     * @param json 配置JSON（可选）
+     * @return 支撑点检查结果
+     */
+    static SupportCheckResult CheckSupportPoint(
+        const std::map<std::string, std::vector<double>> &pointsDict,
+        double groundHeight = 0.0,
+        double tolerance = 1.0,
+        const std::string &json = "");
+
+    /**
+     * @brief 生成线框播放数据
+     * @param frameRate 帧率
+     * @param modelCode 模型代码
+     * @param positions 位置数组
+     * @param quaternions 四元数数组
+     * @return P_OPERATION对象
+     */
+    static P_OPERATION MakeOperation(
+        int frameRate,
+        const std::string &modelCode,
+        const std::vector<std::vector<double>> &positions,
+        const std::vector<std::vector<double>> &quaternions);
+
+    /**
+     * @brief 等待函数（用于模拟延迟）
+     * @param milliseconds 等待的毫秒数
+     */
+    static void Wait(int milliseconds)
+    {
+        std::this_thread::sleep_for(std::chrono::milliseconds(milliseconds));
+    }
+
+private:
+    /**
+     * @brief 从JSON创建默认配置
+     * @return 默认机器人步态请求
+     */
+    static RobotGaitRequest CreateDefaultRequest();
+
+    /**
+     * @brief 加载并验证JSON数据
+     * @param json JSON字符串
+     * @return 验证后的RobotGaitRequest对象
+     */
+    static RobotGaitRequest LoadAndValidateJson(const std::string &jsonInput);
+};
+
+#endif // KINEMATICS_HELPER_H