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
|
||||
51
inc/QuadrupedRobotSimulation/CompleteJsonExporter.h
Normal file
51
inc/QuadrupedRobotSimulation/CompleteJsonExporter.h
Normal file
@@ -0,0 +1,51 @@
|
||||
// CompleteJsonExporter.h
|
||||
#ifndef COMPLETE_JSON_EXPORTER_H
|
||||
#define COMPLETE_JSON_EXPORTER_H
|
||||
|
||||
#include "BaseClass.h"
|
||||
#include "KinematicsSimulation.h"
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <map>
|
||||
#include <sstream>
|
||||
#include <iomanip>
|
||||
|
||||
class CompleteJsonExporter
|
||||
{
|
||||
public:
|
||||
// 主导出函数
|
||||
static std::string ExportCompleteDataJsonString(const QuadrupedRobotSimulation &simulation);
|
||||
|
||||
// 各个组件的创建函数
|
||||
static GaitInfo CreateGaitInfo(const QuadrupedRobotSimulation &simulation);
|
||||
static SystemParameters CreateSystemParameters(const QuadrupedRobotSimulation &simulation);
|
||||
static TrajectoryData CreateTrajectoryData(const QuadrupedRobotSimulation &simulation);
|
||||
static MotorDataExport CreateMotorData(const QuadrupedRobotSimulation &simulation);
|
||||
static ConstraintData CreateConstraintData(const QuadrupedRobotSimulation &simulation);
|
||||
static StatisticsData CreateStatistics(const QuadrupedRobotSimulation &simulation);
|
||||
static InitialAnglesData CreateInitialAngles(const QuadrupedRobotSimulation &simulation);
|
||||
static PhaseInfo CreatePhaseInfo(const QuadrupedRobotSimulation &simulation);
|
||||
static MotionRangeData CreateMotionRange(const QuadrupedRobotSimulation &simulation);
|
||||
|
||||
private:
|
||||
// 辅助函数
|
||||
static Point2D ArrayToPoint(const std::vector<double> &array);
|
||||
|
||||
// 查找最小值和最大值的模板函数
|
||||
template <typename T>
|
||||
static T FindMin(const std::vector<T> &values);
|
||||
|
||||
template <typename T>
|
||||
static T FindMax(const std::vector<T> &values);
|
||||
|
||||
// 检查所有值是否满足条件的模板函数
|
||||
template <typename T>
|
||||
static bool All(const std::vector<T> &values, std::function<bool(const T &)> predicate);
|
||||
|
||||
// 转换函数
|
||||
static std::string DoubleToString(double value, int precision = 6);
|
||||
};
|
||||
|
||||
#endif // COMPLETE_JSON_EXPORTER_H
|
||||
144
inc/QuadrupedRobotSimulation/KinematicsHelper.h
Normal file
144
inc/QuadrupedRobotSimulation/KinematicsHelper.h
Normal file
@@ -0,0 +1,144 @@
|
||||
// KinematicsHelper.h
|
||||
#ifndef KINEMATICS_HELPER_H
|
||||
#define KINEMATICS_HELPER_H
|
||||
|
||||
#include "BaseClass.h"
|
||||
#include "KinematicsSimulation.h"
|
||||
#include "KinematicsReverse.h"
|
||||
#include "CompleteJsonExporter.h"
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
/**
|
||||
* @brief 工具类：提供便捷的正逆解计算方法
|
||||
*/
|
||||
class KinematicsHelper
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief 模拟机器人完整运动
|
||||
*/
|
||||
static void SimRobot();
|
||||
|
||||
/**
|
||||
* @brief 输入三个电机的值，返回各个连杆的姿态
|
||||
* @param jsonInput JSON输入字符串（可选）
|
||||
* @return JSON格式的所有点计算结果
|
||||
*/
|
||||
static json QuadrupedRobot_CalculateAllPointsFromMotorAngles(const std::string &jsonInput = "");
|
||||
/**
|
||||
* @brief 执行正向运动学计算：计算所有轨迹
|
||||
* @param jsonInput JSON输入字符串（可选）
|
||||
* @return JSON格式的正向运动学计算结果
|
||||
*/
|
||||
static json QuadrupedRobot_PerformForwardKinematics(const std::string &jsonInput = "");
|
||||
/**
|
||||
* @brief 单腿运动计算
|
||||
* @param thigh_angle_deg 大腿电机角度（度）
|
||||
* @param shank_angle_deg 小腿电机角度（度）
|
||||
* @param ankle_angle_deg 脚踝电机角度（度）
|
||||
* @param leg_type 腿类型："LF"、"LH"、"RF"、"RH"
|
||||
* @param json JSON输入字符串（可选）
|
||||
* @return JSON格式的单腿计算结果
|
||||
*/
|
||||
static std::string QuadrupedRobot_CalculateAllPointsOnlyOneLegFromMotorAngles(
|
||||
double thigh_angle_deg, double shank_angle_deg, double ankle_angle_deg,
|
||||
const std::string &leg_type = "LF", const std::string &json = "");
|
||||
|
||||
/**
|
||||
* @brief 获得RobotGaitDataManager机器人管理句柄
|
||||
* @param json JSON输入字符串（可选）
|
||||
* @return 共享指针指向RobotGaitDataManager对象
|
||||
*/
|
||||
static std::shared_ptr<RobotGaitDataManager> RobotGaitDataManagerFromJson(const std::string &jsonInput = "", const std::string &robotID = "0");
|
||||
|
||||
/**
|
||||
* @brief 从JSON字符串创建配置对象
|
||||
* @param jsonInput JSON输入字符串（可选）
|
||||
* @return QuadrupedRobotConfiguration对象
|
||||
*/
|
||||
static QuadrupedRobotConfiguration CreateConfigFromJson(const std::string &jsonInput = "");
|
||||
|
||||
/**
|
||||
* @brief 根据机器人的机构数据和机构类型，计算机器人四个腿的所有轨迹数据
|
||||
* @param config 机器人配置
|
||||
* @return JSON格式的正向运动学计算结果
|
||||
*/
|
||||
static std::string PerformForwardKinematics(const QuadrupedRobotConfiguration &config);
|
||||
|
||||
/**
|
||||
* @brief 导出完整数据为JSON字符串
|
||||
* @param config 机器人配置
|
||||
* @return JSON格式的完整导出数据
|
||||
*/
|
||||
static std::string ExportCompleteData(const QuadrupedRobotConfiguration &config);
|
||||
|
||||
/**
|
||||
* @brief 批量逆向计算
|
||||
* @param motor_data 电机数据映射
|
||||
* @param leg_type 腿类型
|
||||
* @param max_frames 最大帧数（可选）
|
||||
* @param jsonInput 配置JSON（可选）
|
||||
* @return 逆向计算结果向量
|
||||
*/
|
||||
static 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,
|
||||
const std::string &jsonInput = "");
|
||||
|
||||
/**
|
||||
* @brief 计算并验证支撑点
|
||||
* @param pointsDict 点字典
|
||||
* @param groundHeight 地面高度
|
||||
* @param tolerance 容差
|
||||
* @param json 配置JSON（可选）
|
||||
* @return 支撑点检查结果
|
||||
*/
|
||||
static SupportCheckResult CheckSupportPoint(
|
||||
const std::map<std::string, std::vector<double>> &pointsDict,
|
||||
double groundHeight = 0.0,
|
||||
double tolerance = 1.0,
|
||||
const std::string &json = "");
|
||||
|
||||
/**
|
||||
* @brief 生成线框播放数据
|
||||
* @param frameRate 帧率
|
||||
* @param modelCode 模型代码
|
||||
* @param positions 位置数组
|
||||
* @param quaternions 四元数数组
|
||||
* @return P_OPERATION对象
|
||||
*/
|
||||
static P_OPERATION MakeOperation(
|
||||
int frameRate,
|
||||
const std::string &modelCode,
|
||||
const std::vector<std::vector<double>> &positions,
|
||||
const std::vector<std::vector<double>> &quaternions);
|
||||
|
||||
/**
|
||||
* @brief 等待函数（用于模拟延迟）
|
||||
* @param milliseconds 等待的毫秒数
|
||||
*/
|
||||
static void Wait(int milliseconds)
|
||||
{
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(milliseconds));
|
||||
}
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief 从JSON创建默认配置
|
||||
* @return 默认机器人步态请求
|
||||
*/
|
||||
static RobotGaitRequest CreateDefaultRequest();
|
||||
|
||||
/**
|
||||
* @brief 加载并验证JSON数据
|
||||
* @param json JSON字符串
|
||||
* @return 验证后的RobotGaitRequest对象
|
||||
*/
|
||||
static RobotGaitRequest LoadAndValidateJson(const std::string &jsonInput);
|
||||
};
|
||||
|
||||
#endif // KINEMATICS_HELPER_H
|
||||
219
inc/QuadrupedRobotSimulation/KinematicsReverse.h
Normal file
219
inc/QuadrupedRobotSimulation/KinematicsReverse.h
Normal file
@@ -0,0 +1,219 @@
|
||||
// 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 "BaseClass.h"
|
||||
#include "OPERATION.h"
|
||||
#include "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
|
||||
201
inc/QuadrupedRobotSimulation/KinematicsSimulation.h
Normal file
201
inc/QuadrupedRobotSimulation/KinematicsSimulation.h
Normal file
@@ -0,0 +1,201 @@
|
||||
// KinematicsSimulation.h
|
||||
#ifndef KINEMATICS_SIMULATION_H
|
||||
#define KINEMATICS_SIMULATION_H
|
||||
|
||||
#include "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
|
||||
347
inc/QuadrupedRobotSimulation/OPERATION.h
Normal file
347
inc/QuadrupedRobotSimulation/OPERATION.h
Normal file
@@ -0,0 +1,347 @@
|
||||
// OPERATION.h
|
||||
#ifndef OPERATION_H
|
||||
#define OPERATION_H
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
#include "../inc/nlohmann/json.hpp"
|
||||
using json = nlohmann::json;
|
||||
|
||||
// 毫米转米函数
|
||||
inline double MMToM(double MM)
|
||||
{
|
||||
return std::round(MM / 1000.0 * 100000.0) / 100000.0; // 保留5位小数
|
||||
}
|
||||
|
||||
// 帧内容类
|
||||
class C_ObjStates
|
||||
{
|
||||
public:
|
||||
std::string i = ""; // 模型代码
|
||||
std::string tx = ""; // tx
|
||||
std::string ty = ""; // ty
|
||||
std::string tz = ""; // tz
|
||||
std::string qx = ""; // qx
|
||||
std::string qy = ""; // qy
|
||||
std::string qz = ""; // qz
|
||||
std::string qw = ""; // qw
|
||||
};
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(C_ObjStates, i, tx, ty, tz, qx, qy, qz, qw)
|
||||
|
||||
// 工艺类
|
||||
class C_Craft
|
||||
{
|
||||
public:
|
||||
std::string CraftCode;
|
||||
std::string CraftValue;
|
||||
};
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(C_Craft, CraftCode, CraftValue)
|
||||
|
||||
// 帧信号类
|
||||
class C_Signalwrite
|
||||
{
|
||||
public:
|
||||
std::string TagID;
|
||||
std::string TagValue;
|
||||
};
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(C_Signalwrite, TagID, TagValue)
|
||||
|
||||
// 可见性类
|
||||
class C_Visible
|
||||
{
|
||||
public:
|
||||
std::string ModelCode;
|
||||
std::string Visible;
|
||||
};
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(C_Visible, ModelCode, Visible)
|
||||
|
||||
// 旋转类
|
||||
class C_Rotation
|
||||
{
|
||||
public:
|
||||
std::string ModelCode;
|
||||
std::string Axis;
|
||||
int Rate = 0;
|
||||
};
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(C_Rotation, ModelCode, Axis, Rate)
|
||||
|
||||
// 等待类
|
||||
class C_Wait
|
||||
{
|
||||
public:
|
||||
std::string WaitStr;
|
||||
};
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(C_Wait, WaitStr)
|
||||
|
||||
// 附加类
|
||||
class C_Attach
|
||||
{
|
||||
public:
|
||||
std::string AttachToModelCode = "";
|
||||
std::string AttachToModelName = "";
|
||||
bool IsTwoWay = false;
|
||||
bool IsAttach = false;
|
||||
};
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(C_Attach, AttachToModelCode, AttachToModelName, IsTwoWay, IsAttach)
|
||||
|
||||
// 帧类
|
||||
class C_Frames
|
||||
{
|
||||
public:
|
||||
std::string time = "0.01"; // 帧时间
|
||||
std::vector<C_ObjStates> objStates;
|
||||
std::vector<C_Craft> crafts;
|
||||
std::vector<C_Signalwrite> signalwrites;
|
||||
std::vector<C_Visible> visibles;
|
||||
std::vector<C_Rotation> rotations;
|
||||
std::vector<C_Wait> waits;
|
||||
std::vector<C_Attach> attachs;
|
||||
};
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(C_Frames, time, objStates, crafts, signalwrites,
|
||||
visibles, rotations, waits, attachs)
|
||||
|
||||
// 操作类（帧集合）
|
||||
class C_OPERATION
|
||||
{
|
||||
public:
|
||||
std::vector<C_Frames> frames;
|
||||
};
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(C_OPERATION, frames)
|
||||
|
||||
// 项目操作类
|
||||
class P_OPERATION
|
||||
{
|
||||
public:
|
||||
C_OPERATION OPERATION;
|
||||
};
|
||||
|
||||
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(P_OPERATION, OPERATION)
|
||||
// 功能类
|
||||
class P_OPERATION_Func
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* 生成线框播放数据
|
||||
* @param PP_FrameValue 帧率值
|
||||
* @param modelCode 模型代码
|
||||
* @param ptQList 点位置和四元数列表 [x, y, z, qx, qy, qz, qw]
|
||||
* @return 帧列表
|
||||
*/
|
||||
static std::vector<C_Frames> _MakeCraftPlayData_Line_Frame(
|
||||
int PP_FrameValue,
|
||||
const std::string &modelCode,
|
||||
const std::vector<std::vector<double>> &ptQList)
|
||||
{
|
||||
|
||||
std::vector<C_Frames> frames;
|
||||
|
||||
for (size_t i = 0; i < ptQList.size(); i++)
|
||||
{
|
||||
const auto &currQ = ptQList[i];
|
||||
|
||||
if (currQ.size() < 7)
|
||||
{
|
||||
// 数据不足，跳过
|
||||
continue;
|
||||
}
|
||||
|
||||
// 创建对象状态
|
||||
C_ObjStates objState;
|
||||
objState.i = modelCode;
|
||||
objState.tx = std::to_string(MMToM(currQ[0]));
|
||||
objState.ty = std::to_string(MMToM(currQ[1]));
|
||||
objState.tz = std::to_string(MMToM(currQ[2]));
|
||||
objState.qx = std::to_string(currQ[3]);
|
||||
objState.qy = std::to_string(currQ[4]);
|
||||
objState.qz = std::to_string(currQ[5]);
|
||||
objState.qw = std::to_string(currQ[6]);
|
||||
|
||||
// 创建帧
|
||||
C_Frames frame;
|
||||
frame.time = std::to_string(PP_FrameValue * i / 1000.0);
|
||||
frame.objStates.push_back(objState);
|
||||
|
||||
frames.push_back(frame);
|
||||
}
|
||||
|
||||
return frames;
|
||||
}
|
||||
|
||||
/**
|
||||
* 生成线框播放数据（简化版本）
|
||||
* @param frameRate 帧率
|
||||
* @param modelCode 模型代码
|
||||
* @param positions 位置数组 [x, y, z] (毫米)
|
||||
* @param quaternions 四元数数组 [qx, qy, qz, qw]
|
||||
* @return 帧列表
|
||||
*/
|
||||
static std::vector<C_Frames> MakeCraftPlayData_Simple(
|
||||
int frameRate,
|
||||
const std::string &modelCode,
|
||||
const std::vector<std::vector<double>> &positions,
|
||||
const std::vector<std::vector<double>> &quaternions)
|
||||
{
|
||||
|
||||
if (positions.size() != quaternions.size())
|
||||
{
|
||||
// 数据长度不匹配，返回空列表
|
||||
return {};
|
||||
}
|
||||
|
||||
std::vector<C_Frames> frames;
|
||||
|
||||
for (size_t i = 0; i < positions.size(); i++)
|
||||
{
|
||||
if (positions[i].size() < 3 || quaternions[i].size() < 4)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
// 创建对象状态
|
||||
C_ObjStates objState;
|
||||
objState.i = modelCode;
|
||||
objState.tx = std::to_string(MMToM(positions[i][0]));
|
||||
objState.ty = std::to_string(MMToM(positions[i][1]));
|
||||
objState.tz = std::to_string(MMToM(positions[i][2]));
|
||||
objState.qx = std::to_string(quaternions[i][0]);
|
||||
objState.qy = std::to_string(quaternions[i][1]);
|
||||
objState.qz = std::to_string(quaternions[i][2]);
|
||||
objState.qw = std::to_string(quaternions[i][3]);
|
||||
|
||||
// 创建帧
|
||||
C_Frames frame;
|
||||
frame.time = std::to_string(i / static_cast<double>(frameRate));
|
||||
frame.objStates.push_back(objState);
|
||||
|
||||
frames.push_back(frame);
|
||||
}
|
||||
|
||||
return frames;
|
||||
}
|
||||
|
||||
/**
|
||||
* 生成完整的操作对象
|
||||
* @param frameRate 帧率
|
||||
* @param modelCode 模型代码
|
||||
* @param positions 位置数组
|
||||
* @param quaternions 四元数数组
|
||||
* @return P_OPERATION对象
|
||||
*/
|
||||
static P_OPERATION MakeOperation(
|
||||
int frameRate,
|
||||
const std::string &modelCode,
|
||||
const std::vector<std::vector<double>> &positions,
|
||||
const std::vector<std::vector<double>> &quaternions)
|
||||
{
|
||||
|
||||
auto frames = MakeCraftPlayData_Simple(frameRate, modelCode, positions, quaternions);
|
||||
|
||||
P_OPERATION operation;
|
||||
operation.OPERATION.frames = frames;
|
||||
|
||||
return operation;
|
||||
}
|
||||
|
||||
/**
|
||||
* 添加信号写入到帧
|
||||
* @param frame 目标帧
|
||||
* @param tagID 标签ID
|
||||
* @param tagValue 标签值
|
||||
*/
|
||||
static void AddSignalWrite(C_Frames &frame, const std::string &tagID, const std::string &tagValue)
|
||||
{
|
||||
C_Signalwrite signal;
|
||||
signal.TagID = tagID;
|
||||
signal.TagValue = tagValue;
|
||||
frame.signalwrites.push_back(signal);
|
||||
}
|
||||
|
||||
/**
|
||||
* 添加工艺命令到帧
|
||||
* @param frame 目标帧
|
||||
* @param craftCode 工艺代码
|
||||
* @param craftValue 工艺值
|
||||
*/
|
||||
static void AddCraft(C_Frames &frame, const std::string &craftCode, const std::string &craftValue)
|
||||
{
|
||||
C_Craft craft;
|
||||
craft.CraftCode = craftCode;
|
||||
craft.CraftValue = craftValue;
|
||||
frame.crafts.push_back(craft);
|
||||
}
|
||||
|
||||
/**
|
||||
* 添加可见性控制到帧
|
||||
* @param frame 目标帧
|
||||
* @param modelCode 模型代码
|
||||
* @param visible 是否可见 ("true" 或 "false")
|
||||
*/
|
||||
static void AddVisible(C_Frames &frame, const std::string &modelCode, const std::string &visible)
|
||||
{
|
||||
C_Visible visibleCmd;
|
||||
visibleCmd.ModelCode = modelCode;
|
||||
visibleCmd.Visible = visible;
|
||||
frame.visibles.push_back(visibleCmd);
|
||||
}
|
||||
|
||||
/**
|
||||
* 添加旋转控制到帧
|
||||
* @param frame 目标帧
|
||||
* @param modelCode 模型代码
|
||||
* @param axis 旋转轴 ("x", "y", "z")
|
||||
* @param rate 旋转速率
|
||||
*/
|
||||
static void AddRotation(C_Frames &frame, const std::string &modelCode,
|
||||
const std::string &axis, int rate)
|
||||
{
|
||||
C_Rotation rotation;
|
||||
rotation.ModelCode = modelCode;
|
||||
rotation.Axis = axis;
|
||||
rotation.Rate = rate;
|
||||
frame.rotations.push_back(rotation);
|
||||
}
|
||||
|
||||
/**
|
||||
* 添加等待命令到帧
|
||||
* @param frame 目标帧
|
||||
* @param waitStr 等待字符串
|
||||
*/
|
||||
static void AddWait(C_Frames &frame, const std::string &waitStr)
|
||||
{
|
||||
C_Wait wait;
|
||||
wait.WaitStr = waitStr;
|
||||
frame.waits.push_back(wait);
|
||||
}
|
||||
|
||||
/**
|
||||
* 添加附加命令到帧
|
||||
* @param frame 目标帧
|
||||
* @param attachToModelCode 附加目标模型代码
|
||||
* @param attachToModelName 附加目标模型名称
|
||||
* @param isTwoWay 是否双向附加
|
||||
* @param isAttach 是否附加（true=附加，false=分离）
|
||||
*/
|
||||
static void AddAttach(C_Frames &frame, const std::string &attachToModelCode,
|
||||
const std::string &attachToModelName,
|
||||
bool isTwoWay, bool isAttach)
|
||||
{
|
||||
C_Attach attach;
|
||||
attach.AttachToModelCode = attachToModelCode;
|
||||
attach.AttachToModelName = attachToModelName;
|
||||
attach.IsTwoWay = isTwoWay;
|
||||
attach.IsAttach = isAttach;
|
||||
frame.attachs.push_back(attach);
|
||||
}
|
||||
};
|
||||
|
||||
#endif // OPERATION_H
|
||||
173
inc/QuadrupedRobotSimulation/RobotConfig.hpp
Normal file
173
inc/QuadrupedRobotSimulation/RobotConfig.hpp
Normal file
@@ -0,0 +1,173 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <map>
|
||||
#include <nlohmann/json.hpp>
|
||||
#include <fstream>
|
||||
#include <sstream>
|
||||
#include <iostream> // 必须包含这个头文件
|
||||
#include <iomanip>
|
||||
// 前置声明
|
||||
struct GaitInfo;
|
||||
struct SystemParameters;
|
||||
struct ComponentID;
|
||||
struct LegModelIDs;
|
||||
struct ModelID;
|
||||
struct LegParam;
|
||||
struct LegParameters;
|
||||
struct RobotBody;
|
||||
struct RequestParameters;
|
||||
|
||||
// GaitInfo 结构
|
||||
struct GaitInfo
|
||||
{
|
||||
std::string GaitType;
|
||||
double Period;
|
||||
double Frequency;
|
||||
double StepTime;
|
||||
double SupportTime;
|
||||
double SwingTime;
|
||||
double SupportRatio;
|
||||
double SwingRatio;
|
||||
int TotalFrames;
|
||||
double TotalTime;
|
||||
};
|
||||
|
||||
// SystemParameters 结构
|
||||
struct SystemParameters
|
||||
{
|
||||
double A_x;
|
||||
double A_y;
|
||||
double L10;
|
||||
double L20;
|
||||
double L30;
|
||||
double StepLength;
|
||||
double StepHeight;
|
||||
double X;
|
||||
double DeltaX;
|
||||
double L21;
|
||||
double L22;
|
||||
double L23;
|
||||
double Beta1;
|
||||
double BB2_BC_Angle;
|
||||
double C1_B_Length;
|
||||
double CC1;
|
||||
double L31;
|
||||
double C2C3_Length;
|
||||
double Lead;
|
||||
double D1_L_Offset;
|
||||
double D2_L_Offset;
|
||||
double C4_D2_Offset;
|
||||
double ThighMotorReduction;
|
||||
double ShankMotorReduction;
|
||||
double AnkleMotorReduction;
|
||||
};
|
||||
|
||||
// ComponentID 结构
|
||||
struct ComponentID
|
||||
{
|
||||
std::string name;
|
||||
std::string uuid;
|
||||
};
|
||||
|
||||
// LegModelIDs 结构
|
||||
struct LegModelIDs
|
||||
{
|
||||
std::vector<ComponentID> components;
|
||||
};
|
||||
|
||||
// ModelID 结构
|
||||
struct ModelID
|
||||
{
|
||||
LegModelIDs LF;
|
||||
LegModelIDs LH;
|
||||
LegModelIDs RF;
|
||||
LegModelIDs RH;
|
||||
};
|
||||
|
||||
// LegParam 结构
|
||||
struct LegParam
|
||||
{
|
||||
double thigh_angle_deg;
|
||||
double shank_angle_deg;
|
||||
double ankle_angle_deg;
|
||||
};
|
||||
|
||||
// LegParameters 结构
|
||||
struct LegParameters
|
||||
{
|
||||
LegParam LF;
|
||||
LegParam LH;
|
||||
LegParam RF;
|
||||
LegParam RH;
|
||||
};
|
||||
|
||||
// RobotBody 结构
|
||||
struct RobotBody
|
||||
{
|
||||
std::string BodyCode;
|
||||
double x;
|
||||
double y;
|
||||
double z;
|
||||
double qx;
|
||||
double qy;
|
||||
double qz;
|
||||
double qw;
|
||||
};
|
||||
|
||||
// RequestParameters 主结构
|
||||
struct RequestParameters
|
||||
{
|
||||
GaitInfo GaitInfo;
|
||||
SystemParameters SystemParameters;
|
||||
ModelID ModelID;
|
||||
LegParameters Param;
|
||||
RobotBody RobotBody;
|
||||
std::string RobotID = "0";
|
||||
double t_percentage;
|
||||
// 初始化静态变量 左腿和右腿
|
||||
std::map<std::string, std::vector<double>> old_initial_points_dictLF;
|
||||
std::map<std::string, std::vector<double>> old_initial_points_dictLH;
|
||||
// 静态方法
|
||||
static RequestParameters fromJsonString(const std::string &jsonStr);
|
||||
static RequestParameters fromJsonFile(const std::string &filename);
|
||||
// 静态函数版本
|
||||
static RequestParameters &updateFromJson(const nlohmann::json &j, RequestParameters &req_param);
|
||||
|
||||
// 序列化为JSON字符串
|
||||
std::string toJsonString() const;
|
||||
|
||||
// 保存到文件
|
||||
bool saveToFile(const std::string &filename) const;
|
||||
// 显示所有数据的调试函数
|
||||
void debugPrint() const;
|
||||
};
|
||||
|
||||
// JSON序列化/反序列化声明
|
||||
void to_json(nlohmann::json &j, const GaitInfo &g);
|
||||
void from_json(const nlohmann::json &j, GaitInfo &g);
|
||||
|
||||
void to_json(nlohmann::json &j, const SystemParameters &s);
|
||||
void from_json(const nlohmann::json &j, SystemParameters &s);
|
||||
|
||||
void to_json(nlohmann::json &j, const ComponentID &c);
|
||||
void from_json(const nlohmann::json &j, ComponentID &c);
|
||||
|
||||
void to_json(nlohmann::json &j, const LegModelIDs &l);
|
||||
void from_json(const nlohmann::json &j, LegModelIDs &l);
|
||||
|
||||
void to_json(nlohmann::json &j, const ModelID &m);
|
||||
void from_json(const nlohmann::json &j, ModelID &m);
|
||||
|
||||
void to_json(nlohmann::json &j, const LegParam &l);
|
||||
void from_json(const nlohmann::json &j, LegParam &l);
|
||||
|
||||
void to_json(nlohmann::json &j, const LegParameters &l);
|
||||
void from_json(const nlohmann::json &j, LegParameters &l);
|
||||
|
||||
void to_json(nlohmann::json &j, const RobotBody &r);
|
||||
void from_json(const nlohmann::json &j, RobotBody &r);
|
||||
|
||||
void to_json(nlohmann::json &j, const RequestParameters &c);
|
||||
void from_json(const nlohmann::json &j, RequestParameters &c);
|
||||
109
inc/QuadrupedRobotSimulation/graph_utils.hpp
Normal file
109
inc/QuadrupedRobotSimulation/graph_utils.hpp
Normal file
@@ -0,0 +1,109 @@
|
||||
// graph_utils.hpp
|
||||
#pragma once
|
||||
#include <iostream>
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
class GraphUtils
|
||||
{
|
||||
private:
|
||||
// 定义边类型
|
||||
using Edge = std::pair<std::string, std::string>;
|
||||
using EdgeMap = std::map<std::string, Edge>;
|
||||
|
||||
// 静态数据成员
|
||||
static EdgeMap graphData;
|
||||
|
||||
// 初始化静态数据
|
||||
static EdgeMap initGraphData()
|
||||
{
|
||||
EdgeMap data;
|
||||
data["AB"] = {"A", "B"};
|
||||
data["AB1"] = {"A", "B1"};
|
||||
data["B1B2"] = {"B1", "B2"};
|
||||
data["BC"] = {"B", "C"};
|
||||
data["C2C3"] = {"C2", "C3"};
|
||||
data["C4C3"] = {"C4", "C3"};
|
||||
data["CC4"] = {"C", "C4"};
|
||||
return data;
|
||||
}
|
||||
|
||||
public:
|
||||
// 静态方法 - 获取所有边
|
||||
static const EdgeMap &getGraph()
|
||||
{
|
||||
static EdgeMap instance = initGraphData();
|
||||
return instance;
|
||||
}
|
||||
|
||||
// 获取边的两个节点
|
||||
static std::pair<std::string, std::string> getNodes(const std::string &edgeName)
|
||||
{
|
||||
const auto &graph = getGraph();
|
||||
auto it = graph.find(edgeName);
|
||||
if (it != graph.end())
|
||||
{
|
||||
return it->second;
|
||||
}
|
||||
return {"", ""}; // 返回空值表示未找到
|
||||
}
|
||||
|
||||
// 获取所有边名
|
||||
static std::vector<std::string> getEdgeNames()
|
||||
{
|
||||
std::vector<std::string> names;
|
||||
const auto &graph = getGraph();
|
||||
names.reserve(graph.size());
|
||||
for (const auto &pair : graph)
|
||||
{
|
||||
names.push_back(pair.first);
|
||||
}
|
||||
return names;
|
||||
}
|
||||
|
||||
// 导出为字符串
|
||||
static std::string toString()
|
||||
{
|
||||
const auto &graph = getGraph();
|
||||
std::string result = "{\n";
|
||||
for (const auto &pair : graph)
|
||||
{
|
||||
result += " \"" + pair.first + "\": [\"" +
|
||||
pair.second.first + "\", \"" +
|
||||
pair.second.second + "\"],\n";
|
||||
}
|
||||
// 移除最后一个逗号
|
||||
if (!graph.empty())
|
||||
{
|
||||
result.pop_back(); // 移除换行符
|
||||
result.pop_back(); // 移除逗号
|
||||
result += "\n";
|
||||
}
|
||||
result += "}";
|
||||
return result;
|
||||
}
|
||||
|
||||
// 查找包含节点的边
|
||||
static std::vector<std::string> findEdgesWithNode(const std::string &node)
|
||||
{
|
||||
std::vector<std::string> result;
|
||||
const auto &graph = getGraph();
|
||||
|
||||
for (const auto &pair : graph)
|
||||
{
|
||||
if (pair.second.first == node || pair.second.second == node)
|
||||
{
|
||||
result.push_back(pair.first);
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
// 显示图形
|
||||
static void display()
|
||||
{
|
||||
std::cout << "Graph Structure:" << std::endl;
|
||||
std::cout << toString() << std::endl;
|
||||
}
|
||||
};
|
||||
Reference in New Issue
Block a user