Initial commit
This commit is contained in:
zhangshun
706
inc/QuadrupedRobotSimulation/BaseClass.h
Normal file
706
inc/QuadrupedRobotSimulation/BaseClass.h
Normal file
@@ -0,0 +1,706 @@
|
||||
// BaseClass.h - 完整修复版
|
||||
#ifndef BASE_CLASS_H
|
||||
#define BASE_CLASS_H
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
#include <stdexcept>
|
||||
#include <iostream>
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
#include <set>
|
||||
#include <functional>
|
||||
#include <sstream>
|
||||
#include <iomanip>
|
||||
#include "../inc/nlohmann/json.hpp"
|
||||
#include "OPERATION.h"
|
||||
#include "SharedGeometry.h" // 包含共享的几何类
|
||||
#include "RobotConfig.hpp"
|
||||
|
||||
using json = nlohmann::json;
|
||||
|
||||
// 1. 首先完整定义 CheckResultItem 结构
|
||||
struct CheckResultItem
|
||||
{
|
||||
std::string Condition; // 添加缺失的成员变量
|
||||
bool Passed = false;
|
||||
std::optional<double> Actual;
|
||||
std::optional<double> Expected;
|
||||
std::optional<double> Tolerance;
|
||||
std::map<std::string, double> AdditionalData;
|
||||
|
||||
CheckResultItem() : Condition(""), Passed(false)
|
||||
{
|
||||
AdditionalData = std::map<std::string, double>();
|
||||
}
|
||||
|
||||
CheckResultItem(std::string cond, bool passed = false)
|
||||
: Condition(std::move(cond)), Passed(passed)
|
||||
{
|
||||
AdditionalData = std::map<std::string, double>();
|
||||
}
|
||||
};
|
||||
|
||||
// 3. 现在安全地声明 CheckResultItem 的序列化函数
|
||||
void to_json(json &j, const CheckResultItem &item);
|
||||
void from_json(const json &j, CheckResultItem &item);
|
||||
// 3. 前向声明 ModelID
|
||||
struct ModelID;
|
||||
|
||||
// 4. 在namespace nlohmann中前向声明特化
|
||||
namespace nlohmann
|
||||
{
|
||||
// 为 std::optional 添加序列化支持
|
||||
template <typename T>
|
||||
struct adl_serializer<std::optional<T>>;
|
||||
|
||||
// 为 std::vector<std::map<std::string, std::string>> 的序列化声明
|
||||
template <>
|
||||
struct adl_serializer<std::vector<std::map<std::string, std::string>>>;
|
||||
|
||||
// 为 Vector2D 的序列化声明
|
||||
template <>
|
||||
struct adl_serializer<Vector2D>;
|
||||
|
||||
// 为 ModelID 的序列化声明
|
||||
template <>
|
||||
struct adl_serializer<ModelID>;
|
||||
}
|
||||
|
||||
struct RobotGaitRequest
|
||||
{
|
||||
RequestParameters req_param;
|
||||
};
|
||||
|
||||
struct LinkModelIDInfo
|
||||
{
|
||||
std::string LinkName;
|
||||
std::string ModelID;
|
||||
std::string StartPoint;
|
||||
std::string EndPoint;
|
||||
std::string Description;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(LinkModelIDInfo, LinkName, ModelID, StartPoint, EndPoint, Description)
|
||||
};
|
||||
|
||||
struct LegLinkDisplay
|
||||
{
|
||||
std::string LegName;
|
||||
std::string LinkName;
|
||||
std::string ModelID;
|
||||
std::string StartPoint;
|
||||
std::string EndPoint;
|
||||
std::string Description;
|
||||
int Index = 0;
|
||||
std::string UniqueID;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(LegLinkDisplay, LegName, LinkName, ModelID, StartPoint, EndPoint, Description, Index, UniqueID)
|
||||
};
|
||||
|
||||
struct RobotData
|
||||
{
|
||||
GaitInfo GaitInfo;
|
||||
SystemParameters SystemParameters;
|
||||
std::vector<LinkModelIDInfo> LF;
|
||||
std::vector<LinkModelIDInfo> LH;
|
||||
std::vector<LinkModelIDInfo> RF;
|
||||
std::vector<LinkModelIDInfo> RH;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(RobotData, GaitInfo, SystemParameters, LF, LH, RF, RH)
|
||||
};
|
||||
|
||||
struct Point2D
|
||||
{
|
||||
double X = 0.0;
|
||||
double Y = 0.0;
|
||||
|
||||
Point2D() = default;
|
||||
Point2D(double x, double y);
|
||||
|
||||
friend void to_json(json &j, const Point2D &p);
|
||||
friend void from_json(const json &j, Point2D &p);
|
||||
};
|
||||
|
||||
struct FrameData
|
||||
{
|
||||
int FrameNumber = 0;
|
||||
double Time = 0.0;
|
||||
int ShiftedFrameNumber = 0;
|
||||
|
||||
Point2D A_Point;
|
||||
Point2D B_Point;
|
||||
Point2D C_Point;
|
||||
Point2D L_Point;
|
||||
Point2D D1_Point;
|
||||
Point2D D2_Point;
|
||||
Point2D C4_Point;
|
||||
Point2D B1_Point;
|
||||
Point2D B2_Point;
|
||||
Point2D C1_Point;
|
||||
Point2D C2_Point;
|
||||
Point2D C3_Point;
|
||||
|
||||
double AB_Horizontal_Angle = 0.0;
|
||||
double AB_BC_Angle = 0.0;
|
||||
double BC_CL_Angle = 0.0;
|
||||
double AB1_AB_Angle = 0.0;
|
||||
|
||||
double C3_C4_Distance = 0.0;
|
||||
double C2_C3_Distance = 0.0;
|
||||
double C2_C4_Distance = 0.0;
|
||||
double C2_C3_C4_Angle = 0.0;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(FrameData, FrameNumber, Time, ShiftedFrameNumber,
|
||||
A_Point, B_Point, C_Point, L_Point, D1_Point, D2_Point,
|
||||
C4_Point, B1_Point, B2_Point, C1_Point, C2_Point, C3_Point,
|
||||
AB_Horizontal_Angle, AB_BC_Angle, BC_CL_Angle, AB1_AB_Angle,
|
||||
C3_C4_Distance, C2_C3_Distance, C2_C4_Distance, C2_C3_C4_Angle)
|
||||
};
|
||||
|
||||
struct LegTrajectory
|
||||
{
|
||||
std::string LegCode;
|
||||
int PhaseShift = 0;
|
||||
bool IsRightLeg = false;
|
||||
std::vector<FrameData> Frames;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(LegTrajectory, LegCode, PhaseShift, IsRightLeg, Frames)
|
||||
};
|
||||
|
||||
struct TrajectoryData
|
||||
{
|
||||
int TotalFrames = 0;
|
||||
double StepTime = 0.0;
|
||||
std::map<std::string, LegTrajectory> Legs;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(TrajectoryData, TotalFrames, StepTime, Legs)
|
||||
};
|
||||
|
||||
struct MotorInfo
|
||||
{
|
||||
double Angle = 0.0;
|
||||
double AngleIncrement = 0.0;
|
||||
double Speed = 0.0;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(MotorInfo, Angle, AngleIncrement, Speed)
|
||||
};
|
||||
|
||||
struct MotorFrameData
|
||||
{
|
||||
int FrameNumber = 0;
|
||||
double StartTime = 0.0;
|
||||
double EndTime = 0.0;
|
||||
|
||||
MotorInfo Thigh;
|
||||
MotorInfo Shank;
|
||||
MotorInfo Ankle;
|
||||
|
||||
double C3C4_Distance = 0.0;
|
||||
double C3C4_DistanceIncrement = 0.0;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(MotorFrameData, FrameNumber, StartTime, EndTime,
|
||||
Thigh, Shank, Ankle, C3C4_Distance, C3C4_DistanceIncrement)
|
||||
};
|
||||
|
||||
struct LegMotorData
|
||||
{
|
||||
std::string LegCode;
|
||||
std::vector<MotorFrameData> Frames;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(LegMotorData, LegCode, Frames)
|
||||
};
|
||||
|
||||
struct MotorDataExport
|
||||
{
|
||||
double StepTime = 0.0;
|
||||
std::map<std::string, LegMotorData> Legs;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(MotorDataExport, StepTime, Legs)
|
||||
};
|
||||
|
||||
struct MinMax
|
||||
{
|
||||
double Min = 0.0;
|
||||
double Max = 0.0;
|
||||
double Range = 0.0;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(MinMax, Min, Max, Range)
|
||||
};
|
||||
|
||||
struct AngleConstraint
|
||||
{
|
||||
double MinAllowed = 0.0;
|
||||
double MaxAllowed = 0.0;
|
||||
double MinActual = 0.0;
|
||||
double MaxActual = 0.0;
|
||||
bool IsValid = false;
|
||||
std::string CheckType;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(AngleConstraint, MinAllowed, MaxAllowed, MinActual, MaxActual, IsValid, CheckType)
|
||||
};
|
||||
|
||||
struct DistanceConstraint
|
||||
{
|
||||
double MinAllowed = 0.0;
|
||||
double MaxAllowed = 0.0;
|
||||
double MinActual = 0.0;
|
||||
double MaxActual = 0.0;
|
||||
bool IsValid = false;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(DistanceConstraint, MinAllowed, MaxAllowed, MinActual, MaxActual, IsValid)
|
||||
};
|
||||
|
||||
struct PerpendicularityConstraint
|
||||
{
|
||||
double MaxDotProduct = 0.0;
|
||||
double MinDotProduct = 0.0;
|
||||
bool IsValid = false;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(PerpendicularityConstraint, MaxDotProduct, MinDotProduct, IsValid)
|
||||
};
|
||||
|
||||
struct ConstraintViolation
|
||||
{
|
||||
std::string ConstraintType;
|
||||
int Frame = 0;
|
||||
double Time = 0.0;
|
||||
double Value = 0.0;
|
||||
std::string AllowedRange;
|
||||
std::string Severity;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(ConstraintViolation, ConstraintType, Frame, Time, Value, AllowedRange, Severity)
|
||||
};
|
||||
|
||||
struct ConstraintData
|
||||
{
|
||||
AngleConstraint AB1_AB_Constraint;
|
||||
DistanceConstraint C3C4_Constraint;
|
||||
DistanceConstraint CL_Distance_Constraint;
|
||||
PerpendicularityConstraint Perpendicularity_Constraint;
|
||||
std::vector<ConstraintViolation> ConstraintViolations;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(ConstraintData, AB1_AB_Constraint, C3C4_Constraint,
|
||||
CL_Distance_Constraint, Perpendicularity_Constraint, ConstraintViolations)
|
||||
};
|
||||
|
||||
struct AngleStatistics
|
||||
{
|
||||
MinMax AB_Horizontal;
|
||||
MinMax AB_BC;
|
||||
MinMax BC_CL;
|
||||
MinMax AB1_AB;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(AngleStatistics, AB_Horizontal, AB_BC, BC_CL, AB1_AB)
|
||||
};
|
||||
|
||||
struct DistanceStatistics
|
||||
{
|
||||
MinMax C3_C4;
|
||||
MinMax C2_C3;
|
||||
MinMax C2_C4;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(DistanceStatistics, C3_C4, C2_C3, C2_C4)
|
||||
};
|
||||
|
||||
struct MotorSpeedStatistics
|
||||
{
|
||||
MinMax Thigh;
|
||||
MinMax Shank;
|
||||
MinMax Ankle;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(MotorSpeedStatistics, Thigh, Shank, Ankle)
|
||||
};
|
||||
|
||||
struct TrajectoryStatistics
|
||||
{
|
||||
int TotalFrames = 0;
|
||||
double TotalTime = 0.0;
|
||||
int SwingFrames = 0;
|
||||
int SupportFrames = 0;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(TrajectoryStatistics, TotalFrames, TotalTime, SwingFrames, SupportFrames)
|
||||
};
|
||||
|
||||
struct StatisticsData
|
||||
{
|
||||
AngleStatistics AngleStatistics;
|
||||
DistanceStatistics DistanceStatistics;
|
||||
MotorSpeedStatistics MotorSpeedStatistics;
|
||||
TrajectoryStatistics TrajectoryStatistics;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(StatisticsData, AngleStatistics, DistanceStatistics,
|
||||
MotorSpeedStatistics, TrajectoryStatistics)
|
||||
};
|
||||
|
||||
struct RawInitialAngles
|
||||
{
|
||||
double Thigh = 0.0;
|
||||
double Shank = 0.0;
|
||||
double Ankle = 0.0;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(RawInitialAngles, Thigh, Shank, Ankle)
|
||||
};
|
||||
|
||||
struct CalibratedAngles
|
||||
{
|
||||
double Thigh = 0.0;
|
||||
double Shank = 0.0;
|
||||
double Ankle = 0.0;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(CalibratedAngles, Thigh, Shank, Ankle)
|
||||
};
|
||||
|
||||
struct Adjustments
|
||||
{
|
||||
double ThighAdjustment = 0.0;
|
||||
double ShankAdjustment = 0.0;
|
||||
double AnkleAdjustment = 0.0;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(Adjustments, ThighAdjustment, ShankAdjustment, AnkleAdjustment)
|
||||
};
|
||||
|
||||
struct LegInitialAngles
|
||||
{
|
||||
std::string LegCode;
|
||||
RawInitialAngles RawInitialAngles;
|
||||
CalibratedAngles CalibratedAngles;
|
||||
Adjustments Adjustments;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(LegInitialAngles, LegCode, RawInitialAngles, CalibratedAngles, Adjustments)
|
||||
};
|
||||
|
||||
struct InitialAnglesData
|
||||
{
|
||||
std::map<std::string, LegInitialAngles> Legs;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(InitialAnglesData, Legs)
|
||||
};
|
||||
|
||||
struct PhaseInfo
|
||||
{
|
||||
std::string GaitType;
|
||||
double TimeLF = 0.0;
|
||||
double TimeLH = 0.0;
|
||||
double TimeRF = 0.0;
|
||||
double TimeRH = 0.0;
|
||||
int PhaseLF = 0;
|
||||
int PhaseLH = 0;
|
||||
int PhaseRF = 0;
|
||||
int PhaseRH = 0;
|
||||
double SupportRatio = 0.0;
|
||||
double SwingRatio = 0.0;
|
||||
double SupportTime = 0.0;
|
||||
double SwingTime = 0.0;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(PhaseInfo, GaitType, TimeLF, TimeLH, TimeRF, TimeRH,
|
||||
PhaseLF, PhaseLH, PhaseRF, PhaseRH, SupportRatio,
|
||||
SwingRatio, SupportTime, SwingTime)
|
||||
};
|
||||
|
||||
struct MotionRange
|
||||
{
|
||||
MinMax X;
|
||||
MinMax Y;
|
||||
double TotalRange = 0.0;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(MotionRange, X, Y, TotalRange)
|
||||
};
|
||||
|
||||
struct LegMotionRange
|
||||
{
|
||||
std::string LegCode;
|
||||
int PhaseShift = 0;
|
||||
MotionRange L_Point;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(LegMotionRange, LegCode, PhaseShift, L_Point)
|
||||
};
|
||||
|
||||
struct MotionRangeData
|
||||
{
|
||||
std::map<std::string, LegMotionRange> Legs;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(MotionRangeData, Legs)
|
||||
};
|
||||
|
||||
struct SplitModelID
|
||||
{
|
||||
std::vector<std::map<std::string, std::vector<std::string>>> LF;
|
||||
std::vector<std::map<std::string, std::vector<std::string>>> LH;
|
||||
std::vector<std::map<std::string, std::vector<std::string>>> RF;
|
||||
std::vector<std::map<std::string, std::vector<std::string>>> RH;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(SplitModelID, LF, LH, RF, RH)
|
||||
};
|
||||
|
||||
// 支撑点检查结果类
|
||||
struct SupportCheckResult
|
||||
{
|
||||
bool IsSupport = false;
|
||||
std::map<std::string, CheckResultItem> Checks;
|
||||
std::vector<std::string> Reasons;
|
||||
std::vector<std::string> FailedChecks;
|
||||
|
||||
SupportCheckResult();
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(SupportCheckResult, IsSupport, Checks, Reasons, FailedChecks)
|
||||
};
|
||||
|
||||
class MotorData
|
||||
{
|
||||
public:
|
||||
std::vector<double> frame_start_times;
|
||||
std::vector<double> frame_end_times;
|
||||
std::vector<double> AB_angles;
|
||||
std::vector<double> AB_angle_increments;
|
||||
std::vector<double> thigh_motor_speeds;
|
||||
std::vector<double> AB1_AB_angles;
|
||||
std::vector<double> AB1_AB_angle_increments;
|
||||
std::vector<double> shank_motor_speeds;
|
||||
std::vector<double> C3C4_distances;
|
||||
std::vector<double> C3C4_distance_increments;
|
||||
std::vector<double> ankle_motor_angles;
|
||||
std::vector<double> ankle_motor_angle_increments;
|
||||
std::vector<double> ankle_motor_speeds;
|
||||
|
||||
MotorData(int n_frames = 0);
|
||||
void Resize(int n_frames);
|
||||
MotorData Clone() const;
|
||||
|
||||
friend void to_json(json &j, const MotorData &md);
|
||||
friend void from_json(const json &j, MotorData &md);
|
||||
};
|
||||
|
||||
struct ReverseCalculationResult
|
||||
{
|
||||
int Frame = 0;
|
||||
double Time = 0.0;
|
||||
std::map<std::string, std::vector<double>> Points;
|
||||
bool Valid = false;
|
||||
std::vector<std::string> Errors;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(ReverseCalculationResult, Frame, Time, Points, Valid, Errors)
|
||||
};
|
||||
|
||||
struct CompleteExportData
|
||||
{
|
||||
GaitInfo GaitInfo;
|
||||
SystemParameters SystemParameters;
|
||||
TrajectoryData TrajectoryData;
|
||||
MotorDataExport MotorData;
|
||||
ConstraintData ConstraintData;
|
||||
StatisticsData Statistics;
|
||||
InitialAnglesData InitialAngles;
|
||||
PhaseInfo PhaseInfo;
|
||||
MotionRangeData MotionRange;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(CompleteExportData, GaitInfo, SystemParameters,
|
||||
TrajectoryData, MotorData, ConstraintData, Statistics,
|
||||
InitialAngles, PhaseInfo, MotionRange)
|
||||
};
|
||||
|
||||
class ModelIDSplitter
|
||||
{
|
||||
public:
|
||||
// 修改参数类型为 const ModelID&
|
||||
static SplitModelID SplitModelIDToArrays(const ModelID &modelID,
|
||||
const std::map<std::string, std::vector<double>> &pointsDict);
|
||||
|
||||
private:
|
||||
static std::vector<std::map<std::string, std::vector<std::string>>>
|
||||
SplitLegDictionaryDataToArrays(const std::vector<std::map<std::string, std::string>> &legData,
|
||||
const std::set<std::string> &validKeys);
|
||||
|
||||
static std::map<std::string, std::vector<std::string>>
|
||||
SplitDictionaryToArrays(const std::map<std::string, std::string> &dict,
|
||||
const std::set<std::string> &validKeys);
|
||||
|
||||
static std::vector<std::string> SplitKey(const std::string &compositeKey,
|
||||
const std::set<std::string> &validKeys);
|
||||
};
|
||||
|
||||
class QuadrupedRobotConfiguration
|
||||
{
|
||||
public:
|
||||
// 基本几何参数
|
||||
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 s_l;
|
||||
double s_h;
|
||||
double X;
|
||||
|
||||
// 脚部构型参数
|
||||
double D1_L_offset;
|
||||
double D2_L_offset;
|
||||
double C4_D2_offset;
|
||||
|
||||
// 步态控制参数
|
||||
double T;
|
||||
double f;
|
||||
std::string gait_type;
|
||||
double support_ratio;
|
||||
double swing_ratio;
|
||||
|
||||
// 电机参数
|
||||
double thigh_motor_reduction;
|
||||
double shank_motor_reduction;
|
||||
double ankle_motor_reduction;
|
||||
|
||||
// 逆向运动学特有参数
|
||||
double adjusted_shank_angles_factory;
|
||||
|
||||
// 初始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_reference_distance;
|
||||
double deltX;
|
||||
double step;
|
||||
double t_s;
|
||||
double t_w;
|
||||
double timeLF;
|
||||
double timeLH;
|
||||
double timeRF;
|
||||
double timeRH;
|
||||
|
||||
QuadrupedRobotConfiguration();
|
||||
QuadrupedRobotConfiguration(const GaitInfo &gaitInfo, const SystemParameters &sysParams);
|
||||
void FromGaitInfoAndSystemParameters(const GaitInfo &gaitInfo, const SystemParameters &sysParams);
|
||||
void CalculateDerivedParameters();
|
||||
|
||||
struct ValidationResultFrame
|
||||
{
|
||||
int Frame = 0;
|
||||
double ThighAngle = 0.0;
|
||||
double ShankAngle = 0.0;
|
||||
double AnkleAngle = 0.0;
|
||||
bool IsValid = false;
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(ValidationResultFrame, Frame, ThighAngle, ShankAngle, AnkleAngle, IsValid)
|
||||
};
|
||||
|
||||
// 在QuadrupedRobotConfiguration类中添加公共方法（BaseClass.h中）
|
||||
void debugPrint() const;
|
||||
|
||||
struct ValidationResult
|
||||
{
|
||||
bool IsValid = false;
|
||||
std::vector<std::string> Errors;
|
||||
std::vector<std::string> Warnings;
|
||||
|
||||
ValidationResult();
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_INTRUSIVE(ValidationResult, IsValid, Errors, Warnings)
|
||||
};
|
||||
|
||||
ValidationResult Validate() const;
|
||||
|
||||
private:
|
||||
void FromSystemParameters(const SystemParameters &sysParams);
|
||||
void FromGaitInfo(const GaitInfo &gaitInfo);
|
||||
|
||||
friend void to_json(json &j, const QuadrupedRobotConfiguration &config);
|
||||
friend void from_json(const json &j, QuadrupedRobotConfiguration &config);
|
||||
};
|
||||
|
||||
class RobotGaitDataManager
|
||||
{
|
||||
private:
|
||||
RobotGaitRequest _data;
|
||||
static std::mutex _lock;
|
||||
|
||||
public:
|
||||
RobotGaitDataManager();
|
||||
const RobotGaitRequest &GetData() const;
|
||||
void SetData(const RobotGaitRequest &data);
|
||||
void LoadFromJson(const nlohmann::json &j, bool useDefaultForMissing = true);
|
||||
void LoadDefaultData();
|
||||
|
||||
ModelID GetModelIDObject() const;
|
||||
|
||||
GaitInfo GetGaitInfo() const;
|
||||
SystemParameters GetSystemParameters() const;
|
||||
std::map<std::string, std::vector<std::map<std::string, std::string>>> GetModelID() const;
|
||||
LegParameters GetParam() const;
|
||||
RobotBody GetRobotBody() const;
|
||||
std::map<std::string, LegParam> GetParamDictionary() const;
|
||||
|
||||
// 在 RobotGaitDataManager 类声明中修改
|
||||
std::map<std::string, std::vector<double>> Get_old_initial_points_dictLF() const;
|
||||
std::map<std::string, std::vector<double>> Get_old_initial_points_dictLH() const;
|
||||
void Set_old_initial_points_dictLF(const std::map<std::string, std::vector<double>> &L_point);
|
||||
void Set_old_initial_points_dictLH(const std::map<std::string, std::vector<double>> &L_point);
|
||||
|
||||
void SetRobotBody_x(const double x);
|
||||
|
||||
private:
|
||||
RobotGaitRequest CreateDefaultRobotGaitRequest() const;
|
||||
RobotGaitRequest FillMissingDataWithDefaults(const RobotGaitRequest &data) const;
|
||||
static bool IsDefaultValue(double value);
|
||||
};
|
||||
|
||||
class SimulationStatistics
|
||||
{
|
||||
public:
|
||||
int TotalFrames = 0;
|
||||
double TotalTime = 0.0;
|
||||
double MinABAngle = 0.0;
|
||||
double MaxABAngle = 0.0;
|
||||
double MinBCAngle = 0.0;
|
||||
double MaxBCAngle = 0.0;
|
||||
double MinAB1ABAngle = 0.0;
|
||||
double MaxAB1ABAngle = 0.0;
|
||||
double MinC3C4Distance = 0.0;
|
||||
double MaxC3C4Distance = 0.0;
|
||||
double MaxThighSpeed = 0.0;
|
||||
double MaxShankSpeed = 0.0;
|
||||
double MaxAnkleSpeed = 0.0;
|
||||
|
||||
SimulationStatistics() = default;
|
||||
explicit SimulationStatistics(const QuadrupedRobotConfiguration &config);
|
||||
|
||||
std::string ToString() const;
|
||||
|
||||
friend void to_json(json &j, const SimulationStatistics &stats);
|
||||
friend void from_json(const json &j, SimulationStatistics &stats);
|
||||
};
|
||||
|
||||
class SplitModelIDExtensions
|
||||
{
|
||||
public:
|
||||
static std::map<std::string, std::map<std::string, std::vector<std::string>>>
|
||||
ToSimpleDictionary(const SplitModelID &splitModelID);
|
||||
|
||||
private:
|
||||
static std::map<std::string, std::vector<std::string>>
|
||||
MergeLegData(const std::vector<std::map<std::string, std::vector<std::string>>> &legData);
|
||||
};
|
||||
|
||||
// 实用函数
|
||||
Point2D ArrayToPoint(const std::vector<double> &array);
|
||||
|
||||
#endif // BASE_CLASS_H
|
||||
Reference in New Issue
Block a user