meswork/smart_wasm
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
zhangshun
2026-06-01 15:55:59 +08:00
commit 40f9bdb590
1799 changed files with 362227 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

347
inc/QuadrupedRobotSimulation/OPERATION.h Normal file
View 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