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

按功能拆分接口并清理核心乱码注释

Browse Source
This commit is contained in:
zhangshun
2026-06-01 17:59:51 +08:00
parent 12310de782
commit 6aa98241ef
12 changed files with 946 additions and 958 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
include/KinematicsWebAPI.h
View File

@@ -19,6 +19,24 @@ private:
std::function<void(const std::string &)> onLog;
// 按功能域分发请求
json dispatchCommand(const std::string &req_cmd, const json &req_param);
// 机器人运动学与机器人管理接口
json handleRobotCommand(const std::string &req_cmd, const json &req_param);
// SPC 接口
json handleSpcCommand(const std::string &req_cmd, const json &req_param);
// 连杆机构接口
json handleFourBarCommand(const std::string &req_cmd, const json &req_param);
// 四足机器人接口
json handleQuadrupedCommand(const std::string &req_cmd, const json &req_param);
// 未知命令的默认响应
json createUnknownCommandResponse(const std::string &req_cmd, const json &req_param) const;
public:
KinematicsWebAPI();

239
include/Robot.h
View File

@@ -1,17 +1,20 @@
#ifndef ROBOT_H
#ifndef ROBOT_H
#define ROBOT_H
#include <unordered_map>
#include <kdl/chain.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainiksolvervel_pinv.hpp>
#include <kdl/chainiksolverpos_lma.hpp>
#include <kdl/chainiksolverpos_nr.hpp>
#include <kdl/chainiksolverpos_lma.hpp> // 锟斤拷锟斤拷LMA锟斤拷锟斤拷锟酵凤拷募锟<E58B9F>
#include <kdl/chainiksolvervel_pinv.hpp>
#include <kdl/frames.hpp>
#include <kdl/jntarray.hpp>
#include <string>
// 锟斤拷 Robot.h 锟斤拷锟斤拷锟斤拷
#include <unordered_map>
#include <vector>
#include "utils.h"
class Robot
{
private:
@@ -19,228 +22,238 @@ private:
KDL::ChainFkSolverPos_recursive *fkSolver;
KDL::ChainIkSolverVel_pinv *ikVelSolver;
KDL::ChainIkSolverPos_NR *ikSolverNR;
KDL::ChainIkSolverPos_LMA *ikSolverLMA; // 锟斤拷锟斤拷LMA锟斤拷锟斤拷锟<E68BB7>
KDL::ChainIkSolverPos_LMA *ikSolverLMA;
bool m_initialized;
std::unordered_map<std::string, std::string> jointChildLinkUuidMap; // joint锟斤拷锟狡碉拷child link UUID锟斤拷映锟斤拷
// 记录 joint 名称到 child link UUID 的映射，便于前端回写对象姿态。
std::unordered_map<std::string, std::string> jointChildLinkUuidMap;
// 私锟叫革拷锟斤拷锟斤拷锟斤拷
// 内部工具方法。
double quaternionAngleDifference(const double q1[4], const double q2[4]);
int calculateAutoSteps(const double startPose[7], const double endPose[7],
int minSteps, int maxSteps,
double positionResolution, double orientationResolution);
// 锟斤拷锟斤拷私锟叫凤拷锟斤拷
bool parseJointChildLinkUuidsFromUrdf(const std::string &urdfString);
public:
/**
* @brief 默锟较癸拷锟届函锟斤拷
* @brief 默认构造函数。
*/
Robot();
/**
* @brief 锟斤拷URDF锟街凤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟<EFBFBD>
* @param urdfString URDF锟斤拷式锟斤拷锟街凤拷锟斤拷
* @brief 使用 URDF 字符串直接初始化机器人。
* @param urdfString URDF 格式字符串。
*/
explicit Robot(const std::string &urdfString);
/**
* @brief 锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷源
* @brief 释放运动学求解器资源。
*/
~Robot();
int numberOfJoints;
/**
* @brief 锟斤拷URDF锟街凤拷锟斤拷锟斤拷始锟斤拷锟斤拷锟斤拷锟斤拷
* @param urdfString URDF锟斤拷式锟斤拷锟街凤拷锟斤拷
* @return 锟缴癸拷锟斤拷锟斤拷true锟斤拷失锟杰凤拷锟斤拷false
* @brief 根据 URDF 字符串初始化机器人运动学链。
* @param urdfString URDF 格式字符串。
* @return 初始化成功返回 true，否则返回 false。
*/
bool initRobot(const std::string &urdfString);
/**
* @brief 锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷呕锟饺<EFBFBD>joint锟斤拷应锟斤拷child link UUID
* @param jointIndex joint锟斤拷锟<EFBFBD> (1, 2, 3, ...)
* @return 锟斤拷应锟斤拷UUID锟街凤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷蚍祷乜锟斤拷址锟斤拷锟<EFBFBD>
* @brief 根据关节序号获取 child link UUID。
* @param jointIndex 关节序号，从 1 开始。
* @return 找到时返回对应 UUID，未找到返回空字符串。
*/
std::string getJointUuidByIndex(int jointIndex) const;
/**
* @brief 锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷呕锟饺<EFBFBD>joint锟斤拷锟斤拷
* @param jointIndex joint锟斤拷锟<EFBFBD> (1, 2, 3, ...)
* @return joint锟斤拷锟斤拷锟街凤拷锟斤拷锟斤拷锟斤拷 "joint_1", "joint_2" 锟斤拷
* @brief 根据关节序号生成标准关节名称。
* @param jointIndex 关节序号，从 1 开始。
* @return 如 `joint_1`、`joint_2` 这样的关节名称。
*/
static std::string getJointNameByIndex(int jointIndex);
/**
* @brief 锟斤拷取锟斤拷锟叫匡拷锟斤拷joint锟斤拷锟斤拷斜锟<EFBFBD>
* @return 锟斤拷锟斤拷锟斤拷锟叫匡拷锟斤拷joint锟斤拷诺锟斤拷锟斤拷锟<EFBFBD>
* @brief 获取当前已解析到的全部关节序号列表。
* @return 已排序且去重后的关节序号集合。
*/
std::vector<int> getAvailableJointIndices() const;
// 锟斤拷Robot.h锟斤拷锟斤拷锟斤拷
/**
* @brief 兼容多种命名格式查找关节 UUID。
* @param jointIndex 关节序号，从 1 开始。
* @return 找到时返回 UUID，未找到返回空字符串。
*/
std::string findJointUuidByAnyFormat(int jointIndex) const;
/**
* @brief 对姿态序列做逆解，自动进行轨迹插补。
*/
json inversePoseStr(const std::string &pose_str, const std::string &q_init_str);
// 锟斤拷锟斤拷锟斤拷锟斤拷筒锟街碉拷锟斤拷锟斤拷锟斤拷锟斤拷锟截诧拷趾锟斤拷锟斤拷锟<E68BB7>
/**
* @brief 对两点姿态按指定步数插补后做逆解。
*/
json inversePoseStr2PSteps(const std::string &pose_str, const std::string &q_init_str, const std::int32_t steps);
// 锟斤拷锟斤拷锟斤拷牡锟街<E9949F>锟戒不锟斤拷锟叫诧拷趾锟街憋拷臃锟斤拷囟锟接︼拷锟斤拷锟斤拷锟<E68BB7>
/**
* @brief 直接对输入姿态点做逆解，不做相邻点插补。
*/
json inversePoseStrNoDifference(const std::string &pose_str, const std::string &q_init_str);
/**
* @brief 解析姿态字符串，格式为 `x,y,z,qx,qy,qz,qw;...`。
*/
std::vector<std::vector<double>> parsePoseString(const std::string &pose_str);
/**
* @brief 解析关节列表字符串，格式为 `j1,j2,j3,j4,j5,j6;...`。
*/
std::vector<std::vector<double>> parseJointListString(const std::string &pose_str);
std::unordered_map<std::string, std::string> getJointChildLinkUuidMap() const { return jointChildLinkUuidMap; }
std::string getJointChildLinkUuid(const std::string &jointName) const;
std::vector<double> parseJointString(const std::string &joint_str);
json kinematicsForwardAllJointsList(std::vector<double> joints);
json handleKinematicsForwardAllJoints(const std::string &joints_str);
json handleKinematicsForwardAllJoints_objStates(const std::string &joints_str);
/**
* @brief 锟斤拷锟斤拷锟斤拷锟斤拷锟角凤拷锟窖筹拷始锟斤拷
* @return 锟窖筹拷始锟斤拷锟斤拷锟斤拷true
* @brief 计算所有关节位姿，并按前端需要的 objStates 格式组织数据。
*/
json kinematicsForwardAllJointsList(std::vector<double> joints);
/**
* @brief 批量计算多帧关节正解，输出 OPERATION 结构。
*/
json handleKinematicsForwardAllJoints(const std::string &joints_str);
/**
* @brief 兼容旧格式的全部关节正解输出。
*/
json handleKinematicsForwardAllJoints_objStates(const std::string &joints_str);
/**
* @brief 判断机器人是否已初始化。
* @return 已初始化返回 true。
*/
bool isInitialized() const { return m_initialized; }
/**
* @brief 锟斤拷锟斤拷锟剿讹拷学锟斤拷锟<EFBFBD> (NR锟斤拷锟斤拷)
* @param pose 目锟斤拷位锟剿ｏ拷锟斤拷锟斤拷锟斤拷式[x, y, z, qx, qy, qz, qw]
* @param iniJ 锟斤拷始锟截节角度ｏ拷锟斤拷锟斤拷锟斤拷式[6锟斤拷锟截节角讹拷]
* @param resultJoints 锟斤拷锟斤拷锟斤拷锟截节角度ｏ拷锟斤拷锟斤拷锟斤拷式[6锟斤拷锟截节角讹拷]
* @return 锟缴癸拷锟斤拷锟斤拷true锟斤拷失锟杰凤拷锟斤拷false
* @brief 使用 NR 求解器计算逆运动学。
* @param pose 目标位姿，格式为 `[x, y, z, qx, qy, qz, qw]`。
* @param iniJ 初始关节角，格式为 `[j1, ..., j6]`。
* @param resultJoints 输出关节角结果。
* @return 求解成功返回 true，否则返回 false。
*/
bool calculateIK_NR(const double pose[7], const double iniJ[6], double resultJoints[6]);
/**
* @brief 锟斤拷锟斤拷锟剿讹拷学锟斤拷锟<EFBFBD> (LMA锟斤拷锟斤拷)
* @param pose 目锟斤拷位锟剿ｏ拷锟斤拷锟斤拷锟斤拷式[x, y, z, qx, qy, qz, qw]
* @param iniJ 锟斤拷始锟截节角度ｏ拷锟斤拷锟斤拷锟斤拷式[6锟斤拷锟截节角讹拷]
* @param resultJoints 锟斤拷锟斤拷锟斤拷锟截节角度ｏ拷锟斤拷锟斤拷锟斤拷式[6锟斤拷锟截节角讹拷]
* @param eps 锟斤拷锟斤拷锟斤拷锟斤拷 (默锟斤拷: 1e-8)
* @param maxiter 锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷 (默锟斤拷: 3000)
* @param eps_joints 锟截斤拷锟捷诧拷 (默锟斤拷: 1e-12)
* @return 锟缴癸拷锟斤拷锟斤拷true锟斤拷失锟杰凤拷锟斤拷false
* @brief 使用 LMA 求解器计算逆运动学。
* @param pose 目标位姿，格式为 `[x, y, z, qx, qy, qz, qw]`。
* @param iniJ 初始关节角，格式为 `[j1, ..., j6]`。
* @param resultJoints 输出关节角结果。
* @param eps 迭代收敛精度。
* @param maxiter 最大迭代次数。
* @param eps_joints 关节收敛阈值。
* @return 求解成功返回 true，否则返回 false。
*/
bool calculateIK_LMA(const double pose[7], const double iniJ[6], double resultJoints[6],
double eps = 1e-8, int maxiter = 3000, double eps_joints = 1e-12);
/**
* @brief 锟斤拷锟斤拷锟剿讹拷学锟斤拷锟<EFBFBD> - TCP位锟斤拷
* @param joints 锟斤拷锟斤拷亟诮嵌龋锟斤拷锟斤拷锟斤拷锟绞絒6锟斤拷锟截节角讹拷]
* @param tcpPose 锟斤拷锟絋CP位锟剿ｏ拷锟斤拷锟斤拷锟斤拷式[x, y, z, qx, qy, qz, qw]
* @return 锟缴癸拷锟斤拷锟斤拷true锟斤拷失锟杰凤拷锟斤拷false
* @brief 计算 TCP 的正运动学结果。
* @param joints 输入关节角，格式为 `[j1, ..., j6]`。
* @param tcpPose 输出 TCP 位姿，格式为 `[x, y, z, qx, qy, qz, qw]`。
* @return 计算成功返回 true，否则返回 false。
*/
bool calculateFK_TCP(const double joints[6], double tcpPose[7]);
/**
* @brief 锟斤拷锟斤拷锟剿讹拷学锟斤拷锟<EFBFBD> - 锟斤拷锟叫关斤拷位锟斤拷
* @param joints 锟斤拷锟斤拷亟诮嵌龋锟斤拷锟斤拷锟斤拷锟绞絒6锟斤拷锟截节角讹拷]
* @param jointPoses 锟斤拷锟斤拷锟斤拷泄亟锟轿伙拷耍锟斤拷锟斤拷锟斤拷锟绞絒6*7锟斤拷元锟斤拷]
* @return 锟缴癸拷锟斤拷锟斤拷true锟斤拷失锟杰凤拷锟斤拷false
* @brief 计算每个关节节点的正运动学结果。
* @param joints 输入关节角，格式为 `[j1, ..., j6]`。
* @param jointPoses 输出全部关节位姿，共 6 组，每组 7 个值。
* @return 计算成功返回 true，否则返回 false。
*/
bool calculateFK_AllJointsforwardKinematics(const double joints[6], double jointPoses[42]);
bool forwardKinematics(const double joints[6], double jointPoses[42]);
/**
* @brief 锟斤拷取锟斤拷锟斤拷锟斤拷锟剿讹拷锟斤拷
* @return KDL锟剿讹拷锟斤拷锟斤拷锟斤拷
* @brief 获取当前 KDL 运动学链。
*/
const KDL::Chain &getKinematicChain() const { return kinematicChain; }
/**
* @brief 锟斤拷取锟截斤拷锟斤拷锟斤拷
* @return 锟截斤拷锟斤拷锟斤拷
* @brief 获取机器人关节数量。
*/
int getNumberOfJoints() const { return kinematicChain.getNrOfJoints(); }
// 锟届迹锟芥划锟斤拷锟斤拷
/**
* @brief 锟斤拷锟斤拷锟斤拷态锟斤拷墓旒ｏ拷婊<EFBFBD>锟斤拷锟斤拷锟斤拷姹撅拷锟<EFBFBD>
* @param pose1 锟斤拷始位锟斤拷 [x, y, z, qx, qy, qz, qw]
* @param pose2 锟斤拷止位锟斤拷 [x, y, z, qx, qy, qz, qw]
* @param outputPoses 锟斤拷锟斤拷旒ｏ拷锟斤拷锟斤拷锟<EFBFBD>
* @param npoint 锟届迹锟斤拷锟斤拷锟斤拷锟斤拷0锟斤拷示锟皆讹拷锟斤拷锟姐）
* @return 实锟斤拷锟斤拷锟缴的轨迹锟斤拷锟斤拷锟斤拷锟斤拷失锟杰凤拷锟斤拷-1
* @brief 生成姿态轨迹，输出为固定数组。
* @param pose1 起点位姿。
* @param pose2 终点位姿。
* @param outputPoses 输出轨迹数组。
* @param npoint 轨迹点数量，传 0 表示自动计算。
* @return 实际生成的轨迹点数量，失败返回 -1。
*/
int trajectoryPlanning(const double pose1[7], const double pose2[7],
double outputPoses[][7], int npoint = 0);
/**
* @brief 锟斤拷锟斤拷锟斤拷态锟斤拷墓旒ｏ拷婊<EFBFBD>锟斤拷锟斤拷锟斤拷姹撅拷锟斤拷锟斤拷锟斤拷vector锟斤拷
* @param pose1 锟斤拷始位锟斤拷 [x, y, z, qx, qy, qz, qw]
* @param pose2 锟斤拷止位锟斤拷 [x, y, z, qx, qy, qz, qw]
* @param outputPoses 锟斤拷锟斤拷旒ｏ拷锟絭ector锟斤拷锟斤拷锟斤拷预锟饺凤拷锟斤拷锟节存）
* @param npoint 锟届迹锟斤拷锟斤拷锟斤拷锟斤拷0锟斤拷示锟皆讹拷锟斤拷锟姐）
* @return 实锟斤拷锟斤拷锟缴的轨迹锟斤拷锟斤拷锟斤拷锟斤拷失锟杰凤拷锟斤拷-1
* @brief 生成姿态轨迹，输出为二维向量。
* @param pose1 起点位姿。
* @param pose2 终点位姿。
* @param outputPoses 输出轨迹点集合。
* @param npoint 轨迹点数量，传 0 表示自动计算。
* @return 实际生成的轨迹点数量，失败返回 -1。
*/
int trajectoryPlanning(const double pose1[7], const double pose2[7],
std::vector<std::vector<double>> &outputPoses,
int npoint = 0);
/**
* @brief 锟斤拷锟斤拷锟斤拷态锟斤拷墓旒ｏ拷婊<EFBFBD>锟斤拷vector锟芥本锟斤拷
* @param pose1 锟斤拷始位锟斤拷
* @param pose2 锟斤拷止位锟斤拷
* @param outputPoses 锟斤拷锟斤拷旒ｏ拷锟斤拷锟斤拷锟<EFBFBD>
* @param npoint 锟届迹锟斤拷锟斤拷锟斤拷锟斤拷0锟斤拷示锟皆讹拷锟斤拷锟姐）
* @return 实锟斤拷锟斤拷锟缴的轨迹锟斤拷锟斤拷锟斤拷锟斤拷失锟杰凤拷锟斤拷-1
* @brief 使用 `std::vector<double>` 形式输入位姿生成轨迹。
* @param pose1 起点位姿。
* @param pose2 终点位姿。
* @param outputPoses 输出轨迹点集合。
* @param npoint 轨迹点数量，传 0 表示自动计算。
* @return 实际生成的轨迹点数量，失败返回 -1。
*/
int trajectoryPlanning(const std::vector<double> &pose1, const std::vector<double> &pose2,
std::vector<std::vector<double>> &outputPoses, int npoint = 0);
// 锟斤拷锟竭猴拷锟斤拷
/**
* @brief 锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷元锟斤拷之锟斤拷慕嵌炔锟<E78294>
* @param q1 锟斤拷元锟斤拷1 [qx, qy, qz, qw]
* @param q2 锟斤拷元锟斤拷2 [qx, qy, qz, qw]
* @return 锟角度差（锟斤拷锟饺ｏ拷
*/
// static double quaternionAngleDifference(const double q1[4], const double q2[4]);
/**
* @brief 锟皆讹拷锟斤拷锟斤拷旒ｏ拷婊<E68BB7>锟斤拷锟斤拷锟斤拷锟<E68BB7>
* @param startPose 锟斤拷始位锟斤拷 [x, y, z, qx, qy, qz, qw]
* @param endPose 锟斤拷止位锟斤拷 [x, y, z, qx, qy, qz, qw]
* @param minSteps 锟斤拷小锟斤拷锟斤拷
* @param maxSteps 锟斤拷锟斤拷锟<E68BB7>
* @param positionResolution 位锟矫分憋拷锟绞ｏ拷锟阶ｏ拷
* @param orientationResolution 锟斤拷态锟街憋拷锟绞ｏ拷锟斤拷锟饺ｏ拷
* @return 锟狡硷拷锟侥轨迹锟斤拷锟斤拷锟斤拷
*/
// static int calculateAutoSteps(const double startPose[7], const double endPose[7],
// int minSteps = 10, int maxSteps = 100,
// double positionResolution = 0.01,
// double orientationResolution = 0.1);
// 锟斤拷取锟斤拷锟斤拷锟斤拷锟斤拷息
// const KDL::Chain& getKinematicChain() const { return kinematicChain; }
// int getNumberOfJoints() const { return kinematicChain.getNrOfJoints(); }
int calculateAutoSteps(const std::vector<double> &startPose, const std::vector<double> &endPose,
int minSteps, int maxSteps,
double positionResolution, double orientationResolution);
double quaternionAngleDifference(const std::vector<double> &q1, const std::vector<double> &q2);
/**
* @brief 锟斤拷锟斤拷锟剿讹拷学锟斤拷锟<EFBFBD> (LMA锟斤拷锟斤拷) - 锟斤拷锟斤拷vector锟芥本
* @param pose 目锟斤拷位锟斤拷 [x, y, z, qx, qy, qz, qw]
* @param iniJ 锟斤拷始锟截节角讹拷 [6锟斤拷锟截节角讹拷]
* @param eps 锟斤拷锟斤拷锟斤拷锟斤拷
* @param maxiter 锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷
* @param eps_joints 锟截斤拷锟捷诧拷
* @return 锟截节角讹拷vector
* @brief 使用 LMA 求解器计算逆解，输入为数组。
*/
std::vector<double> inverse(const double pose[7], const double iniJ[6],
double eps = 1e-8, int maxiter = 3000,
double eps_joints = 1e-12);
/**
* @brief 使用 LMA 求解器计算逆解，输入为向量。
*/
std::vector<double> inverse(const std::vector<double> &pose,
const std::vector<double> &iniJ,
double eps = 1e-8, int maxiter = 3000,
double eps_joints = 1e-12);
/**
* @brief 使用默认零位初值计算逆解，输入为数组。
*/
std::vector<double> inverse(const double pose[7],
double eps = 1e-8, int maxiter = 3000,
double eps_joints = 1e-12);
/**
* @brief 使用默认零位初值计算逆解，输入为向量。
*/
std::vector<double> inverse(const std::vector<double> &pose,
double eps = 1e-8, int maxiter = 3000,
double eps_joints = 1e-12);
};
#endif // ROBOT_H
#endif // ROBOT_H

60
include/utils.h
View File

@@ -1,37 +1,33 @@
#ifndef UTILS_H
#define UTILS_H
#include <string>
#include <chrono>
#include "nlohmann/json.hpp"
#include <string>
// // ǰ<><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD> httplib <20><> Response <20><>
// namespace httplib {
// class Response;
// }
#include "nlohmann/json.hpp"
using json = nlohmann::json;
namespace utils
{
// ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>غ<EFBFBD><EFBFBD><EFBFBD>
// 时间相关工具。
std::string get_current_time();
std::string get_current_timestamp();
// JSON <EFBFBD><EFBFBD>Ӧ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// 通用 JSON 响应构造。
json create_error_response(int code, const std::string &message, const std::string &details = "");
json create_success_response(const json &data = {}, const std::string &message = "Success");
// UTF-8 <EFBFBD><EFBFBD>֤
// UTF-8 校验与清洗。
bool is_valid_utf8(const std::string &str);
std::string sanitize_utf8(const std::string &str);
// API <EFBFBD><EFBFBD>Ӧ<EFBFBD><EFBFBD>ʽ
// 业务 API 响应格式。
json create_api_response(bool success, int code, const std::string &msg,
const std::string &req_code = "", const std::string &req_from = "",
const std::string &req_cmd = "", const json &res_data = json::object());
// Base64 <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>URDF<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// Base64 与 URDF 处理。
std::string base64_decode(const std::string &encoded_string);
std::string base64_to_urdf(const std::string &base64_urdf);
bool save_base64_urdf_to_file(const std::string &base64_urdf, const std::string &filename);
@@ -39,35 +35,34 @@ namespace utils
bool save_urdf_string_to_file(const std::string &urdf_content, const std::string &filename);
} // namespace utils
class StringUtils
{
public:
/**
* @brief 将包含一个分隔符的字符串拆分成两部分
* @param str 要拆分的字符串
* @param delimiter 分隔符，默认为'_'
* @return std::pair<std::string, std::string> 包含两个部分的pair
* 第一个元素是分隔符前的部分，第二个是分隔符后的部分
* 如果未找到分隔符，第二个元素为空字符串
* @brief 将包含单个分隔符的字符串拆分为两部分。
* @param str 要拆分的字符串。
* @param delimiter 分隔符，默认为 `_`。
* @return 第一个元素为分隔符前的内容，第二个元素为分隔符后的内容。
*/
static std::pair<std::string, std::string> splitByDelimiter(
const std::string &str,
char delimiter = '_');
/**
* @brief 专门拆分"A_B"格式的字符串
* @param str 要拆分的字符串
* @return std::pair<std::string, std::string> 包含两个部分的pair
* @brief 专门拆分 `A_B` 形式的字符串。
* @param str 要拆分的字符串。
* @return 拆分后的两段内容。
*/
static std::pair<std::string, std::string> splitA_B(const std::string &str);
/**
* @brief 将字符串按分隔符拆分成两个部分，支持引用参数返回
* @param str 要拆分的字符串
* @param part1 返回第一部分
* @param part2 返回第二部分
* @param delimiter 分隔符，默认为'_'
* @return bool 如果找到分隔符返回true，否则返回false
* @brief 将字符串按分隔符拆成两段，并通过引用返回结果。
* @param str 要拆分的字符串。
* @param part1 返回第一段。
* @param part2 返回第二段。
* @param delimiter 分隔符，默认为 `_`。
* @return 找到分隔符返回 true，否则返回 false。
*/
static bool splitToTwoParts(
const std::string &str,
@@ -76,14 +71,15 @@ public:
char delimiter = '_');
/**
* @brief 严格拆分，必须包含且只包含一个分隔符
* @param str 要拆分的字符串
* @param delimiter 分隔符，默认为'_'
* @return std::pair<std::string, std::string> 包含两个部分的pair
* @throws std::invalid_argument 如果没有找到分隔符或找到多个分隔符
* @brief 严格拆分字符串，要求只出现一个分隔符。
* @param str 要拆分的字符串。
* @param delimiter 分隔符，默认为 `_`。
* @return 拆分后的两段内容。
* @throws std::invalid_argument 当分隔符不存在或出现多次时抛出异常。
*/
static std::pair<std::string, std::string> splitStrict(
const std::string &str,
char delimiter = '_');
};
#endif // UTILS_H
#endif // UTILS_H

6
scripts/build.ps1
View File

@@ -74,7 +74,11 @@ $startTime = Get-Date
# 编译命令 - 使用当前真实业务模块源码
$compileResult = emcc `
src/KinematicsWebAPI.cpp `
src/api/KinematicsWebAPI.Core.cpp `
src/api/KinematicsWebAPI.RobotCommands.cpp `
src/api/KinematicsWebAPI.SpcCommands.cpp `
src/api/KinematicsWebAPI.FourBarCommands.cpp `
src/api/KinematicsWebAPI.QuadrupedCommands.cpp `
src/main.cpp `
src/Robot.cpp `
src/spc_core.cpp `

609
src/KinematicsWebAPI.cpp
View File

@@ -1,609 +0,0 @@
#include "QuadrupedRobotSimulation/KinematicsHelper.h"
#include "KinematicsWebAPI.h"
#include "FourBarMechanism/CrankSliderMechanism.h"
#include "URDFStrings.h"
#include <iostream>
#include <thread>
#include <chrono>
#include <iomanip>
#include <sstream>
#include <codecvt>
#include <locale>
#include <csignal>
// inversePoseStr2PSteps
KinematicsWebAPI::KinematicsWebAPI()
{
init_func();
}
std::string KinematicsWebAPI::init_func()
{
std::string response_string;
std::string urdf_string = URDFStrings::abb120_urdf; // 使用新的命名空间访问
auto result = RobotManager::initRobot(urdf_string, "9D7EAEF4-1AAB-499E-8783-B6CE016BC6D1");
auto result1 = RobotManager::initRobot(urdf_string, "abb_irb120_3_58");
json error_response = utils::create_api_response(true, 200, "init_func", "", "", "");
response_string = error_response.dump();
return response_string;
}
std::string KinematicsWebAPI::func(std::string sanitized_body)
{
// 添加调试输出
log("func called with body: " + sanitized_body.substr(0, 100));
assert(!sanitized_body.empty() && "sanitized_body should not be empty");
std::string response_string;
try
{
json request_json;
// 构建响应数据
json result;
try
{
request_json = json::parse(sanitized_body);
}
catch (const std::exception &e)
{
json error_response = utils::create_api_response(false, 400, "Invalid JSON format", "", "", "");
response_string = error_response.dump();
return response_string;
}
std::string msg = request_json.value("msg", "");
std::string req_code = request_json.value("req_code", "");
std::string req_from = request_json.value("req_from", "");
std::string req_cmd = request_json.value("req_cmd", "");
json req_param = request_json.value("req_param", json::object());
json res_data;
// 使用if语句直接处理对应的命令，不通过中间函数
if (req_cmd == "Cmd_Kinematics_inverse_pose_str")
{
try
{
log("Handling Cmd_Kinematics_inverse_pose_str command");
std::string pose_str = req_param.value("pose_str", "");
std::string q_init_str = req_param.value("q_init_str", "0,0,0,0,0,0");
std::string robot_uuid = req_param.value("robot_uuid", "default");
auto robot = RobotManager::getRobot(robot_uuid);
if (!robot || !robot->isInitialized())
{
res_data = {{"error", "Robot not found or not initialized"}};
}
else
{
result = robot->inversePoseStr(pose_str, q_init_str);
if (result.empty())
{
res_data = {{"error", "Inverse kinematics calculation failed"}};
}
else
{
res_data = {{"joints", result}, {"success", true}};
}
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to calculate inverse kinematics: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_Kinematics_inverse_pose_str_2PSteps")
{
try
{
log("Handling Cmd_Kinematics_inverse_pose_str_2PSteps command");
std::string pose_str = req_param.value("pose_str", "");
std::string q_init_str = req_param.value("q_init_str", "0,0,0,0,0,0");
std::string robot_uuid = req_param.value("robot_uuid", "default");
std::string steps_str = req_param.value("steps_str", "default");
int steps = std::stoi(steps_str);
auto robot = RobotManager::getRobot(robot_uuid);
if (!robot || !robot->isInitialized())
{
res_data = {{"error", "Robot not found or not initialized"}};
}
else
{
result = robot->inversePoseStr2PSteps(pose_str, q_init_str, steps);
if (result.empty())
{
res_data = {{"error", "Inverse kinematics calculation failed"}};
}
else
{
res_data = {{"joints", result}, {"success", true}};
}
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to calculate inverse kinematics: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_Kinematics_inverse_pose_str_NoDifference")
{
try
{
log("Handling Cmd_Kinematics_inverse_pose_strNoDifference command");
std::string pose_str = req_param.value("pose_str", "");
std::string q_init_str = req_param.value("q_init_str", "0,0,0,0,0,0");
std::string robot_uuid = req_param.value("robot_uuid", "default");
auto robot = RobotManager::getRobot(robot_uuid);
if (!robot || !robot->isInitialized())
{
res_data = {{"error", "Robot not found or not initialized"}};
}
else
{
result = robot->inversePoseStrNoDifference(pose_str, q_init_str);
if (result.empty())
{
res_data = {{"error", "Inverse kinematics calculation failed"}};
}
else
{
res_data = {{"joints", result}, {"success", true}};
}
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to calculate inverse kinematics: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_Kinematics_forward_pose_str")
{
try
{
log("Handling Cmd_Kinematics_forward_pose_str command");
std::string joints_str = req_param.value("q_init_str", "0,0,0,0,0,0");
std::string robot_uuid = req_param.value("robot_uuid", "default");
auto robot = RobotManager::getRobot(robot_uuid);
if (!robot || !robot->isInitialized())
{
res_data = {{"error", "Robot not found or not initialized"}};
}
else
{
auto joints = robot->parseJointString(joints_str);
if (joints.size() != 6)
{
res_data = {{"error", "Invalid joints format"}};
}
else
{
double tcp_pose[7];
if (robot->calculateFK_TCP(joints.data(), tcp_pose))
{
res_data = {
{"position", {tcp_pose[0], tcp_pose[1], tcp_pose[2]}},
{"orientation", {tcp_pose[3], tcp_pose[4], tcp_pose[5], tcp_pose[6]}},
{"joints", joints},
{"success", true}};
}
else
{
res_data = {{"error", "Forward kinematics calculation failed"}};
}
}
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to calculate forward kinematics: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_SelectCraftTree")
{
try
{
log("Handling Cmd_SelectCraftTree command");
std::string tree_id = req_param.value("tree_id", "");
res_data = {
{"success", true},
{"tree_id", tree_id},
{"message", "Craft tree selected successfully"},
{"timestamp", utils::get_current_time()}};
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to select craft tree: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_AddOperationTree")
{
try
{
log("Handling Cmd_AddOperationTree command");
std::string tree_name = req_param.value("name", "");
json operations = req_param.value("operations", json::array());
res_data = {
{"success", true},
{"tree_name", tree_name},
{"operations_count", operations.size()},
{"message", "Operation tree added successfully"},
{"timestamp", utils::get_current_time()}};
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to add operation tree: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_InitRobot")
{
try
{
log("Handling Cmd_InitRobot command");
std::string urdf_base64 = req_param.value("urdf_base64", "");
std::string uuid = req_param.value("robot_uuid", "");
bool force_update = req_param.value("force_update", true);
std::string urdf_content = utils::base64_to_urdf(urdf_base64);
if (!utils::validate_urdf_base64(urdf_base64))
{
res_data = {
{"success", false},
{"message", "Invalid URDF format"},
{"timestamp", utils::get_current_time()}};
}
else
{
auto result = RobotManager::initRobot(urdf_content, uuid, force_update);
res_data = {
{"success", result.first},
{"message", result.second},
{"timestamp", utils::get_current_time()}};
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to initialize robot: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_GetRobot")
{
try
{
log("Handling Cmd_GetRobot command");
std::string uuid = req_param.value("uuid", "");
auto robot = RobotManager::getRobot(uuid);
if (!robot)
{
res_data = {{"error", "Robot not found"}};
}
else
{
res_data = {
{"success", true},
{"uuid", uuid},
{"initialized", robot->isInitialized()},
{"joints_count", robot->getNumberOfJoints()},
{"timestamp", utils::get_current_time()}};
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to get robot: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_RemoveRobot")
{
try
{
log("Handling Cmd_RemoveRobot command");
std::string uuid = req_param.value("uuid", "");
auto result = RobotManager::removeRobot(uuid);
res_data = {
{"success", result.first},
{"message", result.second},
{"timestamp", utils::get_current_time()}};
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to remove robot: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_ListRobots")
{
try
{
log("Handling Cmd_ListRobots command");
bool detail = req_param.value("detail", false);
auto robots = RobotManager::listRobots(detail);
res_data = {
{"success", true},
{"robots", robots},
{"count", robots.size()},
{"timestamp", utils::get_current_time()}};
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to list robots: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_Kinematics_forward_all_joints")
{
try
{
log("Handling Cmd_Kinematics_forward_all_joints command");
std::string joints_str = req_param.value("joints_str", "0,0,0,0,0,0");
std::string robot_uuid = req_param.value("robot_uuid", "default");
auto robot = RobotManager::getRobot(robot_uuid);
if (!robot || !robot->isInitialized())
{
res_data = {{"error", "Robot not found or not initialized"}};
}
else
{
auto joints_poses_array = robot->handleKinematicsForwardAllJoints(joints_str);
res_data = {
{"OPERATION", joints_poses_array},
{"success", true}};
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to calculate forward kinematics for all joints: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_Spc")
{
try
{
log("Handling " + req_cmd + " command");
json result = SpcCalculator::Spc(req_param);
res_data = result;
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to Cmd_Spc: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_FourBar_CrankSlider")
{
// 请求参数示例:
// {
// "L_AB": 0.5,
// "L_BS": 2.0,
// "S_OFS": 0.0,
// "angleDeg": 45.0
// }
try
{
log("Handling " + req_cmd + " command");
// 从请求参数中提取值
double L_AB = req_param.value("L_AB", 0.5);
double L_BS = req_param.value("L_BS", 2.0);
double S_OFS = req_param.value("S_OFS", 0.0);
double angleDeg = req_param.value("angleDeg", 0.0);
log("Parameters: L_AB=" + std::to_string(L_AB) +
", L_BS=" + std::to_string(L_BS) +
", S_OFS=" + std::to_string(S_OFS) +
", angleDeg=" + std::to_string(angleDeg));
// 创建曲柄滑块机构实例
std::unique_ptr<CrankSliderMechanism> mechanism(createCrankSliderMechanism());
// 设置连杆参数
mechanism->setL_AB(L_AB);
mechanism->setL_BS(L_BS);
mechanism->setS_OFS(S_OFS);
// 验证参数
ValidationResult validation = mechanism->validateParameters();
if (!validation.isValid())
{
res_data = {
{"success", false},
{"error", "Invalid parameters"},
{"validation_errors", validation.Errors},
{"validation_warnings", validation.Warnings}};
}
else
{
// 执行计算
MechanismState state = mechanism->calculate(angleDeg);
// 检查是否有错误
if (state.hasError())
{
res_data = {
{"success", false},
{"error", state.ErrorMessage}};
}
else
{
// 构建响应数据
json result;
result["success"] = true;
// 添加点坐标
json points_json;
for (const auto &point_pair : state.Points)
{
json point;
point["x"] = point_pair.second.X;
point["y"] = point_pair.second.Y;
points_json[point_pair.first] = point;
}
result["points"] = points_json;
// 添加姿态信息
json poses_json;
for (const auto &pose_pair : state.Poses)
{
json pose;
pose["tx"] = pose_pair.second.tx;
pose["ty"] = pose_pair.second.ty;
pose["tz"] = pose_pair.second.tz;
pose["qx"] = pose_pair.second.qx;
pose["qy"] = pose_pair.second.qy;
pose["qz"] = pose_pair.second.qz;
pose["qw"] = pose_pair.second.qw;
poses_json[pose_pair.first] = pose;
}
result["poses"] = poses_json;
// 添加角度信息
json angles_json;
for (const auto &angle_pair : state.Angles)
{
angles_json[angle_pair.first] = angle_pair.second;
}
result["angles"] = angles_json;
// 添加输入值
result["input_value"] = state.InputValue;
// 添加警告信息（如果有）
if (state.hasWarning())
{
result["warning"] = state.WarningMessage;
}
// 添加轨迹信息
std::vector<Vector2D> trajectory = mechanism->getTrajectoryPoints();
std::vector<Vector2D> slider_trajectory = mechanism->getSliderTrajectory();
json trajectory_json;
for (size_t i = 0; i < trajectory.size(); i++)
{
json point;
point["x"] = trajectory[i].X;
point["y"] = trajectory[i].Y;
trajectory_json.push_back(point);
}
result["trajectory"] = trajectory_json;
json slider_trajectory_json;
for (size_t i = 0; i < slider_trajectory.size(); i++)
{
json point;
point["x"] = slider_trajectory[i].X;
point["y"] = slider_trajectory[i].Y;
slider_trajectory_json.push_back(point);
}
result["slider_trajectory"] = slider_trajectory_json;
// 添加参数信息
result["parameters"] = {
{"L_AB", L_AB},
{"L_BS", L_BS},
{"S_OFS", S_OFS},
{"angleDeg", angleDeg}};
res_data = result;
}
}
}
catch (const std::exception &e)
{
res_data = {
{"success", false},
{"error", "Failed to Cmd_FourBar_CrankSlider: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_QuadrupedRobot_CalculateAllPointsFromMotorAngles")
{
// json j = json::parse(jsonStr);
std::cout << "[DEBUG 0001] Cmd_QuadrupedRobot_CalculateAllPointsFromMotorAngles 步骤1: 初始化数据结构" << std::endl;
std::string jsonInput = req_param.dump();
result = KinematicsHelper::QuadrupedRobot_CalculateAllPointsFromMotorAngles(jsonInput);
// if (req_param.empty())
// {
// result = KinematicsHelper::QuadrupedRobot_CalculateAllPointsFromMotorAngles();
// }
// else
// {
// std::string jsonInput = req_param.dump();
// result = KinematicsHelper::QuadrupedRobot_CalculateAllPointsFromMotorAngles(jsonInput);
// }
res_data = result;
}
else if (req_cmd == "Cmd_QuadrupedRobot_PerformForwardKinematics")
{
std::cout << "[DEBUG] Cmd_QuadrupedRobot_PerformForwardKinematics = " << std::endl;
std::string jsonInput = req_param.dump();
result = KinematicsHelper::QuadrupedRobot_PerformForwardKinematics(jsonInput);
// if (req_param.empty())
// {
// result = KinematicsHelper::QuadrupedRobot_PerformForwardKinematics();
// }
// else
// {
// std::string jsonInput = req_param.dump();
// result = KinematicsHelper::QuadrupedRobot_PerformForwardKinematics(jsonInput);
// }
res_data = result;
}
else
{
// 未知命令，调用默认处理函数
res_data["success"] = false;
res_data["error"] = "Unknown command: " + req_cmd;
res_data["received_params"] = req_param;
res_data["timestamp"] = std::time(nullptr);
}
json response = utils::create_api_response(true, 0, msg, req_code, req_from, req_cmd, res_data);
response_string = response.dump();
}
catch (const std::exception &e)
{
json error_response = utils::create_api_response(false, 500, "Processing error: " + std::string(e.what()), "", "", "");
response_string = error_response.dump();
}
return response_string;
}
void KinematicsWebAPI::log(const std::string &message)
{
std::cout << "[" << getCurrentTimestamp() << "] " << message << std::endl;
if (onLog)
{
onLog("[" + getCurrentTimestamp() + "] " + message);
}
}
std::string KinematicsWebAPI::getCurrentTimestamp()
{
return utils::get_current_timestamp();
}
bool KinematicsWebAPI::is_running() const
{
return running_;
}

333
src/Robot.cpp
View File

File diff suppressed because it is too large Load Diff

126
src/api/KinematicsWebAPI.Core.cpp Normal file
View File

@@ -0,0 +1,126 @@
#include "KinematicsWebAPI.h"
#include "URDFStrings.h"
#include <cassert>
#include <ctime>
#include <iostream>
#include <string>
namespace
{
bool isRobotCommand(const std::string &req_cmd)
{
return req_cmd == "Cmd_Kinematics_inverse_pose_str" ||
req_cmd == "Cmd_Kinematics_inverse_pose_str_2PSteps" ||
req_cmd == "Cmd_Kinematics_inverse_pose_str_NoDifference" ||
req_cmd == "Cmd_Kinematics_forward_pose_str" ||
req_cmd == "Cmd_Kinematics_forward_all_joints" ||
req_cmd == "Cmd_InitRobot" ||
req_cmd == "Cmd_GetRobot" ||
req_cmd == "Cmd_RemoveRobot" ||
req_cmd == "Cmd_ListRobots" ||
req_cmd == "Cmd_SelectCraftTree" ||
req_cmd == "Cmd_AddOperationTree";
}
} // namespace
KinematicsWebAPI::KinematicsWebAPI()
{
init_func();
}
std::string KinematicsWebAPI::init_func()
{
std::string urdf_string = URDFStrings::abb120_urdf;
RobotManager::initRobot(urdf_string, "9D7EAEF4-1AAB-499E-8783-B6CE016BC6D1");
RobotManager::initRobot(urdf_string, "abb_irb120_3_58");
return utils::create_api_response(true, 200, "init_func", "", "", "").dump();
}
std::string KinematicsWebAPI::func(std::string sanitized_body)
{
log("func called with body: " + sanitized_body.substr(0, 100));
assert(!sanitized_body.empty() && "sanitized_body should not be empty");
try
{
json request_json;
try
{
request_json = json::parse(sanitized_body);
}
catch (const std::exception &)
{
return utils::create_api_response(false, 400, "Invalid JSON format", "", "", "").dump();
}
const std::string msg = request_json.value("msg", "");
const std::string req_code = request_json.value("req_code", "");
const std::string req_from = request_json.value("req_from", "");
const std::string req_cmd = request_json.value("req_cmd", "");
const json req_param = request_json.value("req_param", json::object());
json res_data = dispatchCommand(req_cmd, req_param);
return utils::create_api_response(true, 0, msg, req_code, req_from, req_cmd, res_data).dump();
}
catch (const std::exception &e)
{
return utils::create_api_response(false, 500, "Processing error: " + std::string(e.what()), "", "", "").dump();
}
}
json KinematicsWebAPI::dispatchCommand(const std::string &req_cmd, const json &req_param)
{
if (isRobotCommand(req_cmd))
{
return handleRobotCommand(req_cmd, req_param);
}
if (req_cmd == "Cmd_Spc")
{
return handleSpcCommand(req_cmd, req_param);
}
if (req_cmd == "Cmd_FourBar_CrankSlider")
{
return handleFourBarCommand(req_cmd, req_param);
}
if (req_cmd == "Cmd_QuadrupedRobot_CalculateAllPointsFromMotorAngles" ||
req_cmd == "Cmd_QuadrupedRobot_PerformForwardKinematics")
{
return handleQuadrupedCommand(req_cmd, req_param);
}
return createUnknownCommandResponse(req_cmd, req_param);
}
json KinematicsWebAPI::createUnknownCommandResponse(const std::string &req_cmd, const json &req_param) const
{
return {
{"success", false},
{"error", "Unknown command: " + req_cmd},
{"received_params", req_param},
{"timestamp", std::time(nullptr)}};
}
void KinematicsWebAPI::log(const std::string &message)
{
std::cout << "[" << getCurrentTimestamp() << "] " << message << std::endl;
if (onLog)
{
onLog("[" + getCurrentTimestamp() + "] " + message);
}
}
std::string KinematicsWebAPI::getCurrentTimestamp()
{
return utils::get_current_timestamp();
}
bool KinematicsWebAPI::is_running() const
{
return running_;
}

114
src/api/KinematicsWebAPI.FourBarCommands.cpp Normal file
View File

@@ -0,0 +1,114 @@
#include "KinematicsWebAPI.h"
#include "FourBarMechanism/CrankSliderMechanism.h"
json KinematicsWebAPI::handleFourBarCommand(const std::string &req_cmd, const json &req_param)
{
try
{
log("Handling " + req_cmd + " command");
double L_AB = req_param.value("L_AB", 0.5);
double L_BS = req_param.value("L_BS", 2.0);
double S_OFS = req_param.value("S_OFS", 0.0);
double angleDeg = req_param.value("angleDeg", 0.0);
log("Parameters: L_AB=" + std::to_string(L_AB) +
", L_BS=" + std::to_string(L_BS) +
", S_OFS=" + std::to_string(S_OFS) +
", angleDeg=" + std::to_string(angleDeg));
std::unique_ptr<CrankSliderMechanism> mechanism(createCrankSliderMechanism());
mechanism->setL_AB(L_AB);
mechanism->setL_BS(L_BS);
mechanism->setS_OFS(S_OFS);
ValidationResult validation = mechanism->validateParameters();
if (!validation.isValid())
{
return {
{"success", false},
{"error", "Invalid parameters"},
{"validation_errors", validation.Errors},
{"validation_warnings", validation.Warnings}};
}
MechanismState state = mechanism->calculate(angleDeg);
if (state.hasError())
{
return {
{"success", false},
{"error", state.ErrorMessage}};
}
json result;
result["success"] = true;
json points_json;
for (const auto &point_pair : state.Points)
{
points_json[point_pair.first] = {
{"x", point_pair.second.X},
{"y", point_pair.second.Y}};
}
result["points"] = points_json;
json poses_json;
for (const auto &pose_pair : state.Poses)
{
poses_json[pose_pair.first] = {
{"tx", pose_pair.second.tx},
{"ty", pose_pair.second.ty},
{"tz", pose_pair.second.tz},
{"qx", pose_pair.second.qx},
{"qy", pose_pair.second.qy},
{"qz", pose_pair.second.qz},
{"qw", pose_pair.second.qw}};
}
result["poses"] = poses_json;
json angles_json;
for (const auto &angle_pair : state.Angles)
{
angles_json[angle_pair.first] = angle_pair.second;
}
result["angles"] = angles_json;
result["input_value"] = state.InputValue;
if (state.hasWarning())
{
result["warning"] = state.WarningMessage;
}
json trajectory_json = json::array();
for (const auto &point : mechanism->getTrajectoryPoints())
{
trajectory_json.push_back({
{"x", point.X},
{"y", point.Y}});
}
result["trajectory"] = trajectory_json;
json slider_trajectory_json = json::array();
for (const auto &point : mechanism->getSliderTrajectory())
{
slider_trajectory_json.push_back({
{"x", point.X},
{"y", point.Y}});
}
result["slider_trajectory"] = slider_trajectory_json;
result["parameters"] = {
{"L_AB", L_AB},
{"L_BS", L_BS},
{"S_OFS", S_OFS},
{"angleDeg", angleDeg}};
return result;
}
catch (const std::exception &e)
{
return {
{"success", false},
{"error", "Failed to Cmd_FourBar_CrankSlider: " + std::string(e.what())}};
}
}

21
src/api/KinematicsWebAPI.QuadrupedCommands.cpp Normal file
View File

@@ -0,0 +1,21 @@
#include "KinematicsWebAPI.h"
#include "QuadrupedRobotSimulation/KinematicsHelper.h"
#include <iostream>
json KinematicsWebAPI::handleQuadrupedCommand(const std::string &req_cmd, const json &req_param)
{
if (req_cmd == "Cmd_QuadrupedRobot_CalculateAllPointsFromMotorAngles")
{
std::cout << "[DEBUG] Cmd_QuadrupedRobot_CalculateAllPointsFromMotorAngles: 开始计算点位" << std::endl;
return KinematicsHelper::QuadrupedRobot_CalculateAllPointsFromMotorAngles(req_param.dump());
}
if (req_cmd == "Cmd_QuadrupedRobot_PerformForwardKinematics")
{
std::cout << "[DEBUG] Cmd_QuadrupedRobot_PerformForwardKinematics: 开始执行正运动学" << std::endl;
return KinematicsHelper::QuadrupedRobot_PerformForwardKinematics(req_param.dump());
}
return createUnknownCommandResponse(req_cmd, req_param);
}

316
src/api/KinematicsWebAPI.RobotCommands.cpp Normal file
View File

@@ -0,0 +1,316 @@
#include "KinematicsWebAPI.h"
json KinematicsWebAPI::handleRobotCommand(const std::string &req_cmd, const json &req_param)
{
json res_data;
if (req_cmd == "Cmd_Kinematics_inverse_pose_str")
{
try
{
log("Handling Cmd_Kinematics_inverse_pose_str command");
std::string pose_str = req_param.value("pose_str", "");
std::string q_init_str = req_param.value("q_init_str", "0,0,0,0,0,0");
std::string robot_uuid = req_param.value("robot_uuid", "default");
auto robot = RobotManager::getRobot(robot_uuid);
if (!robot || !robot->isInitialized())
{
res_data = {{"error", "Robot not found or not initialized"}};
}
else
{
json result = robot->inversePoseStr(pose_str, q_init_str);
if (result.empty())
{
res_data = {{"error", "Inverse kinematics calculation failed"}};
}
else
{
res_data = {{"joints", result}, {"success", true}};
}
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to calculate inverse kinematics: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_Kinematics_inverse_pose_str_2PSteps")
{
try
{
log("Handling Cmd_Kinematics_inverse_pose_str_2PSteps command");
std::string pose_str = req_param.value("pose_str", "");
std::string q_init_str = req_param.value("q_init_str", "0,0,0,0,0,0");
std::string robot_uuid = req_param.value("robot_uuid", "default");
std::string steps_str = req_param.value("steps_str", "default");
int steps = std::stoi(steps_str);
auto robot = RobotManager::getRobot(robot_uuid);
if (!robot || !robot->isInitialized())
{
res_data = {{"error", "Robot not found or not initialized"}};
}
else
{
json result = robot->inversePoseStr2PSteps(pose_str, q_init_str, steps);
if (result.empty())
{
res_data = {{"error", "Inverse kinematics calculation failed"}};
}
else
{
res_data = {{"joints", result}, {"success", true}};
}
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to calculate inverse kinematics: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_Kinematics_inverse_pose_str_NoDifference")
{
try
{
log("Handling Cmd_Kinematics_inverse_pose_str_NoDifference command");
std::string pose_str = req_param.value("pose_str", "");
std::string q_init_str = req_param.value("q_init_str", "0,0,0,0,0,0");
std::string robot_uuid = req_param.value("robot_uuid", "default");
auto robot = RobotManager::getRobot(robot_uuid);
if (!robot || !robot->isInitialized())
{
res_data = {{"error", "Robot not found or not initialized"}};
}
else
{
json result = robot->inversePoseStrNoDifference(pose_str, q_init_str);
if (result.empty())
{
res_data = {{"error", "Inverse kinematics calculation failed"}};
}
else
{
res_data = {{"joints", result}, {"success", true}};
}
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to calculate inverse kinematics: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_Kinematics_forward_pose_str")
{
try
{
log("Handling Cmd_Kinematics_forward_pose_str command");
std::string joints_str = req_param.value("q_init_str", "0,0,0,0,0,0");
std::string robot_uuid = req_param.value("robot_uuid", "default");
auto robot = RobotManager::getRobot(robot_uuid);
if (!robot || !robot->isInitialized())
{
res_data = {{"error", "Robot not found or not initialized"}};
}
else
{
auto joints = robot->parseJointString(joints_str);
if (joints.size() != 6)
{
res_data = {{"error", "Invalid joints format"}};
}
else
{
double tcp_pose[7];
if (robot->calculateFK_TCP(joints.data(), tcp_pose))
{
res_data = {
{"position", {tcp_pose[0], tcp_pose[1], tcp_pose[2]}},
{"orientation", {tcp_pose[3], tcp_pose[4], tcp_pose[5], tcp_pose[6]}},
{"joints", joints},
{"success", true}};
}
else
{
res_data = {{"error", "Forward kinematics calculation failed"}};
}
}
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to calculate forward kinematics: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_Kinematics_forward_all_joints")
{
try
{
log("Handling Cmd_Kinematics_forward_all_joints command");
std::string joints_str = req_param.value("joints_str", "0,0,0,0,0,0");
std::string robot_uuid = req_param.value("robot_uuid", "default");
auto robot = RobotManager::getRobot(robot_uuid);
if (!robot || !robot->isInitialized())
{
res_data = {{"error", "Robot not found or not initialized"}};
}
else
{
res_data = {
{"OPERATION", robot->handleKinematicsForwardAllJoints(joints_str)},
{"success", true}};
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to calculate forward kinematics for all joints: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_InitRobot")
{
try
{
log("Handling Cmd_InitRobot command");
std::string urdf_base64 = req_param.value("urdf_base64", "");
std::string uuid = req_param.value("robot_uuid", "");
bool force_update = req_param.value("force_update", true);
std::string urdf_content = utils::base64_to_urdf(urdf_base64);
if (!utils::validate_urdf_base64(urdf_base64))
{
res_data = {
{"success", false},
{"message", "Invalid URDF format"},
{"timestamp", utils::get_current_time()}};
}
else
{
auto result = RobotManager::initRobot(urdf_content, uuid, force_update);
res_data = {
{"success", result.first},
{"message", result.second},
{"timestamp", utils::get_current_time()}};
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to initialize robot: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_GetRobot")
{
try
{
log("Handling Cmd_GetRobot command");
std::string uuid = req_param.value("uuid", "");
auto robot = RobotManager::getRobot(uuid);
if (!robot)
{
res_data = {{"error", "Robot not found"}};
}
else
{
res_data = {
{"success", true},
{"uuid", uuid},
{"initialized", robot->isInitialized()},
{"joints_count", robot->getNumberOfJoints()},
{"timestamp", utils::get_current_time()}};
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to get robot: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_RemoveRobot")
{
try
{
log("Handling Cmd_RemoveRobot command");
std::string uuid = req_param.value("uuid", "");
auto result = RobotManager::removeRobot(uuid);
res_data = {
{"success", result.first},
{"message", result.second},
{"timestamp", utils::get_current_time()}};
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to remove robot: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_ListRobots")
{
try
{
log("Handling Cmd_ListRobots command");
bool detail = req_param.value("detail", false);
auto robots = RobotManager::listRobots(detail);
res_data = {
{"success", true},
{"robots", robots},
{"count", robots.size()},
{"timestamp", utils::get_current_time()}};
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to list robots: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_SelectCraftTree")
{
try
{
log("Handling Cmd_SelectCraftTree command");
std::string tree_id = req_param.value("tree_id", "");
res_data = {
{"success", true},
{"tree_id", tree_id},
{"message", "Craft tree selected successfully"},
{"timestamp", utils::get_current_time()}};
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to select craft tree: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_AddOperationTree")
{
try
{
log("Handling Cmd_AddOperationTree command");
std::string tree_name = req_param.value("name", "");
json operations = req_param.value("operations", json::array());
res_data = {
{"success", true},
{"tree_name", tree_name},
{"operations_count", operations.size()},
{"message", "Operation tree added successfully"},
{"timestamp", utils::get_current_time()}};
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to add operation tree: " + std::string(e.what())}};
}
}
return res_data;
}

14
src/api/KinematicsWebAPI.SpcCommands.cpp Normal file
View File

@@ -0,0 +1,14 @@
#include "KinematicsWebAPI.h"
json KinematicsWebAPI::handleSpcCommand(const std::string &req_cmd, const json &req_param)
{
try
{
log("Handling " + req_cmd + " command");
return SpcCalculator::Spc(req_param);
}
catch (const std::exception &e)
{
return {{"error", "Failed to Cmd_Spc: " + std::string(e.what())}};
}
}

48
src/utils.cpp
View File

@@ -8,8 +8,6 @@
#include <algorithm>
#include <stdexcept>
#include <stdexcept>
// 实现 splitByDelimiter 方法
std::pair<std::string, std::string> StringUtils::splitByDelimiter(
const std::string &str,
@@ -88,13 +86,13 @@ std::pair<std::string, std::string> StringUtils::splitStrict(
namespace utils
{
// Base64<EFBFBD>ַ<EFBFBD><EFBFBD><EFBFBD>
// Base64 字符表
const std::string base64_chars =
"ABCDEFGHIJKLMNOPQRSTUVWXYZ"
"abcdefghijklmnopqrstuvwxyz"
"0123456789+/";
// <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD>Ϊ<EFBFBD><EFBFBD>Ч<EFBFBD><EFBFBD>base64<EFBFBD>ַ<EFBFBD>
// 判断字符是否为有效的 Base64 字符
static bool is_base64(unsigned char c)
{
return (isalnum(c) || (c == '+') || (c == '/'));
@@ -134,7 +132,7 @@ namespace utils
// res.set_header("Access-Control-Allow-Methods", "GET, POST, PUT, DELETE, OPTIONS, PATCH");
// res.set_header("Access-Control-Allow-Headers", "Content-Type, Authorization, X-Requested-With, X-API-Key, Accept, Origin");
// res.set_header("Access-Control-Expose-Headers", "Content-Length, Content-Type, X-Request-Id");
// res.set_header("Access-Control-Max-Age", "86400"); // 24Сʱ
// res.set_header("Access-Control-Max-Age", "86400"); // 24 小时
// res.set_header("Vary", "Origin");
// }
@@ -171,11 +169,11 @@ namespace utils
unsigned char c = str[i];
if (c <= 0x7F)
{
continue; // ASCII<EFBFBD>ַ<EFBFBD>
continue; // ASCII 字符
}
else if ((c & 0xE0) == 0xC0)
{
// 2<EFBFBD>ֽ<EFBFBD>UTF-8
// 2 字节 UTF-8
if (i + 1 >= str.size() || (str[i + 1] & 0xC0) != 0x80)
{
return false;
@@ -184,7 +182,7 @@ namespace utils
}
else if ((c & 0xF0) == 0xE0)
{
// 3<EFBFBD>ֽ<EFBFBD>UTF-8
// 3 字节 UTF-8
if (i + 2 >= str.size() || (str[i + 1] & 0xC0) != 0x80 || (str[i + 2] & 0xC0) != 0x80)
{
return false;
@@ -193,7 +191,7 @@ namespace utils
}
else if ((c & 0xF8) == 0xF0)
{
// 4<EFBFBD>ֽ<EFBFBD>UTF-8
// 4 字节 UTF-8
if (i + 3 >= str.size() || (str[i + 1] & 0xC0) != 0x80 ||
(str[i + 2] & 0xC0) != 0x80 || (str[i + 3] & 0xC0) != 0x80)
{
@@ -203,7 +201,7 @@ namespace utils
}
else
{
return false; // <EFBFBD><EFBFBD>Ч<EFBFBD><EFBFBD>UTF-8<EFBFBD>ֽ<EFBFBD>
return false; // 非法 UTF-8 字节
}
}
return true;
@@ -219,11 +217,11 @@ namespace utils
unsigned char c = str[i];
if (c <= 0x7F)
{
result += c; // ASCII<EFBFBD>ַ<EFBFBD>
result += c; // ASCII 字符
}
else if ((c & 0xE0) == 0xC0)
{
// 2<EFBFBD>ֽ<EFBFBD>UTF-8
// 2 字节 UTF-8
if (i + 1 < str.size() && (str[i + 1] & 0xC0) == 0x80)
{
result += c;
@@ -233,7 +231,7 @@ namespace utils
}
else if ((c & 0xF0) == 0xE0)
{
// 3<EFBFBD>ֽ<EFBFBD>UTF-8
// 3 字节 UTF-8
if (i + 2 < str.size() && (str[i + 1] & 0xC0) == 0x80 && (str[i + 2] & 0xC0) == 0x80)
{
result += c;
@@ -244,7 +242,7 @@ namespace utils
}
else if ((c & 0xF8) == 0xF0)
{
// 4<EFBFBD>ֽ<EFBFBD>UTF-8
// 4 字节 UTF-8
if (i + 3 < str.size() && (str[i + 1] & 0xC0) == 0x80 &&
(str[i + 2] & 0xC0) == 0x80 && (str[i + 3] & 0xC0) == 0x80)
{
@@ -255,7 +253,7 @@ namespace utils
i += 3;
}
}
// <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><EFBFBD>UTF-8<EFBFBD>ֽ<EFBFBD>
// 跳过非法 UTF-8 字节
}
return result;
}
@@ -285,7 +283,7 @@ namespace utils
return response;
}
// Base64<EFBFBD><EFBFBD><EFBFBD>뺯<EFBFBD><EFBFBD>
// Base64 解码函数
std::string base64_decode(const std::string &encoded_string)
{
int in_len = encoded_string.size();
@@ -295,7 +293,7 @@ namespace utils
unsigned char char_array_4[4], char_array_3[3];
std::string ret;
// <EFBFBD>Ƴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܵĻ<EFBFBD><EFBFBD>з<EFBFBD><EFBFBD>Ϳո<EFBFBD>
// 去掉输入中的换行和空格
std::string clean_encoded;
for (char c : encoded_string)
{
@@ -346,7 +344,7 @@ namespace utils
size_t pos = base64_chars.find(char_array_4[j]);
if (pos == std::string::npos && j >= i)
{
// <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ַ<EFBFBD>λ<EFBFBD><EFBFBD>Ϊ0
// 补齐的占位字符按 0 处理
char_array_4[j] = 0;
}
else if (pos != std::string::npos)
@@ -372,14 +370,14 @@ namespace utils
return ret;
}
// <EFBFBD><EFBFBD>base64<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>URDFת<EFBFBD><EFBFBD>Ϊ<EFBFBD>ַ<EFBFBD><EFBFBD><EFBFBD>
// 将 Base64 编码的 URDF 转成字符串
std::string base64_to_urdf(const std::string &base64_urdf)
{
try
{
std::string urdf_content = base64_decode(base64_urdf);
// <EFBFBD><EFBFBD>֤<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><EFBFBD>URDF/XML
// 校验解码结果是否像合法的 URDF/XML
if (urdf_content.find("<?xml") != std::string::npos ||
urdf_content.find("<robot") != std::string::npos)
{
@@ -396,7 +394,7 @@ namespace utils
}
}
// <EFBFBD><EFBFBD>base64<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>URDF<EFBFBD><EFBFBD><EFBFBD>浽<EFBFBD>ļ<EFBFBD>
// 将 Base64 编码的 URDF 保存到文件
bool save_base64_urdf_to_file(const std::string &base64_urdf, const std::string &filename)
{
try
@@ -411,7 +409,7 @@ namespace utils
}
}
// <EFBFBD><EFBFBD>URDF<EFBFBD>ַ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>浽<EFBFBD>ļ<EFBFBD>
// 将 URDF 字符串保存到文件
bool save_urdf_string_to_file(const std::string &urdf_content, const std::string &filename)
{
try
@@ -437,14 +435,14 @@ namespace utils
}
}
// <EFBFBD><EFBFBD>֤base64<EFBFBD>ַ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><EFBFBD>URDF
// 校验 Base64 字符串是否可解析为合法 URDF
bool validate_urdf_base64(const std::string &base64_urdf)
{
try
{
std::string urdf_content = base64_to_urdf(base64_urdf);
// <EFBFBD>򵥵<EFBFBD>URDF<EFBFBD><EFBFBD>֤
// 基础 URDF 结构校验
bool has_xml_decl = urdf_content.find("<?xml") != std::string::npos;
bool has_robot_tag = urdf_content.find("<robot") != std::string::npos;
bool has_link_tag = urdf_content.find("<link") != std::string::npos;
@@ -458,4 +456,4 @@ namespace utils
}
}
} // namespace utils
} // namespace utils