202 lines
9.1 KiB
C++
202 lines
9.1 KiB
C++
// 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
|