diff --git a/include/KinematicsWebAPI.h b/include/KinematicsWebAPI.h index 8e2719e..2b2a5da 100644 --- a/include/KinematicsWebAPI.h +++ b/include/KinematicsWebAPI.h @@ -19,6 +19,24 @@ private: std::function 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(); diff --git a/include/Robot.h b/include/Robot.h index a5fb4c9..e94ccde 100644 --- a/include/Robot.h +++ b/include/Robot.h @@ -1,17 +1,20 @@ -#ifndef ROBOT_H +#ifndef ROBOT_H #define ROBOT_H -#include + #include #include -#include +#include #include -#include // 锟斤拷锟斤拷LMA锟斤拷锟斤拷锟酵凤拷募锟� +#include #include #include + #include -// 锟斤拷 Robot.h 锟斤拷锟斤拷锟斤拷 +#include #include + #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锟斤拷锟斤拷锟� + KDL::ChainIkSolverPos_LMA *ikSolverLMA; bool m_initialized; - std::unordered_map jointChildLinkUuidMap; // joint锟斤拷锟狡碉拷child link UUID锟斤拷映锟斤拷 + // 记录 joint 名称到 child link UUID 的映射，便于前端回写对象姿态。 + std::unordered_map 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锟街凤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟� - * @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 锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷呕锟饺�joint锟斤拷应锟斤拷child link UUID - * @param jointIndex joint锟斤拷锟� (1, 2, 3, ...) - * @return 锟斤拷应锟斤拷UUID锟街凤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷蚍祷乜锟斤拷址锟斤拷锟� + * @brief 根据关节序号获取 child link UUID。 + * @param jointIndex 关节序号，从 1 开始。 + * @return 找到时返回对应 UUID，未找到返回空字符串。 */ std::string getJointUuidByIndex(int jointIndex) const; /** - * @brief 锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷呕锟饺�joint锟斤拷锟斤拷 - * @param jointIndex joint锟斤拷锟� (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锟斤拷锟斤拷斜锟� - * @return 锟斤拷锟斤拷锟斤拷锟叫匡拷锟斤拷joint锟斤拷诺锟斤拷锟斤拷锟� + * @brief 获取当前已解析到的全部关节序号列表。 + * @return 已排序且去重后的关节序号集合。 */ std::vector 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); - // 锟斤拷锟斤拷锟斤拷锟斤拷筒锟街碉拷锟斤拷锟斤拷锟斤拷锟斤拷锟截诧拷趾锟斤拷锟斤拷锟� + + /** + * @brief 对两点姿态按指定步数插补后做逆解。 + */ json inversePoseStr2PSteps(const std::string &pose_str, const std::string &q_init_str, const std::int32_t steps); - // 锟斤拷锟斤拷锟斤拷牡锟街�锟戒不锟斤拷锟叫诧拷趾锟街憋拷臃锟斤拷囟锟接︼拷锟斤拷锟斤拷锟� + + /** + * @brief 直接对输入姿态点做逆解，不做相邻点插补。 + */ json inversePoseStrNoDifference(const std::string &pose_str, const std::string &q_init_str); + + /** + * @brief 解析姿态字符串，格式为 `x,y,z,qx,qy,qz,qw;...`。 + */ std::vector> parsePoseString(const std::string &pose_str); + + /** + * @brief 解析关节列表字符串，格式为 `j1,j2,j3,j4,j5,j6;...`。 + */ std::vector> parseJointListString(const std::string &pose_str); std::unordered_map getJointChildLinkUuidMap() const { return jointChildLinkUuidMap; } std::string getJointChildLinkUuid(const std::string &jointName) const; std::vector parseJointString(const std::string &joint_str); - json kinematicsForwardAllJointsList(std::vector 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 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 锟斤拷锟斤拷锟剿讹拷学锟斤拷锟� (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 锟斤拷锟斤拷锟剿讹拷学锟斤拷锟� (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 锟斤拷锟斤拷锟剿讹拷学锟斤拷锟� - 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 锟斤拷锟斤拷锟剿讹拷学锟斤拷锟� - 锟斤拷锟叫关斤拷位锟斤拷 - * @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 锟斤拷锟斤拷锟斤拷态锟斤拷墓旒ｏ拷婊�锟斤拷锟斤拷锟斤拷姹撅拷锟� - * @param pose1 锟斤拷始位锟斤拷 [x, y, z, qx, qy, qz, qw] - * @param pose2 锟斤拷止位锟斤拷 [x, y, z, qx, qy, qz, qw] - * @param outputPoses 锟斤拷锟斤拷旒ｏ拷锟斤拷锟斤拷锟� - * @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 锟斤拷锟斤拷锟斤拷态锟斤拷墓旒ｏ拷婊�锟斤拷锟斤拷锟斤拷姹撅拷锟斤拷锟斤拷锟斤拷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> &outputPoses, int npoint = 0); /** - * @brief 锟斤拷锟斤拷锟斤拷态锟斤拷墓旒ｏ拷婊�锟斤拷vector锟芥本锟斤拷 - * @param pose1 锟斤拷始位锟斤拷 - * @param pose2 锟斤拷止位锟斤拷 - * @param outputPoses 锟斤拷锟斤拷旒ｏ拷锟斤拷锟斤拷锟� - * @param npoint 锟届迹锟斤拷锟斤拷锟斤拷锟斤拷0锟斤拷示锟皆讹拷锟斤拷锟姐） - * @return 实锟斤拷锟斤拷锟缴的轨迹锟斤拷锟斤拷锟斤拷锟斤拷失锟杰凤拷锟斤拷-1 + * @brief 使用 `std::vector` 形式输入位姿生成轨迹。 + * @param pose1 起点位姿。 + * @param pose2 终点位姿。 + * @param outputPoses 输出轨迹点集合。 + * @param npoint 轨迹点数量，传 0 表示自动计算。 + * @return 实际生成的轨迹点数量，失败返回 -1。 */ int trajectoryPlanning(const std::vector &pose1, const std::vector &pose2, std::vector> &outputPoses, int npoint = 0); - // 锟斤拷锟竭猴拷锟斤拷 - /** - * @brief 锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷元锟斤拷之锟斤拷慕嵌炔锟� - * @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 锟皆讹拷锟斤拷锟斤拷旒ｏ拷婊�锟斤拷锟斤拷锟斤拷锟� - * @param startPose 锟斤拷始位锟斤拷 [x, y, z, qx, qy, qz, qw] - * @param endPose 锟斤拷止位锟斤拷 [x, y, z, qx, qy, qz, qw] - * @param minSteps 锟斤拷小锟斤拷锟斤拷 - * @param maxSteps 锟斤拷锟斤拷锟� - * @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 &startPose, const std::vector &endPose, int minSteps, int maxSteps, double positionResolution, double orientationResolution); double quaternionAngleDifference(const std::vector &q1, const std::vector &q2); /** - * @brief 锟斤拷锟斤拷锟剿讹拷学锟斤拷锟� (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 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 inverse(const std::vector &pose, const std::vector &iniJ, double eps = 1e-8, int maxiter = 3000, double eps_joints = 1e-12); + /** + * @brief 使用默认零位初值计算逆解，输入为数组。 + */ std::vector inverse(const double pose[7], double eps = 1e-8, int maxiter = 3000, double eps_joints = 1e-12); + /** + * @brief 使用默认零位初值计算逆解，输入为向量。 + */ std::vector inverse(const std::vector &pose, double eps = 1e-8, int maxiter = 3000, double eps_joints = 1e-12); }; -#endif // ROBOT_H \ No newline at end of file +#endif // ROBOT_H diff --git a/include/utils.h b/include/utils.h index fc49965..e630c1d 100644 --- a/include/utils.h +++ b/include/utils.h @@ -1,37 +1,33 @@ #ifndef UTILS_H #define UTILS_H -#include #include -#include "nlohmann/json.hpp" +#include -// // ǰ������ httplib �� Response �� -// namespace httplib { -// class Response; -// } +#include "nlohmann/json.hpp" using json = nlohmann::json; namespace utils { - // ʱ����غ��� + // 时间相关工具。 std::string get_current_time(); std::string get_current_timestamp(); - // JSON ��Ӧ���� + // 通用 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 ��֤ + // UTF-8 校验与清洗。 bool is_valid_utf8(const std::string &str); std::string sanitize_utf8(const std::string &str); - // API ��Ӧ��ʽ + // 业务 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 �����URDF�������� + // 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 包含两个部分的pair - * 第一个元素是分隔符前的部分，第二个是分隔符后的部分 - * 如果未找到分隔符，第二个元素为空字符串 + * @brief 将包含单个分隔符的字符串拆分为两部分。 + * @param str 要拆分的字符串。 + * @param delimiter 分隔符，默认为 `_`。 + * @return 第一个元素为分隔符前的内容，第二个元素为分隔符后的内容。 */ static std::pair splitByDelimiter( const std::string &str, char delimiter = '_'); /** - * @brief 专门拆分"A_B"格式的字符串 - * @param str 要拆分的字符串 - * @return std::pair 包含两个部分的pair + * @brief 专门拆分 `A_B` 形式的字符串。 + * @param str 要拆分的字符串。 + * @return 拆分后的两段内容。 */ static std::pair 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 包含两个部分的pair - * @throws std::invalid_argument 如果没有找到分隔符或找到多个分隔符 + * @brief 严格拆分字符串，要求只出现一个分隔符。 + * @param str 要拆分的字符串。 + * @param delimiter 分隔符，默认为 `_`。 + * @return 拆分后的两段内容。 + * @throws std::invalid_argument 当分隔符不存在或出现多次时抛出异常。 */ static std::pair splitStrict( const std::string &str, char delimiter = '_'); }; -#endif // UTILS_H \ No newline at end of file + +#endif // UTILS_H diff --git a/scripts/build.ps1 b/scripts/build.ps1 index 84dc8c6..fae65b0 100644 --- a/scripts/build.ps1 +++ b/scripts/build.ps1 @@ -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 ` diff --git a/src/KinematicsWebAPI.cpp b/src/KinematicsWebAPI.cpp deleted file mode 100644 index d0b4cfd..0000000 --- a/src/KinematicsWebAPI.cpp +++ /dev/null @@ -1,609 +0,0 @@ - -#include "QuadrupedRobotSimulation/KinematicsHelper.h" -#include "KinematicsWebAPI.h" -#include "FourBarMechanism/CrankSliderMechanism.h" -#include "URDFStrings.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -// 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 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 trajectory = mechanism->getTrajectoryPoints(); - std::vector 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_; -} diff --git a/src/Robot.cpp b/src/Robot.cpp index d1e72eb..f9aa2cf 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -19,7 +19,7 @@ using namespace std; Robot::Robot() : m_initialized(false), fkSolver(nullptr), ikVelSolver(nullptr), ikSolverNR(nullptr), ikSolverLMA(nullptr) { - // ���캯����ʼ���б���ȷ��ʼ�����г�Ա + // 构造函数中显式初始化全部求解器指针。 } Robot::Robot(const std::string &urdfString) : m_initialized(false), fkSolver(nullptr), @@ -37,28 +37,29 @@ Robot::~Robot() delete ikSolverNR; delete ikSolverLMA; } -// ��initRobot��������ӽ���joint-child link UUIDӳ��ĵ��� + +// 根据 URDF 初始化机器人，并提取关节与 child link 的 UUID 映射。 bool Robot::initRobot(const std::string &urdfString) { try { KDL::Tree tree; - // ��URDF�ַ�������������ģ�� + // 从 URDF 字符串解析 KDL Tree。 if (!kdl_parser::treeFromString(urdfString, tree)) { std::cerr << "Failed to parse URDF from string" << std::endl; return false; } - // ��ȡ��base��tool0���˶��� + // 提取 base 到 tool0 的运动学链。 if (!tree.getChain("base", "tool0", kinematicChain)) { std::cerr << "Failed to get chain from base to tool0" << std::endl; return false; } - // ���ؽ����� + // 当前逻辑只支持 6 轴机器人链。 if (kinematicChain.getNrOfJoints() != 6) { std::cerr << "Robot chain must have exactly 6 joints, got: " @@ -66,18 +67,18 @@ bool Robot::initRobot(const std::string &urdfString) return false; } - // ����joint��child link UUIDӳ�� + // 解析 joint 到 child link UUID 的映射。 if (!parseJointChildLinkUuidsFromUrdf(urdfString)) { std::cerr << "Warning: Failed to parse joint child link UUIDs from URDF" << std::endl; } - // ��ʼ���˶�ѧ����� + // 初始化 FK / IK 求解器。 fkSolver = new KDL::ChainFkSolverPos_recursive(kinematicChain); ikVelSolver = new KDL::ChainIkSolverVel_pinv(kinematicChain); ikSolverNR = new KDL::ChainIkSolverPos_NR(kinematicChain, *fkSolver, *ikVelSolver, 100, 1e-6); - // LMA�����������Ҫʱ��̬��������Ϊ��Ҫ���� + // LMA 求解器按调用时动态构造，便于传入不同收敛参数。 m_initialized = true; std::cout << "Robot initialized successfully with " << kinematicChain.getNrOfJoints() << " joints" << std::endl; @@ -90,7 +91,7 @@ bool Robot::initRobot(const std::string &urdfString) return false; } } -// ����������Ż�ȡjoint���� +// 根据序号生成标准关节名。 std::string Robot::getJointNameByIndex(int jointIndex) { if (jointIndex < 1) @@ -103,7 +104,7 @@ std::string Robot::getJointNameByIndex(int jointIndex) return ss.str(); } -// ����������Ż�ȡjoint��Ӧ��child link UUID +// 根据关节序号获取 child link UUID。 std::string Robot::getJointUuidByIndex(int jointIndex) const { std::string jointName = getJointNameByIndex(jointIndex); @@ -115,7 +116,7 @@ std::string Robot::getJointUuidByIndex(int jointIndex) const return getJointChildLinkUuid(jointName); } -// ��ȡ���п���joint����б� +// 获取当前已经解析到的全部关节序号。 std::vector Robot::getAvailableJointIndices() const { std::vector indices; @@ -124,25 +125,25 @@ std::vector Robot::getAvailableJointIndices() const { const std::string &jointName = pair.first; - // ���joint�����Ƿ���� "joint_X" ��ʽ + // 只统计 `joint_X` 这类标准格式。 if (jointName.find("joint_") == 0) { try { - // ��ȡ���ֲ��� + // 取出序号部分。 std::string indexStr = jointName.substr(6); // "joint_".length() = 6 int index = std::stoi(indexStr); indices.push_back(index); } catch (const std::exception &e) { - // ���ת��ʧ�ܣ��������joint + // 非数字后缀直接忽略。 continue; } } } - // ����ȥ�� + // 排序并去重。 std::sort(indices.begin(), indices.end()); indices.erase(std::unique(indices.begin(), indices.end()), indices.end()); @@ -153,25 +154,25 @@ std::string Robot::findJointUuidByAnyFormat(int jointIndex) const if (jointIndex < 1) return ""; - // ���Զ��ֿ��ܵ�joint���Ƹ�ʽ + // 兼容多种可能的关节命名格式。 std::vector possibleNames; - // ��ʽ1: "joint_X" + // 格式 1: `joint_X` std::stringstream ss1; ss1 << "joint_" << jointIndex; possibleNames.push_back(ss1.str()); - // ��ʽ2: "jointX" (���»���) + // 格式 2: `jointX` std::stringstream ss2; ss2 << "joint" << jointIndex; possibleNames.push_back(ss2.str()); - // ��ʽ3: "joint_XX" (��λ��) + // 格式 3: `joint_XX` std::stringstream ss3; ss3 << "joint_" << std::setw(2) << std::setfill('0') << jointIndex; possibleNames.push_back(ss3.str()); - // �������п��ܵ����� + // 逐个尝试匹配。 for (const auto &name : possibleNames) { auto it = jointChildLinkUuidMap.find(name); @@ -183,21 +184,20 @@ std::string Robot::findJointUuidByAnyFormat(int jointIndex) const return ""; } -// ��ӽ���joint-child link UUIDӳ��ķ��� -// ��ӽ���joint-child link UUIDӳ��ķ��� +// 解析 URDF 中的 joint 与 child link UUID 映射。 bool Robot::parseJointChildLinkUuidsFromUrdf(const std::string &urdfString) { try { jointChildLinkUuidMap.clear(); - // ʹ��������ʽƥ��joint��ǩ��child link��UUID - // ʹ�� [\\s\\S]*? ��ƥ��������з����ڵ��κ��ַ� + // 使用正则提取 joint 标签及其 child link UUID。 + // `[\\s\\S]*?` 用于跨行匹配 joint 标签内部内容。 std::regex jointRegex(R"(]*>[\\s\\S]*? Child link: " << childLinkName << " -> UUID: " << uuid << std::endl; @@ -219,12 +219,12 @@ bool Robot::parseJointChildLinkUuidsFromUrdf(const std::string &urdfString) searchStart = matches.suffix().first; } - // ���û���ҵ��κ�joint�����Ը��򵥵�ƥ�䷽�� + // 如果首轮没有命中，则退化为更宽松的解析方式。 if (!foundAny) { std::cout << "Trying alternative parsing method..." << std::endl; - // ����2: �ֱ�ƥ��joint���ƺ�child link + // 方案二：分别匹配 joint 名称和 child link。 std::regex jointNameRegex(R"()"); std::smatch endMatch; if (std::regex_search(jointStart, urdfString.cend(), endMatch, jointEndRegex)) { jointEnd = endMatch[0].second; - // ��joint��ǩ��Χ�ڲ���child link + // 在 joint 片段中提取 child link UUID。 std::string jointContent(jointStart, jointEnd); if (std::regex_search(jointContent, childMatches, childLinkRegex)) { @@ -272,12 +272,12 @@ bool Robot::parseJointChildLinkUuidsFromUrdf(const std::string &urdfString) } } - // ����Ƿ��ҵ���joint + // 如果最终仍未找到任何关节映射，则输出 URDF 片段辅助排查。 if (jointChildLinkUuidMap.empty()) { std::cerr << "No joint child link UUIDs found in URDF" << std::endl; - // �������URDF�������ڵ��� + // 仅打印前 500 个字符，避免日志过长。 std::cout << "First 500 characters of URDF:" << std::endl; std::cout << urdfString.substr(0, 500) << std::endl; @@ -294,7 +294,7 @@ bool Robot::parseJointChildLinkUuidsFromUrdf(const std::string &urdfString) return false; } } -// ��ӻ�ȡ�ض�joint��child link UUID�ķ��� +// 根据关节名称获取 child link UUID。 std::string Robot::getJointChildLinkUuid(const std::string &jointName) const { auto it = jointChildLinkUuidMap.find(jointName); @@ -302,7 +302,7 @@ std::string Robot::getJointChildLinkUuid(const std::string &jointName) const { return it->second; } - return ""; // ���ؿ��ַ�����ʾδ�ҵ� + return ""; // 返回空字符串表示未找到。 } bool Robot::calculateIK_NR(const double pose[7], const double iniJ[6], double resultJoints[6]) @@ -315,7 +315,7 @@ bool Robot::calculateIK_NR(const double pose[7], const double iniJ[6], double re try { - // ����������ת��ΪKDL���� + // 将输入位姿转换为 KDL Frame。 KDL::Vector position(pose[0], pose[1], pose[2]); KDL::Rotation rotation = KDL::Rotation::Quaternion(pose[3], pose[4], pose[5], pose[6]); KDL::Frame targetFrame(rotation, position); @@ -328,12 +328,12 @@ bool Robot::calculateIK_NR(const double pose[7], const double iniJ[6], double re KDL::JntArray result(6); - // ִ�������˶�ѧ��� (NR����) + // 执行 NR 逆运动学求解。 int status = ikSolverNR->CartToJnt(initialJoints, targetFrame, result); if (status >= 0) { - // �����ת����double���� + // 将结果写回输出数组。 for (int i = 0; i < 6; i++) { resultJoints[i] = result(i); @@ -342,13 +342,13 @@ bool Robot::calculateIK_NR(const double pose[7], const double iniJ[6], double re } else { - std::cerr << "NR�����˶�ѧ���ʧ�ܣ�������: " << status << std::endl; + std::cerr << "NR 逆运动学求解失败，状态码: " << status << std::endl; return false; } } catch (const std::exception &e) { - std::cerr << "NR�����˶�ѧ����쳣: " << e.what() << std::endl; + std::cerr << "NR 逆运动学求解异常: " << e.what() << std::endl; return false; } } @@ -364,10 +364,10 @@ bool Robot::calculateIK_LMA(const double pose[7], const double iniJ[6], double r try { - // ����LMA����� (ÿ�ε��ö������µģ���Ϊ�������ܲ�ͬ) + // 每次调用时动态构造 LMA 求解器，便于传入不同参数。 KDL::ChainIkSolverPos_LMA ikSolverLMA(kinematicChain, eps, maxiter, eps_joints); - // ����������ת��ΪKDL���� + // 将输入位姿转换为 KDL Frame。 KDL::Vector position(pose[0], pose[1], pose[2]); KDL::Rotation rotation = KDL::Rotation::Quaternion(pose[3], pose[4], pose[5], pose[6]); KDL::Frame targetFrame(rotation, position); @@ -380,12 +380,12 @@ bool Robot::calculateIK_LMA(const double pose[7], const double iniJ[6], double r KDL::JntArray result(6); - // ִ�������˶�ѧ��� (LMA����) + // 执行 LMA 逆运动学求解。 int status = ikSolverLMA.CartToJnt(initialJoints, targetFrame, result); if (status >= 0) { - // �����ת����double���� + // 将结果写回输出数组。 for (int i = 0; i < 6; i++) { resultJoints[i] = result(i); @@ -394,13 +394,13 @@ bool Robot::calculateIK_LMA(const double pose[7], const double iniJ[6], double r } else { - std::cerr << "LMA�����˶�ѧ���ʧ�ܣ�������: " << status << std::endl; + std::cerr << "LMA 逆运动学求解失败，状态码: " << status << std::endl; return false; } } catch (const std::exception &e) { - std::cerr << "LMA�����˶�ѧ����쳣: " << e.what() << std::endl; + std::cerr << "LMA 逆运动学求解异常: " << e.what() << std::endl; return false; } } @@ -415,7 +415,7 @@ bool Robot::calculateFK_TCP(const double joints[6], double tcpPose[7]) try { - // ����������ת��ΪKDL���� + // 将关节数组转换为 KDL 关节对象。 KDL::JntArray jointArray(6); for (int i = 0; i < 6; i++) { @@ -424,17 +424,17 @@ bool Robot::calculateFK_TCP(const double joints[6], double tcpPose[7]) KDL::Frame tcpFrame; - // ִ�������˶�ѧ��⣨ĩ��ִ������ + // 计算 TCP 正运动学结果。 int status = fkSolver->JntToCart(jointArray, tcpFrame); if (status >= 0) { - // ��ȡλ�� + // 提取位置。 tcpPose[0] = tcpFrame.p.x(); tcpPose[1] = tcpFrame.p.y(); tcpPose[2] = tcpFrame.p.z(); - // ��ȡ��Ԫ�� + // 提取四元数。 double x, y, z, w; tcpFrame.M.GetQuaternion(x, y, z, w); tcpPose[3] = x; @@ -446,13 +446,13 @@ bool Robot::calculateFK_TCP(const double joints[6], double tcpPose[7]) } else { - std::cerr << "�����˶�ѧ���ʧ�ܣ�������: " << status << std::endl; + std::cerr << "TCP 正运动学计算失败，状态码: " << status << std::endl; return false; } } catch (const std::exception &e) { - std::cerr << "�����˶�ѧ����쳣: " << e.what() << std::endl; + std::cerr << "TCP 正运动学计算异常: " << e.what() << std::endl; return false; } } @@ -513,31 +513,31 @@ bool Robot::calculateFK_AllJointsforwardKinematics(const double joints[6], doubl try { - // ����������ת��ΪKDL���� + // 将关节数组转换为 KDL 关节对象。 KDL::JntArray jointArray(6); for (int i = 0; i < 6; i++) { jointArray(i) = joints[i]; } - // Ϊÿ���ؽڼ��������˶�ѧ + // 逐段计算每个关节节点的位姿。 for (int segIndex = 1; segIndex <= 6; segIndex++) { KDL::Frame frame; - // �����segIndex���ؽڵ�λ�� + // 计算当前段的关节位姿。 int status = fkSolver->JntToCart(jointArray, frame, segIndex); if (status >= 0) { int baseIndex = (segIndex - 1) * 7; - // ��ȡλ�� + // 提取位置。 jointPoses[baseIndex + 0] = frame.p.x(); jointPoses[baseIndex + 1] = frame.p.y(); jointPoses[baseIndex + 2] = frame.p.z(); - // ��ȡ��Ԫ�� + // 提取四元数。 double x, y, z, w; frame.M.GetQuaternion(x, y, z, w); jointPoses[baseIndex + 3] = x; @@ -547,7 +547,7 @@ bool Robot::calculateFK_AllJointsforwardKinematics(const double joints[6], doubl } else { - std::cerr << "�ؽ� " << segIndex << " �����˶�ѧ���ʧ�ܣ�������: " << status << std::endl; + std::cerr << "关节 " << segIndex << " 正运动学计算失败，状态码: " << status << std::endl; return false; } } @@ -556,22 +556,15 @@ bool Robot::calculateFK_AllJointsforwardKinematics(const double joints[6], doubl } catch (const std::exception &e) { - std::cerr << "�ؽ������˶�ѧ����쳣: " << e.what() << std::endl; + std::cerr << "全部关节正运动学计算异常: " << e.what() << std::endl; return false; } } /** - * @brief �Ż��汾������Ԥ�ȷ����ڴ�Ĺ켣�滮���� - * @param pose1 ��ʼλ�� - * @param pose2 ��ֹλ�� - * @param outputPoses ����켣���������ɺ����ڲ���䣩 - * @param npoint �켣��������0��ʾ�Զ����㣩 - * @return ʵ�����ɵĹ켣��������ʧ�ܷ���-1 + * @brief 计算四元数之间的夹角差，用于轨迹步数估算。 */ -// �����ĸ��������͹켣�滮���� - double Robot::quaternionAngleDifference(const double q1[4], const double q2[4]) { double dot = q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2] + q1[3] * q2[3]; @@ -584,22 +577,22 @@ int Robot::calculateAutoSteps(const double startPose[7], const double endPose[7] double positionResolution = 0.01, double orientationResolution = 0.1) { - // ����λ�þ��� + // 计算位置距离。 double dx = endPose[0] - startPose[0]; double dy = endPose[1] - startPose[1]; double dz = endPose[2] - startPose[2]; double distance = std::sqrt(dx * dx + dy * dy + dz * dz); - // ������̬���� + // 计算姿态差异。 double startQuat[4] = {startPose[3], startPose[4], startPose[5], startPose[6]}; double endQuat[4] = {endPose[3], endPose[4], endPose[5], endPose[6]}; double angleDiff = quaternionAngleDifference(startQuat, endQuat); - // ����λ�ú���̬�ֱ��ʼ������貽�� + // 分别根据位置和姿态分辨率估算步数。 int stepsFromPosition = static_cast(ceil(distance / positionResolution)); int stepsFromOrientation = static_cast(ceil(angleDiff / orientationResolution)); - // ȡ�����еĽϴ�ֵ����������minSteps��maxSteps֮�� + // 取较大值，并限制在最小/最大步数范围内。 int autoSteps = max(stepsFromPosition, stepsFromOrientation); autoSteps = max(minSteps, min(maxSteps, autoSteps)); @@ -616,22 +609,22 @@ int Robot::trajectoryPlanning(const double pose1[7], const double pose2[7], return -1; } - // �Զ�������� + // 未指定步数时自动估算。 if (npoint <= 0) { npoint = calculateAutoSteps(pose1, pose2); - cout << "�Զ�����켣������: " << npoint << endl; + cout << "自动计算轨迹点数: " << npoint << endl; } if (npoint <= 0) { - cerr << "�켣�������������0" << endl; + cerr << "轨迹点数量必须大于 0" << endl; return -1; } try { - // ��double[7]ת��ΪKDL Frame + // 将数组位姿转换为 KDL Frame。 KDL::Vector pos1(pose1[0], pose1[1], pose1[2]); KDL::Rotation rot1 = KDL::Rotation::Quaternion(pose1[3], pose1[4], pose1[5], pose1[6]); KDL::Frame f1(rot1, pos1); @@ -640,11 +633,11 @@ int Robot::trajectoryPlanning(const double pose1[7], const double pose2[7], KDL::Rotation rot2 = KDL::Rotation::Quaternion(pose2[3], pose2[4], pose2[5], pose2[6]); KDL::Frame f2(rot2, pos2); - // �޸ģ�ʹ��Ĭ�Ϲ��캯������ָ����ת��ֵ�� + // 使用圆角路径插补并配合单轴旋转插值。 // KDL::Path_RoundedComposite* path = new KDL::Path_RoundedComposite( - // 0.002, // ��Ч�뾶 - // 0.01 // �������� - // // �Ƴ�������IJ�����new KDL::RotationalInterpolation_SingleAxis() + // 0.002, // 有效圆角半径 + // 0.01 // 几何公差 + // // 旋转插值器使用单轴插值 //); Path_RoundedComposite *path = new Path_RoundedComposite(0.002, 0.01, new RotationalInterpolation_SingleAxis()); @@ -652,19 +645,19 @@ int Robot::trajectoryPlanning(const double pose1[7], const double pose2[7], path->Add(f1); path->Add(f2); - // ���·������ + // 完成路径构造。 path->Finish(); // if (!path->Finish()) { - // cerr << "·������ʧ��" << endl; + // cerr << "路径构造失败" << endl; // delete path; // return -1; // } - // ���������ٶ����� + // 设置速度曲线。 KDL::VelocityProfile *velpref = new KDL::VelocityProfile_Trap(0.5, 0.1); velpref->SetProfile(0, path->PathLength()); - // �����켣 + // 生成轨迹对象。 KDL::Trajectory *traject = new KDL::Trajectory_Segment(path, velpref); double duration = traject->Duration(); @@ -672,18 +665,18 @@ int Robot::trajectoryPlanning(const double pose1[7], const double pose2[7], int frameIndex = 0; - // ���ɹ켣�� + // 采样轨迹点。 for (int i = 0; i < npoint; i++) { double time = i * timeStep; if (i == npoint - 1) { - time = duration; // ȷ����ȷ�����յ� + time = duration; // 确保最后一个点落在终点。 } KDL::Frame current_pose = traject->Pos(time); - // ת����double[7]��ʽ + // 转回 `[x, y, z, qx, qy, qz, qw]` 数组格式。 outputPoses[frameIndex][0] = current_pose.p.x(); outputPoses[frameIndex][1] = current_pose.p.y(); outputPoses[frameIndex][2] = current_pose.p.z(); @@ -698,25 +691,20 @@ int Robot::trajectoryPlanning(const double pose1[7], const double pose2[7], frameIndex++; } - // ������Դ - delete traject; // ����Զ�ɾ��path��velpref + // 释放轨迹对象，内部会一并清理 path 和 velpref。 + delete traject; return frameIndex; } catch (const exception &e) { - cerr << "�켣�滮�쳣: " << e.what() << endl; + cerr << "轨迹规划异常: " << e.what() << endl; return -1; } } /** - * @brief ������̬��Ĺ켣�滮������汾�� - * @param pose1 ��ʼλ�� [x, y, z, qx, qy, qz, qw] - * @param pose2 ��ֹλ�� [x, y, z, qx, qy, qz, qw] - * @param outputPoses ����켣�����飨����Ԥ�ȷ����ڴ棩 - * @param npoint �켣��������0��ʾ�Զ����㣩 - * @return ʵ�����ɵĹ켣��������ʧ�ܷ���-1 + * @brief 生成位姿轨迹，输出为二维向量版本。 */ int Robot::trajectoryPlanning(const double pose1[7], const double pose2[7], std::vector> &outputPoses, int npoint) @@ -728,22 +716,22 @@ int Robot::trajectoryPlanning(const double pose1[7], const double pose2[7], return -1; } - // �Զ�������� + // 未指定步数时自动估算。 if (npoint <= 0) { npoint = calculateAutoSteps(pose1, pose2); - std::cout << "�Զ�����켣������: " << npoint << std::endl; + std::cout << "自动计算轨迹点数: " << npoint << std::endl; } if (npoint <= 0) { - std::cerr << "��Ч�ĵ���" << std::endl; + std::cerr << "无效的轨迹点数量" << std::endl; return -1; } try { - // ������ת��ΪKDL Frame + // 将数组位姿转换为 KDL Frame。 KDL::Vector pos1(pose1[0], pose1[1], pose1[2]); KDL::Rotation rot1 = KDL::Rotation::Quaternion(pose1[3], pose1[4], pose1[5], pose1[6]); KDL::Frame f1(rot1, pos1); @@ -752,14 +740,14 @@ int Robot::trajectoryPlanning(const double pose1[7], const double pose2[7], KDL::Rotation rot2 = KDL::Rotation::Quaternion(pose2[3], pose2[4], pose2[5], pose2[6]); KDL::Frame f2(rot2, pos2); - // ����·���͹켣 + // 构造路径与轨迹对象。 Path_RoundedComposite *path = new Path_RoundedComposite(0.002, 0.01, new RotationalInterpolation_SingleAxis()); path->Add(f1); path->Add(f2); path->Finish(); // if (!path->Finish()) { - // std::cerr << "·������ʧ��" << std::endl; + // std::cerr << "路径构造失败" << std::endl; // delete path; // return -1; // } @@ -772,11 +760,11 @@ int Robot::trajectoryPlanning(const double pose1[7], const double pose2[7], double duration = traject->Duration(); double timeStep = duration / npoint; - // ׼����� - ����Ԥ�ȷ��䣬�ɺ����ڲ����� + // 预分配输出空间，减少 push_back 过程中的扩容成本。 outputPoses.clear(); outputPoses.reserve(npoint); - // ���ɹ켣�� + // 采样轨迹点。 for (int i = 0; i < npoint; i++) { double time = (i == npoint - 1) ? duration : i * timeStep; @@ -797,20 +785,20 @@ int Robot::trajectoryPlanning(const double pose1[7], const double pose2[7], outputPoses.push_back(std::move(point)); } - // ������Դ - delete traject; // ����Զ�ɾ��path��velpref + // 释放轨迹对象，内部会一并清理 path 和 velpref。 + delete traject; return outputPoses.size(); } catch (const std::exception &e) { - std::cerr << "�켣�滮�쳣: " << e.what() << std::endl; + std::cerr << "轨迹规划异常: " << e.what() << std::endl; return -1; } } /** - * @brief ������̬��Ĺ켣�滮��vector�汾�� + * @brief 生成位姿轨迹，输入输出都使用 `std::vector`。 */ int Robot::trajectoryPlanning(const std::vector &pose1, const std::vector &pose2, @@ -820,11 +808,11 @@ int Robot::trajectoryPlanning(const std::vector &pose1, if (pose1.size() != 7 || pose2.size() != 7) { - std::cerr << "����λ�˱�����7ά����" << std::endl; + std::cerr << "输入位姿必须是 7 维数组" << std::endl; return -1; } - // ת��Ϊ���鲢��������汾 + // 转换为数组后复用主实现。 double pose1_arr[7], pose2_arr[7]; std::copy(pose1.begin(), pose1.end(), pose1_arr); std::copy(pose2.begin(), pose2.end(), pose2_arr); @@ -833,13 +821,7 @@ int Robot::trajectoryPlanning(const std::vector &pose1, } /** - * @brief �����˶�ѧ��� (LMA����) - ����vector�汾 - * @param pose Ŀ��λ�� [x, y, z, qx, qy, qz, qw] - * @param iniJ ��ʼ�ؽڽǶ� [6���ؽڽǶ�] - * @param eps �������� - * @param maxiter ���������� - * @param eps_joints �ؽ��ݲ� - * @return �ؽڽǶ�vector��������ʧ�ܷ��ؿ�vector + * @brief 使用 LMA 求解逆运动学，输出关节角向量。 */ std::vector Robot::inverse(const double pose[7], const double iniJ[6], double eps, int maxiter, double eps_joints) @@ -847,28 +829,28 @@ std::vector Robot::inverse(const double pose[7], const double iniJ[6], std::vector result; - // ���������Ƿ��ʼ�� + // 先检查机器人是否已经完成初始化。 if (!isInitialized()) { - std::cerr << "����: ������δ��ʼ��" << std::endl; + std::cerr << "错误: 机器人尚未初始化" << std::endl; return result; } - // ������������Ч�� + // 校验输入参数。 if (pose == nullptr || iniJ == nullptr) { - std::cerr << "����: �����������Ϊnullptr" << std::endl; + std::cerr << "错误: 输入参数不能为 nullptr" << std::endl; return result; } double resultJoints[6]; - // �������е�LMA��⺯�� + // 调用底层 LMA 求解器。 bool success = calculateIK_LMA(pose, iniJ, resultJoints, eps, maxiter, eps_joints); if (success) { - // ������ת��Ϊvector + // 将结果拷贝到 vector。 result.reserve(6); for (int i = 0; i < 6; i++) { @@ -877,16 +859,16 @@ std::vector Robot::inverse(const double pose[7], const double iniJ[6], } else { - // ���ʧ�ܣ����ؿ�vector + // 求解失败时返回空结果。 result.clear(); - std::cerr << "����: �����˶�ѧ���ʧ��" << std::endl; + std::cerr << "错误: 逆运动学求解失败" << std::endl; } return result; } /** - * @brief ���ذ汾��ʹ��vector��Ϊ���� + * @brief `std::vector` 版本的逆运动学接口。 */ std::vector Robot::inverse(const std::vector &pose, const std::vector &iniJ, @@ -895,33 +877,28 @@ std::vector Robot::inverse(const std::vector &pose, if (pose.size() != 7) { - std::cerr << "����: Ŀ��λ��ӦΪ7ά, ʵ��Ϊ " << pose.size() << " ά" << std::endl; + std::cerr << "错误: 目标位姿应为 7 维，当前为 " << pose.size() << " 维" << std::endl; return std::vector(); } if (iniJ.size() != 6) { - std::cerr << "����: ��ʼ�ؽڽǶ�ӦΪ6ά, ʵ��Ϊ " << iniJ.size() << " ά" << std::endl; + std::cerr << "错误: 初始关节角应为 6 维，当前为 " << iniJ.size() << " 维" << std::endl; return std::vector(); } - // ת��Ϊ���鲢��������汾 + // 转为数组后复用主实现。 return inverse(pose.data(), iniJ.data(), eps, maxiter, eps_joints); } /** - * @brief ���ذ汾��ʹ��Ĭ�ϳ�ʼ�ؽڽǶ� - * @param pose Ŀ��λ�� - * @param eps �������� - * @param maxiter ���������� - * @param eps_joints �ؽ��ݲ� - * @return �ؽڽǶ�vector + * @brief 使用零位初始关节角进行逆解。 */ std::vector Robot::inverse(const double pose[7], double eps, int maxiter, double eps_joints) { - // ʹ�����ʼ�ؽڽǶ� + // 使用零位关节角作为默认初值。 double zeroJoints[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; return inverse(pose, zeroJoints, eps, maxiter, eps_joints); } @@ -932,7 +909,7 @@ std::vector Robot::inverse(const std::vector &pose, if (pose.size() != 7) { - std::cerr << "����: Ŀ��λ��ӦΪ7ά" << std::endl; + std::cerr << "错误: 目标位姿应为 7 维" << std::endl; return std::vector(); } @@ -940,17 +917,17 @@ std::vector Robot::inverse(const std::vector &pose, return inverse(pose, zeroJoints, eps, maxiter, eps_joints); } -// ������ĵ�֮�䲻���в�֣�ֱ�ӷ��ض�Ӧ������ +// 对输入姿态逐点逆解，不执行轨迹插补。 json Robot::inversePoseStrNoDifference(const std::string &pose_str, const std::string &q_init_str) { - // ���� JSON ���� + // 准备输出 JSON 数组。 json joints_poses_array = json::array(); json joints_array = json::array(); std::vector> posePoints = parsePoseString(pose_str); if (posePoints.empty()) { - return joints_poses_array; // ���ؿ����� + return joints_poses_array; // 返回空数组。 } std::vector initJoints = parseJointString(q_init_str); @@ -960,17 +937,17 @@ json Robot::inversePoseStrNoDifference(const std::string &pose_str, const std::s { std::vector inverseSolution = inverse(posePoints[0], initJoints); - // �������Ƿ�ɹ� + // 单点逆解成功则直接返回。 if (!inverseSolution.empty()) { - // �����ת��Ϊ JSON ���� + // 转为 JSON 数组。 joints_array = inverseSolution; joints_poses_array.push_back(joints_array); return joints_poses_array; } else { - return joints_poses_array; // ���ؿ����� + return joints_poses_array; // 返回空数组。 } } else @@ -980,16 +957,16 @@ json Robot::inversePoseStrNoDifference(const std::string &pose_str, const std::s std::vector startPose = posePoints[i]; std::vector inverseSolution = inverse(startPose, initJoints); - // �������Ƿ�ɹ� + // 逆解成功则更新当前初值。 if (!inverseSolution.empty()) { allJointSolutions.push_back(inverseSolution); - initJoints = inverseSolution; // ���³�ʼ�ؽڽǶ� + initJoints = inverseSolution; // 用上一点结果作为下一点初值。 } else { - std::cerr << "���ʧ��" << std::endl; - // ʹ����һ�γɹ��Ľ���ʼֵ + std::cerr << "逆解失败" << std::endl; + // 失败时复用上一次成功结果，避免轨迹中断。 if (!allJointSolutions.empty()) { allJointSolutions.push_back(allJointSolutions.back()); @@ -1001,7 +978,7 @@ json Robot::inversePoseStrNoDifference(const std::string &pose_str, const std::s } } - // �����йؽڽ�ת��Ϊ JSON ���� + // 汇总全部关节解。 for (const auto &jointSolution : allJointSolutions) { joints_array = jointSolution; @@ -1012,17 +989,17 @@ json Robot::inversePoseStrNoDifference(const std::string &pose_str, const std::s } } -// ������ĵ�֮������Զ���ֺ󣬷��ز�ֺ����� +// 对姿态段进行自动插补后，再逐点做逆解。 json Robot::inversePoseStr(const std::string &pose_str, const std::string &q_init_str) { - // ���� JSON ���� + // 准备输出 JSON 数组。 json joints_poses_array = json::array(); json joints_array = json::array(); std::vector> posePoints = parsePoseString(pose_str); if (posePoints.empty()) { - return joints_poses_array; // ���ؿ����� + return joints_poses_array; // 返回空数组。 } std::vector initJoints = parseJointString(q_init_str); @@ -1032,17 +1009,17 @@ json Robot::inversePoseStr(const std::string &pose_str, const std::string &q_ini { std::vector inverseSolution = inverse(posePoints[0], initJoints); - // �������Ƿ�ɹ� + // 单点逆解成功则直接返回。 if (!inverseSolution.empty()) { - // �����ת��Ϊ JSON ���� + // 转为 JSON 数组。 joints_array = inverseSolution; joints_poses_array.push_back(joints_array); return joints_poses_array; } else { - return joints_poses_array; // ���ؿ����� + return joints_poses_array; // 返回空数组。 } } else @@ -1058,7 +1035,7 @@ json Robot::inversePoseStr(const std::string &pose_str, const std::string &q_ini if (pointCount <= 0) { - std::cerr << "�켣�滮ʧ��" << std::endl; + std::cerr << "轨迹规划失败" << std::endl; continue; } @@ -1066,16 +1043,16 @@ json Robot::inversePoseStr(const std::string &pose_str, const std::string &q_ini { std::vector inverseSolution = inverse(pose, initJoints); - // �������Ƿ�ɹ� + // 逆解成功则更新当前初值。 if (!inverseSolution.empty()) { allJointSolutions.push_back(inverseSolution); - initJoints = inverseSolution; // ���³�ʼ�ؽڽǶ� + initJoints = inverseSolution; // 用上一点结果作为下一点初值。 } else { - std::cerr << "���ʧ��" << std::endl; - // ʹ����һ�γɹ��Ľ���ʼֵ + std::cerr << "逆解失败" << std::endl; + // 失败时复用上一次成功结果，避免轨迹中断。 if (!allJointSolutions.empty()) { allJointSolutions.push_back(allJointSolutions.back()); @@ -1088,7 +1065,7 @@ json Robot::inversePoseStr(const std::string &pose_str, const std::string &q_ini } } - // �����йؽڽ�ת��Ϊ JSON ���� + // 汇总全部关节解。 for (const auto &jointSolution : allJointSolutions) { joints_array = jointSolution; @@ -1099,17 +1076,17 @@ json Robot::inversePoseStr(const std::string &pose_str, const std::string &q_ini } } -// �������㰴�����steps���в�ֵ����������ز�ֺ�ĵ�ĵ���� +// 对两点姿态按指定步数插补后，逐点计算逆解。 json Robot::inversePoseStr2PSteps(const std::string &pose_str, const std::string &q_init_str, const std::int32_t steps) { - // ���� JSON ���� + // 准备输出 JSON 数组。 json joints_poses_array = json::array(); json joints_array = json::array(); std::vector> posePoints = parsePoseString(pose_str); if (posePoints.empty()) { - return joints_poses_array; // ���ؿ����� + return joints_poses_array; // 返回空数组。 } std::vector initJoints = parseJointString(q_init_str); @@ -1119,17 +1096,17 @@ json Robot::inversePoseStr2PSteps(const std::string &pose_str, const std::string { std::vector inverseSolution = inverse(posePoints[0], initJoints); - // �������Ƿ�ɹ� + // 单点逆解成功则直接返回。 if (!inverseSolution.empty()) { - // �����ת��Ϊ JSON ���� + // 转为 JSON 数组。 joints_array = inverseSolution; joints_poses_array.push_back(joints_array); return joints_poses_array; } else { - return joints_poses_array; // ���ؿ����� + return joints_poses_array; // 返回空数组。 } } else @@ -1143,16 +1120,16 @@ json Robot::inversePoseStr2PSteps(const std::string &pose_str, const std::string { std::vector inverseSolution = inverse(pose, initJoints); - // �������Ƿ�ɹ� + // 逆解成功则更新当前初值。 if (!inverseSolution.empty()) { allJointSolutions.push_back(inverseSolution); - initJoints = inverseSolution; // ���³�ʼ�ؽڽǶ� + initJoints = inverseSolution; // 用上一点结果作为下一点初值。 } else { - std::cerr << "���ʧ��" << std::endl; - // ʹ����һ�γɹ��Ľ���ʼֵ + std::cerr << "逆解失败" << std::endl; + // 失败时复用上一次成功结果，避免轨迹中断。 if (!allJointSolutions.empty()) { allJointSolutions.push_back(allJointSolutions.back()); @@ -1164,7 +1141,7 @@ json Robot::inversePoseStr2PSteps(const std::string &pose_str, const std::string } } - // �����йؽڽ�ת��Ϊ JSON ���� + // 汇总全部关节解。 for (const auto &jointSolution : allJointSolutions) { joints_array = jointSolution; @@ -1218,7 +1195,7 @@ std::vector> Robot::parsePoseString(const std::string &pose_ if (values.size() != 7) { - throw invalid_argument("ÿ����̬��������7��ֵ (x,y,z,qx,qy,qz,qw)"); + throw invalid_argument("每个姿态都必须包含 7 个值 (x,y,z,qx,qy,qz,qw)"); } result.push_back(values); @@ -1275,7 +1252,7 @@ std::vector> Robot::parseJointListString(const std::string & if (values.size() != 6) { - throw invalid_argument("ÿ����̬��������7��ֵ (x,y,z,qx,qy,qz,qw)"); + throw invalid_argument("每组关节角都必须包含 6 个值 (j1,j2,j3,j4,j5,j6)"); } result.push_back(values); @@ -1331,7 +1308,7 @@ json Robot::kinematicsForwardAllJointsList(std::vector joints) // json joints_array1_id = json::array(); json joints_array_objStates = json::array(); - // ʹ�ýṹ�����ݣ�����ǰ�˽��� + // 输出前端需要的 objStates 结构。 for (int i = 0; i < numberOfJoints; i++) { const int base_index = i * VALUES_PER_JOINT; @@ -1358,7 +1335,7 @@ json Robot::kinematicsForwardAllJointsList(std::vector joints) }; - // ���ذ������ָ�ʽ�Ķ��� + // 返回按时间帧组织的结果。 return { //{"array_format", joints_array}, //{"key_value_format", joints_array1}, @@ -1390,10 +1367,10 @@ json Robot::handleKinematicsForwardAllJoints(const std::string &joints_str) { json result = kinematicsForwardAllJointsList(joints[i]); - // ����Ƿ��д��� + // 遇到错误则立即返回。 if (result.contains("error")) { - return result; // ֱ�ӷ��ش��� + return result; // 直接返回错误信息。 } frames.push_back(result["objStates_time"]); @@ -1431,10 +1408,10 @@ json Robot::handleKinematicsForwardAllJoints_objStates(const std::string &joints { json result = kinematicsForwardAllJointsList(joints[i]); - // ����Ƿ��д��� + // 遇到错误则立即返回。 if (result.contains("error")) { - return result; // ֱ�ӷ��ش��� + return result; // 直接返回错误信息。 } joints_poses_array.push_back(result["array_format"]); diff --git a/src/api/KinematicsWebAPI.Core.cpp b/src/api/KinematicsWebAPI.Core.cpp new file mode 100644 index 0000000..1b8f9a4 --- /dev/null +++ b/src/api/KinematicsWebAPI.Core.cpp @@ -0,0 +1,126 @@ +#include "KinematicsWebAPI.h" +#include "URDFStrings.h" + +#include +#include +#include +#include + +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_; +} diff --git a/src/api/KinematicsWebAPI.FourBarCommands.cpp b/src/api/KinematicsWebAPI.FourBarCommands.cpp new file mode 100644 index 0000000..d685c35 --- /dev/null +++ b/src/api/KinematicsWebAPI.FourBarCommands.cpp @@ -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 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())}}; + } +} diff --git a/src/api/KinematicsWebAPI.QuadrupedCommands.cpp b/src/api/KinematicsWebAPI.QuadrupedCommands.cpp new file mode 100644 index 0000000..7552547 --- /dev/null +++ b/src/api/KinematicsWebAPI.QuadrupedCommands.cpp @@ -0,0 +1,21 @@ +#include "KinematicsWebAPI.h" +#include "QuadrupedRobotSimulation/KinematicsHelper.h" + +#include + +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); +} diff --git a/src/api/KinematicsWebAPI.RobotCommands.cpp b/src/api/KinematicsWebAPI.RobotCommands.cpp new file mode 100644 index 0000000..7f9eb11 --- /dev/null +++ b/src/api/KinematicsWebAPI.RobotCommands.cpp @@ -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; +} diff --git a/src/api/KinematicsWebAPI.SpcCommands.cpp b/src/api/KinematicsWebAPI.SpcCommands.cpp new file mode 100644 index 0000000..2f6b436 --- /dev/null +++ b/src/api/KinematicsWebAPI.SpcCommands.cpp @@ -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())}}; + } +} diff --git a/src/utils.cpp b/src/utils.cpp index 87a1f28..239d97f 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -8,8 +8,6 @@ #include #include -#include - // 实现 splitByDelimiter 方法 std::pair StringUtils::splitByDelimiter( const std::string &str, @@ -88,13 +86,13 @@ std::pair StringUtils::splitStrict( namespace utils { - // Base64�ַ��� + // Base64 字符表 const std::string base64_chars = "ABCDEFGHIJKLMNOPQRSTUVWXYZ" "abcdefghijklmnopqrstuvwxyz" "0123456789+/"; - // ����Ƿ�Ϊ��Ч��base64�ַ� + // 判断字符是否为有效的 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�ַ� + continue; // ASCII 字符 } else if ((c & 0xE0) == 0xC0) { - // 2�ֽ�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�ֽ�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�ֽ�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; // ��Ч��UTF-8�ֽ� + return false; // 非法 UTF-8 字节 } } return true; @@ -219,11 +217,11 @@ namespace utils unsigned char c = str[i]; if (c <= 0x7F) { - result += c; // ASCII�ַ� + result += c; // ASCII 字符 } else if ((c & 0xE0) == 0xC0) { - // 2�ֽ�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�ֽ�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�ֽ�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; } } - // ������Ч��UTF-8�ֽ� + // 跳过非法 UTF-8 字节 } return result; } @@ -285,7 +283,7 @@ namespace utils return response; } - // Base64���뺯�� + // 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; - // �Ƴ����ܵĻ��з��Ϳո� + // 去掉输入中的换行和空格 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) { - // ��������ַ�λ��Ϊ0 + // 补齐的占位字符按 0 处理 char_array_4[j] = 0; } else if (pos != std::string::npos) @@ -372,14 +370,14 @@ namespace utils return ret; } - // ��base64�����URDFת��Ϊ�ַ��� + // 将 Base64 编码的 URDF 转成字符串 std::string base64_to_urdf(const std::string &base64_urdf) { try { std::string urdf_content = base64_decode(base64_urdf); - // ��֤�������Ƿ�����Ч��URDF/XML + // 校验解码结果是否像合法的 URDF/XML if (urdf_content.find("