HiveCoreRD/hivecore_robot_vision
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

1

Browse Source
This commit is contained in:
liangyuxuan
2026-01-21 16:06:22 +08:00
parent b04196de57
commit 1602b1423d
1 changed files with 13 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
vision_detect/vision_detect/VisionDetect/utils/grasp_refine.py
View File

@@ -60,6 +60,8 @@ def collision_detector(
return 2
if hand_top_box.any():
return 3
else:
iou *= 0.5
left_finger_box = (
(points[2] < (z + left_size[2])) & (points[2] > z -0.05)
@@ -213,8 +215,9 @@ def refine_grasp_pose(
if search_mode:
right_num = left_num = 0
# already in left side
if left_last and not right_last:
y_min = refine[1]
y_min = refine[1] - 0.004
while left_num < 10:
left_num += 1
refine[1] += 0.004
@@ -225,11 +228,12 @@ def refine_grasp_pose(
refine[1] = (refine[1] - 0.004 + y_min) / 2
break
else:
refine[1] = y_min + 0.2
refine[1] = y_min + 0.02
print(f"left_num = {left_num}")
elif not left_last and right_last:
y_max = refine[1]
# already in right side
y_max = refine[1] + 0.004
while right_num < 10:
right_num += 1
refine[1] -= 0.004
@@ -240,12 +244,13 @@ def refine_grasp_pose(
refine[1] = (refine[1] + 0.004 + y_max) / 2
break
else:
refine[1] = y_max - 0.2
refine[1] = y_max - 0.02
print(f"right_num = {right_num}")
elif not left_last and not right_last:
# in middle
y_cur = refine[1]
while left_num < 6:
while left_num < 10:
left_num += 1
refine[1] += 0.004
collision_code = collision_detector(
@@ -255,11 +260,11 @@ def refine_grasp_pose(
y_max = refine[1] - 0.004
break
else:
y_max = refine[1] + 0.024
y_max = y_cur + 0.040
print(f"left_num = {left_num}")
refine[1] = y_cur
while right_num < 6:
while right_num < 10:
right_num += 1
refine[1] -= 0.004
collision_code = collision_detector(
@@ -269,7 +274,7 @@ def refine_grasp_pose(
refine[1] = (refine[1] + 0.004 + y_max) / 2
break
else:
refine[1] = (y_cur - 0.024 + y_max) / 2
refine[1] = (y_cur - 0.040 + y_max) / 2
print(f"right_num = {right_num}")
position = target_position + (expect_orientation @ refine.T).T