SmartBench/CMVR-IOT
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: keep vision click approach orientation

Browse Source
This commit is contained in:
lixiaolong 2026-09-08 16:01:16 +08:00
parent 6731c1b19b
commit 356999547c
1 changed files with 20 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
cmvr-iot-test/src/main/java/com/cmvr/test/flow/runtime/operator/edge/VisionLocateOperateService.java
View File

@ -144,9 +144,15 @@ public class VisionLocateOperateService implements EdgeOperateService {
approachMm = Math.max(1.0, Math.min(20.0, approachMm)); approachMm = Math.max(1.0, Math.min(20.0, approachMm));
double pressMm = Math.max(-20.0, Math.min(20.0, doubleValue(input, "pressDistanceMm", 0.0))); double pressMm = Math.max(-20.0, Math.min(20.0, doubleValue(input, "pressDistanceMm", 0.0)));
double[][] currentTcp = VisionGeometry.multiply(currentFlange, flangeToTcp); double[][] currentTcp = VisionGeometry.multiply(currentFlange, flangeToTcp);
// The contact direction is the only authoritative click direction.
// Build it once at the contact point, then change height only for the
// approach pose. Re-solving the approach orientation independently is
// unsafe when the fitted depth normal contains small fluctuations.
double[] toolZ = scale(surfaceNormal, -1.0); double[] toolZ = scale(surfaceNormal, -1.0);
double[][] contactTcp = targetToolPose(currentTcp, toolZ, add(objectPoint, scale(toolZ, pressMm / 1000.0))); double[] contactPosition = add(objectPoint, scale(toolZ, pressMm / 1000.0));
double[][] approachTcp = targetToolPose(currentTcp, toolZ, add(objectPoint, scale(toolZ, -approachMm / 1000.0))); double[][] contactTcp = targetToolPose(currentTcp, toolZ, contactPosition);
double[] approachPosition = add(contactPosition, scale(toolZ, -(approachMm + pressMm) / 1000.0));
double[][] approachTcp = translatedPose(contactTcp, approachPosition);
double[][] tcpToFlange = VisionGeometry.rigidInverse(flangeToTcp); double[][] tcpToFlange = VisionGeometry.rigidInverse(flangeToTcp);
double[][] contactFlange = VisionGeometry.multiply(contactTcp, tcpToFlange); double[][] contactFlange = VisionGeometry.multiply(contactTcp, tcpToFlange);
double[][] approachFlange = VisionGeometry.multiply(approachTcp, tcpToFlange); double[][] approachFlange = VisionGeometry.multiply(approachTcp, tcpToFlange);
@ -532,6 +538,7 @@ public class VisionLocateOperateService implements EdgeOperateService {
} }
private double[][] targetToolPose(double[][] currentTcp, double[] targetZ, double[] position) { private double[][] targetToolPose(double[][] currentTcp, double[] targetZ, double[] position) {
targetZ = normalize(targetZ);
double[] xSeed = {currentTcp[0][0], currentTcp[1][0], currentTcp[2][0]}; double[] xSeed = {currentTcp[0][0], currentTcp[1][0], currentTcp[2][0]};
double[] x = subtract(xSeed, scale(targetZ, dot(xSeed, targetZ))); double[] x = subtract(xSeed, scale(targetZ, dot(xSeed, targetZ)));
if (length(x) < 1e-8) { if (length(x) < 1e-8) {
@ -549,6 +556,17 @@ public class VisionLocateOperateService implements EdgeOperateService {
return result; return result;
} }
/** Keep the contact orientation exactly while changing only TCP height. */
private double[][] translatedPose(double[][] pose, double[] position) {
double[][] result = new double[4][4];
for (int row = 0; row < 3; row++) {
System.arraycopy(pose[row], 0, result[row], 0, 3);
result[row][3] = position[row];
}
result[3][3] = 1.0;
return result;
}
private double[] rotate(double[][] matrix, double[] value) { private double[] rotate(double[][] matrix, double[] value) {
return new double[]{matrix[0][0]*value[0]+matrix[0][1]*value[1]+matrix[0][2]*value[2], return new double[]{matrix[0][0]*value[0]+matrix[0][1]*value[1]+matrix[0][2]*value[2],
matrix[1][0]*value[0]+matrix[1][1]*value[1]+matrix[1][2]*value[2], matrix[1][0]*value[0]+matrix[1][1]*value[1]+matrix[1][2]*value[2],