220 lines
8.8 KiB
C++
220 lines
8.8 KiB
C++
// KinematicsReverse.h
|
||
#ifndef KINEMATICS_REVERSE_H
|
||
#define KINEMATICS_REVERSE_H
|
||
|
||
#include <vector>
|
||
#include <string>
|
||
#include <map>
|
||
#include <cmath>
|
||
#include <memory>
|
||
#include <algorithm>
|
||
#include <functional>
|
||
#include <optional>
|
||
|
||
#include "utils.h"
|
||
#include "QuadrupedRobotSimulation/BaseClass.h"
|
||
#include "QuadrupedRobotSimulation/OPERATION.h"
|
||
#include "SharedGeometry/SharedGeometry.h"
|
||
|
||
class ReverseKinematicsCalculator
|
||
{
|
||
private:
|
||
// 统一配置
|
||
QuadrupedRobotConfiguration _config;
|
||
std::shared_ptr<RobotGaitDataManager> _manager;
|
||
|
||
// 基本输入参数
|
||
double adjusted_shank_angles_factory;
|
||
std::vector<double> A_point;
|
||
double L10;
|
||
double L20;
|
||
double L30;
|
||
double L21;
|
||
double L22;
|
||
double L23;
|
||
double beta1;
|
||
double CC1;
|
||
double L31;
|
||
double C2C3_length;
|
||
double lead1;
|
||
double D1_L_offset;
|
||
double D2_L_offset;
|
||
double C4_D2_offset;
|
||
|
||
// 初始C3C4距离
|
||
double initial_C3C4_LF;
|
||
double initial_C3C4_LH;
|
||
double initial_C3C4_RF;
|
||
double initial_C3C4_RH;
|
||
|
||
// 计算得到的中间变量
|
||
double _BB2_BC_angle_rad;
|
||
double _C1_B_length;
|
||
double _CC4_distance;
|
||
|
||
// 数学常量
|
||
static constexpr double PI = 3.14159265358979323846;
|
||
static constexpr double DegToRad = PI / 180.0;
|
||
static constexpr double RadToDeg = 180.0 / PI;
|
||
|
||
// 静态变量（模拟C#的静态字段）
|
||
std::map<std::string, std::vector<double>> old_initial_points_dictLF;
|
||
std::map<std::string, std::vector<double>> old_initial_points_dictLH;
|
||
static bool IsSupporLF;
|
||
static bool IsSupporLH;
|
||
static double BodyPosition_x;
|
||
static double BodyPosition_y;
|
||
static double BodyPosition_z;
|
||
static double BodyPosition_qx;
|
||
static double BodyPosition_qy;
|
||
static double BodyPosition_qz;
|
||
static double BodyPosition_qw;
|
||
static const std::string BodyCode;
|
||
|
||
public:
|
||
// 构造函数
|
||
ReverseKinematicsCalculator(std::shared_ptr<RobotGaitDataManager> manager = nullptr);
|
||
|
||
void debugPrintParamDict(const std::map<std::string, LegParam> ¶mDict) const;
|
||
void debugPrintLegCalculation(const std::string &legName, const LegParam &legParam,
|
||
const std::map<std::string, std::vector<double>> &points_dict,
|
||
const ModelID &modelID) const;
|
||
void debugPrintFinalJson(const std::vector<C_ObjStates> &objStates) const;
|
||
// 主计算函数
|
||
std::string CalculateAllPointsFromMotorAnglesJsonStr();
|
||
std::string CalculateAllPointsOnlyOneLegFromMotorAnglesJsonStr(
|
||
double thigh_angle_deg, double shank_angle_deg, double ankle_angle_deg,
|
||
const std::string &leg_type = "LF", bool IsReset = false);
|
||
|
||
// 逆向计算主函数
|
||
std::map<std::string, std::vector<double>> CalculateAllPointsFromMotorAnglesReverse(
|
||
double thigh_angle_deg, double shank_angle_deg, double ankle_angle_deg,
|
||
const std::string &leg_type = "LF", bool IsReset = false);
|
||
|
||
// 判断是否为支撑点
|
||
bool IsSupportPoint(const std::map<std::string, std::vector<double>> &pointsDict,
|
||
double groundHeight = 0.0, double tolerance = 1.0);
|
||
|
||
// 验证计算结果
|
||
std::pair<bool, std::vector<std::string>> ValidateReverseCalculation(
|
||
const std::map<std::string, std::vector<double>> &points_dict,
|
||
double thigh_angle, double shank_angle, double ankle_angle);
|
||
|
||
// 批量计算
|
||
std::vector<ReverseCalculationResult> BatchReverseCalculation(
|
||
const std::map<std::string, std::vector<double>> &motor_data,
|
||
const std::string &leg_type = "LF", std::optional<int> max_frames = std::nullopt);
|
||
|
||
// 获取配置
|
||
QuadrupedRobotConfiguration GetConfig() const { return _config; }
|
||
|
||
// 更新配置
|
||
void UpdateConfiguration(const QuadrupedRobotConfiguration &newConfig);
|
||
|
||
private:
|
||
// 从配置加载参数
|
||
void LoadParametersFromConfig();
|
||
|
||
// 计算派生参数
|
||
void CalculateDerivedParameters();
|
||
|
||
// 验证配置
|
||
void ValidateConfiguration();
|
||
|
||
// 计算点C（四杆机构解析解）
|
||
std::optional<Vector2D> CalculatePointC(const Vector2D &A, const Vector2D &D,
|
||
const Vector2D &B, double l2, double l3);
|
||
|
||
// 选择C点（确保C和B在AD同侧）
|
||
Vector2D SelectCPointByADSide(const Vector2D &A, const Vector2D &D,
|
||
const Vector2D &B, const Vector2D &C1, const Vector2D &C2);
|
||
|
||
// 根据大腿角度计算B点
|
||
std::vector<double> CalculateBFromThighAngleReverse(double thigh_angle_deg);
|
||
|
||
// 根据小腿角度计算B1点
|
||
std::vector<double> CalculateB1FromShankAngleReverse(const std::vector<double> &B_p,
|
||
double shank_angle_deg);
|
||
|
||
// 根据四杆机构计算B2点
|
||
std::pair<std::vector<double>, std::string> CalculateB2FromFourbarReverse(
|
||
const std::vector<double> &A_p, const std::vector<double> &B_p,
|
||
const std::vector<double> &B1_p, double L22, double L23);
|
||
|
||
// 根据B和B2计算C点
|
||
std::vector<double> CalculateCFromBAndB2Reverse(const std::vector<double> &B1,
|
||
const std::vector<double> &B2,
|
||
double L, double theta_deg);
|
||
|
||
// 反向计算C1点
|
||
std::vector<double> CalculateC1PointReverse(const std::vector<double> &B_p,
|
||
const std::vector<double> &C_p);
|
||
|
||
// 根据C1点计算C2点
|
||
std::vector<double> CalculateC2PointFromC1Reverse(const std::vector<double> &C1_p,
|
||
const std::vector<double> &B_p,
|
||
const std::vector<double> &C_point);
|
||
|
||
// 根据脚踝角度计算C3C4距离
|
||
double CalculateC3C4DistanceFromAnkleAngleReverse(double ankle_angle_deg,
|
||
const std::string &leg_type = "LF");
|
||
|
||
// 已知三角形三边和两个顶点，求第三个顶点
|
||
std::vector<double> FindThirdVertex(const std::vector<double> &C,
|
||
const std::vector<double> &C2,
|
||
double a, double b, double c);
|
||
|
||
// 根据C和C2点计算C4点
|
||
std::vector<double> CalculateC4PointFromCAndC2Reverse(const std::vector<double> &C_p,
|
||
const std::vector<double> &C2_p,
|
||
double ankle_angle_deg,
|
||
const std::string &leg_type = "LF");
|
||
|
||
// 根据C和C4点计算L点
|
||
std::vector<double> CalculateLPointsFromCAndC4Reverse(const std::vector<double> &C,
|
||
const std::vector<double> &C4);
|
||
|
||
// 求直角三角形直角顶点
|
||
std::vector<double> FindRightAngleThirdVertex(const std::vector<double> &C,
|
||
const std::vector<double> &L,
|
||
double a, double b);
|
||
|
||
// 根据C和L点计算D1点
|
||
std::vector<double> CalculateD1PointsFromCAndLReverse(const std::vector<double> &C_p,
|
||
const std::vector<double> &L_p);
|
||
|
||
// 求直角三角形直角顶点D2
|
||
std::vector<double> FindRightAngleVertexD2(const std::vector<double> &L,
|
||
const std::vector<double> &C4,
|
||
double a, double b);
|
||
|
||
// 根据L和C4点计算D2点
|
||
std::vector<double> CalculateD2PointsFromCAndLReverse(const std::vector<double> &L,
|
||
const std::vector<double> &C4);
|
||
|
||
// 求直角三角形直角顶点C3
|
||
std::vector<double> FindRightAngleVertex(const std::vector<double> &C2,
|
||
const std::vector<double> &C4, double L);
|
||
|
||
// 根据C2和C4点计算C3点
|
||
std::vector<double> CalculateC3PointFromC2AndC4Reverse(const std::vector<double> &C2_point,
|
||
const std::vector<double> &C4_point);
|
||
|
||
// 从字典安全获取点坐标
|
||
std::vector<double> GetPointSafe(const std::map<std::string, std::vector<double>> &pointsDict,
|
||
const std::string &key);
|
||
|
||
// 获取模型ID
|
||
// 新版本：
|
||
// std::string GetValueModelID(
|
||
// const ModelID &modelID, // 改为 ModelID 类型
|
||
// const std::string &legName, const std::string &key);
|
||
|
||
const LegModelIDs &GetLegModelIDs(const ModelID &modelID, const std::string &legName);
|
||
|
||
// 辅助函数：旋转向量
|
||
std::vector<double> RotateVector(double angle, const std::vector<double> &vector);
|
||
};
|
||
|
||
#endif // KINEMATICS_REVERSE_H
|