diff --git a/docs/robot_kinematics_capabilities.md b/docs/robot_kinematics_capabilities.md index f781298..c223a52 100644 --- a/docs/robot_kinematics_capabilities.md +++ b/docs/robot_kinematics_capabilities.md @@ -160,15 +160,60 @@ ### 奇异点检测 -当前没有显式计算雅可比矩阵条件数或可操作度。 +模块已支持基于 TCP 雅可比矩阵的奇异点检测。 -尚未支持： +当前支持： -- 奇异点预警。 -- 腕部奇异。 -- 肩部奇异。 -- 肘部奇异。 -- 接近奇异点时的速度放大风险提示。 +- 计算 6x6 TCP 雅可比矩阵。 +- 计算雅可比矩阵奇异值。 +- 输出最小奇异值、最大奇异值、条件数和 manipulability。 +- 输出 rank。 +- 根据阈值给出 `normal`、`warning`、`singular` 风险等级。 +- 在 three.js 测试页面中通过“奇异点”按钮直接查看当前关节姿态风险。 + +对应接口： + +- `Cmd_Kinematics_check_singularity` + +请求示例： + +```json +{ + "msg": "singularity check", + "req_code": "CHECK_SINGULARITY_001", + "req_from": "client", + "req_cmd": "Cmd_Kinematics_check_singularity", + "req_param": { + "robot_uuid": "abb_irb120_3_58", + "joints_str": "0.15,-0.25,0.35,0.1,-0.2,0.3" + } +} +``` + +可选阈值参数： + +```json +{ + "singular_threshold": 0.0001, + "warning_threshold": 0.01, + "condition_threshold": 1000000, + "condition_warning_threshold": 10000 +} +``` + +响应中的关键字段： + +- `risk_level`：`normal`、`warning` 或 `singular`。 +- `is_singular`：是否已判定为奇异。 +- `is_near_singular`：是否接近奇异。 +- `rank`：雅可比矩阵秩。 +- `singular_values`：奇异值数组。 +- `min_singular_value`：最小奇异值。 +- `condition_number`：条件数。 +- `manipulability`：可操作度指标。 +- `jacobian`：6x6 TCP 雅可比矩阵。 + +注意：当前检测给出数值风险等级，尚未进一步分类为腕部奇异、肩部奇异或肘部奇异。 ### 速度、加速度和 jerk 约束 @@ -259,12 +304,11 @@ 如果继续按工业机器人实际使用场景增强，建议优先级如下： -1. 增加奇异点检测和雅可比分析。 -2. 增加多 IK 解和构型选择。 -3. 增加关节连续性和最短路径处理。 -4. 增加速度、加速度和轨迹时间参数化。 -5. 增加工具坐标系和工件坐标系管理。 -6. 增加 PTP、LIN、CIRC 等工业运动指令。 -7. 增加碰撞检测。 +1. 增加多 IK 解和构型选择。 +2. 增加关节连续性和最短路径处理。 +3. 增加速度、加速度和轨迹时间参数化。 +4. 增加工具坐标系和工件坐标系管理。 +5. 增加 PTP、LIN、CIRC 等工业运动指令。 +6. 增加碰撞检测。 +7. 增加奇异类型分类，例如腕部、肩部、肘部奇异。 8. 增加控制器通讯和实时执行相关能力。 - diff --git a/include/Robot.h b/include/Robot.h index fe60379..cd48ce7 100644 --- a/include/Robot.h +++ b/include/Robot.h @@ -6,6 +6,7 @@ #include #include #include +#include #include #include #include @@ -193,6 +194,21 @@ public: bool calculateFK_AllJointsforwardKinematics(const double joints[6], double jointPoses[42]); bool forwardKinematics(const double joints[6], double jointPoses[42]); + /** + * @brief 检查当前关节姿态是否接近奇异点。 + * @param joints_str 输入关节角，格式为 `j1,j2,j3,j4,j5,j6`。 + * @param singularThreshold 最小奇异值低于该阈值时判定为奇异。 + * @param warningThreshold 最小奇异值低于该阈值时判定为接近奇异。 + * @param conditionThreshold 条件数超过该阈值时判定为奇异。 + * @param conditionWarningThreshold 条件数超过该阈值时判定为接近奇异。 + * @return 奇异点检测结果 JSON。 + */ + json checkSingularity(const std::string &joints_str, + double singularThreshold = 1e-4, + double warningThreshold = 1e-2, + double conditionThreshold = 1e6, + double conditionWarningThreshold = 1e4); + /** * @brief 获取当前 KDL 运动学链。 */ diff --git a/public/robot_kinematics_three_test.html b/public/robot_kinematics_three_test.html index c48d7eb..dfdef20 100644 --- a/public/robot_kinematics_three_test.html +++ b/public/robot_kinematics_three_test.html @@ -420,6 +420,9 @@
+ @@ -492,6 +495,10 @@ 关节数 0
+
+ 奇异风险 + - +
three.js 0.184.0 @@ -546,11 +553,13 @@ targetQw: document.getElementById("target-qw"), tcpValue: document.getElementById("tcp-value"), jointCountValue: document.getElementById("joint-count-value"), + singularityValue: document.getElementById("singularity-value"), toast: document.getElementById("toast"), initBtn: document.getElementById("init-btn"), registerUrdfBtn: document.getElementById("register-urdf-btn"), resetBtn: document.getElementById("reset-btn"), fkBtn: document.getElementById("fk-btn"), + singularityBtn: document.getElementById("singularity-btn"), ikBtn: document.getElementById("ik-btn"), applyIkBtn: document.getElementById("apply-ik-btn"), targetCurrentBtn: document.getElementById("target-current-btn"), @@ -731,6 +740,20 @@ }; } + // 生成奇异点检测请求。 + function buildSingularityRequest() { + return { + msg: "threejs singularity check", + req_code: `THREE_SINGULARITY_${Date.now()}`, + req_from: "threejs_test", + req_cmd: "Cmd_Kinematics_check_singularity", + req_param: { + robot_uuid: elements.robotUuid.value.trim(), + joints_str: buildJointString(), + }, + }; + } + // 生成逆解请求。 function buildInverseRequest() { const pose = [ @@ -883,6 +906,27 @@ } } + // 执行奇异点检测并显示风险等级。 + async function runSingularityCheck() { + await ensureInitialized(); + setBusy(true, "奇异点检测中"); + + const request = buildSingularityRequest(); + try { + renderJson(request, null); + const response = await api.processBusinessRequest(request); + renderJson(request, response); + updateSingularityMetric(response?.res_data); + + const ok = response.success && response.res_data?.success; + setStatus(ok ? "奇异点检测完成" : "奇异点检测失败", Boolean(ok)); + } catch (error) { + showError(error); + } finally { + setBusy(false); + } + } + // 执行逆解并保存待应用的关节解。 async function runInverseKinematics() { await ensureInitialized(); @@ -1114,6 +1158,22 @@ } } + // 更新奇异点风险指标。 + function updateSingularityMetric(result) { + if (!result?.success) { + elements.singularityValue.textContent = "-"; + return; + } + + const riskText = { + normal: "正常", + warning: "接近", + singular: "奇异", + }[result.risk_level] ?? result.risk_level; + const minSv = Number(result.min_singular_value); + elements.singularityValue.textContent = `${riskText} / ${Number.isFinite(minSv) ? minSv.toExponential(2) : "-"}`; + } + // 设置按钮忙碌状态。 function setBusy(isBusy, label) { [ @@ -1121,6 +1181,7 @@ elements.registerUrdfBtn, elements.resetBtn, elements.fkBtn, + elements.singularityBtn, elements.ikBtn, elements.targetCurrentBtn, ].forEach((button) => { @@ -1188,6 +1249,7 @@ elements.registerUrdfBtn.addEventListener("click", registerUrdfFromFile); elements.resetBtn.addEventListener("click", resetJoints); elements.fkBtn.addEventListener("click", runForwardKinematics); + elements.singularityBtn.addEventListener("click", runSingularityCheck); elements.ikBtn.addEventListener("click", runInverseKinematics); elements.applyIkBtn.addEventListener("click", applyInverseSolution); elements.targetCurrentBtn.addEventListener("click", updateTcpTargetFromCurrent); diff --git a/scripts/build.ps1 b/scripts/build.ps1 index fae65b0..41af6bf 100644 --- a/scripts/build.ps1 +++ b/scripts/build.ps1 @@ -133,6 +133,7 @@ $compileResult = emcc ` -s USE_PTHREADS=0 ` -O3 ` -Wno-deprecated-literal-operator ` + -Wno-deprecated-declarations ` -o public/wasm/smart_math.js 2>&1 $endTime = Get-Date diff --git a/scripts/run_wasm_tests.js b/scripts/run_wasm_tests.js index a6436fa..bfe5fe6 100644 --- a/scripts/run_wasm_tests.js +++ b/scripts/run_wasm_tests.js @@ -142,6 +142,19 @@ function buildInverseRequest(command, robotUuid, poseStr, qInitStr, extra = {}) }; } +function buildSingularityRequest(robotUuid, joints) { + return { + msg: "singularity check", + req_code: "AUTO_SINGULARITY", + req_from: "wasm_test", + req_cmd: "Cmd_Kinematics_check_singularity", + req_param: { + robot_uuid: robotUuid, + joints_str: jointArrayToString(joints), + }, + }; +} + // 构造 link 名称被整体替换的 URDF，用于验证初始化能自动推导运动学链。 function buildRenamedUrdfFromDocs() { const urdfPath = path.join(rootDir, "docs", "urdf.xml"); @@ -195,6 +208,25 @@ async function runJointLimitCase(module) { return response; } +// 检查奇异点分析接口会返回完整的雅可比 SVD 指标。 +async function runSingularityCheckCase(module) { + const response = callBusinessApi(module, buildSingularityRequest("abb_irb120_3_58", [0.15, -0.25, 0.35, 0.1, -0.2, 0.3])); + ensure(response.success === true, "singularity check request failed"); + ensure(getByPath(response, "res_data.success") === true, "singularity check result is not successful"); + ensure(Array.isArray(getByPath(response, "res_data.singular_values")), "singular values are missing"); + ensure(getByPath(response, "res_data.singular_values").length === 6, "singular value count should be 6"); + ensure(typeof getByPath(response, "res_data.condition_number") === "number", "condition number is missing"); + ensure(typeof getByPath(response, "res_data.risk_level") === "string", "risk level is missing"); + return { + details: { + riskLevel: getByPath(response, "res_data.risk_level"), + rank: getByPath(response, "res_data.rank"), + minSingularValue: getByPath(response, "res_data.min_singular_value"), + conditionNumber: getByPath(response, "res_data.condition_number"), + }, + }; +} + function assertStaticCase(response, assertions) { for (const assertion of assertions) { const actual = getByPath(response, assertion.path); @@ -414,6 +446,10 @@ const suite = [ id: "kinematics_joint_limit_rejects_fk", type: "joint_limit", }, + { + id: "kinematics_singularity_check", + type: "singularity_check", + }, { id: "spc_basic_5x30", type: "static", @@ -587,6 +623,8 @@ async function runCase(module, testCase) { return runRenamedUrdfChainCase(module, testCase); case "joint_limit": return runJointLimitCase(module, testCase); + case "singularity_check": + return runSingularityCheckCase(module, testCase); default: throw new Error(`Unknown test type: ${testCase.type}`); } diff --git a/src/Robot.cpp b/src/Robot.cpp index 8f2804d..46e0612 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -797,6 +798,122 @@ bool Robot::calculateFK_AllJointsforwardKinematics(const double joints[6], doubl } } +// 基于 TCP 雅可比矩阵的奇异值分析，判断当前姿态是否接近奇异点。 +json Robot::checkSingularity(const std::string &joints_str, + double singularThreshold, + double warningThreshold, + double conditionThreshold, + double conditionWarningThreshold) +{ + try + { + if (!m_initialized) + { + return json{{"success", false}, {"error", "Robot not initialized"}}; + } + + std::vector joints = parseJointString(joints_str); + if (joints.size() != 6) + { + return json{{"success", false}, {"error", "Invalid joints format"}}; + } + + if (!validateJointLimits(joints, "Singularity check input joints")) + { + return json{{"success", false}, {"error", "Joint value out of limits"}}; + } + + KDL::JntArray jointArray(6); + for (int i = 0; i < 6; i++) + { + jointArray(i) = joints[i]; + } + + KDL::Jacobian jacobian(kinematicChain.getNrOfJoints()); + KDL::ChainJntToJacSolver jacSolver(kinematicChain); + const int status = jacSolver.JntToJac(jointArray, jacobian); + if (status < 0) + { + return json{{"success", false}, {"error", "Jacobian calculation failed"}, {"status", status}}; + } + + Eigen::JacobiSVD svd(jacobian.data, Eigen::ComputeThinU | Eigen::ComputeThinV); + const auto singularValues = svd.singularValues(); + + json singularValuesJson = json::array(); + double minSingularValue = std::numeric_limits::infinity(); + double maxSingularValue = 0.0; + double manipulability = 1.0; + int rank = 0; + + for (int i = 0; i < singularValues.size(); i++) + { + const double value = singularValues(i); + singularValuesJson.push_back(value); + minSingularValue = std::min(minSingularValue, value); + maxSingularValue = std::max(maxSingularValue, value); + manipulability *= value; + if (value > singularThreshold) + { + rank++; + } + } + + if (!std::isfinite(minSingularValue)) + { + minSingularValue = 0.0; + } + + const double conditionNumber = minSingularValue <= 0.0 + ? std::numeric_limits::infinity() + : maxSingularValue / minSingularValue; + const bool isSingular = minSingularValue <= singularThreshold || + conditionNumber >= conditionThreshold || + rank < static_cast(kinematicChain.getNrOfJoints()); + const bool isNearSingular = !isSingular && + (minSingularValue <= warningThreshold || + conditionNumber >= conditionWarningThreshold); + const std::string riskLevel = isSingular ? "singular" : (isNearSingular ? "warning" : "normal"); + + json jacobianJson = json::array(); + for (int row = 0; row < jacobian.data.rows(); row++) + { + json rowJson = json::array(); + for (int col = 0; col < jacobian.data.cols(); col++) + { + rowJson.push_back(jacobian.data(row, col)); + } + jacobianJson.push_back(rowJson); + } + + return { + {"success", true}, + {"is_singular", isSingular}, + {"is_near_singular", isNearSingular}, + {"risk_level", riskLevel}, + {"rank", rank}, + {"joint_count", kinematicChain.getNrOfJoints()}, + {"min_singular_value", minSingularValue}, + {"max_singular_value", maxSingularValue}, + {"condition_number", conditionNumber}, + {"manipulability", manipulability}, + {"singular_values", singularValuesJson}, + {"thresholds", { + {"singular", singularThreshold}, + {"warning", warningThreshold}, + {"condition", conditionThreshold}, + {"condition_warning", conditionWarningThreshold}, + }}, + {"joints", joints}, + {"jacobian", jacobianJson}, + }; + } + catch (const std::exception &e) + { + return json{{"success", false}, {"error", "Failed to check singularity: " + std::string(e.what())}}; + } +} + /** * @brief 计算四元数之间的夹角差，用于轨迹步数估算。 */ diff --git a/src/api/KinematicsWebAPI.Core.cpp b/src/api/KinematicsWebAPI.Core.cpp index 61685bc..1881935 100644 --- a/src/api/KinematicsWebAPI.Core.cpp +++ b/src/api/KinematicsWebAPI.Core.cpp @@ -15,6 +15,7 @@ bool isRobotCommand(const std::string &req_cmd) 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_Kinematics_check_singularity" || req_cmd == "Cmd_InitRobot" || req_cmd == "Cmd_GetRobot" || req_cmd == "Cmd_RemoveRobot" || diff --git a/src/api/KinematicsWebAPI.RobotCommands.cpp b/src/api/KinematicsWebAPI.RobotCommands.cpp index 7f9eb11..97e18ca 100644 --- a/src/api/KinematicsWebAPI.RobotCommands.cpp +++ b/src/api/KinematicsWebAPI.RobotCommands.cpp @@ -175,6 +175,39 @@ json KinematicsWebAPI::handleRobotCommand(const std::string &req_cmd, const json res_data = {{"error", "Failed to calculate forward kinematics for all joints: " + std::string(e.what())}}; } } + else if (req_cmd == "Cmd_Kinematics_check_singularity") + { + try + { + log("Handling Cmd_Kinematics_check_singularity command"); + + std::string joints_str = req_param.value("joints_str", req_param.value("q_init_str", "0,0,0,0,0,0")); + std::string robot_uuid = req_param.value("robot_uuid", "default"); + double singular_threshold = req_param.value("singular_threshold", 1e-4); + double warning_threshold = req_param.value("warning_threshold", 1e-2); + double condition_threshold = req_param.value("condition_threshold", 1e6); + double condition_warning_threshold = req_param.value("condition_warning_threshold", 1e4); + + auto robot = RobotManager::getRobot(robot_uuid); + if (!robot || !robot->isInitialized()) + { + res_data = {{"error", "Robot not found or not initialized"}}; + } + else + { + res_data = robot->checkSingularity( + joints_str, + singular_threshold, + warning_threshold, + condition_threshold, + condition_warning_threshold); + } + } + catch (const std::exception &e) + { + res_data = {{"error", "Failed to check singularity: " + std::string(e.what())}}; + } + } else if (req_cmd == "Cmd_InitRobot") { try