修改跟随法向量方向移动问题

CU/Detect.py

@@ -39,7 +39,10 @@ class Detect:
 
 
             position = Real_Position().init_position(*target_position[:3], *noraml_base[:3])
-            position.Z = position.Z
+            # position.Z = position.Z
+            position.a = uvw[0]
+            position.b = uvw[1]
+            position.c = uvw[2]
             self.detect_position = position
             self.detect_status = DetectStatus.DOk
 

CU/EMV.py

@@ -36,7 +36,7 @@ def send_command(command):
             print(f"收到响应: {binascii.hexlify(response)}")
             # 校验响应
             if response == byte_data:
-                print("命令成功下发，继电器已执行操作。")
+                # print("命令成功下发，继电器已执行操作。")
                 return True
             else:
                 print("命令下发失败，响应与请求不符。")
@@ -67,7 +67,7 @@ def close(grasp, shake, throw):
         if send_command(valve_commands[1]['close']):
             time.sleep(1)
     if shake:
-        print("关闭电磁阀 2")
+        # print("关闭电磁阀 2")
         if send_command(valve_commands[2]['close']):
             time.sleep(0.05)
     if throw:

CU/Feeding.py

@@ -20,6 +20,7 @@ from enum import Enum, IntEnum
 from COM.COM_Robot import RobotClient, DetectType
 from Model.RobotModel import CMDInstructRequest, MoveType
 from Trace.handeye_calibration import getPosition
+from Trace.handeye_calibration import getxyz
 from Util.util_math import get_distance
 from Util.util_time import CRisOrFall
 from Vision.camera_coordinate_dete import Detection
@@ -132,21 +133,21 @@ class FeedLine:
     def set_take_position(self,position:Real_Position,dynamic_height=0):
         for i in range(len(self.feeding_to_end)):
             if self.feeding_to_end[i].status == FeedStatus.FTake.value:
-                if position != None:
-                    befor_take_position = Real_Position().init_position(position.X,
-                                                                        position.Y,
-                                                                        position.Z+dynamic_height,
-                                                                        position.U,
-                                                                        position.V,
-                                                                        position.W)
-                    after_take_position = Real_Position().init_position(position.X,
-                                                                        position.Y,
-                                                                        position.Z+dynamic_height,
-                                                                        position.U,
-                                                                        position.V,
-                                                                        position.W)
-                    self.feeding_to_end[i - 1].set_position(befor_take_position)
-                    self.feeding_to_end[i + 1].set_position(after_take_position)
+                xyz = getxyz(position.X, position.Y, position.Z, position.a, position.b, position.c)
+                befor_take_position = Real_Position().init_position(xyz[0],
+                                                                    xyz[1],
+                                                                    xyz[2],
+                                                                    position.U,
+                                                                    position.V,
+                                                                    position.W)
+                after_take_position = Real_Position().init_position(xyz[0],
+                                                                    xyz[1],
+                                                                    xyz[2],
+                                                                    position.U,
+                                                                    position.V,
+                                                                    position.W)
+                self.feeding_to_end[i - 1].set_position(befor_take_position)
+                self.feeding_to_end[i + 1].set_position(after_take_position)
 
                 self.feeding_to_end[i].set_position(position)
 

Config/FeedLine.ini

@@ -46,12 +46,12 @@ status = 3
 linetype = 0
 
 [Position8]
-x = 1430.494385
-y = 1765.716187
+x = 1445.789185
+y = 1707.384888
 z = 2050.0
-u = 1.57722
-v = 4.174088
-w = -87.506218
+u = 2.204855
+v = 3.428981
+w = -85.25634
 id = 8
 order = 8
 lineid = 1
@@ -59,12 +59,12 @@ status = 6
 linetype = 0
 
 [Position9]
-x = 1427.58313
-y = 1724.46875
-z = 1831.32312
-u = 1.923308
-v = 3.454441
-w = -87.299934
+x = 1445.789185
+y = 1707.384888
+z = 1826.260132
+u = 2.204855
+v = 3.428981
+w = -85.25634
 id = 9
 order = 9
 lineid = 1
@@ -92,7 +92,7 @@ u = 5.812903
 v = 5.431066
 w = -168.01712
 id = 12
-order = 14
+order = 13
 lineid = 1
 status = 9
 linetype = 0
@@ -189,12 +189,12 @@ status = 3
 linetype = 0
 
 [Position4]
-x = 1556.585449
-y = 1663.580322
-z = 2331.091553
-u = 48.062508
-v = -0.372809
-w = -96.427994
+x = 1510.92981
+y = 1653.713745
+z = 2381.065186
+u = 60.821259
+v = -4.995515
+w = -99.228653
 id = 4
 order = 11
 lineid = 1
@@ -202,28 +202,15 @@ status = 8
 linetype = 0
 
 [Position11]
-x = 1331.740112
-y = 1787.211548
-z = 2307.181641
-u = -34.621368
-v = 19.412609
-w = -132.657135
+x = 1256.956909
+y = 1809.304443
+z = 2368.663574
+u = -45.444492
+v = 18.997807
+w = -131.11731
 id = 11
 order = 12
 lineid = 1
 status = 8
 linetype = 0
 
-[Position17]
-x = 1364.639404
-y = 1892.286743
-z = 2276.495361
-u = 11.343127
-v = 48.749588
-w = -110.22995
-id = 17
-order = 13
-lineid = 1
-status = 8
-linetype = 0
-

Constant.py

@@ -5,7 +5,7 @@ IO_EmergencyPoint = 2
 max_log_len = 100
 bag_height = 10 # 一袋的高度
 position_accuracy_action = 0.1 #动作时的位置精度6  这个精度要高 必须到位置才做动作
-position_accuracy_command = 300 #命令时的位置精度
+position_accuracy_command = 500 #命令时的位置精度
 manual_adjust_accuracy = 1
 # speed = 10
 # shake_speed = 20
@@ -13,7 +13,7 @@ manual_adjust_accuracy = 1
 # return_speed = 10
 feedLine_set_section = 'FeedLine'
 position_set_section = 'Position'
-feedLine_set_file = f'.{os.sep}Config{os.sep}feedLine.ini'
+feedLine_set_file = f'.{os.sep}Config{os.sep}FeedLine.ini'
 MAX_Position_num = 1000
 MAX_Line_num = 10
 set_ini = 'Seting.ini'

Model/Position.py

@@ -16,12 +16,15 @@ class Position:
 
 
     def compare(self,position,is_action=False):
-        distance = math.sqrt((self.X-position.X)**2+
-                             (self.Y-position.Y)**2+
-                             (self.Z - position.Z)**2+
-                             (self.U - position.U)**2+
-                             (self.V - position.V)**2+
-                             (self.W - position.W) ** 2)
+        # distance = math.sqrt((self.X-position.X)**2+
+        #                      (self.Y-position.Y)**2+
+        #                      (self.Z - position.Z)**2+
+        #                      (self.U - position.U)**2+
+        #                      (self.V - position.V)**2+
+        #                      (self.W - position.W) ** 2)
+        distance = math.sqrt((self.X - position.X) ** 2 +
+                             (self.Y - position.Y) ** 2 +
+                             (self.Z - position.Z) ** 2 )
         if distance<=(position_accuracy_action if is_action else position_accuracy_command):
             return True
         else:

Seting.ini

@@ -59,9 +59,9 @@ smooth = 9
 dynamic_height = 350.0
 
 [Speed]
-debug_speed = 80
+debug_speed = 100
 feed_speed = 100
-reset_speed = 80
+reset_speed = 100
 
 [Origin]
 x = 204.996765

Trace/handeye_calibration.py

@@ -76,7 +76,7 @@ def getPosition(x,y,z,a,b,c,rotation,points):
     # 单位化方向向量
     short_edge_direction = edge_vector / np.linalg.norm(edge_vector)
 
-    delta = -200#沿法向量方向抬高和压低，-指表示抬高，+值表示压低
+    delta = -10#沿法向量方向抬高和压低，-指表示抬高，+值表示压低
     angle = np.asarray([a,b,c])
     noraml = camera2robot[:3, :3]@angle
     normal_vector = noraml / np.linalg.norm(noraml)
@@ -87,13 +87,13 @@ def getPosition(x,y,z,a,b,c,rotation,points):
     return target_position,noraml_base
 
 def getxyz(x,y,z,a,b,c):
-    target = np.asarray([x, y, z, 1])
+    target = np.asarray([x, y, z])
     camera2robot = np.loadtxt('./Trace/com_pose2.txt', delimiter=' ')
-    target_position_raw = np.dot(camera2robot, target)
+    # target_position_raw = np.dot(camera2robot, target)
     delta = -500  # 沿法向量方向抬高和压低，-指表示抬高，+值表示压低
     angle = np.asarray([a, b, c])
     noraml = camera2robot[:3, :3] @ angle
     normal_vector = noraml / np.linalg.norm(noraml)
-    target_position = target_position_raw[:3] + delta * normal_vector
+    target_position = target + delta * normal_vector
 
     return target_position