// KinematicsSimulation.h #ifndef KINEMATICS_SIMULATION_H #define KINEMATICS_SIMULATION_H #include "QuadrupedRobotSimulation/BaseClass.h" #include #include #include #include #include #include #include class QuadrupedRobotSimulation { private: // 常量 static constexpr double PI = 3.14159265358979323846; public: // 使用统一配置 QuadrupedRobotConfiguration _config; // ============== 从配置继承的字段 ============== // 主几何参数 double A_x; // 固定点A的X坐标 double A_y; // 固定点A的Y坐标 std::vector 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> Trajectory_C; // C点轨迹 std::vector> Trajectory_B; // B点轨迹 std::vector> Trajectory_D1; // D1点轨迹 std::vector> Trajectory_D2; // D2点轨迹 std::vector> Trajectory_C4; // C4点轨迹 std::vector> Trajectory_B1; // B1点轨迹 std::vector> Trajectory_B2; // B2点轨迹 std::vector> Trajectory_C1; // C1点轨迹 std::vector> Trajectory_C2; // C2点轨迹 std::vector> Trajectory_C3; // C3点轨迹 // ============== 角度和距离数据列表 ============== std::vector AB_horizontal_angles; // AB连杆与水平夹角（度） std::vector AB_BC_angles; // AB与BC夹角（度） std::vector BC_CL_angles; // BC与CL夹角（度） std::vector AB1_AB_angles; // AB1与AB夹角θ1（度） std::vector BCL_angles; // B-C-L夹角（度） std::vector C3_C4_distances; // C3-C4距离（mm） std::vector C2_C3_distances; // C2-C3距离（mm） std::vector C2_C4_distances; // C2-C4距离（mm） std::vector C2_C3_C4_angles; // ∠C2C3C4角度（度） std::vector CL_distances; // C-L距离（mm） std::vector 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> 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> CalculateSupportPoints(const std::vector &L_point); std::vector Calculate_B_Position(const std::vector &A, const std::vector &C, double AB_length, double BC_length); double Calculate_BCL_Angle(const std::vector &B_point, const std::vector &C_point, const std::vector &L_point); std::vector CalculateSwingPointWithConstraints(const std::vector &L_point, double target_BCL_angle, const std::vector &prev_C, double &prev_angle); void RecalculateAllPoints(); // 辅助点计算方法 std::vector Calculate_B2_Position(const std::vector &B, const std::vector &C, double B_B2_length, double BB2_BC_angle); std::vector Calculate_B1_Position(const std::vector &A, const std::vector &B2, double A_B1_length, double B1B2_length); std::vector Calculate_C1_Position(const std::vector &B, const std::vector &C, double C1_B_length); std::vector Calculate_C2_Position(const std::vector &C1, const std::vector &B, const std::vector &C, double C1C2_length); std::vector Calculate_C3_Position(const std::vector &C2, const std::vector &C4, double C2C3_length); double Calculate_C2C3C4_Angle(const std::vector &C2, const std::vector &C3, const std::vector &C4); double Distance(const std::vector &p1, const std::vector &p2); // 电机数据计算方法 MotorData CalculateIncrementsAndVelocities(); MotorData CalculateOtherLegData(const MotorData &baseData, int shift, bool isRightLeg); MotorData AdjustMotorAngles(const MotorData &motorData, const std::vector &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 Linspace(double start, double end, int num); std::vector> Zeros(int rows, int cols); std::vector> Flip(const std::vector> &array); std::vector> VStack(const std::vector> &array1, const std::vector> &array2); int PythonRoundExact(double value); // 数组操作函数 std::vector ShiftArray(const std::vector &array, int shiftFrames); }; #endif // KINEMATICS_SIMULATION_H