规范模块目录和第三方依赖结构

include/QuadrupedRobotSimulation/KinematicsSimulation.h

@@ -0,0 +1,201 @@
+// KinematicsSimulation.h
+#ifndef KINEMATICS_SIMULATION_H
+#define KINEMATICS_SIMULATION_H
+
+#include "QuadrupedRobotSimulation/BaseClass.h"
+#include <vector>
+#include <string>
+#include <cmath>
+#include <algorithm>
+#include <stdexcept>
+#include <iostream>
+#include <functional>
+
+class QuadrupedRobotSimulation
+{
+private:
+    // 常量
+    static constexpr double PI = 3.14159265358979323846;
+
+public:
+    // 使用统一配置
+    QuadrupedRobotConfiguration _config;
+
+    // ============== 从配置继承的字段 ==============
+    // 主几何参数
+    double A_x;                  // 固定点A的X坐标
+    double A_y;                  // 固定点A的Y坐标
+    std::vector<double> A_point; // A点坐标数组
+    double L10;                  // AB连杆长度（大腿长度）
+    double L20;                  // BC连杆长度（小腿上段长度）
+    double L30;                  // C点到L点（脚端点）的垂直距离
+    double s_l;                  // 步长
+    double s_h;                  // 步高
+    double X;                    // 下铰链点与大腿铰链点投影水平距离
+
+    // 小腿连杆机构参数
+
+    double L22;   // B1到B2的连杆长度
+    double L23;   // B到B2的连杆长度
+    double beta1; // B-B2与BC的夹角（度）
+    double CC1;   // C到C1点的距离
+
+    // 脚踝连杆机构参数
+    double L31;         // C1到C2的连杆长度
+    double C2C3_length; // C2到C3的连杆长度（固定长度）
+    double lead1;       // 丝杠导程（mm）
+
+    // 脚部构型参数
+    double D1_L_offset;  // D1点与L点的水平偏移（左侧）
+    double D2_L_offset;  // D2点与L点的水平偏移（右侧）
+    double C4_D2_offset; // C4点与D2点的垂直距离
+
+    // 电机参数
+    double thigh_motor_reduction; // 大腿电机减速比
+    double shank_motor_reduction; // 小腿电机减速比
+    double ankle_motor_reduction; // 脚踝电机减速比
+
+    // 逆向运动学参数
+    double adjusted_shank_angles_factory; // 小腿角度调整参数
+
+    // ============== 计算得到的中间变量 ==============
+    double BB2_BC_angle; // B-B2与BC的夹角（弧度）
+    double C1_B_length;  // C1到B点的距离
+
+    // ============== 步态相位时间 ==============
+    double timeLF = 0; // 左前腿相位时间
+    double timeLH = 0; // 左后腿相位时间
+    double timeRF = 0; // 右前腿相位时间
+    double timeRH = 0; // 右后腿相位时间
+
+    // ============== 轨迹数据列表 ==============
+
+    std::vector<std::vector<double>> Trajectory_C;  // C点轨迹
+    std::vector<std::vector<double>> Trajectory_B;  // B点轨迹
+    std::vector<std::vector<double>> Trajectory_D1; // D1点轨迹
+    std::vector<std::vector<double>> Trajectory_D2; // D2点轨迹
+    std::vector<std::vector<double>> Trajectory_C4; // C4点轨迹
+    std::vector<std::vector<double>> Trajectory_B1; // B1点轨迹
+    std::vector<std::vector<double>> Trajectory_B2; // B2点轨迹
+    std::vector<std::vector<double>> Trajectory_C1; // C1点轨迹
+    std::vector<std::vector<double>> Trajectory_C2; // C2点轨迹
+    std::vector<std::vector<double>> Trajectory_C3; // C3点轨迹
+
+    // ============== 角度和距离数据列表 ==============
+    std::vector<double> AB_horizontal_angles; // AB连杆与水平夹角（度）
+    std::vector<double> AB_BC_angles;         // AB与BC夹角（度）
+    std::vector<double> BC_CL_angles;         // BC与CL夹角（度）
+    std::vector<double> AB1_AB_angles;        // AB1与AB夹角θ1（度）
+    std::vector<double> BCL_angles;           // B-C-L夹角（度）
+    std::vector<double> C3_C4_distances;      // C3-C4距离（mm）
+    std::vector<double> C2_C3_distances;      // C2-C3距离（mm）
+    std::vector<double> C2_C4_distances;      // C2-C4距离（mm）
+    std::vector<double> C2_C3_C4_angles;      // ∠C2C3C4角度（度）
+    std::vector<double> CL_distances;         // C-L距离（mm）
+    std::vector<double> dot_products;         // 点积验证数据
+
+    // ============== 电机数据对象 ==============
+    MotorData motor_data_LF; // 左前腿电机数据
+    MotorData motor_data_LH; // 左后腿电机数据
+    MotorData motor_data_RF; // 右前腿电机数据
+    MotorData motor_data_RH; // 右后腿电机数据
+
+    // 原始电机数据（调整前）
+    MotorData motor_data_LF_raw;
+    MotorData motor_data_LH_raw;
+    MotorData motor_data_RF_raw;
+    MotorData motor_data_RH_raw;
+
+    std::string gait_type;      // 步态类型："walk"、"trot"、"standup"
+                                // 步态控制参数
+    double T;                   // 步态周期时间（秒）
+    double f;                   // 控制频率（Hz）
+    double step;                // 时间步长（秒）
+    double t_s;                 // 支撑阶段时间
+    double t_w;                 // 摆动阶段时间
+    double support_ratio = 0.5; // 支撑阶段比例
+    double swing_ratio = 0.5;   // 摆动阶段比例
+
+    double deltX; // x方向偏移距离
+    double L21;   // A到B1的连杆长度
+
+    std::vector<std::vector<double>> Trajectory_L; // L点（脚端点）轨迹
+
+    // 构造函数
+    QuadrupedRobotSimulation(const QuadrupedRobotConfiguration &config = QuadrupedRobotConfiguration());
+    QuadrupedRobotSimulation();
+
+    // 获取配置对象
+    const QuadrupedRobotConfiguration &GetConfig() const { return _config; }
+
+    // 主计算函数
+    std::string CalculateAllTrajectories_JsonStr();
+    void CalculateAllTrajectories();
+    std::string CalculateAllTrajectoriesJsonString();
+
+    // 设置步态参数
+    void SetGaitParameters();
+
+private:
+    // 初始化方法
+    void LoadParametersFromConfig();
+    void InitializeParameters();
+
+    // 轨迹计算方法
+    void CalculateTrajectoryL_V2();
+    void CalculateTrajectoryC();
+    void CalculateTrajectoryB();
+    void CalculateAllPoints();
+    void CalculateAllAngles();
+    void CheckConstraints();
+    void CalculateMotorData();
+
+    // 辅助计算方法
+    std::vector<std::vector<double>> CalculateSupportPoints(const std::vector<double> &L_point);
+    std::vector<double> Calculate_B_Position(const std::vector<double> &A, const std::vector<double> &C,
+                                             double AB_length, double BC_length);
+    double Calculate_BCL_Angle(const std::vector<double> &B_point, const std::vector<double> &C_point,
+                               const std::vector<double> &L_point);
+    std::vector<double> CalculateSwingPointWithConstraints(const std::vector<double> &L_point,
+                                                           double target_BCL_angle,
+                                                           const std::vector<double> &prev_C,
+                                                           double &prev_angle);
+    void RecalculateAllPoints();
+
+    // 辅助点计算方法
+    std::vector<double> Calculate_B2_Position(const std::vector<double> &B, const std::vector<double> &C,
+                                              double B_B2_length, double BB2_BC_angle);
+    std::vector<double> Calculate_B1_Position(const std::vector<double> &A, const std::vector<double> &B2,
+                                              double A_B1_length, double B1B2_length);
+    std::vector<double> Calculate_C1_Position(const std::vector<double> &B, const std::vector<double> &C,
+                                              double C1_B_length);
+    std::vector<double> Calculate_C2_Position(const std::vector<double> &C1, const std::vector<double> &B,
+                                              const std::vector<double> &C, double C1C2_length);
+    std::vector<double> Calculate_C3_Position(const std::vector<double> &C2, const std::vector<double> &C4,
+                                              double C2C3_length);
+    double Calculate_C2C3C4_Angle(const std::vector<double> &C2, const std::vector<double> &C3,
+                                  const std::vector<double> &C4);
+    double Distance(const std::vector<double> &p1, const std::vector<double> &p2);
+
+    // 电机数据计算方法
+    MotorData CalculateIncrementsAndVelocities();
+    MotorData CalculateOtherLegData(const MotorData &baseData, int shift, bool isRightLeg);
+    MotorData AdjustMotorAngles(const MotorData &motorData, const std::vector<double> &frame_start_times, double step);
+    MotorData ReverseShankData(const MotorData &motorData, double step, double shank_motor_reduction);
+    MotorData AdjustAnkleAngles(const MotorData &data);
+
+    // 数学辅助函数
+    double NonlinearAngleFunction(double t, double start_angle_deg, double end_angle_deg,
+                                  const std::string &function_type, double curvature);
+    std::vector<double> Linspace(double start, double end, int num);
+    std::vector<std::vector<double>> Zeros(int rows, int cols);
+    std::vector<std::vector<double>> Flip(const std::vector<std::vector<double>> &array);
+    std::vector<std::vector<double>> VStack(const std::vector<std::vector<double>> &array1,
+                                            const std::vector<std::vector<double>> &array2);
+    int PythonRoundExact(double value);
+
+    // 数组操作函数
+    std::vector<double> ShiftArray(const std::vector<double> &array, int shiftFrames);
+};
+
+#endif // KINEMATICS_SIMULATION_H