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

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