715 lines
25 KiB
C++
715 lines
25 KiB
C++
// CompleteJsonExporter.cpp
|
||
#include "QuadrupedRobotSimulation/CompleteJsonExporter.h"
|
||
#include "QuadrupedRobotSimulation/BaseClass.h"
|
||
#include "QuadrupedRobotSimulation/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();
|
||
}
|