From 356999547c09df77987b60b81b16e4aab8ec1ab4 Mon Sep 17 00:00:00 2001
From: lixiaolong <702156524@qq.com>
Date: Tue, 8 Sep 2026 16:01:16 +0800
Subject: [PATCH] fix: keep vision click approach orientation

---
 .../edge/VisionLocateOperateService.java      | 22 +++++++++++++++++--
 1 file changed, 20 insertions(+), 2 deletions(-)

diff --git a/cmvr-iot-test/src/main/java/com/cmvr/test/flow/runtime/operator/edge/VisionLocateOperateService.java b/cmvr-iot-test/src/main/java/com/cmvr/test/flow/runtime/operator/edge/VisionLocateOperateService.java
index ac99f17..79f5a20 100644
--- a/cmvr-iot-test/src/main/java/com/cmvr/test/flow/runtime/operator/edge/VisionLocateOperateService.java
+++ b/cmvr-iot-test/src/main/java/com/cmvr/test/flow/runtime/operator/edge/VisionLocateOperateService.java
@@ -144,9 +144,15 @@ public class VisionLocateOperateService implements EdgeOperateService {
         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[][] 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[][] contactTcp = targetToolPose(currentTcp, toolZ, add(objectPoint, scale(toolZ, pressMm / 1000.0)));
-        double[][] approachTcp = targetToolPose(currentTcp, toolZ, add(objectPoint, scale(toolZ, -approachMm / 1000.0)));
+        double[] contactPosition = add(objectPoint, scale(toolZ, pressMm / 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[][] contactFlange = VisionGeometry.multiply(contactTcp, 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) {
+        targetZ = normalize(targetZ);
         double[] xSeed = {currentTcp[0][0], currentTcp[1][0], currentTcp[2][0]};
         double[] x = subtract(xSeed, scale(targetZ, dot(xSeed, targetZ)));
         if (length(x) < 1e-8) {
@@ -549,6 +556,17 @@ public class VisionLocateOperateService implements EdgeOperateService {
         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) {
         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],