Initial commit
This commit is contained in:
zhangshun
714
src/QuadrupedRobotSimulation/CompleteJsonExporter.cpp
Normal file
714
src/QuadrupedRobotSimulation/CompleteJsonExporter.cpp
Normal file
@@ -0,0 +1,714 @@
|
||||
// CompleteJsonExporter.cpp
|
||||
#include "CompleteJsonExporter.h"
|
||||
#include "BaseClass.h"
|
||||
#include "KinematicsSimulation.h"
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <functional>
|
||||
#include <sstream>
|
||||
#include <iomanip>
|
||||
#include <limits>
|
||||
|
||||
// 主导出函数
|
||||
std::string CompleteJsonExporter::ExportCompleteDataJsonString(const QuadrupedRobotSimulation &simulation)
|
||||
{
|
||||
try
|
||||
{
|
||||
// 创建数据对象
|
||||
CompleteExportData exportData;
|
||||
|
||||
// 基本信息
|
||||
exportData.GaitInfo = CreateGaitInfo(simulation);
|
||||
|
||||
// 系统参数
|
||||
exportData.SystemParameters = CreateSystemParameters(simulation);
|
||||
|
||||
// 轨迹数据（所有腿）
|
||||
exportData.TrajectoryData = CreateTrajectoryData(simulation);
|
||||
|
||||
// 电机数据（所有腿）
|
||||
exportData.MotorData = CreateMotorData(simulation);
|
||||
|
||||
// 约束检查数据
|
||||
exportData.ConstraintData = CreateConstraintData(simulation);
|
||||
|
||||
// 统计信息
|
||||
exportData.Statistics = CreateStatistics(simulation);
|
||||
|
||||
// 初始角度数据
|
||||
exportData.InitialAngles = CreateInitialAngles(simulation);
|
||||
|
||||
// 相位信息
|
||||
exportData.PhaseInfo = CreatePhaseInfo(simulation);
|
||||
|
||||
// 运动范围统计
|
||||
exportData.MotionRange = CreateMotionRange(simulation);
|
||||
|
||||
// 序列化为JSON
|
||||
json j = exportData;
|
||||
return j.dump(2); // 缩进2个空格
|
||||
}
|
||||
catch (const std::exception &ex)
|
||||
{
|
||||
std::cerr << "导出JSON数据时出错: " << ex.what() << std::endl;
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
// 创建步态信息
|
||||
GaitInfo CompleteJsonExporter::CreateGaitInfo(const QuadrupedRobotSimulation &simulation)
|
||||
{
|
||||
GaitInfo gaitInfo;
|
||||
gaitInfo.GaitType = simulation.gait_type;
|
||||
gaitInfo.Period = simulation.T;
|
||||
gaitInfo.Frequency = simulation.f;
|
||||
gaitInfo.StepTime = simulation.step;
|
||||
gaitInfo.SupportTime = simulation.t_s;
|
||||
gaitInfo.SwingTime = simulation.t_w;
|
||||
gaitInfo.SupportRatio = simulation.support_ratio;
|
||||
gaitInfo.SwingRatio = simulation.swing_ratio;
|
||||
gaitInfo.TotalFrames = simulation.Trajectory_L.size();
|
||||
gaitInfo.TotalTime = simulation.Trajectory_L.size() * simulation.step;
|
||||
|
||||
return gaitInfo;
|
||||
}
|
||||
|
||||
// 创建系统参数
|
||||
SystemParameters CompleteJsonExporter::CreateSystemParameters(const QuadrupedRobotSimulation &simulation)
|
||||
{
|
||||
SystemParameters systemParams;
|
||||
|
||||
// 基本参数
|
||||
systemParams.L10 = simulation.L10;
|
||||
systemParams.L20 = simulation.L20;
|
||||
systemParams.L30 = simulation.L30;
|
||||
|
||||
// 步态设计参数
|
||||
systemParams.StepLength = simulation.s_l;
|
||||
systemParams.StepHeight = simulation.s_h;
|
||||
systemParams.X = simulation.X;
|
||||
systemParams.DeltaX = simulation.deltX;
|
||||
|
||||
// 小腿连杆参数
|
||||
systemParams.L21 = simulation.L21;
|
||||
systemParams.L22 = simulation.L22;
|
||||
systemParams.L23 = simulation.L23;
|
||||
systemParams.Beta1 = simulation.beta1;
|
||||
systemParams.BB2_BC_Angle = simulation.BB2_BC_angle;
|
||||
systemParams.C1_B_Length = simulation.C1_B_length;
|
||||
|
||||
// 脚踝连杆参数
|
||||
systemParams.CC1 = simulation.CC1;
|
||||
systemParams.L31 = simulation.L31;
|
||||
systemParams.C2C3_Length = simulation.C2C3_length;
|
||||
systemParams.Lead = simulation.lead1;
|
||||
|
||||
// 脚部构型参数
|
||||
systemParams.D1_L_Offset = simulation.D1_L_offset;
|
||||
systemParams.D2_L_Offset = simulation.D2_L_offset;
|
||||
systemParams.C4_D2_Offset = simulation.C4_D2_offset;
|
||||
|
||||
// 电机参数
|
||||
systemParams.ThighMotorReduction = simulation.thigh_motor_reduction;
|
||||
systemParams.ShankMotorReduction = simulation.shank_motor_reduction;
|
||||
systemParams.AnkleMotorReduction = simulation.ankle_motor_reduction;
|
||||
|
||||
return systemParams;
|
||||
}
|
||||
|
||||
// 创建轨迹数据
|
||||
TrajectoryData CompleteJsonExporter::CreateTrajectoryData(const QuadrupedRobotSimulation &simulation)
|
||||
{
|
||||
int n_frames = simulation.Trajectory_L.size();
|
||||
|
||||
// 计算相位偏移帧数
|
||||
int phase_LH = static_cast<int>(simulation.timeLH * n_frames);
|
||||
int phase_RF = static_cast<int>(simulation.timeRF * n_frames);
|
||||
int phase_RH = static_cast<int>(simulation.timeRH * n_frames);
|
||||
|
||||
TrajectoryData trajectoryData;
|
||||
trajectoryData.TotalFrames = n_frames;
|
||||
trajectoryData.StepTime = simulation.step;
|
||||
|
||||
// 为每条腿创建轨迹数据
|
||||
std::vector<std::string> legCodes = {"LF", "LH", "RF", "RH"};
|
||||
std::vector<int> phaseShifts = {0, phase_LH, phase_RF, phase_RH};
|
||||
|
||||
for (size_t i = 0; i < legCodes.size(); i++)
|
||||
{
|
||||
std::string legCode = legCodes[i];
|
||||
int phaseShift = phaseShifts[i];
|
||||
bool isRightLeg = legCode[0] == 'R'; // 检查是否以'R'开头
|
||||
|
||||
LegTrajectory legTraj;
|
||||
legTraj.LegCode = legCode;
|
||||
legTraj.PhaseShift = phaseShift;
|
||||
legTraj.IsRightLeg = isRightLeg;
|
||||
|
||||
for (int frame = 0; frame < n_frames; frame++)
|
||||
{
|
||||
int shiftedIdx = (frame + phaseShift) % n_frames;
|
||||
|
||||
FrameData frameData;
|
||||
frameData.FrameNumber = frame;
|
||||
frameData.Time = frame * simulation.step;
|
||||
frameData.ShiftedFrameNumber = shiftedIdx;
|
||||
|
||||
// 关键点坐标
|
||||
frameData.A_Point = Point2D(simulation.A_point[0], simulation.A_point[1]);
|
||||
frameData.B_Point = ArrayToPoint(simulation.Trajectory_B[shiftedIdx]);
|
||||
frameData.C_Point = ArrayToPoint(simulation.Trajectory_C[shiftedIdx]);
|
||||
frameData.L_Point = ArrayToPoint(simulation.Trajectory_L[shiftedIdx]);
|
||||
frameData.D1_Point = ArrayToPoint(simulation.Trajectory_D1[shiftedIdx]);
|
||||
frameData.D2_Point = ArrayToPoint(simulation.Trajectory_D2[shiftedIdx]);
|
||||
frameData.C4_Point = ArrayToPoint(simulation.Trajectory_C4[shiftedIdx]);
|
||||
frameData.B1_Point = ArrayToPoint(simulation.Trajectory_B1[shiftedIdx]);
|
||||
frameData.B2_Point = ArrayToPoint(simulation.Trajectory_B2[shiftedIdx]);
|
||||
frameData.C1_Point = ArrayToPoint(simulation.Trajectory_C1[shiftedIdx]);
|
||||
frameData.C2_Point = ArrayToPoint(simulation.Trajectory_C2[shiftedIdx]);
|
||||
frameData.C3_Point = ArrayToPoint(simulation.Trajectory_C3[shiftedIdx]);
|
||||
|
||||
// 角度数据
|
||||
frameData.AB_Horizontal_Angle = simulation.AB_horizontal_angles[shiftedIdx];
|
||||
frameData.AB_BC_Angle = simulation.AB_BC_angles[shiftedIdx];
|
||||
frameData.BC_CL_Angle = simulation.BC_CL_angles[shiftedIdx];
|
||||
frameData.AB1_AB_Angle = simulation.AB1_AB_angles[shiftedIdx];
|
||||
|
||||
// 距离数据
|
||||
frameData.C3_C4_Distance = simulation.C3_C4_distances[shiftedIdx];
|
||||
frameData.C2_C3_Distance = simulation.C2_C3_distances[shiftedIdx];
|
||||
frameData.C2_C4_Distance = simulation.C2_C4_distances[shiftedIdx];
|
||||
frameData.C2_C3_C4_Angle = simulation.C2_C3_C4_angles[shiftedIdx];
|
||||
|
||||
// 如果是右侧腿，调整大腿角度（镜像变换）
|
||||
if (isRightLeg)
|
||||
{
|
||||
frameData.AB_Horizontal_Angle = 540 - frameData.AB_Horizontal_Angle;
|
||||
// 确保角度在0-360度范围内
|
||||
while (frameData.AB_Horizontal_Angle >= 360)
|
||||
{
|
||||
frameData.AB_Horizontal_Angle -= 360;
|
||||
}
|
||||
while (frameData.AB_Horizontal_Angle < 0)
|
||||
{
|
||||
frameData.AB_Horizontal_Angle += 360;
|
||||
}
|
||||
}
|
||||
|
||||
legTraj.Frames.push_back(frameData);
|
||||
}
|
||||
|
||||
trajectoryData.Legs[legCode] = legTraj;
|
||||
}
|
||||
|
||||
return trajectoryData;
|
||||
}
|
||||
|
||||
// 创建电机数据
|
||||
MotorDataExport CompleteJsonExporter::CreateMotorData(const QuadrupedRobotSimulation &simulation)
|
||||
{
|
||||
MotorDataExport motorData;
|
||||
motorData.StepTime = simulation.step;
|
||||
|
||||
// 获取四条腿的电机数据
|
||||
std::map<std::string, MotorData> legsData = {
|
||||
{"LF", simulation.motor_data_LF},
|
||||
{"LH", simulation.motor_data_LH},
|
||||
{"RF", simulation.motor_data_RF},
|
||||
{"RH", simulation.motor_data_RH}};
|
||||
|
||||
for (const auto &kvp : legsData)
|
||||
{
|
||||
std::string legCode = kvp.first;
|
||||
const MotorData &data = kvp.second;
|
||||
|
||||
LegMotorData legMotorData;
|
||||
legMotorData.LegCode = legCode;
|
||||
|
||||
int n_frames = data.frame_start_times.size();
|
||||
for (int i = 0; i < n_frames; i++)
|
||||
{
|
||||
MotorFrameData frameData;
|
||||
frameData.FrameNumber = i;
|
||||
frameData.StartTime = data.frame_start_times[i];
|
||||
frameData.EndTime = data.frame_end_times[i];
|
||||
|
||||
// 大腿电机数据
|
||||
frameData.Thigh.Angle = data.AB_angles[i];
|
||||
frameData.Thigh.AngleIncrement = data.AB_angle_increments[i];
|
||||
frameData.Thigh.Speed = data.thigh_motor_speeds[i];
|
||||
|
||||
// 小腿电机数据
|
||||
frameData.Shank.Angle = data.AB1_AB_angles[i];
|
||||
frameData.Shank.AngleIncrement = data.AB1_AB_angle_increments[i];
|
||||
frameData.Shank.Speed = data.shank_motor_speeds[i];
|
||||
|
||||
// 脚踝电机数据
|
||||
frameData.Ankle.Angle = data.ankle_motor_angles[i];
|
||||
frameData.Ankle.AngleIncrement = data.ankle_motor_angle_increments[i];
|
||||
frameData.Ankle.Speed = data.ankle_motor_speeds[i];
|
||||
|
||||
// 丝杠相关数据
|
||||
frameData.C3C4_Distance = data.C3C4_distances[i];
|
||||
frameData.C3C4_DistanceIncrement = data.C3C4_distance_increments[i];
|
||||
|
||||
legMotorData.Frames.push_back(frameData);
|
||||
}
|
||||
|
||||
motorData.Legs[legCode] = legMotorData;
|
||||
}
|
||||
|
||||
return motorData;
|
||||
}
|
||||
|
||||
// 创建约束检查数据
|
||||
ConstraintData CompleteJsonExporter::CreateConstraintData(const QuadrupedRobotSimulation &simulation)
|
||||
{
|
||||
const double AB1_AB_min = 23.0;
|
||||
const double AB1_AB_max = 130.0;
|
||||
const double C3_C4_min = 125.0;
|
||||
const double C3_C4_max = 205.0;
|
||||
|
||||
ConstraintData constraintData;
|
||||
|
||||
// 计算AB1-AB角度的绝对值的最大最小值
|
||||
double ab1_ab_min_actual = std::numeric_limits<double>::max();
|
||||
double ab1_ab_max_actual = std::numeric_limits<double>::lowest();
|
||||
bool ab1_ab_all_valid = true;
|
||||
|
||||
for (double angle : simulation.AB1_AB_angles)
|
||||
{
|
||||
double abs_angle = std::abs(angle);
|
||||
ab1_ab_min_actual = std::min(ab1_ab_min_actual, abs_angle);
|
||||
ab1_ab_max_actual = std::max(ab1_ab_max_actual, abs_angle);
|
||||
|
||||
if (abs_angle < AB1_AB_min || abs_angle > AB1_AB_max)
|
||||
{
|
||||
ab1_ab_all_valid = false;
|
||||
}
|
||||
}
|
||||
|
||||
// 计算C3-C4距离的最大最小值
|
||||
double c3c4_min_actual = std::numeric_limits<double>::max();
|
||||
double c3c4_max_actual = std::numeric_limits<double>::lowest();
|
||||
bool c3c4_all_valid = true;
|
||||
|
||||
for (double distance : simulation.C3_C4_distances)
|
||||
{
|
||||
c3c4_min_actual = std::min(c3c4_min_actual, distance);
|
||||
c3c4_max_actual = std::max(c3c4_max_actual, distance);
|
||||
|
||||
if (distance < C3_C4_min || distance > C3_C4_max)
|
||||
{
|
||||
c3c4_all_valid = false;
|
||||
}
|
||||
}
|
||||
|
||||
// 计算CL距离的最大最小值
|
||||
double cl_min_actual = std::numeric_limits<double>::max();
|
||||
double cl_max_actual = std::numeric_limits<double>::lowest();
|
||||
bool cl_all_valid = true;
|
||||
|
||||
for (double distance : simulation.CL_distances)
|
||||
{
|
||||
cl_min_actual = std::min(cl_min_actual, distance);
|
||||
cl_max_actual = std::max(cl_max_actual, distance);
|
||||
|
||||
if (std::abs(distance - simulation.L30) >= 1e-6)
|
||||
{
|
||||
cl_all_valid = false;
|
||||
}
|
||||
}
|
||||
|
||||
// 计算点积的最大最小值
|
||||
double dot_min_actual = std::numeric_limits<double>::max();
|
||||
double dot_max_actual = std::numeric_limits<double>::lowest();
|
||||
bool dot_all_valid = true;
|
||||
|
||||
for (double dot_product : simulation.dot_products)
|
||||
{
|
||||
dot_min_actual = std::min(dot_min_actual, dot_product);
|
||||
dot_max_actual = std::max(dot_max_actual, dot_product);
|
||||
|
||||
if (std::abs(dot_product) >= 1e-6)
|
||||
{
|
||||
dot_all_valid = false;
|
||||
}
|
||||
}
|
||||
|
||||
// 设置约束数据
|
||||
constraintData.AB1_AB_Constraint.MinAllowed = AB1_AB_min;
|
||||
constraintData.AB1_AB_Constraint.MaxAllowed = AB1_AB_max;
|
||||
constraintData.AB1_AB_Constraint.MinActual = ab1_ab_min_actual;
|
||||
constraintData.AB1_AB_Constraint.MaxActual = ab1_ab_max_actual;
|
||||
constraintData.AB1_AB_Constraint.IsValid = ab1_ab_all_valid;
|
||||
constraintData.AB1_AB_Constraint.CheckType = "绝对值检查";
|
||||
|
||||
constraintData.C3C4_Constraint.MinAllowed = C3_C4_min;
|
||||
constraintData.C3C4_Constraint.MaxAllowed = C3_C4_max;
|
||||
constraintData.C3C4_Constraint.MinActual = c3c4_min_actual;
|
||||
constraintData.C3C4_Constraint.MaxActual = c3c4_max_actual;
|
||||
constraintData.C3C4_Constraint.IsValid = c3c4_all_valid;
|
||||
|
||||
constraintData.CL_Distance_Constraint.MinAllowed = simulation.L30;
|
||||
constraintData.CL_Distance_Constraint.MaxAllowed = simulation.L30;
|
||||
constraintData.CL_Distance_Constraint.MinActual = cl_min_actual;
|
||||
constraintData.CL_Distance_Constraint.MaxActual = cl_max_actual;
|
||||
constraintData.CL_Distance_Constraint.IsValid = cl_all_valid;
|
||||
|
||||
constraintData.Perpendicularity_Constraint.MaxDotProduct = dot_max_actual;
|
||||
constraintData.Perpendicularity_Constraint.MinDotProduct = dot_min_actual;
|
||||
constraintData.Perpendicularity_Constraint.IsValid = dot_all_valid;
|
||||
|
||||
// 收集约束违反信息
|
||||
std::vector<ConstraintViolation> violations;
|
||||
|
||||
// 检查AB1-AB约束违反
|
||||
for (size_t i = 0; i < simulation.AB1_AB_angles.size(); i++)
|
||||
{
|
||||
double abs_angle = std::abs(simulation.AB1_AB_angles[i]);
|
||||
if (abs_angle < AB1_AB_min || abs_angle > AB1_AB_max)
|
||||
{
|
||||
ConstraintViolation violation;
|
||||
violation.ConstraintType = "AB1_AB_Angle";
|
||||
violation.Frame = static_cast<int>(i);
|
||||
violation.Time = i * simulation.step;
|
||||
violation.Value = simulation.AB1_AB_angles[i];
|
||||
violation.AllowedRange = DoubleToString(AB1_AB_min) + "~" + DoubleToString(AB1_AB_max);
|
||||
violation.Severity = "Error";
|
||||
|
||||
violations.push_back(violation);
|
||||
}
|
||||
}
|
||||
|
||||
// 检查C3-C4约束违反
|
||||
for (size_t i = 0; i < simulation.C3_C4_distances.size(); i++)
|
||||
{
|
||||
double distance = simulation.C3_C4_distances[i];
|
||||
if (distance < C3_C4_min || distance > C3_C4_max)
|
||||
{
|
||||
ConstraintViolation violation;
|
||||
violation.ConstraintType = "C3C4_Distance";
|
||||
violation.Frame = static_cast<int>(i);
|
||||
violation.Time = i * simulation.step;
|
||||
violation.Value = distance;
|
||||
violation.AllowedRange = DoubleToString(C3_C4_min) + "~" + DoubleToString(C3_C4_max);
|
||||
violation.Severity = "Error";
|
||||
|
||||
violations.push_back(violation);
|
||||
}
|
||||
}
|
||||
|
||||
constraintData.ConstraintViolations = violations;
|
||||
|
||||
return constraintData;
|
||||
}
|
||||
|
||||
// 创建统计信息
|
||||
StatisticsData CompleteJsonExporter::CreateStatistics(const QuadrupedRobotSimulation &simulation)
|
||||
{
|
||||
StatisticsData statistics;
|
||||
|
||||
// 计算角度统计
|
||||
double ab_horizontal_min = FindMin(simulation.AB_horizontal_angles);
|
||||
double ab_horizontal_max = FindMax(simulation.AB_horizontal_angles);
|
||||
|
||||
double ab_bc_min = FindMin(simulation.AB_BC_angles);
|
||||
double ab_bc_max = FindMax(simulation.AB_BC_angles);
|
||||
|
||||
double bc_cl_min = FindMin(simulation.BC_CL_angles);
|
||||
double bc_cl_max = FindMax(simulation.BC_CL_angles);
|
||||
|
||||
double ab1_ab_min = FindMin(simulation.AB1_AB_angles);
|
||||
double ab1_ab_max = FindMax(simulation.AB1_AB_angles);
|
||||
|
||||
// 角度统计
|
||||
statistics.AngleStatistics.AB_Horizontal.Min = ab_horizontal_min;
|
||||
statistics.AngleStatistics.AB_Horizontal.Max = ab_horizontal_max;
|
||||
statistics.AngleStatistics.AB_Horizontal.Range = ab_horizontal_max - ab_horizontal_min;
|
||||
|
||||
statistics.AngleStatistics.AB_BC.Min = ab_bc_min;
|
||||
statistics.AngleStatistics.AB_BC.Max = ab_bc_max;
|
||||
statistics.AngleStatistics.AB_BC.Range = ab_bc_max - ab_bc_min;
|
||||
|
||||
statistics.AngleStatistics.BC_CL.Min = bc_cl_min;
|
||||
statistics.AngleStatistics.BC_CL.Max = bc_cl_max;
|
||||
statistics.AngleStatistics.BC_CL.Range = bc_cl_max - bc_cl_min;
|
||||
|
||||
statistics.AngleStatistics.AB1_AB.Min = ab1_ab_min;
|
||||
statistics.AngleStatistics.AB1_AB.Max = ab1_ab_max;
|
||||
statistics.AngleStatistics.AB1_AB.Range = ab1_ab_max - ab1_ab_min;
|
||||
|
||||
// 计算距离统计
|
||||
double c3c4_min = FindMin(simulation.C3_C4_distances);
|
||||
double c3c4_max = FindMax(simulation.C3_C4_distances);
|
||||
|
||||
double c2c3_min = FindMin(simulation.C2_C3_distances);
|
||||
double c2c3_max = FindMax(simulation.C2_C3_distances);
|
||||
|
||||
double c2c4_min = FindMin(simulation.C2_C4_distances);
|
||||
double c2c4_max = FindMax(simulation.C2_C4_distances);
|
||||
|
||||
// 距离统计
|
||||
statistics.DistanceStatistics.C3_C4.Min = c3c4_min;
|
||||
statistics.DistanceStatistics.C3_C4.Max = c3c4_max;
|
||||
statistics.DistanceStatistics.C3_C4.Range = c3c4_max - c3c4_min;
|
||||
|
||||
statistics.DistanceStatistics.C2_C3.Min = c2c3_min;
|
||||
statistics.DistanceStatistics.C2_C3.Max = c2c3_max;
|
||||
statistics.DistanceStatistics.C2_C3.Range = c2c3_max - c2c3_min;
|
||||
|
||||
statistics.DistanceStatistics.C2_C4.Min = c2c4_min;
|
||||
statistics.DistanceStatistics.C2_C4.Max = c2c4_max;
|
||||
statistics.DistanceStatistics.C2_C4.Range = c2c4_max - c2c4_min;
|
||||
|
||||
// 计算电机速度统计（使用左前腿的数据）
|
||||
double thigh_min = FindMin(simulation.motor_data_LF.thigh_motor_speeds);
|
||||
double thigh_max = FindMax(simulation.motor_data_LF.thigh_motor_speeds);
|
||||
|
||||
double shank_min = FindMin(simulation.motor_data_LF.shank_motor_speeds);
|
||||
double shank_max = FindMax(simulation.motor_data_LF.shank_motor_speeds);
|
||||
|
||||
double ankle_min = FindMin(simulation.motor_data_LF.ankle_motor_speeds);
|
||||
double ankle_max = FindMax(simulation.motor_data_LF.ankle_motor_speeds);
|
||||
|
||||
// 电机速度统计
|
||||
statistics.MotorSpeedStatistics.Thigh.Min = thigh_min;
|
||||
statistics.MotorSpeedStatistics.Thigh.Max = thigh_max;
|
||||
statistics.MotorSpeedStatistics.Thigh.Range = std::abs(thigh_max - thigh_min);
|
||||
|
||||
statistics.MotorSpeedStatistics.Shank.Min = shank_min;
|
||||
statistics.MotorSpeedStatistics.Shank.Max = shank_max;
|
||||
statistics.MotorSpeedStatistics.Shank.Range = std::abs(shank_max - shank_min);
|
||||
|
||||
statistics.MotorSpeedStatistics.Ankle.Min = ankle_min;
|
||||
statistics.MotorSpeedStatistics.Ankle.Max = ankle_max;
|
||||
statistics.MotorSpeedStatistics.Ankle.Range = std::abs(ankle_max - ankle_min);
|
||||
|
||||
// 轨迹统计
|
||||
statistics.TrajectoryStatistics.TotalFrames = simulation.Trajectory_L.size();
|
||||
statistics.TrajectoryStatistics.TotalTime = simulation.Trajectory_L.size() * simulation.step;
|
||||
statistics.TrajectoryStatistics.SwingFrames = static_cast<int>(std::round(simulation.t_w / simulation.step));
|
||||
statistics.TrajectoryStatistics.SupportFrames = static_cast<int>(std::round(simulation.t_s / simulation.step));
|
||||
|
||||
return statistics;
|
||||
}
|
||||
|
||||
// 创建初始角度数据
|
||||
InitialAnglesData CompleteJsonExporter::CreateInitialAngles(const QuadrupedRobotSimulation &simulation)
|
||||
{
|
||||
InitialAnglesData initialAngles;
|
||||
|
||||
// 获取原始初始角度
|
||||
std::map<std::string, RawInitialAngles> rawInitialAngles;
|
||||
|
||||
rawInitialAngles["LF"] = RawInitialAngles{
|
||||
.Thigh = simulation.AB_horizontal_angles[0],
|
||||
.Shank = simulation.AB1_AB_angles[0],
|
||||
.Ankle = simulation.C3_C4_distances[0]};
|
||||
|
||||
// 计算相位偏移帧数
|
||||
int n_frames = simulation.Trajectory_L.size();
|
||||
int phase_LH = static_cast<int>(simulation.timeLH * n_frames);
|
||||
int phase_RF = static_cast<int>(simulation.timeRF * n_frames);
|
||||
int phase_RH = static_cast<int>(simulation.timeRH * n_frames);
|
||||
|
||||
// 添加其他腿的初始角度
|
||||
rawInitialAngles["LH"] = RawInitialAngles{
|
||||
.Thigh = simulation.AB_horizontal_angles[phase_LH],
|
||||
.Shank = simulation.AB1_AB_angles[phase_LH],
|
||||
.Ankle = simulation.C3_C4_distances[phase_LH]};
|
||||
|
||||
rawInitialAngles["RF"] = RawInitialAngles{
|
||||
.Thigh = simulation.AB_horizontal_angles[phase_RF],
|
||||
.Shank = simulation.AB1_AB_angles[phase_RF],
|
||||
.Ankle = simulation.C3_C4_distances[phase_RF]};
|
||||
|
||||
rawInitialAngles["RH"] = RawInitialAngles{
|
||||
.Thigh = simulation.AB_horizontal_angles[phase_RH],
|
||||
.Shank = simulation.AB1_AB_angles[phase_RH],
|
||||
.Ankle = simulation.C3_C4_distances[phase_RH]};
|
||||
|
||||
// 创建每条腿的初始角度数据
|
||||
std::vector<std::string> legCodes = {"LF", "LH", "RF", "RH"};
|
||||
std::vector<MotorData> motorDatas = {
|
||||
simulation.motor_data_LF,
|
||||
simulation.motor_data_LH,
|
||||
simulation.motor_data_RF,
|
||||
simulation.motor_data_RH};
|
||||
|
||||
for (size_t i = 0; i < legCodes.size(); i++)
|
||||
{
|
||||
std::string legCode = legCodes[i];
|
||||
const MotorData &motorData = motorDatas[i];
|
||||
const RawInitialAngles &rawAngles = rawInitialAngles[legCode];
|
||||
|
||||
LegInitialAngles legInitialAngles;
|
||||
legInitialAngles.LegCode = legCode;
|
||||
|
||||
// 调试初始标定位置（原始值）
|
||||
legInitialAngles.RawInitialAngles = rawAngles;
|
||||
|
||||
// 标定后位置（调整后的值）
|
||||
legInitialAngles.CalibratedAngles.Thigh = motorData.AB_angles[0];
|
||||
legInitialAngles.CalibratedAngles.Shank = motorData.AB1_AB_angles[0];
|
||||
legInitialAngles.CalibratedAngles.Ankle = motorData.ankle_motor_angles[0];
|
||||
|
||||
// 调整信息
|
||||
legInitialAngles.Adjustments.ThighAdjustment = motorData.AB_angles[0] - rawAngles.Thigh;
|
||||
legInitialAngles.Adjustments.ShankAdjustment = motorData.AB1_AB_angles[0] - rawAngles.Shank;
|
||||
legInitialAngles.Adjustments.AnkleAdjustment = motorData.ankle_motor_angles[0] - rawAngles.Ankle;
|
||||
|
||||
initialAngles.Legs[legCode] = legInitialAngles;
|
||||
}
|
||||
|
||||
return initialAngles;
|
||||
}
|
||||
|
||||
// 创建相位信息
|
||||
PhaseInfo CompleteJsonExporter::CreatePhaseInfo(const QuadrupedRobotSimulation &simulation)
|
||||
{
|
||||
PhaseInfo phaseInfo;
|
||||
int n_frames = simulation.Trajectory_L.size();
|
||||
|
||||
phaseInfo.GaitType = simulation.gait_type;
|
||||
phaseInfo.TimeLF = simulation.timeLF;
|
||||
phaseInfo.TimeLH = simulation.timeLH;
|
||||
phaseInfo.TimeRF = simulation.timeRF;
|
||||
phaseInfo.TimeRH = simulation.timeRH;
|
||||
phaseInfo.PhaseLF = 0;
|
||||
phaseInfo.PhaseLH = static_cast<int>(simulation.timeLH * n_frames);
|
||||
phaseInfo.PhaseRF = static_cast<int>(simulation.timeRF * n_frames);
|
||||
phaseInfo.PhaseRH = static_cast<int>(simulation.timeRH * n_frames);
|
||||
phaseInfo.SupportRatio = simulation.support_ratio;
|
||||
phaseInfo.SwingRatio = simulation.swing_ratio;
|
||||
phaseInfo.SupportTime = simulation.t_s;
|
||||
phaseInfo.SwingTime = simulation.t_w;
|
||||
|
||||
return phaseInfo;
|
||||
}
|
||||
|
||||
// 创建运动范围数据
|
||||
MotionRangeData CompleteJsonExporter::CreateMotionRange(const QuadrupedRobotSimulation &simulation)
|
||||
{
|
||||
MotionRangeData motionRange;
|
||||
|
||||
// 为每条腿计算运动范围
|
||||
std::vector<std::string> legCodes = {"LF", "LH", "RF", "RH"};
|
||||
std::vector<int> phaseShifts = {
|
||||
0,
|
||||
static_cast<int>(simulation.timeLH * simulation.Trajectory_L.size()),
|
||||
static_cast<int>(simulation.timeRF * simulation.Trajectory_L.size()),
|
||||
static_cast<int>(simulation.timeRH * simulation.Trajectory_L.size())};
|
||||
|
||||
for (size_t i = 0; i < legCodes.size(); i++)
|
||||
{
|
||||
std::string legCode = legCodes[i];
|
||||
int phaseShift = phaseShifts[i];
|
||||
|
||||
// 计算L点运动范围
|
||||
std::vector<double> x_values, y_values;
|
||||
for (size_t idx = 0; idx < simulation.Trajectory_L.size(); idx++)
|
||||
{
|
||||
int shiftedIdx = (idx + phaseShift) % simulation.Trajectory_L.size();
|
||||
const auto &point = simulation.Trajectory_L[shiftedIdx];
|
||||
x_values.push_back(point[0]);
|
||||
y_values.push_back(point[1]);
|
||||
}
|
||||
|
||||
double x_min = FindMin(x_values);
|
||||
double x_max = FindMax(x_values);
|
||||
double y_min = FindMin(y_values);
|
||||
double y_max = FindMax(y_values);
|
||||
|
||||
LegMotionRange legMotionRange;
|
||||
legMotionRange.LegCode = legCode;
|
||||
legMotionRange.PhaseShift = phaseShift;
|
||||
|
||||
legMotionRange.L_Point.X.Min = x_min;
|
||||
legMotionRange.L_Point.X.Max = x_max;
|
||||
legMotionRange.L_Point.X.Range = x_max - x_min;
|
||||
|
||||
legMotionRange.L_Point.Y.Min = y_min;
|
||||
legMotionRange.L_Point.Y.Max = y_max;
|
||||
legMotionRange.L_Point.Y.Range = y_max - y_min;
|
||||
|
||||
legMotionRange.L_Point.TotalRange = std::sqrt(
|
||||
std::pow(x_max - x_min, 2) +
|
||||
std::pow(y_max - y_min, 2));
|
||||
|
||||
motionRange.Legs[legCode] = legMotionRange;
|
||||
}
|
||||
|
||||
return motionRange;
|
||||
}
|
||||
|
||||
// 辅助函数：将数组转换为Point2D
|
||||
Point2D CompleteJsonExporter::ArrayToPoint(const std::vector<double> &array)
|
||||
{
|
||||
if (array.size() >= 2)
|
||||
{
|
||||
return Point2D(array[0], array[1]);
|
||||
}
|
||||
return Point2D(0.0, 0.0);
|
||||
}
|
||||
|
||||
// 模板函数：查找最小值
|
||||
template <typename T>
|
||||
T CompleteJsonExporter::FindMin(const std::vector<T> &values)
|
||||
{
|
||||
if (values.empty())
|
||||
{
|
||||
return T();
|
||||
}
|
||||
T min_val = values[0];
|
||||
for (const auto &val : values)
|
||||
{
|
||||
if (val < min_val)
|
||||
{
|
||||
min_val = val;
|
||||
}
|
||||
}
|
||||
return min_val;
|
||||
}
|
||||
|
||||
// 模板函数：查找最大值
|
||||
template <typename T>
|
||||
T CompleteJsonExporter::FindMax(const std::vector<T> &values)
|
||||
{
|
||||
if (values.empty())
|
||||
{
|
||||
return T();
|
||||
}
|
||||
T max_val = values[0];
|
||||
for (const auto &val : values)
|
||||
{
|
||||
if (val > max_val)
|
||||
{
|
||||
max_val = val;
|
||||
}
|
||||
}
|
||||
return max_val;
|
||||
}
|
||||
|
||||
// 模板函数：检查所有值是否满足条件
|
||||
template <typename T>
|
||||
bool CompleteJsonExporter::All(const std::vector<T> &values, std::function<bool(const T &)> predicate)
|
||||
{
|
||||
for (const auto &val : values)
|
||||
{
|
||||
if (!predicate(val))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// 辅助函数：将double转换为字符串
|
||||
std::string CompleteJsonExporter::DoubleToString(double value, int precision)
|
||||
{
|
||||
std::ostringstream oss;
|
||||
oss << std::fixed << std::setprecision(precision) << value;
|
||||
return oss.str();
|
||||
}
|
||||
Reference in New Issue
Block a user