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-16 12:56:26 +08:00
parent a100e44ac0
commit 0c7dc9e05b
8 changed files with 327 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

74
docs/robot_kinematics_capabilities.md
View File

@@ -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. 增加控制器通讯和实时执行相关能力。

16
include/Robot.h
View File

@@ -6,6 +6,7 @@
#include <kdl/chainiksolverpos_lma.hpp>
#include <kdl/chainiksolverpos_nr.hpp>
#include <kdl/chainiksolverpos_nr_jl.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainiksolvervel_pinv.hpp>
#include <kdl/frames.hpp>
#include <kdl/jntarray.hpp>
@@ -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 运动学链。
*/

62
public/robot_kinematics_three_test.html
View File

@@ -420,6 +420,9 @@
<div id="joint-controls"></div>
<div class="actions">
<button id="fk-btn" type="button">正解</button>
<button id="singularity-btn" class="secondary" type="button">
奇异点
</button>
<button id="target-current-btn" class="secondary" type="button">
取当前位姿
</button>
@@ -492,6 +495,10 @@
<span class="metric-label">关节数</span>
<span class="metric-value" id="joint-count-value">0</span>
</div>
<div class="metric">
<span class="metric-label">奇异风险</span>
<span class="metric-value" id="singularity-value">-</span>
</div>
<div class="metric">
<span class="metric-label">three.js</span>
<span class="metric-value">0.184.0</span>
@@ -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);

1
scripts/build.ps1
View File

@@ -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

38
scripts/run_wasm_tests.js
View File

@@ -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}`);
}

117
src/Robot.cpp
View File

@@ -11,6 +11,7 @@
#include <kdl/velocityprofile_trap.hpp>
#include <kdl/trajectory_segment.hpp>
#include <kdl/rotational_interpolation_sa.hpp>
#include <Eigen/SVD>
#include <limits>
#include <regex>
#include <vector>
@@ -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<double> 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<Eigen::MatrixXd> svd(jacobian.data, Eigen::ComputeThinU | Eigen::ComputeThinV);
const auto singularValues = svd.singularValues();
json singularValuesJson = json::array();
double minSingularValue = std::numeric_limits<double>::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<double>::infinity()
: maxSingularValue / minSingularValue;
const bool isSingular = minSingularValue <= singularThreshold ||
conditionNumber >= conditionThreshold ||
rank < static_cast<int>(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 计算四元数之间的夹角差，用于轨迹步数估算。
*/

1
src/api/KinematicsWebAPI.Core.cpp
View File

@@ -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" ||

33
src/api/KinematicsWebAPI.RobotCommands.cpp
View File

@@ -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