xiongyi/AutoControlSystem-git
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/master'

Browse Source 
# Conflicts:
#	CU/Detect.py
This commit is contained in:
cdeyw
2025-02-27 10:49:22 +08:00
parent 308ac2229c ddce5ec9d5
commit d1d0d1a5e5
3 changed files with 20 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
CU/Detect.py
View File

@ -38,7 +38,7 @@ class Detect:
target_position, noraml_base = getPosition(*xyz, *uvw, None, points) target_position, noraml_base = getPosition(*xyz, *uvw, None, points)
position = Real_Position().init_position(*target_position[:3], *noraml_base[:3]) position = Real_Position().init_position(*target_position[:3], *noraml_base[:3])
position.Z = position.Z position.Z = position.Z + 200
self.detect_position = position self.detect_position = position
self.detect_status = DetectStatus.DOk self.detect_status = DetectStatus.DOk

15
CU/Feeding.py
View File

@ -19,7 +19,7 @@ from Model.Position import Real_Position, Detection_Position
from enum import Enum, IntEnum from enum import Enum, IntEnum
from COM.COM_Robot import RobotClient, DetectType from COM.COM_Robot import RobotClient, DetectType
from Model.RobotModel import CMDInstructRequest, MoveType 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_math import get_distance
from Util.util_time import CRisOrFall from Util.util_time import CRisOrFall
from Vision.camera_coordinate_dete import Detection from Vision.camera_coordinate_dete import Detection
@ -133,15 +133,16 @@ class FeedLine:
for i in range(len(self.feeding_to_end)): for i in range(len(self.feeding_to_end)):
if self.feeding_to_end[i].status == FeedStatus.FTake.value: if self.feeding_to_end[i].status == FeedStatus.FTake.value:
if position != None: if position != None:
befor_take_position = Real_Position().init_position(position.X, xyz = getxyz(position.X, position.Y, position.Z, position.a, position.b, position.c)
position.Y, befor_take_position = Real_Position().init_position(xyz[0],
position.Z+dynamic_height, xyz[1],
xyz[2],
position.U, position.U,
position.V, position.V,
position.W) position.W)
after_take_position = Real_Position().init_position(position.X, after_take_position = Real_Position().init_position(xyz[0],
position.Y, xyz[1],
position.Z+dynamic_height, xyz[2],
position.U, position.U,
position.V, position.V,
position.W) position.W)

12
Trace/handeye_calibration.py
View File

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