Initial commit

src/QuadrupedRobotSimulation/CompleteJsonExporter.cpp

@@ -0,0 +1,714 @@
+// CompleteJsonExporter.cpp
+#include "CompleteJsonExporter.h"
+#include "BaseClass.h"
+#include "KinematicsSimulation.h"
+#include <algorithm>
+#include <cmath>
+#include <functional>
+#include <sstream>
+#include <iomanip>
+#include <limits>
+
+// 主导出函数
+std::string CompleteJsonExporter::ExportCompleteDataJsonString(const QuadrupedRobotSimulation &simulation)
+{
+    try
+    {
+        // 创建数据对象
+        CompleteExportData exportData;
+
+        // 基本信息
+        exportData.GaitInfo = CreateGaitInfo(simulation);
+
+        // 系统参数
+        exportData.SystemParameters = CreateSystemParameters(simulation);
+
+        // 轨迹数据（所有腿）
+        exportData.TrajectoryData = CreateTrajectoryData(simulation);
+
+        // 电机数据（所有腿）
+        exportData.MotorData = CreateMotorData(simulation);
+
+        // 约束检查数据
+        exportData.ConstraintData = CreateConstraintData(simulation);
+
+        // 统计信息
+        exportData.Statistics = CreateStatistics(simulation);
+
+        // 初始角度数据
+        exportData.InitialAngles = CreateInitialAngles(simulation);
+
+        // 相位信息
+        exportData.PhaseInfo = CreatePhaseInfo(simulation);
+
+        // 运动范围统计
+        exportData.MotionRange = CreateMotionRange(simulation);
+
+        // 序列化为JSON
+        json j = exportData;
+        return j.dump(2); // 缩进2个空格
+    }
+    catch (const std::exception &ex)
+    {
+        std::cerr << "导出JSON数据时出错: " << ex.what() << std::endl;
+        throw;
+    }
+}
+
+// 创建步态信息
+GaitInfo CompleteJsonExporter::CreateGaitInfo(const QuadrupedRobotSimulation &simulation)
+{
+    GaitInfo gaitInfo;
+    gaitInfo.GaitType = simulation.gait_type;
+    gaitInfo.Period = simulation.T;
+    gaitInfo.Frequency = simulation.f;
+    gaitInfo.StepTime = simulation.step;
+    gaitInfo.SupportTime = simulation.t_s;
+    gaitInfo.SwingTime = simulation.t_w;
+    gaitInfo.SupportRatio = simulation.support_ratio;
+    gaitInfo.SwingRatio = simulation.swing_ratio;
+    gaitInfo.TotalFrames = simulation.Trajectory_L.size();
+    gaitInfo.TotalTime = simulation.Trajectory_L.size() * simulation.step;
+
+    return gaitInfo;
+}
+
+// 创建系统参数
+SystemParameters CompleteJsonExporter::CreateSystemParameters(const QuadrupedRobotSimulation &simulation)
+{
+    SystemParameters systemParams;
+
+    // 基本参数
+    systemParams.L10 = simulation.L10;
+    systemParams.L20 = simulation.L20;
+    systemParams.L30 = simulation.L30;
+
+    // 步态设计参数
+    systemParams.StepLength = simulation.s_l;
+    systemParams.StepHeight = simulation.s_h;
+    systemParams.X = simulation.X;
+    systemParams.DeltaX = simulation.deltX;
+
+    // 小腿连杆参数
+    systemParams.L21 = simulation.L21;
+    systemParams.L22 = simulation.L22;
+    systemParams.L23 = simulation.L23;
+    systemParams.Beta1 = simulation.beta1;
+    systemParams.BB2_BC_Angle = simulation.BB2_BC_angle;
+    systemParams.C1_B_Length = simulation.C1_B_length;
+
+    // 脚踝连杆参数
+    systemParams.CC1 = simulation.CC1;
+    systemParams.L31 = simulation.L31;
+    systemParams.C2C3_Length = simulation.C2C3_length;
+    systemParams.Lead = simulation.lead1;
+
+    // 脚部构型参数
+    systemParams.D1_L_Offset = simulation.D1_L_offset;
+    systemParams.D2_L_Offset = simulation.D2_L_offset;
+    systemParams.C4_D2_Offset = simulation.C4_D2_offset;
+
+    // 电机参数
+    systemParams.ThighMotorReduction = simulation.thigh_motor_reduction;
+    systemParams.ShankMotorReduction = simulation.shank_motor_reduction;
+    systemParams.AnkleMotorReduction = simulation.ankle_motor_reduction;
+
+    return systemParams;
+}
+
+// 创建轨迹数据
+TrajectoryData CompleteJsonExporter::CreateTrajectoryData(const QuadrupedRobotSimulation &simulation)
+{
+    int n_frames = simulation.Trajectory_L.size();
+
+    // 计算相位偏移帧数
+    int phase_LH = static_cast<int>(simulation.timeLH * n_frames);
+    int phase_RF = static_cast<int>(simulation.timeRF * n_frames);
+    int phase_RH = static_cast<int>(simulation.timeRH * n_frames);
+
+    TrajectoryData trajectoryData;
+    trajectoryData.TotalFrames = n_frames;
+    trajectoryData.StepTime = simulation.step;
+
+    // 为每条腿创建轨迹数据
+    std::vector<std::string> legCodes = {"LF", "LH", "RF", "RH"};
+    std::vector<int> phaseShifts = {0, phase_LH, phase_RF, phase_RH};
+
+    for (size_t i = 0; i < legCodes.size(); i++)
+    {
+        std::string legCode = legCodes[i];
+        int phaseShift = phaseShifts[i];
+        bool isRightLeg = legCode[0] == 'R'; // 检查是否以'R'开头
+
+        LegTrajectory legTraj;
+        legTraj.LegCode = legCode;
+        legTraj.PhaseShift = phaseShift;
+        legTraj.IsRightLeg = isRightLeg;
+
+        for (int frame = 0; frame < n_frames; frame++)
+        {
+            int shiftedIdx = (frame + phaseShift) % n_frames;
+
+            FrameData frameData;
+            frameData.FrameNumber = frame;
+            frameData.Time = frame * simulation.step;
+            frameData.ShiftedFrameNumber = shiftedIdx;
+
+            // 关键点坐标
+            frameData.A_Point = Point2D(simulation.A_point[0], simulation.A_point[1]);
+            frameData.B_Point = ArrayToPoint(simulation.Trajectory_B[shiftedIdx]);
+            frameData.C_Point = ArrayToPoint(simulation.Trajectory_C[shiftedIdx]);
+            frameData.L_Point = ArrayToPoint(simulation.Trajectory_L[shiftedIdx]);
+            frameData.D1_Point = ArrayToPoint(simulation.Trajectory_D1[shiftedIdx]);
+            frameData.D2_Point = ArrayToPoint(simulation.Trajectory_D2[shiftedIdx]);
+            frameData.C4_Point = ArrayToPoint(simulation.Trajectory_C4[shiftedIdx]);
+            frameData.B1_Point = ArrayToPoint(simulation.Trajectory_B1[shiftedIdx]);
+            frameData.B2_Point = ArrayToPoint(simulation.Trajectory_B2[shiftedIdx]);
+            frameData.C1_Point = ArrayToPoint(simulation.Trajectory_C1[shiftedIdx]);
+            frameData.C2_Point = ArrayToPoint(simulation.Trajectory_C2[shiftedIdx]);
+            frameData.C3_Point = ArrayToPoint(simulation.Trajectory_C3[shiftedIdx]);
+
+            // 角度数据
+            frameData.AB_Horizontal_Angle = simulation.AB_horizontal_angles[shiftedIdx];
+            frameData.AB_BC_Angle = simulation.AB_BC_angles[shiftedIdx];
+            frameData.BC_CL_Angle = simulation.BC_CL_angles[shiftedIdx];
+            frameData.AB1_AB_Angle = simulation.AB1_AB_angles[shiftedIdx];
+
+            // 距离数据
+            frameData.C3_C4_Distance = simulation.C3_C4_distances[shiftedIdx];
+            frameData.C2_C3_Distance = simulation.C2_C3_distances[shiftedIdx];
+            frameData.C2_C4_Distance = simulation.C2_C4_distances[shiftedIdx];
+            frameData.C2_C3_C4_Angle = simulation.C2_C3_C4_angles[shiftedIdx];
+
+            // 如果是右侧腿，调整大腿角度（镜像变换）
+            if (isRightLeg)
+            {
+                frameData.AB_Horizontal_Angle = 540 - frameData.AB_Horizontal_Angle;
+                // 确保角度在0-360度范围内
+                while (frameData.AB_Horizontal_Angle >= 360)
+                {
+                    frameData.AB_Horizontal_Angle -= 360;
+                }
+                while (frameData.AB_Horizontal_Angle < 0)
+                {
+                    frameData.AB_Horizontal_Angle += 360;
+                }
+            }
+
+            legTraj.Frames.push_back(frameData);
+        }
+
+        trajectoryData.Legs[legCode] = legTraj;
+    }
+
+    return trajectoryData;
+}
+
+// 创建电机数据
+MotorDataExport CompleteJsonExporter::CreateMotorData(const QuadrupedRobotSimulation &simulation)
+{
+    MotorDataExport motorData;
+    motorData.StepTime = simulation.step;
+
+    // 获取四条腿的电机数据
+    std::map<std::string, MotorData> legsData = {
+        {"LF", simulation.motor_data_LF},
+        {"LH", simulation.motor_data_LH},
+        {"RF", simulation.motor_data_RF},
+        {"RH", simulation.motor_data_RH}};
+
+    for (const auto &kvp : legsData)
+    {
+        std::string legCode = kvp.first;
+        const MotorData &data = kvp.second;
+
+        LegMotorData legMotorData;
+        legMotorData.LegCode = legCode;
+
+        int n_frames = data.frame_start_times.size();
+        for (int i = 0; i < n_frames; i++)
+        {
+            MotorFrameData frameData;
+            frameData.FrameNumber = i;
+            frameData.StartTime = data.frame_start_times[i];
+            frameData.EndTime = data.frame_end_times[i];
+
+            // 大腿电机数据
+            frameData.Thigh.Angle = data.AB_angles[i];
+            frameData.Thigh.AngleIncrement = data.AB_angle_increments[i];
+            frameData.Thigh.Speed = data.thigh_motor_speeds[i];
+
+            // 小腿电机数据
+            frameData.Shank.Angle = data.AB1_AB_angles[i];
+            frameData.Shank.AngleIncrement = data.AB1_AB_angle_increments[i];
+            frameData.Shank.Speed = data.shank_motor_speeds[i];
+
+            // 脚踝电机数据
+            frameData.Ankle.Angle = data.ankle_motor_angles[i];
+            frameData.Ankle.AngleIncrement = data.ankle_motor_angle_increments[i];
+            frameData.Ankle.Speed = data.ankle_motor_speeds[i];
+
+            // 丝杠相关数据
+            frameData.C3C4_Distance = data.C3C4_distances[i];
+            frameData.C3C4_DistanceIncrement = data.C3C4_distance_increments[i];
+
+            legMotorData.Frames.push_back(frameData);
+        }
+
+        motorData.Legs[legCode] = legMotorData;
+    }
+
+    return motorData;
+}
+
+// 创建约束检查数据
+ConstraintData CompleteJsonExporter::CreateConstraintData(const QuadrupedRobotSimulation &simulation)
+{
+    const double AB1_AB_min = 23.0;
+    const double AB1_AB_max = 130.0;
+    const double C3_C4_min = 125.0;
+    const double C3_C4_max = 205.0;
+
+    ConstraintData constraintData;
+
+    // 计算AB1-AB角度的绝对值的最大最小值
+    double ab1_ab_min_actual = std::numeric_limits<double>::max();
+    double ab1_ab_max_actual = std::numeric_limits<double>::lowest();
+    bool ab1_ab_all_valid = true;
+
+    for (double angle : simulation.AB1_AB_angles)
+    {
+        double abs_angle = std::abs(angle);
+        ab1_ab_min_actual = std::min(ab1_ab_min_actual, abs_angle);
+        ab1_ab_max_actual = std::max(ab1_ab_max_actual, abs_angle);
+
+        if (abs_angle < AB1_AB_min || abs_angle > AB1_AB_max)
+        {
+            ab1_ab_all_valid = false;
+        }
+    }
+
+    // 计算C3-C4距离的最大最小值
+    double c3c4_min_actual = std::numeric_limits<double>::max();
+    double c3c4_max_actual = std::numeric_limits<double>::lowest();
+    bool c3c4_all_valid = true;
+
+    for (double distance : simulation.C3_C4_distances)
+    {
+        c3c4_min_actual = std::min(c3c4_min_actual, distance);
+        c3c4_max_actual = std::max(c3c4_max_actual, distance);
+
+        if (distance < C3_C4_min || distance > C3_C4_max)
+        {
+            c3c4_all_valid = false;
+        }
+    }
+
+    // 计算CL距离的最大最小值
+    double cl_min_actual = std::numeric_limits<double>::max();
+    double cl_max_actual = std::numeric_limits<double>::lowest();
+    bool cl_all_valid = true;
+
+    for (double distance : simulation.CL_distances)
+    {
+        cl_min_actual = std::min(cl_min_actual, distance);
+        cl_max_actual = std::max(cl_max_actual, distance);
+
+        if (std::abs(distance - simulation.L30) >= 1e-6)
+        {
+            cl_all_valid = false;
+        }
+    }
+
+    // 计算点积的最大最小值
+    double dot_min_actual = std::numeric_limits<double>::max();
+    double dot_max_actual = std::numeric_limits<double>::lowest();
+    bool dot_all_valid = true;
+
+    for (double dot_product : simulation.dot_products)
+    {
+        dot_min_actual = std::min(dot_min_actual, dot_product);
+        dot_max_actual = std::max(dot_max_actual, dot_product);
+
+        if (std::abs(dot_product) >= 1e-6)
+        {
+            dot_all_valid = false;
+        }
+    }
+
+    // 设置约束数据
+    constraintData.AB1_AB_Constraint.MinAllowed = AB1_AB_min;
+    constraintData.AB1_AB_Constraint.MaxAllowed = AB1_AB_max;
+    constraintData.AB1_AB_Constraint.MinActual = ab1_ab_min_actual;
+    constraintData.AB1_AB_Constraint.MaxActual = ab1_ab_max_actual;
+    constraintData.AB1_AB_Constraint.IsValid = ab1_ab_all_valid;
+    constraintData.AB1_AB_Constraint.CheckType = "绝对值检查";
+
+    constraintData.C3C4_Constraint.MinAllowed = C3_C4_min;
+    constraintData.C3C4_Constraint.MaxAllowed = C3_C4_max;
+    constraintData.C3C4_Constraint.MinActual = c3c4_min_actual;
+    constraintData.C3C4_Constraint.MaxActual = c3c4_max_actual;
+    constraintData.C3C4_Constraint.IsValid = c3c4_all_valid;
+
+    constraintData.CL_Distance_Constraint.MinAllowed = simulation.L30;
+    constraintData.CL_Distance_Constraint.MaxAllowed = simulation.L30;
+    constraintData.CL_Distance_Constraint.MinActual = cl_min_actual;
+    constraintData.CL_Distance_Constraint.MaxActual = cl_max_actual;
+    constraintData.CL_Distance_Constraint.IsValid = cl_all_valid;
+
+    constraintData.Perpendicularity_Constraint.MaxDotProduct = dot_max_actual;
+    constraintData.Perpendicularity_Constraint.MinDotProduct = dot_min_actual;
+    constraintData.Perpendicularity_Constraint.IsValid = dot_all_valid;
+
+    // 收集约束违反信息
+    std::vector<ConstraintViolation> violations;
+
+    // 检查AB1-AB约束违反
+    for (size_t i = 0; i < simulation.AB1_AB_angles.size(); i++)
+    {
+        double abs_angle = std::abs(simulation.AB1_AB_angles[i]);
+        if (abs_angle < AB1_AB_min || abs_angle > AB1_AB_max)
+        {
+            ConstraintViolation violation;
+            violation.ConstraintType = "AB1_AB_Angle";
+            violation.Frame = static_cast<int>(i);
+            violation.Time = i * simulation.step;
+            violation.Value = simulation.AB1_AB_angles[i];
+            violation.AllowedRange = DoubleToString(AB1_AB_min) + "~" + DoubleToString(AB1_AB_max);
+            violation.Severity = "Error";
+
+            violations.push_back(violation);
+        }
+    }
+
+    // 检查C3-C4约束违反
+    for (size_t i = 0; i < simulation.C3_C4_distances.size(); i++)
+    {
+        double distance = simulation.C3_C4_distances[i];
+        if (distance < C3_C4_min || distance > C3_C4_max)
+        {
+            ConstraintViolation violation;
+            violation.ConstraintType = "C3C4_Distance";
+            violation.Frame = static_cast<int>(i);
+            violation.Time = i * simulation.step;
+            violation.Value = distance;
+            violation.AllowedRange = DoubleToString(C3_C4_min) + "~" + DoubleToString(C3_C4_max);
+            violation.Severity = "Error";
+
+            violations.push_back(violation);
+        }
+    }
+
+    constraintData.ConstraintViolations = violations;
+
+    return constraintData;
+}
+
+// 创建统计信息
+StatisticsData CompleteJsonExporter::CreateStatistics(const QuadrupedRobotSimulation &simulation)
+{
+    StatisticsData statistics;
+
+    // 计算角度统计
+    double ab_horizontal_min = FindMin(simulation.AB_horizontal_angles);
+    double ab_horizontal_max = FindMax(simulation.AB_horizontal_angles);
+
+    double ab_bc_min = FindMin(simulation.AB_BC_angles);
+    double ab_bc_max = FindMax(simulation.AB_BC_angles);
+
+    double bc_cl_min = FindMin(simulation.BC_CL_angles);
+    double bc_cl_max = FindMax(simulation.BC_CL_angles);
+
+    double ab1_ab_min = FindMin(simulation.AB1_AB_angles);
+    double ab1_ab_max = FindMax(simulation.AB1_AB_angles);
+
+    // 角度统计
+    statistics.AngleStatistics.AB_Horizontal.Min = ab_horizontal_min;
+    statistics.AngleStatistics.AB_Horizontal.Max = ab_horizontal_max;
+    statistics.AngleStatistics.AB_Horizontal.Range = ab_horizontal_max - ab_horizontal_min;
+
+    statistics.AngleStatistics.AB_BC.Min = ab_bc_min;
+    statistics.AngleStatistics.AB_BC.Max = ab_bc_max;
+    statistics.AngleStatistics.AB_BC.Range = ab_bc_max - ab_bc_min;
+
+    statistics.AngleStatistics.BC_CL.Min = bc_cl_min;
+    statistics.AngleStatistics.BC_CL.Max = bc_cl_max;
+    statistics.AngleStatistics.BC_CL.Range = bc_cl_max - bc_cl_min;
+
+    statistics.AngleStatistics.AB1_AB.Min = ab1_ab_min;
+    statistics.AngleStatistics.AB1_AB.Max = ab1_ab_max;
+    statistics.AngleStatistics.AB1_AB.Range = ab1_ab_max - ab1_ab_min;
+
+    // 计算距离统计
+    double c3c4_min = FindMin(simulation.C3_C4_distances);
+    double c3c4_max = FindMax(simulation.C3_C4_distances);
+
+    double c2c3_min = FindMin(simulation.C2_C3_distances);
+    double c2c3_max = FindMax(simulation.C2_C3_distances);
+
+    double c2c4_min = FindMin(simulation.C2_C4_distances);
+    double c2c4_max = FindMax(simulation.C2_C4_distances);
+
+    // 距离统计
+    statistics.DistanceStatistics.C3_C4.Min = c3c4_min;
+    statistics.DistanceStatistics.C3_C4.Max = c3c4_max;
+    statistics.DistanceStatistics.C3_C4.Range = c3c4_max - c3c4_min;
+
+    statistics.DistanceStatistics.C2_C3.Min = c2c3_min;
+    statistics.DistanceStatistics.C2_C3.Max = c2c3_max;
+    statistics.DistanceStatistics.C2_C3.Range = c2c3_max - c2c3_min;
+
+    statistics.DistanceStatistics.C2_C4.Min = c2c4_min;
+    statistics.DistanceStatistics.C2_C4.Max = c2c4_max;
+    statistics.DistanceStatistics.C2_C4.Range = c2c4_max - c2c4_min;
+
+    // 计算电机速度统计（使用左前腿的数据）
+    double thigh_min = FindMin(simulation.motor_data_LF.thigh_motor_speeds);
+    double thigh_max = FindMax(simulation.motor_data_LF.thigh_motor_speeds);
+
+    double shank_min = FindMin(simulation.motor_data_LF.shank_motor_speeds);
+    double shank_max = FindMax(simulation.motor_data_LF.shank_motor_speeds);
+
+    double ankle_min = FindMin(simulation.motor_data_LF.ankle_motor_speeds);
+    double ankle_max = FindMax(simulation.motor_data_LF.ankle_motor_speeds);
+
+    // 电机速度统计
+    statistics.MotorSpeedStatistics.Thigh.Min = thigh_min;
+    statistics.MotorSpeedStatistics.Thigh.Max = thigh_max;
+    statistics.MotorSpeedStatistics.Thigh.Range = std::abs(thigh_max - thigh_min);
+
+    statistics.MotorSpeedStatistics.Shank.Min = shank_min;
+    statistics.MotorSpeedStatistics.Shank.Max = shank_max;
+    statistics.MotorSpeedStatistics.Shank.Range = std::abs(shank_max - shank_min);
+
+    statistics.MotorSpeedStatistics.Ankle.Min = ankle_min;
+    statistics.MotorSpeedStatistics.Ankle.Max = ankle_max;
+    statistics.MotorSpeedStatistics.Ankle.Range = std::abs(ankle_max - ankle_min);
+
+    // 轨迹统计
+    statistics.TrajectoryStatistics.TotalFrames = simulation.Trajectory_L.size();
+    statistics.TrajectoryStatistics.TotalTime = simulation.Trajectory_L.size() * simulation.step;
+    statistics.TrajectoryStatistics.SwingFrames = static_cast<int>(std::round(simulation.t_w / simulation.step));
+    statistics.TrajectoryStatistics.SupportFrames = static_cast<int>(std::round(simulation.t_s / simulation.step));
+
+    return statistics;
+}
+
+// 创建初始角度数据
+InitialAnglesData CompleteJsonExporter::CreateInitialAngles(const QuadrupedRobotSimulation &simulation)
+{
+    InitialAnglesData initialAngles;
+
+    // 获取原始初始角度
+    std::map<std::string, RawInitialAngles> rawInitialAngles;
+
+    rawInitialAngles["LF"] = RawInitialAngles{
+        .Thigh = simulation.AB_horizontal_angles[0],
+        .Shank = simulation.AB1_AB_angles[0],
+        .Ankle = simulation.C3_C4_distances[0]};
+
+    // 计算相位偏移帧数
+    int n_frames = simulation.Trajectory_L.size();
+    int phase_LH = static_cast<int>(simulation.timeLH * n_frames);
+    int phase_RF = static_cast<int>(simulation.timeRF * n_frames);
+    int phase_RH = static_cast<int>(simulation.timeRH * n_frames);
+
+    // 添加其他腿的初始角度
+    rawInitialAngles["LH"] = RawInitialAngles{
+        .Thigh = simulation.AB_horizontal_angles[phase_LH],
+        .Shank = simulation.AB1_AB_angles[phase_LH],
+        .Ankle = simulation.C3_C4_distances[phase_LH]};
+
+    rawInitialAngles["RF"] = RawInitialAngles{
+        .Thigh = simulation.AB_horizontal_angles[phase_RF],
+        .Shank = simulation.AB1_AB_angles[phase_RF],
+        .Ankle = simulation.C3_C4_distances[phase_RF]};
+
+    rawInitialAngles["RH"] = RawInitialAngles{
+        .Thigh = simulation.AB_horizontal_angles[phase_RH],
+        .Shank = simulation.AB1_AB_angles[phase_RH],
+        .Ankle = simulation.C3_C4_distances[phase_RH]};
+
+    // 创建每条腿的初始角度数据
+    std::vector<std::string> legCodes = {"LF", "LH", "RF", "RH"};
+    std::vector<MotorData> motorDatas = {
+        simulation.motor_data_LF,
+        simulation.motor_data_LH,
+        simulation.motor_data_RF,
+        simulation.motor_data_RH};
+
+    for (size_t i = 0; i < legCodes.size(); i++)
+    {
+        std::string legCode = legCodes[i];
+        const MotorData &motorData = motorDatas[i];
+        const RawInitialAngles &rawAngles = rawInitialAngles[legCode];
+
+        LegInitialAngles legInitialAngles;
+        legInitialAngles.LegCode = legCode;
+
+        // 调试初始标定位置（原始值）
+        legInitialAngles.RawInitialAngles = rawAngles;
+
+        // 标定后位置（调整后的值）
+        legInitialAngles.CalibratedAngles.Thigh = motorData.AB_angles[0];
+        legInitialAngles.CalibratedAngles.Shank = motorData.AB1_AB_angles[0];
+        legInitialAngles.CalibratedAngles.Ankle = motorData.ankle_motor_angles[0];
+
+        // 调整信息
+        legInitialAngles.Adjustments.ThighAdjustment = motorData.AB_angles[0] - rawAngles.Thigh;
+        legInitialAngles.Adjustments.ShankAdjustment = motorData.AB1_AB_angles[0] - rawAngles.Shank;
+        legInitialAngles.Adjustments.AnkleAdjustment = motorData.ankle_motor_angles[0] - rawAngles.Ankle;
+
+        initialAngles.Legs[legCode] = legInitialAngles;
+    }
+
+    return initialAngles;
+}
+
+// 创建相位信息
+PhaseInfo CompleteJsonExporter::CreatePhaseInfo(const QuadrupedRobotSimulation &simulation)
+{
+    PhaseInfo phaseInfo;
+    int n_frames = simulation.Trajectory_L.size();
+
+    phaseInfo.GaitType = simulation.gait_type;
+    phaseInfo.TimeLF = simulation.timeLF;
+    phaseInfo.TimeLH = simulation.timeLH;
+    phaseInfo.TimeRF = simulation.timeRF;
+    phaseInfo.TimeRH = simulation.timeRH;
+    phaseInfo.PhaseLF = 0;
+    phaseInfo.PhaseLH = static_cast<int>(simulation.timeLH * n_frames);
+    phaseInfo.PhaseRF = static_cast<int>(simulation.timeRF * n_frames);
+    phaseInfo.PhaseRH = static_cast<int>(simulation.timeRH * n_frames);
+    phaseInfo.SupportRatio = simulation.support_ratio;
+    phaseInfo.SwingRatio = simulation.swing_ratio;
+    phaseInfo.SupportTime = simulation.t_s;
+    phaseInfo.SwingTime = simulation.t_w;
+
+    return phaseInfo;
+}
+
+// 创建运动范围数据
+MotionRangeData CompleteJsonExporter::CreateMotionRange(const QuadrupedRobotSimulation &simulation)
+{
+    MotionRangeData motionRange;
+
+    // 为每条腿计算运动范围
+    std::vector<std::string> legCodes = {"LF", "LH", "RF", "RH"};
+    std::vector<int> phaseShifts = {
+        0,
+        static_cast<int>(simulation.timeLH * simulation.Trajectory_L.size()),
+        static_cast<int>(simulation.timeRF * simulation.Trajectory_L.size()),
+        static_cast<int>(simulation.timeRH * simulation.Trajectory_L.size())};
+
+    for (size_t i = 0; i < legCodes.size(); i++)
+    {
+        std::string legCode = legCodes[i];
+        int phaseShift = phaseShifts[i];
+
+        // 计算L点运动范围
+        std::vector<double> x_values, y_values;
+        for (size_t idx = 0; idx < simulation.Trajectory_L.size(); idx++)
+        {
+            int shiftedIdx = (idx + phaseShift) % simulation.Trajectory_L.size();
+            const auto &point = simulation.Trajectory_L[shiftedIdx];
+            x_values.push_back(point[0]);
+            y_values.push_back(point[1]);
+        }
+
+        double x_min = FindMin(x_values);
+        double x_max = FindMax(x_values);
+        double y_min = FindMin(y_values);
+        double y_max = FindMax(y_values);
+
+        LegMotionRange legMotionRange;
+        legMotionRange.LegCode = legCode;
+        legMotionRange.PhaseShift = phaseShift;
+
+        legMotionRange.L_Point.X.Min = x_min;
+        legMotionRange.L_Point.X.Max = x_max;
+        legMotionRange.L_Point.X.Range = x_max - x_min;
+
+        legMotionRange.L_Point.Y.Min = y_min;
+        legMotionRange.L_Point.Y.Max = y_max;
+        legMotionRange.L_Point.Y.Range = y_max - y_min;
+
+        legMotionRange.L_Point.TotalRange = std::sqrt(
+            std::pow(x_max - x_min, 2) +
+            std::pow(y_max - y_min, 2));
+
+        motionRange.Legs[legCode] = legMotionRange;
+    }
+
+    return motionRange;
+}
+
+// 辅助函数：将数组转换为Point2D
+Point2D CompleteJsonExporter::ArrayToPoint(const std::vector<double> &array)
+{
+    if (array.size() >= 2)
+    {
+        return Point2D(array[0], array[1]);
+    }
+    return Point2D(0.0, 0.0);
+}
+
+// 模板函数：查找最小值
+template <typename T>
+T CompleteJsonExporter::FindMin(const std::vector<T> &values)
+{
+    if (values.empty())
+    {
+        return T();
+    }
+    T min_val = values[0];
+    for (const auto &val : values)
+    {
+        if (val < min_val)
+        {
+            min_val = val;
+        }
+    }
+    return min_val;
+}
+
+// 模板函数：查找最大值
+template <typename T>
+T CompleteJsonExporter::FindMax(const std::vector<T> &values)
+{
+    if (values.empty())
+    {
+        return T();
+    }
+    T max_val = values[0];
+    for (const auto &val : values)
+    {
+        if (val > max_val)
+        {
+            max_val = val;
+        }
+    }
+    return max_val;
+}
+
+// 模板函数：检查所有值是否满足条件
+template <typename T>
+bool CompleteJsonExporter::All(const std::vector<T> &values, std::function<bool(const T &)> predicate)
+{
+    for (const auto &val : values)
+    {
+        if (!predicate(val))
+        {
+            return false;
+        }
+    }
+    return true;
+}
+
+// 辅助函数：将double转换为字符串
+std::string CompleteJsonExporter::DoubleToString(double value, int precision)
+{
+    std::ostringstream oss;
+    oss << std::fixed << std::setprecision(precision) << value;
+    return oss.str();
+}

src/QuadrupedRobotSimulation/KinematicsHelper.cpp

@@ -0,0 +1,320 @@
+// KinematicsHelper.cpp
+#include "KinematicsHelper.h"
+#include "RobotConfig.hpp"
+#include <iostream>
+#include <sstream>
+#include <iomanip>
+
+// 声明网络发送函数（需要根据实际网络库实现）
+extern void SendMsg(const std::string &msg);
+static std::unordered_map<std::string, std::shared_ptr<RobotGaitDataManager>> instanceMap;
+
+void KinematicsHelper::SimRobot()
+{
+    double thigh_angle_deg = 0;
+    double shank_angle_deg = 0;
+    double ankle_angle_deg = 0;
+    std::string leg_type = "LF";
+
+    auto manager = KinematicsHelper::RobotGaitDataManagerFromJson();
+    auto simulation = std::make_shared<ReverseKinematicsCalculator>(manager);
+
+    nlohmann::json jsonData = KinematicsHelper::QuadrupedRobot_PerformForwardKinematics();
+
+    try
+    {
+        // 使用不同的变量名避免冲突
+        CompleteExportData exportData = jsonData.get<CompleteExportData>();
+
+        int pointCount = exportData.MotorData.Legs.at("LH").Frames.size();
+
+        for (int i = 0; i < pointCount; i++)
+        {
+            // 左前腿
+            leg_type = "LF";
+            thigh_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Thigh.Angle;
+            shank_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Shank.Angle;
+            ankle_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Ankle.Angle;
+
+            std::string c_Frames = KinematicsHelper::QuadrupedRobot_CalculateAllPointsOnlyOneLegFromMotorAngles(
+                thigh_angle_deg, shank_angle_deg, ankle_angle_deg, leg_type);
+
+            // 发送消息（需要实现SendMsg函数）
+            // SendMsg(PP.Prefix + PP.ObjectHeader + Cmd_Protocol.Frames + c_Frames);
+
+            // 左后腿
+            leg_type = "LH";
+            thigh_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Thigh.Angle;
+            shank_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Shank.Angle;
+            ankle_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Ankle.Angle;
+
+            c_Frames = KinematicsHelper::QuadrupedRobot_CalculateAllPointsOnlyOneLegFromMotorAngles(
+                thigh_angle_deg, shank_angle_deg, ankle_angle_deg, leg_type);
+            // SendMsg(PP.Prefix + PP.ObjectHeader + Cmd_Protocol.Frames + c_Frames);
+
+            // 右前腿
+            leg_type = "RF";
+            thigh_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Thigh.Angle;
+            shank_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Shank.Angle;
+            ankle_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Ankle.Angle;
+
+            c_Frames = KinematicsHelper::QuadrupedRobot_CalculateAllPointsOnlyOneLegFromMotorAngles(
+                thigh_angle_deg, shank_angle_deg, ankle_angle_deg, leg_type);
+            // SendMsg(PP.Prefix + PP.ObjectHeader + Cmd_Protocol.Frames + c_Frames);
+
+            // 右后腿
+            leg_type = "RH";
+            thigh_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Thigh.Angle;
+            shank_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Shank.Angle;
+            ankle_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Ankle.Angle;
+
+            c_Frames = KinematicsHelper::QuadrupedRobot_CalculateAllPointsOnlyOneLegFromMotorAngles(
+                thigh_angle_deg, shank_angle_deg, ankle_angle_deg, leg_type);
+            // SendMsg(PP.Prefix + PP.ObjectHeader + Cmd_Protocol.Frames + c_Frames);
+
+            // 等待200毫秒
+            Wait(200);
+        }
+    }
+    catch (const std::exception &e)
+    {
+        std::cerr << "Error in SimRobot: " << e.what() << std::endl;
+    }
+}
+
+json KinematicsHelper::QuadrupedRobot_CalculateAllPointsFromMotorAngles(const std::string &jsonInput)
+{
+
+    std::cout << "[DEBUG] json KinematicsHelper::QuadrupedRobot_CalculateAllPointsFromMotorAngles(const std::string &jsonInput) " << std::endl;
+
+    std::string jsonStr = "{}";
+    auto manager = RobotGaitDataManagerFromJson(jsonInput);
+    ReverseKinematicsCalculator simulation(manager);
+    jsonStr = simulation.CalculateAllPointsFromMotorAnglesJsonStr();
+    json jsonObj = json::parse(jsonStr);
+    return jsonObj;
+}
+json KinematicsHelper::QuadrupedRobot_PerformForwardKinematics(const std::string &jsonInput)
+{
+
+    std::cout << "[DEBUG] json KinematicsHelper::QuadrupedRobot_PerformForwardKinematics(const std::string &jsonInput) " << std::endl;
+
+    auto manager = RobotGaitDataManagerFromJson(jsonInput);
+    QuadrupedRobotConfiguration config(manager->GetGaitInfo(), manager->GetSystemParameters());
+    QuadrupedRobotSimulation simulation(config);
+    std::string jsonStr = simulation.CalculateAllTrajectoriesJsonString();
+    json jsonObj = json::parse(jsonStr);
+    return jsonObj;
+}
+std::string KinematicsHelper::QuadrupedRobot_CalculateAllPointsOnlyOneLegFromMotorAngles(
+    double thigh_angle_deg, double shank_angle_deg, double ankle_angle_deg,
+    const std::string &leg_type, const std::string &jsonInput)
+{
+    auto manager = RobotGaitDataManagerFromJson(jsonInput);
+    ReverseKinematicsCalculator simulation(manager);
+    std::string c_Frames_jsonStr = simulation.CalculateAllPointsOnlyOneLegFromMotorAnglesJsonStr(
+        thigh_angle_deg, shank_angle_deg, ankle_angle_deg, leg_type);
+    return c_Frames_jsonStr;
+}
+
+std::shared_ptr<RobotGaitDataManager> KinematicsHelper::RobotGaitDataManagerFromJson(const std::string &jsonInput, const std::string &robotID)
+{
+
+    std::string RobotID;
+    RobotID = robotID;
+    // 尝试解析JSON
+    nlohmann::json j;
+    j = nlohmann::json::parse(jsonInput);
+
+        if (j.contains("RobotID"))
+    {
+        RobotID = j.at("RobotID").get<std::string>();
+    }
+    auto it = instanceMap.find(RobotID);
+    if (it != instanceMap.end())
+    {
+        // 已存在：重新加载默认数据
+        auto manager = it->second;
+        if (jsonInput.empty())
+        {
+            manager->LoadDefaultData();
+        }
+        else
+        {
+            manager->LoadFromJson(j);
+        }
+        return manager;
+    }
+    else
+    {
+
+        // 不存在：创建新的并加载默认数据
+        auto manager = std::make_shared<RobotGaitDataManager>();
+
+        manager->LoadDefaultData();
+
+        manager->LoadFromJson(jsonInput);
+
+        instanceMap[robotID] = manager;
+        return manager;
+    }
+    auto manager = std::make_shared<RobotGaitDataManager>();
+
+    return manager;
+}
+
+QuadrupedRobotConfiguration KinematicsHelper::CreateConfigFromJson(const std::string &jsonInput)
+{
+    auto manager = RobotGaitDataManagerFromJson(jsonInput);
+    return QuadrupedRobotConfiguration(manager->GetGaitInfo(), manager->GetSystemParameters());
+}
+
+std::string KinematicsHelper::PerformForwardKinematics(const QuadrupedRobotConfiguration &config)
+{
+    QuadrupedRobotSimulation simulation(config);
+    std::string jsonStr = simulation.CalculateAllTrajectories_JsonStr();
+    return jsonStr;
+}
+
+std::string KinematicsHelper::ExportCompleteData(const QuadrupedRobotConfiguration &config)
+{
+    QuadrupedRobotSimulation simulation(config);
+    return CompleteJsonExporter::ExportCompleteDataJsonString(simulation);
+}
+
+std::vector<ReverseCalculationResult> KinematicsHelper::BatchReverseCalculation(
+    const std::map<std::string, std::vector<double>> &motor_data,
+    const std::string &leg_type,
+    std::optional<int> max_frames,
+    const std::string &jsonInput)
+{
+    auto manager = RobotGaitDataManagerFromJson(jsonInput);
+    ReverseKinematicsCalculator calculator(manager);
+    return calculator.BatchReverseCalculation(motor_data, leg_type, max_frames);
+}
+
+SupportCheckResult KinematicsHelper::CheckSupportPoint(
+    const std::map<std::string, std::vector<double>> &pointsDict,
+    double groundHeight,
+    double tolerance,
+    const std::string &jsonInput)
+{
+    auto manager = RobotGaitDataManagerFromJson(jsonInput);
+    ReverseKinematicsCalculator calculator(manager);
+
+    // 注意：原C#代码中IsSupportPoint返回bool，但BaseClass.h中有SupportCheckResult结构
+    // 这里需要根据实际实现调整
+    bool isSupport = calculator.IsSupportPoint(pointsDict, groundHeight, tolerance);
+
+    SupportCheckResult result;
+    result.IsSupport = isSupport;
+    // 这里可以添加更多的检查结果填充逻辑
+
+    return result;
+}
+
+P_OPERATION KinematicsHelper::MakeOperation(
+    int frameRate,
+    const std::string &modelCode,
+    const std::vector<std::vector<double>> &positions,
+    const std::vector<std::vector<double>> &quaternions)
+{
+    return P_OPERATION_Func::MakeOperation(frameRate, modelCode, positions, quaternions);
+}
+
+RobotGaitRequest KinematicsHelper::CreateDefaultRequest()
+{
+    RobotGaitRequest request;
+
+    // 设置默认步态信息
+    request.req_param.GaitInfo.GaitType = "walk";
+    request.req_param.GaitInfo.Period = 2.0;
+    request.req_param.GaitInfo.Frequency = 50.0;
+    request.req_param.GaitInfo.StepTime = 0.02;
+    request.req_param.GaitInfo.SupportTime = 1.0;
+    request.req_param.GaitInfo.SwingTime = 1.0;
+    request.req_param.GaitInfo.SupportRatio = 0.5;
+    request.req_param.GaitInfo.SwingRatio = 0.5;
+    request.req_param.GaitInfo.TotalFrames = 100;
+    request.req_param.GaitInfo.TotalTime = 2.0;
+
+    // 设置默认系统参数
+    request.req_param.SystemParameters.A_x = 0.0;
+    request.req_param.SystemParameters.A_y = 0.0;
+    request.req_param.SystemParameters.L10 = 100.0;
+    request.req_param.SystemParameters.L20 = 100.0;
+    request.req_param.SystemParameters.L30 = 50.0;
+    request.req_param.SystemParameters.StepLength = 200.0;
+    request.req_param.SystemParameters.StepHeight = 50.0;
+    request.req_param.SystemParameters.X = 50.0;
+    request.req_param.SystemParameters.DeltaX = 0.0;
+    request.req_param.SystemParameters.L21 = 80.0;
+    request.req_param.SystemParameters.L22 = 60.0;
+    request.req_param.SystemParameters.L23 = 70.0;
+    request.req_param.SystemParameters.Beta1 = 30.0;
+    request.req_param.SystemParameters.BB2_BC_Angle = 30.0;
+    request.req_param.SystemParameters.C1_B_Length = 50.0;
+    request.req_param.SystemParameters.CC1 = 40.0;
+    request.req_param.SystemParameters.L31 = 60.0;
+    request.req_param.SystemParameters.C2C3_Length = 50.0;
+    request.req_param.SystemParameters.Lead = 5.0;
+    request.req_param.SystemParameters.D1_L_Offset = 20.0;
+    request.req_param.SystemParameters.D2_L_Offset = 20.0;
+    request.req_param.SystemParameters.C4_D2_Offset = 15.0;
+    request.req_param.SystemParameters.ThighMotorReduction = 10.0;
+    request.req_param.SystemParameters.ShankMotorReduction = 10.0;
+    request.req_param.SystemParameters.AnkleMotorReduction = 10.0;
+
+    // 设置默认机器人身体位置
+    request.req_param.RobotBody.BodyCode = "Body";
+    request.req_param.RobotBody.x = 0.0;
+    request.req_param.RobotBody.y = 0.0;
+    request.req_param.RobotBody.z = 300.0;
+    request.req_param.RobotBody.qx = 0.0;
+    request.req_param.RobotBody.qy = 0.0;
+    request.req_param.RobotBody.qz = 0.0;
+    request.req_param.RobotBody.qw = 1.0;
+
+    // 设置默认模型ID
+    // 这里可以根据需要添加默认模型ID
+
+    // 设置默认参数
+    request.req_param.Param.LF.thigh_angle_deg = -49.1033472630546;
+    request.req_param.Param.LF.shank_angle_deg = 7.50678016014155;
+    request.req_param.Param.LF.ankle_angle_deg = -183.115048990549;
+
+    request.req_param.Param.LH.thigh_angle_deg = -38.4042396745178;
+    request.req_param.Param.LH.shank_angle_deg = -9.70116099369837;
+    request.req_param.Param.LH.ankle_angle_deg = 107.000051175049;
+
+    request.req_param.Param.RF.thigh_angle_deg = 38.4042396745178;
+    request.req_param.Param.RF.shank_angle_deg = 9.70116099369837;
+    request.req_param.Param.RF.ankle_angle_deg = 107.000051175049;
+
+    request.req_param.Param.RH.thigh_angle_deg = 49.1033472630546;
+    request.req_param.Param.RH.shank_angle_deg = -7.50678016014155;
+    request.req_param.Param.RH.ankle_angle_deg = -183.115048990549;
+
+    return request;
+}
+
+RobotGaitRequest KinematicsHelper::LoadAndValidateJson(const std::string &jsonInput)
+{
+    if (jsonInput.empty())
+    {
+        return CreateDefaultRequest();
+    }
+
+    try
+    {
+
+        RobotGaitRequest request;
+        request.req_param = RequestParameters::fromJsonString(jsonInput);
+        return request;
+    }
+    catch (const std::exception &e)
+    {
+        std::cerr << "Error parsing JSON: " << e.what() << std::endl;
+        return CreateDefaultRequest();
+    }
+}

src/QuadrupedRobotSimulation/RobotConfig.cpp

@@ -0,0 +1,846 @@
+#include "RobotConfig.hpp"
+// 统一的更新函数：手动从JSON更新现有对象
+RequestParameters &RequestParameters::updateFromJson(const nlohmann::json &j, RequestParameters &req_param)
+{
+    if (j.is_null() || !j.is_object())
+        return req_param; // 修正：返回传入的引用
+
+    try
+    {
+        // 更新 GaitInfo
+        if (j.contains("GaitInfo"))
+        {
+            const auto &gaitJson = j.at("GaitInfo");
+            if (gaitJson.is_object())
+            {
+                if (gaitJson.contains("GaitType"))
+                    req_param.GaitInfo.GaitType = gaitJson.at("GaitType").get<std::string>();
+                if (gaitJson.contains("Period"))
+                    req_param.GaitInfo.Period = gaitJson.at("Period").get<double>();
+                if (gaitJson.contains("Frequency"))
+                    req_param.GaitInfo.Frequency = gaitJson.at("Frequency").get<double>();
+                if (gaitJson.contains("StepTime"))
+                    req_param.GaitInfo.StepTime = gaitJson.at("StepTime").get<double>();
+                if (gaitJson.contains("SupportTime"))
+                    req_param.GaitInfo.SupportTime = gaitJson.at("SupportTime").get<double>();
+                if (gaitJson.contains("SwingTime"))
+                    req_param.GaitInfo.SwingTime = gaitJson.at("SwingTime").get<double>();
+                if (gaitJson.contains("SupportRatio"))
+                    req_param.GaitInfo.SupportRatio = gaitJson.at("SupportRatio").get<double>();
+                if (gaitJson.contains("SwingRatio"))
+                    req_param.GaitInfo.SwingRatio = gaitJson.at("SwingRatio").get<double>();
+                if (gaitJson.contains("TotalFrames"))
+                    req_param.GaitInfo.TotalFrames = gaitJson.at("TotalFrames").get<int>();
+                if (gaitJson.contains("TotalTime"))
+                    req_param.GaitInfo.TotalTime = gaitJson.at("TotalTime").get<double>();
+            }
+        }
+
+        // 更新 SystemParameters
+        if (j.contains("SystemParameters"))
+        {
+            const auto &sysJson = j.at("SystemParameters");
+            if (sysJson.is_object())
+            {
+                if (sysJson.contains("A_x"))
+                    req_param.SystemParameters.A_x = sysJson.at("A_x").get<double>();
+                if (sysJson.contains("A_y"))
+                    req_param.SystemParameters.A_y = sysJson.at("A_y").get<double>();
+                if (sysJson.contains("L10"))
+                    req_param.SystemParameters.L10 = sysJson.at("L10").get<double>();
+                if (sysJson.contains("L20"))
+                    req_param.SystemParameters.L20 = sysJson.at("L20").get<double>();
+                if (sysJson.contains("L30"))
+                    req_param.SystemParameters.L30 = sysJson.at("L30").get<double>();
+                if (sysJson.contains("StepLength"))
+                    req_param.SystemParameters.StepLength = sysJson.at("StepLength").get<double>();
+                if (sysJson.contains("StepHeight"))
+                    req_param.SystemParameters.StepHeight = sysJson.at("StepHeight").get<double>();
+                if (sysJson.contains("X"))
+                    req_param.SystemParameters.X = sysJson.at("X").get<double>();
+                if (sysJson.contains("DeltaX"))
+                    req_param.SystemParameters.DeltaX = sysJson.at("DeltaX").get<double>();
+                if (sysJson.contains("L21"))
+                    req_param.SystemParameters.L21 = sysJson.at("L21").get<double>();
+                if (sysJson.contains("L22"))
+                    req_param.SystemParameters.L22 = sysJson.at("L22").get<double>();
+                if (sysJson.contains("L23"))
+                    req_param.SystemParameters.L23 = sysJson.at("L23").get<double>();
+                if (sysJson.contains("Beta1"))
+                    req_param.SystemParameters.Beta1 = sysJson.at("Beta1").get<double>();
+                if (sysJson.contains("BB2_BC_Angle"))
+                    req_param.SystemParameters.BB2_BC_Angle = sysJson.at("BB2_BC_Angle").get<double>();
+                if (sysJson.contains("C1_B_Length"))
+                    req_param.SystemParameters.C1_B_Length = sysJson.at("C1_B_Length").get<double>();
+                if (sysJson.contains("CC1"))
+                    req_param.SystemParameters.CC1 = sysJson.at("CC1").get<double>();
+                if (sysJson.contains("L31"))
+                    req_param.SystemParameters.L31 = sysJson.at("L31").get<double>();
+                if (sysJson.contains("C2C3_Length"))
+                    req_param.SystemParameters.C2C3_Length = sysJson.at("C2C3_Length").get<double>();
+                if (sysJson.contains("Lead"))
+                    req_param.SystemParameters.Lead = sysJson.at("Lead").get<double>();
+                if (sysJson.contains("D1_L_Offset"))
+                    req_param.SystemParameters.D1_L_Offset = sysJson.at("D1_L_Offset").get<double>();
+                if (sysJson.contains("D2_L_Offset"))
+                    req_param.SystemParameters.D2_L_Offset = sysJson.at("D2_L_Offset").get<double>();
+                if (sysJson.contains("C4_D2_Offset"))
+                    req_param.SystemParameters.C4_D2_Offset = sysJson.at("C4_D2_Offset").get<double>();
+                if (sysJson.contains("ThighMotorReduction"))
+                    req_param.SystemParameters.ThighMotorReduction = sysJson.at("ThighMotorReduction").get<double>();
+                if (sysJson.contains("ShankMotorReduction"))
+                    req_param.SystemParameters.ShankMotorReduction = sysJson.at("ShankMotorReduction").get<double>();
+                if (sysJson.contains("AnkleMotorReduction"))
+                    req_param.SystemParameters.AnkleMotorReduction = sysJson.at("AnkleMotorReduction").get<double>();
+            }
+        }
+
+        // 更新 ModelID
+        if (j.contains("ModelID"))
+        {
+            const auto &modelJson = j.at("ModelID");
+            if (modelJson.is_object())
+            {
+                // 更新 LF
+                if (modelJson.contains("LF"))
+                {
+                    const auto &lfJson = modelJson.at("LF");
+                    if (lfJson.is_array())
+                    {
+                        req_param.ModelID.LF.components.clear();
+                        for (const auto &item : lfJson)
+                        {
+                            if (item.is_object())
+                            {
+                                ComponentID comp;
+                                auto it = item.begin();
+                                if (it != item.end())
+                                {
+                                    comp.name = it.key();
+                                    comp.uuid = it.value().get<std::string>();
+                                    req_param.ModelID.LF.components.push_back(comp);
+                                }
+                            }
+                        }
+                    }
+                }
+
+                // 更新 LH
+                if (modelJson.contains("LH"))
+                {
+                    const auto &lhJson = modelJson.at("LH");
+                    if (lhJson.is_array())
+                    {
+                        req_param.ModelID.LH.components.clear();
+                        for (const auto &item : lhJson)
+                        {
+                            if (item.is_object())
+                            {
+                                ComponentID comp;
+                                auto it = item.begin();
+                                if (it != item.end())
+                                {
+                                    comp.name = it.key();
+                                    comp.uuid = it.value().get<std::string>();
+                                    req_param.ModelID.LH.components.push_back(comp);
+                                }
+                            }
+                        }
+                    }
+                }
+
+                // 更新 RF
+                if (modelJson.contains("RF"))
+                {
+                    const auto &rfJson = modelJson.at("RF");
+                    if (rfJson.is_array())
+                    {
+                        req_param.ModelID.RF.components.clear();
+                        for (const auto &item : rfJson)
+                        {
+                            if (item.is_object())
+                            {
+                                ComponentID comp;
+                                auto it = item.begin();
+                                if (it != item.end())
+                                {
+                                    comp.name = it.key();
+                                    comp.uuid = it.value().get<std::string>();
+                                    req_param.ModelID.RF.components.push_back(comp);
+                                }
+                            }
+                        }
+                    }
+                }
+
+                // 更新 RH
+                if (modelJson.contains("RH"))
+                {
+                    const auto &rhJson = modelJson.at("RH");
+                    if (rhJson.is_array())
+                    {
+                        req_param.ModelID.RH.components.clear();
+                        for (const auto &item : rhJson)
+                        {
+                            if (item.is_object())
+                            {
+                                ComponentID comp;
+                                auto it = item.begin();
+                                if (it != item.end())
+                                {
+                                    comp.name = it.key();
+                                    comp.uuid = it.value().get<std::string>();
+                                    req_param.ModelID.RH.components.push_back(comp);
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+        }
+
+        // 更新 Param (LegParameters)
+        if (j.contains("Param"))
+        {
+            const auto &paramJson = j.at("Param");
+            if (paramJson.is_object())
+            {
+                // 更新 LF
+                if (paramJson.contains("LF"))
+                {
+                    const auto &lfJson = paramJson.at("LF");
+                    if (lfJson.is_object())
+                    {
+                        if (lfJson.contains("thigh_angle_deg"))
+                            req_param.Param.LF.thigh_angle_deg = lfJson.at("thigh_angle_deg").get<double>();
+                        if (lfJson.contains("shank_angle_deg"))
+                            req_param.Param.LF.shank_angle_deg = lfJson.at("shank_angle_deg").get<double>();
+                        if (lfJson.contains("ankle_angle_deg"))
+                            req_param.Param.LF.ankle_angle_deg = lfJson.at("ankle_angle_deg").get<double>();
+                    }
+                }
+
+                // 更新 LH
+                if (paramJson.contains("LH"))
+                {
+                    const auto &lhJson = paramJson.at("LH");
+                    if (lhJson.is_object())
+                    {
+                        if (lhJson.contains("thigh_angle_deg"))
+                            req_param.Param.LH.thigh_angle_deg = lhJson.at("thigh_angle_deg").get<double>();
+                        if (lhJson.contains("shank_angle_deg"))
+                            req_param.Param.LH.shank_angle_deg = lhJson.at("shank_angle_deg").get<double>();
+                        if (lhJson.contains("ankle_angle_deg"))
+                            req_param.Param.LH.ankle_angle_deg = lhJson.at("ankle_angle_deg").get<double>();
+                    }
+                }
+
+                // 更新 RF
+                if (paramJson.contains("RF"))
+                {
+                    const auto &rfJson = paramJson.at("RF");
+                    if (rfJson.is_object())
+                    {
+                        if (rfJson.contains("thigh_angle_deg"))
+                            req_param.Param.RF.thigh_angle_deg = rfJson.at("thigh_angle_deg").get<double>();
+                        if (rfJson.contains("shank_angle_deg"))
+                            req_param.Param.RF.shank_angle_deg = rfJson.at("shank_angle_deg").get<double>();
+                        if (rfJson.contains("ankle_angle_deg"))
+                            req_param.Param.RF.ankle_angle_deg = rfJson.at("ankle_angle_deg").get<double>();
+                    }
+                }
+
+                // 更新 RH
+                if (paramJson.contains("RH"))
+                {
+                    const auto &rhJson = paramJson.at("RH");
+                    if (rhJson.is_object())
+                    {
+                        if (rhJson.contains("thigh_angle_deg"))
+                            req_param.Param.RH.thigh_angle_deg = rhJson.at("thigh_angle_deg").get<double>();
+                        if (rhJson.contains("shank_angle_deg"))
+                            req_param.Param.RH.shank_angle_deg = rhJson.at("shank_angle_deg").get<double>();
+                        if (rhJson.contains("ankle_angle_deg"))
+                            req_param.Param.RH.ankle_angle_deg = rhJson.at("ankle_angle_deg").get<double>();
+                    }
+                }
+            }
+        }
+
+        // 更新 RobotBody
+        if (j.contains("RobotBody"))
+        {
+            const auto &bodyJson = j.at("RobotBody");
+            if (bodyJson.is_object())
+            {
+                if (bodyJson.contains("BodyCode"))
+                    req_param.RobotBody.BodyCode = bodyJson.at("BodyCode").get<std::string>();
+                if (bodyJson.contains("x"))
+                    req_param.RobotBody.x = bodyJson.at("x").get<double>();
+                if (bodyJson.contains("y"))
+                    req_param.RobotBody.y = bodyJson.at("y").get<double>();
+                if (bodyJson.contains("z"))
+                    req_param.RobotBody.z = bodyJson.at("z").get<double>();
+                if (bodyJson.contains("qx"))
+                    req_param.RobotBody.qx = bodyJson.at("qx").get<double>();
+                if (bodyJson.contains("qy"))
+                    req_param.RobotBody.qy = bodyJson.at("qy").get<double>();
+                if (bodyJson.contains("qz"))
+                    req_param.RobotBody.qz = bodyJson.at("qz").get<double>();
+                if (bodyJson.contains("qw"))
+                    req_param.RobotBody.qw = bodyJson.at("qw").get<double>();
+            }
+        }
+
+        // 更新 t_percentage
+        if (j.contains("t_percentage"))
+        {
+            req_param.t_percentage = j.at("t_percentage").get<double>();
+        }
+        if (j.contains("RobotID"))
+        {
+            req_param.RobotID = j.at("RobotID").get<std::string>();
+        }
+
+        return req_param; // 返回引用
+    }
+    catch (const nlohmann::json::exception &e)
+    {
+        std::cerr << "[ERROR] 更新RequestParameters失败: " << e.what() << std::endl;
+        std::cerr << "[ERROR] 当前JSON: " << j.dump(2) << std::endl;
+        throw;
+    }
+}
+// GaitInfo 序列化/反序列化实现
+void to_json(nlohmann::json &j, const GaitInfo &g)
+{
+    j = nlohmann::json{
+        {"GaitType", g.GaitType},
+        {"Period", g.Period},
+        {"Frequency", g.Frequency},
+        {"StepTime", g.StepTime},
+        {"SupportTime", g.SupportTime},
+        {"SwingTime", g.SwingTime},
+        {"SupportRatio", g.SupportRatio},
+        {"SwingRatio", g.SwingRatio},
+        {"TotalFrames", g.TotalFrames},
+        {"TotalTime", g.TotalTime}};
+}
+
+void from_json(const nlohmann::json &j, GaitInfo &g)
+{
+    if (j.is_null() || !j.is_object())
+        return;
+
+    if (j.contains("GaitType"))
+        j.at("GaitType").get_to(g.GaitType);
+    if (j.contains("Period"))
+        j.at("Period").get_to(g.Period);
+    if (j.contains("Frequency"))
+        j.at("Frequency").get_to(g.Frequency);
+    if (j.contains("StepTime"))
+        j.at("StepTime").get_to(g.StepTime);
+    if (j.contains("SupportTime"))
+        j.at("SupportTime").get_to(g.SupportTime);
+    if (j.contains("SwingTime"))
+        j.at("SwingTime").get_to(g.SwingTime);
+    if (j.contains("SupportRatio"))
+        j.at("SupportRatio").get_to(g.SupportRatio);
+    if (j.contains("SwingRatio"))
+        j.at("SwingRatio").get_to(g.SwingRatio);
+    if (j.contains("TotalFrames"))
+        j.at("TotalFrames").get_to(g.TotalFrames);
+    if (j.contains("TotalTime"))
+        j.at("TotalTime").get_to(g.TotalTime);
+}
+
+// SystemParameters 序列化/反序列化实现
+void to_json(nlohmann::json &j, const SystemParameters &s)
+{
+    j = nlohmann::json{
+        {"A_x", s.A_x},
+        {"A_y", s.A_y},
+        {"L10", s.L10},
+        {"L20", s.L20},
+        {"L30", s.L30},
+        {"StepLength", s.StepLength},
+        {"StepHeight", s.StepHeight},
+        {"X", s.X},
+        {"DeltaX", s.DeltaX},
+        {"L21", s.L21},
+        {"L22", s.L22},
+        {"L23", s.L23},
+        {"Beta1", s.Beta1},
+        {"BB2_BC_Angle", s.BB2_BC_Angle},
+        {"C1_B_Length", s.C1_B_Length},
+        {"CC1", s.CC1},
+        {"L31", s.L31},
+        {"C2C3_Length", s.C2C3_Length},
+        {"Lead", s.Lead},
+        {"D1_L_Offset", s.D1_L_Offset},
+        {"D2_L_Offset", s.D2_L_Offset},
+        {"C4_D2_Offset", s.C4_D2_Offset},
+        {"ThighMotorReduction", s.ThighMotorReduction},
+        {"ShankMotorReduction", s.ShankMotorReduction},
+        {"AnkleMotorReduction", s.AnkleMotorReduction}};
+}
+
+void from_json(const nlohmann::json &j, SystemParameters &s)
+{
+    if (j.is_null() || !j.is_object())
+        return;
+
+    if (j.contains("A_x"))
+        j.at("A_x").get_to(s.A_x);
+    if (j.contains("A_y"))
+        j.at("A_y").get_to(s.A_y);
+    if (j.contains("L10"))
+        j.at("L10").get_to(s.L10);
+    if (j.contains("L20"))
+        j.at("L20").get_to(s.L20);
+    if (j.contains("L30"))
+        j.at("L30").get_to(s.L30);
+    if (j.contains("StepLength"))
+        j.at("StepLength").get_to(s.StepLength);
+    if (j.contains("StepHeight"))
+        j.at("StepHeight").get_to(s.StepHeight);
+    if (j.contains("X"))
+        j.at("X").get_to(s.X);
+    if (j.contains("DeltaX"))
+        j.at("DeltaX").get_to(s.DeltaX);
+    if (j.contains("L21"))
+        j.at("L21").get_to(s.L21);
+    if (j.contains("L22"))
+        j.at("L22").get_to(s.L22);
+    if (j.contains("L23"))
+        j.at("L23").get_to(s.L23);
+    if (j.contains("Beta1"))
+        j.at("Beta1").get_to(s.Beta1);
+    if (j.contains("BB2_BC_Angle"))
+        j.at("BB2_BC_Angle").get_to(s.BB2_BC_Angle);
+    if (j.contains("C1_B_Length"))
+        j.at("C1_B_Length").get_to(s.C1_B_Length);
+    if (j.contains("CC1"))
+        j.at("CC1").get_to(s.CC1);
+    if (j.contains("L31"))
+        j.at("L31").get_to(s.L31);
+    if (j.contains("C2C3_Length"))
+        j.at("C2C3_Length").get_to(s.C2C3_Length);
+    if (j.contains("Lead"))
+        j.at("Lead").get_to(s.Lead);
+    if (j.contains("D1_L_Offset"))
+        j.at("D1_L_Offset").get_to(s.D1_L_Offset);
+    if (j.contains("D2_L_Offset"))
+        j.at("D2_L_Offset").get_to(s.D2_L_Offset);
+    if (j.contains("C4_D2_Offset"))
+        j.at("C4_D2_Offset").get_to(s.C4_D2_Offset);
+    if (j.contains("ThighMotorReduction"))
+        j.at("ThighMotorReduction").get_to(s.ThighMotorReduction);
+    if (j.contains("ShankMotorReduction"))
+        j.at("ShankMotorReduction").get_to(s.ShankMotorReduction);
+    if (j.contains("AnkleMotorReduction"))
+        j.at("AnkleMotorReduction").get_to(s.AnkleMotorReduction);
+}
+
+// ComponentID 序列化/反序列化实现
+void to_json(nlohmann::json &j, const ComponentID &c)
+{
+    j = nlohmann::json{{c.name, c.uuid}};
+}
+
+void from_json(const nlohmann::json &j, ComponentID &c)
+{
+    if (j.is_null() || !j.is_object())
+        return;
+
+    auto it = j.begin();
+    if (it != j.end())
+    {
+        c.name = it.key();
+        c.uuid = it.value();
+    }
+}
+
+// LegModelIDs 序列化/反序列化实现
+void to_json(nlohmann::json &j, const LegModelIDs &l)
+{
+
+    j = nlohmann::json::array();
+    for (const auto &component : l.components)
+    {
+        j.push_back(component);
+    }
+}
+
+void from_json(const nlohmann::json &j, LegModelIDs &l)
+{
+    l.components.clear();
+    if (j.is_null() || !j.is_array())
+        return;
+
+    for (const auto &item : j)
+    {
+        ComponentID comp;
+        from_json(item, comp);
+        l.components.push_back(comp);
+    }
+}
+
+// ModelID 序列化/反序列化实现
+void to_json(nlohmann::json &j, const ModelID &m)
+{
+    j = nlohmann::json{
+        {"LF", m.LF},
+        {"LH", m.LH},
+        {"RF", m.RF},
+        {"RH", m.RH}};
+}
+
+void from_json(const nlohmann::json &j, ModelID &m)
+{
+    if (j.is_null() || !j.is_object())
+        return;
+
+    if (j.contains("LF"))
+        j.at("LF").get_to(m.LF);
+    if (j.contains("LH"))
+        j.at("LH").get_to(m.LH);
+    if (j.contains("RF"))
+        j.at("RF").get_to(m.RF);
+    if (j.contains("RH"))
+        j.at("RH").get_to(m.RH);
+}
+
+// LegParam 序列化/反序列化实现
+void to_json(nlohmann::json &j, const LegParam &l)
+{
+    j = nlohmann::json{
+        {"thigh_angle_deg", l.thigh_angle_deg},
+        {"shank_angle_deg", l.shank_angle_deg},
+        {"ankle_angle_deg", l.ankle_angle_deg}};
+}
+
+void from_json(const nlohmann::json &j, LegParam &l)
+{
+    if (j.is_null() || !j.is_object())
+        return;
+
+    if (j.contains("thigh_angle_deg"))
+        j.at("thigh_angle_deg").get_to(l.thigh_angle_deg);
+    if (j.contains("shank_angle_deg"))
+        j.at("shank_angle_deg").get_to(l.shank_angle_deg);
+    if (j.contains("ankle_angle_deg"))
+        j.at("ankle_angle_deg").get_to(l.ankle_angle_deg);
+}
+
+// LegParameters 序列化/反序列化实现
+void to_json(nlohmann::json &j, const LegParameters &l)
+{
+    j = nlohmann::json{
+        {"LF", l.LF},
+        {"LH", l.LH},
+        {"RF", l.RF},
+        {"RH", l.RH}};
+}
+
+void from_json(const nlohmann::json &j, LegParameters &l)
+{
+    if (j.is_null() || !j.is_object())
+        return;
+
+    if (j.contains("LF"))
+        j.at("LF").get_to(l.LF);
+    if (j.contains("LH"))
+        j.at("LH").get_to(l.LH);
+    if (j.contains("RF"))
+        j.at("RF").get_to(l.RF);
+    if (j.contains("RH"))
+        j.at("RH").get_to(l.RH);
+}
+
+// RobotBody 序列化/反序列化实现
+void to_json(nlohmann::json &j, const RobotBody &r)
+{
+    j = nlohmann::json{
+        {"BodyCode", r.BodyCode},
+        {"x", r.x},
+        {"y", r.y},
+        {"z", r.z},
+        {"qx", r.qx},
+        {"qy", r.qy},
+        {"qz", r.qz},
+        {"qw", r.qw}};
+}
+
+void from_json(const nlohmann::json &j, RobotBody &r)
+{
+    if (j.is_null() || !j.is_object())
+        return;
+
+    if (j.contains("BodyCode"))
+        j.at("BodyCode").get_to(r.BodyCode);
+    if (j.contains("x"))
+        j.at("x").get_to(r.x);
+    if (j.contains("y"))
+        j.at("y").get_to(r.y);
+    if (j.contains("z"))
+        j.at("z").get_to(r.z);
+    if (j.contains("qx"))
+        j.at("qx").get_to(r.qx);
+    if (j.contains("qy"))
+        j.at("qy").get_to(r.qy);
+    if (j.contains("qz"))
+        j.at("qz").get_to(r.qz);
+    if (j.contains("qw"))
+        j.at("qw").get_to(r.qw);
+}
+
+// RequestParameters 序列化/反序列化实现
+void to_json(nlohmann::json &j, const RequestParameters &c)
+{
+    j = nlohmann::json{
+        {"GaitInfo", c.GaitInfo},
+        {"SystemParameters", c.SystemParameters},
+        {"ModelID", c.ModelID},
+        {"Param", c.Param},
+        {"RobotBody", c.RobotBody},
+        {"RobotID", c.RobotID},
+        {"t_percentage", c.t_percentage}};
+}
+
+void from_json(const nlohmann::json &j, RequestParameters &c)
+{
+    if (j.is_null() || !j.is_object())
+        return;
+
+    try
+    {
+        if (j.contains("GaitInfo"))
+            j.at("GaitInfo").get_to(c.GaitInfo);
+        if (j.contains("SystemParameters"))
+            j.at("SystemParameters").get_to(c.SystemParameters);
+        if (j.contains("ModelID"))
+            j.at("ModelID").get_to(c.ModelID);
+        if (j.contains("Param"))
+            j.at("Param").get_to(c.Param);
+        if (j.contains("RobotBody"))
+            j.at("RobotBody").get_to(c.RobotBody);
+        if (j.contains("t_percentage"))
+            j.at("t_percentage").get_to(c.t_percentage);
+        if (j.contains("RobotID"))
+            j.at("RobotID").get_to(c.RobotID);
+    }
+    catch (const nlohmann::json::exception &e)
+    {
+        std::cerr << "[ERROR] 解析RequestParameters失败: " << e.what() << std::endl;
+        std::cerr << "[ERROR] 当前JSON: " << j.dump(2) << std::endl;
+        throw;
+    }
+}
+void RequestParameters::debugPrint() const
+{
+    std::cout << "========================================" << std::endl;
+    std::cout << "RequestParameters DEBUG PRINT" << std::endl;
+    std::cout << "========================================" << std::endl;
+
+    // GaitInfo
+    std::cout << "=== GaitInfo ===" << std::endl;
+    std::cout << "  GaitType:     " << GaitInfo.GaitType << std::endl;
+    std::cout << "  Period:       " << GaitInfo.Period << std::endl;
+    std::cout << "  Frequency:    " << GaitInfo.Frequency << std::endl;
+    std::cout << "  StepTime:     " << GaitInfo.StepTime << std::endl;
+    std::cout << "  SupportTime:  " << GaitInfo.SupportTime << std::endl;
+    std::cout << "  SwingTime:    " << GaitInfo.SwingTime << std::endl;
+    std::cout << "  SupportRatio: " << GaitInfo.SupportRatio << std::endl;
+    std::cout << "  SwingRatio:   " << GaitInfo.SwingRatio << std::endl;
+    std::cout << "  TotalFrames:  " << GaitInfo.TotalFrames << std::endl;
+    std::cout << "  TotalTime:    " << GaitInfo.TotalTime << std::endl;
+
+    // SystemParameters (关键字段)
+    std::cout << "\n=== SystemParameters (key fields) ===" << std::endl;
+    std::cout << "  A_x: " << SystemParameters.A_x << std::endl;
+    std::cout << "  A_y: " << SystemParameters.A_y << std::endl;
+    std::cout << "  L10: " << SystemParameters.L10 << std::endl;
+    std::cout << "  L20: " << SystemParameters.L20 << std::endl;
+    std::cout << "  L30: " << SystemParameters.L30 << std::endl;
+    std::cout << "  StepLength: " << SystemParameters.StepLength << std::endl;
+    std::cout << "  StepHeight: " << SystemParameters.StepHeight << std::endl;
+    std::cout << "  X: " << SystemParameters.X << std::endl;
+    std::cout << "  DeltaX: " << SystemParameters.DeltaX << std::endl;
+    std::cout << "  L21: " << SystemParameters.L21 << std::endl;
+    std::cout << "  L22: " << SystemParameters.L22 << std::endl;
+    std::cout << "  L23: " << SystemParameters.L23 << std::endl;
+    std::cout << "  Beta1: " << SystemParameters.Beta1 << std::endl;
+    std::cout << "  BB2_BC_Angle: " << SystemParameters.BB2_BC_Angle << std::endl;
+    std::cout << "  C1_B_Length: " << SystemParameters.C1_B_Length << std::endl;
+    std::cout << "  CC1: " << SystemParameters.CC1 << std::endl;
+    std::cout << "  L31: " << SystemParameters.L31 << std::endl;
+    std::cout << "  C2C3_Length: " << SystemParameters.C2C3_Length << std::endl;
+    std::cout << "  Lead: " << SystemParameters.Lead << std::endl;
+    std::cout << "  D1_L_Offset: " << SystemParameters.D1_L_Offset << std::endl;
+    std::cout << "  D2_L_Offset: " << SystemParameters.D2_L_Offset << std::endl;
+    std::cout << "  C4_D2_Offset: " << SystemParameters.C4_D2_Offset << std::endl;
+    std::cout << "  ThighMotorReduction: " << SystemParameters.ThighMotorReduction << std::endl;
+    std::cout << "  ShankMotorReduction: " << SystemParameters.ShankMotorReduction << std::endl;
+    std::cout << "  AnkleMotorReduction: " << SystemParameters.AnkleMotorReduction << std::endl;
+
+    // ModelID详细内容
+    std::cout << "\n=== ModelID Details ===" << std::endl;
+
+    // LF
+    std::cout << "  LF [" << ModelID.LF.components.size() << " components]:" << std::endl;
+    for (size_t i = 0; i < ModelID.LF.components.size(); ++i)
+    {
+        std::cout << "    [" << i << "] " << ModelID.LF.components[i].name
+                  << ": " << ModelID.LF.components[i].uuid << std::endl;
+    }
+
+    // LH
+    std::cout << "  LH [" << ModelID.LH.components.size() << " components]:" << std::endl;
+    for (size_t i = 0; i < ModelID.LH.components.size(); ++i)
+    {
+        std::cout << "    [" << i << "] " << ModelID.LH.components[i].name
+                  << ": " << ModelID.LH.components[i].uuid << std::endl;
+    }
+
+    // RF
+    std::cout << "  RF [" << ModelID.RF.components.size() << " components]:" << std::endl;
+    for (size_t i = 0; i < ModelID.RF.components.size(); ++i)
+    {
+        std::cout << "    [" << i << "] " << ModelID.RF.components[i].name
+                  << ": " << ModelID.RF.components[i].uuid << std::endl;
+    }
+
+    // RH
+    std::cout << "  RH [" << ModelID.RH.components.size() << " components]:" << std::endl;
+    for (size_t i = 0; i < ModelID.RH.components.size(); ++i)
+    {
+        std::cout << "    [" << i << "] " << ModelID.RH.components[i].name
+                  << ": " << ModelID.RH.components[i].uuid << std::endl;
+    }
+
+    // LegParameters
+    std::cout << "\n=== Param (LegParameters) ===" << std::endl;
+
+    std::cout << "  LF:" << std::endl;
+    std::cout << "    thigh_angle_deg: " << Param.LF.thigh_angle_deg << std::endl;
+    std::cout << "    shank_angle_deg: " << Param.LF.shank_angle_deg << std::endl;
+    std::cout << "    ankle_angle_deg: " << Param.LF.ankle_angle_deg << std::endl;
+
+    std::cout << "  LH:" << std::endl;
+    std::cout << "    thigh_angle_deg: " << Param.LH.thigh_angle_deg << std::endl;
+    std::cout << "    shank_angle_deg: " << Param.LH.shank_angle_deg << std::endl;
+    std::cout << "    ankle_angle_deg: " << Param.LH.ankle_angle_deg << std::endl;
+
+    std::cout << "  RF:" << std::endl;
+    std::cout << "    thigh_angle_deg: " << Param.RF.thigh_angle_deg << std::endl;
+    std::cout << "    shank_angle_deg: " << Param.RF.shank_angle_deg << std::endl;
+    std::cout << "    ankle_angle_deg: " << Param.RF.ankle_angle_deg << std::endl;
+
+    std::cout << "  RH:" << std::endl;
+    std::cout << "    thigh_angle_deg: " << Param.RH.thigh_angle_deg << std::endl;
+    std::cout << "    shank_angle_deg: " << Param.RH.shank_angle_deg << std::endl;
+    std::cout << "    ankle_angle_deg: " << Param.RH.ankle_angle_deg << std::endl;
+
+    // RobotBody
+    std::cout << "\n=== RobotBody ===" << std::endl;
+    std::cout << "  BodyCode: " << RobotBody.BodyCode << std::endl;
+    std::cout << "  x: " << RobotBody.x << std::endl;
+    std::cout << "  y: " << RobotBody.y << std::endl;
+    std::cout << "  z: " << RobotBody.z << std::endl;
+    std::cout << "  qx: " << RobotBody.qx << std::endl;
+    std::cout << "  qy: " << RobotBody.qy << std::endl;
+    std::cout << "  qz: " << RobotBody.qz << std::endl;
+    std::cout << "  qw: " << RobotBody.qw << std::endl;
+
+    // Other parameters
+    std::cout << "\n=== Other Parameters  ===" << std::endl;
+    std::cout << "  t_percentage: " << t_percentage << std::endl;
+    std::cout << "  RobotID: " << RobotID << std::endl;
+
+    std::cout << "========================================" << std::endl;
+    std::cout << "END DEBUG PRINT" << std::endl;
+    std::cout << "========================================" << std::endl;
+}
+
+// RequestParameters 成员函数实现
+RequestParameters RequestParameters::fromJsonString(const std::string &jsonStr)
+{
+    try
+    {
+        auto j = nlohmann::json::parse(jsonStr);
+
+        // 如果不包含req_param，尝试直接解析
+        if (j.contains("GaitInfo") || j.contains("SystemParameters") ||
+            j.contains("ModelID") || j.contains("Param") || j.contains("RobotBody") || j.contains("t_percentage") || j.contains("RobotID"))
+        {
+            // 看起来是直接RequestParameters格式
+            return j.get<RequestParameters>();
+        }
+        else
+        {
+
+            if (j.contains("req_param") && !j["req_param"].is_null())
+            {
+                // req_param是一个嵌套的JSON对象，直接获取它
+                auto req_param_json = j["req_param"];
+                if (req_param_json.contains("GaitInfo") || req_param_json.contains("SystemParameters") ||
+                    req_param_json.contains("ModelID") || req_param_json.contains("Param") || req_param_json.contains("RobotBody") || req_param_json.contains("t_percentage") || req_param_json.contains("RobotID"))
+                {
+                    // 看起来是直接RequestParameters格式
+                    return req_param_json.get<RequestParameters>();
+                }
+            }
+        }
+
+        // 返回默认对象
+        return RequestParameters();
+    }
+    catch (const nlohmann::json::exception &e)
+    {
+        std::cerr << "[ERROR] JSON解析失败: " << e.what() << std::endl;
+        return RequestParameters(); // 返回默认对象
+    }
+    catch (...)
+    {
+        std::cerr << "[ERROR] 未知错误解析JSON" << std::endl;
+        return RequestParameters(); // 返回默认对象
+    }
+}
+
+RequestParameters RequestParameters::fromJsonFile(const std::string &filename)
+{
+    std::ifstream file(filename);
+    if (!file.is_open())
+    {
+        throw std::runtime_error("Cannot open file: " + filename);
+    }
+
+    std::stringstream buffer;
+    buffer << file.rdbuf();
+    return fromJsonString(buffer.str());
+}
+
+std::string RequestParameters::toJsonString() const
+{
+    nlohmann::json j = *this;
+    return j.dump(2); // 缩进2个空格
+}
+
+bool RequestParameters::saveToFile(const std::string &filename) const
+{
+    try
+    {
+        std::ofstream file(filename);
+        if (!file.is_open())
+        {
+            return false;
+        }
+        file << toJsonString();
+        return true;
+    }
+    catch (...)
+    {
+        return false;
+    }
+}

src/QuadrupedRobotSimulation/SharedGeometry.cpp

@@ -0,0 +1,211 @@
+// SharedGeometry.cpp
+#include "SharedGeometry.h"
+#include <stdexcept>
+#include <cmath>
+#include <algorithm>
+
+// 如果没有定义M_PI，则定义它
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+// ==================== Vector2D 实现 ====================
+
+Vector2D::Vector2D() : X(0.0), Y(0.0) {}
+
+Vector2D::Vector2D(double x, double y) : X(x), Y(y) {}
+
+Vector2D Vector2D::operator+(const Vector2D &other) const
+{
+    return Vector2D(X + other.X, Y + other.Y);
+}
+
+Vector2D Vector2D::operator-(const Vector2D &other) const
+{
+    return Vector2D(X - other.X, Y - other.Y);
+}
+
+Vector2D Vector2D::operator*(double scalar) const
+{
+    return Vector2D(X * scalar, Y * scalar);
+}
+
+double Vector2D::distanceTo(const Vector2D &other) const
+{
+    double dx = X - other.X;
+    double dy = Y - other.Y;
+    return std::sqrt(dx * dx + dy * dy);
+}
+
+double Vector2D::length() const
+{
+    return std::sqrt(X * X + Y * Y);
+}
+
+Vector2D Vector2D::normalized() const
+{
+    double len = length();
+    if (len > 0.0)
+    {
+        return Vector2D(X / len, Y / len);
+    }
+    return Vector2D(0.0, 0.0);
+}
+
+Vector2D Vector2D::Zero()
+{
+    return Vector2D(0.0, 0.0);
+}
+
+double Vector2D::Distance(const Vector2D &v1, const Vector2D &v2)
+{
+    return std::sqrt(std::pow(v2.X - v1.X, 2) + std::pow(v2.Y - v1.Y, 2));
+}
+
+double Vector2D::Cross(const Vector2D &other) const
+{
+    return X * other.Y - Y * other.X;
+}
+
+double Vector2D::Dot(const Vector2D &other) const
+{
+    return X * other.X + Y * other.Y;
+}
+
+// ==================== Pose7 实现 ====================
+
+Pose7::Pose7() : tx(0.0), ty(0.0), tz(0.0), qx(0.0), qy(0.0), qz(0.0), qw(1.0) {}
+
+// ==================== PoseCalculator 实现 ====================
+
+Pose7 PoseCalculator::CalculatePoseAndQuaternion(const Vector2D &start, const Vector2D &end)
+{
+    Pose7 pose;
+    pose.tx = start.X;
+    pose.ty = start.Y;
+    pose.tz = 0.0;
+
+    Vector2D direction = end - start;
+    double length = direction.length();
+
+    if (length > 0.0)
+    {
+        direction = direction * (1.0 / length);
+        double angle = std::atan2(direction.Y, direction.X);
+
+        // 转换为四元数（绕Z轴旋转）
+        double halfAngle = angle * 0.5;
+        pose.qw = std::cos(halfAngle);
+        pose.qz = std::sin(halfAngle);
+        pose.qx = 0.0;
+        pose.qy = 0.0;
+    }
+    else
+    {
+        pose.qw = 1.0;
+        pose.qx = 0.0;
+        pose.qy = 0.0;
+        pose.qz = 0.0;
+    }
+
+    return pose;
+}
+
+std::vector<double> PoseCalculator::CalculatePoseAndQuaternionArray(double x1, double y1, double x2, double y2)
+{
+    double x_D = x1;
+    double y_D = y1;
+    double x_G = x2;
+    double y_G = y2;
+
+    // 方向向量
+    double dx = x_G - x_D;
+    double dy = y_G - y_D;
+
+    double v_norm = std::sqrt(dx * dx + dy * dy);
+
+    // 使用小的epsilon值而不是直接比较0
+    if (std::abs(v_norm) < 1e-10)
+    {
+        throw std::invalid_argument("两个点位置相同，无法计算姿态。");
+    }
+
+    // 单位化方向向量
+    double vx = dx / v_norm;
+    double vy = dy / v_norm;
+
+    // 四元数（绕Z轴旋转）
+    double theta = std::atan2(vy, vx);
+    double halfTheta = theta / 2.0;
+    double qw = std::cos(halfTheta);
+    double qx = 0.0;
+    double qy = 0.0;
+    double qz = std::sin(halfTheta);
+
+    // 返回长度为7的数组
+    return {x_D, y_D, 0.0, qw, qx, qy, qz};
+}
+
+double PoseCalculator::MMToM(double MM)
+{
+    return Round(MM / 1000.0, 6);
+}
+
+double PoseCalculator::Round(double value, int decimals)
+{
+    double factor = std::pow(10.0, decimals);
+    return std::round(value * factor) / factor;
+}
+
+std::string PoseCalculator::ToString(double value, int decimals)
+{
+    std::ostringstream oss;
+    oss << std::fixed << std::setprecision(decimals) << value;
+    return oss.str();
+}
+
+C_ObjStates PoseCalculator::CalculatePoseAndQuaternion_C_ObjStates(const std::string &i,
+                                                                   double x1, double y1,
+                                                                   double x2, double y2)
+{
+    // 如果需要实现这个方法，需要C_ObjStates的定义
+    // 这里先抛出一个异常，提醒需要实现
+    // throw std::runtime_error("C_ObjStates需要额外的定义，请提供C_ObjStates类的定义");
+
+    // 示例代码（需要C_ObjStates类）：
+
+    double x_D = MMToM(x1);
+    double y_D = MMToM(y1);
+    double x_G = MMToM(x2);
+    double y_G = MMToM(y2);
+
+    double dx = x_G - x_D;
+    double dy = y_G - y_D;
+    double v_norm = std::sqrt(dx * dx + dy * dy);
+
+    if (std::abs(v_norm) < 1e-10)
+    {
+        throw std::invalid_argument("两个点位置相同，无法计算姿态。");
+    }
+
+    double vx = dx / v_norm;
+    double vy = dy / v_norm;
+    double theta = std::atan2(vy, vx);
+    double halfTheta = theta / 2.0;
+    double qw = std::cos(halfTheta);
+    double qx = 0.0;
+    double qy = 0.0;
+    double qz = std::sin(halfTheta);
+
+    C_ObjStates c_ObjStates;
+    c_ObjStates.i = i;
+    c_ObjStates.tx = ToString(x_D);
+    c_ObjStates.ty = ToString(y_D);
+    c_ObjStates.tz = "0";
+    c_ObjStates.qx = ToString(qx);
+    c_ObjStates.qy = ToString(qy);
+    c_ObjStates.qz = ToString(qz);
+    c_ObjStates.qw = ToString(qw);
+
+    return c_ObjStates;
+}