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

修改抓取位置微调

Browse Source
This commit is contained in:
liangyuxuan
2026-01-19 14:06:09 +08:00
parent 039bb2d165
commit 611b529075
8 changed files with 174 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
calibration_mat/eye_in_right_hand.json
View File

@@ -1,22 +1,22 @@
{
"T": [
[
0.013635791720580838,
0.9992765703636767,
-0.03550212819481636,
-0.08645650535173793
0.02515190926712163,
0.9984434279780899,
-0.049780544267610596,
-0.08312977955463981
],
[
-0.9998865276218899,
0.013399556063222661,
-0.006883587549256321,
0.036196137264974
-0.9996337211449028,
0.025617085950903107,
0.008728599966714646,
0.059044674332170574
],
[
-0.006402895000908794,
0.03559196285001416,
0.999345893630474,
0.014407883180676354
0.00999024575340213,
0.04954276975245833,
0.9987220378839358,
0.017378234075134728
],
[
0.0,

24
tools/pt2vino.py Normal file
View File

@@ -0,0 +1,24 @@
import argparse
from ultralytics import YOLO
def main(checkpoint_path):
model = YOLO(checkpoint_path)
model.export(
format="openvino",
imgsz=(1280, 720),
dynamic=True,
simplify=True,
half=True,
workspace=0.8,
batch=1,
device="cpu"
)
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("checkpoint_path", type=str)
args = parser.parse_args()
main(args.checkpoint_path)
# main(checkpoint_path="checkpoint/medical_sense-seg.pt")

BIN
vision_detect/checkpoints/hivecorebox-seg.onnx
View File

Binary file not shown.

BIN
vision_detect/checkpoints/medical_sense-seg.pt
View File

Binary file not shown.

BIN
vision_detect/checkpoints/yolo11n-seg.onnx
View File

Binary file not shown.

BIN
vision_detect/checkpoints/yolo11s-seg.onnx
View File

Binary file not shown.

6
vision_detect/vision_detect/VisionDetect/node/detect_node.py
View File

@@ -198,6 +198,8 @@ class DetectNode(InitBase):
full_points = create_o3d_pcd(
depth_img, self.camera_size, self.k, **self.calculate_configs
)
time_full_points = time.time()
print(f"create full points: {(time_full_points - time3) * 1000}")
if self.calculate_mode == "E2E" and self.detect_mode == 'Detect' and E2E_SIGN:
self.calculate_configs["orign_point_clouds"] = create_o3d_pcd(
@@ -230,7 +232,8 @@ class DetectNode(InitBase):
if self.p == "left" or self.p == "right":
position = rmat[0:3, 3]
rmat, sign = refine_grasp_pose(
full_points, self.calculate_configs.get("voxel_size"), position
full_points, self.calculate_configs.get("voxel_size"), position,
search_mode=True
)
if not sign:
self.get_logger().warning(self.WARNING_LOG_MAP[str(rmat)])
@@ -240,7 +243,6 @@ class DetectNode(InitBase):
print(f"Refine: {(time_e - time_c) * 1000} ms")
self.get_logger().info(f"grab_width: {grab_width}")
# rmat = self.hand_eye_mat @ rmat
x, y, z, rw, rx, ry, rz = rmat2quat(self.hand_eye_mat @ rmat)
pose = Pose()

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

@@ -10,6 +10,7 @@ def collision_detector(
refine: np.ndarray,
volume: list[float],
iou: float = 0.001,
search_mode: bool = False,
**kwargs
) -> int:
"""
@@ -20,6 +21,7 @@ def collision_detector(
refine: np.ndarray (3, ), Grab target poes coordinate system
volume: list [left, right]
iou : float
search_mode: bool, Default False
**kwargs:
"grab_width": float
"hand_size": list [width, height, length]
@@ -29,40 +31,47 @@ def collision_detector(
output:
collision_code: int
"""
hand_size = kwargs.get('hand_size', [0.090, 0.063, 0.13])
hand_size = kwargs.get('hand_size', [0.113, 0.063, 0.13])
left_size = kwargs.get('left_size', [0.006, 0.037, 0.086])
right_size = kwargs.get('right_size', [0.006, 0.037, 0.086])
grab_width = kwargs.get('grab_width', 0.10) * 0.95
x, y, z = refine
hand_top_box = (
(points[2] < z) & (points[2] > (z - hand_size[2]))
& (points[0] < x) & (points[0] > (x - hand_size[1]/2))
& (points[1] < (y + hand_size[0]/2)) & (points[1] > (y - hand_size[0]/2))
)
hand_bottom_box = (
(points[2] < z) & (points[2] > (z - hand_size[2]))
& (points[0] < (x + hand_size[1]/2)) & (points[0] > x)
& (points[1] < (y + hand_size[0]/2)) & (points[1] > (y - hand_size[0]/2))
)
if not search_mode:
hand_top_box = (
(points[2] < z) & (points[2] > (z - hand_size[2]))
& (points[0] < (x - hand_size[1]*1/4)) & (points[0] > (x - hand_size[1]/2))
& (points[1] < (y + hand_size[0]/2)) & (points[1] > (y - hand_size[0]/2))
)
hand_center_box = (
(points[2] < z) & (points[2] > (z - hand_size[2]))
& (points[0] < (x + hand_size[1]*1/4)) & (points[0] > (x - hand_size[1]*1/4))
& (points[1] < (y + hand_size[0]/2)) & (points[1] > (y - hand_size[0]/2))
)
hand_bottom_box = (
(points[2] < z) & (points[2] > (z - hand_size[2]))
& (points[0] < (x + hand_size[1]/2)) & (points[0] > (x + hand_size[1]*1/4))
& (points[1] < (y + hand_size[0]/2)) & (points[1] > (y - hand_size[0]/2))
)
if (hand_top_box.any() and hand_bottom_box.any()) or hand_center_box.any():
return 1
if hand_bottom_box.any():
return 2
if hand_top_box.any():
return 3
left_finger_box = (
(points[2] < (z + left_size[2])) & (points[2] > z)
(points[2] < (z + left_size[2])) & (points[2] > z -0.05)
& (points[0] < (x + left_size[1]/2)) & (points[0] > (x - left_size[1]/2))
& (points[1] < (y + grab_width/2 + left_size[0])) & (points[1] > (y + grab_width/2))
)
right_finger_box = (
(points[2] < (z + right_size[2])) & (points[2] > z)
(points[2] < (z + right_size[2])) & (points[2] > z - 0.05)
& (points[0] < (x + right_size[1]/2)) & (points[0] > (x - right_size[1]/2))
& (points[1] < (y - grab_width/2)) & (points[1] > (y-(grab_width/2 + right_size[0])))
)
if hand_top_box.any() and hand_bottom_box.any():
return 1
if hand_bottom_box.any():
return 2
if hand_top_box.any():
return 3
left_iou = left_finger_box.sum() / (volume[0]+1e-6)
right_iou = right_finger_box.sum() / (volume[1]+1e-6)
@@ -82,6 +91,7 @@ def refine_grasp_pose(
voxel_size: float,
target_position: np.ndarray,
expect_orientation: np.ndarray | None = None,
search_mode: bool = False,
**kwargs
):
"""
@@ -92,6 +102,7 @@ def refine_grasp_pose(
voxel_size: float
target_position: np.ndarray (3, ), grab target position
expect_orientation: np.ndarray (3, 3), expect grab target orientation
search_mode: bool, Default False
**kwargs:
"grab_width": float
"hand_size": list [width, height, length]
@@ -105,7 +116,7 @@ def refine_grasp_pose(
refine = np.zeros(3)
grab_width = kwargs.get('grab_width', 0.10)
hand_size = kwargs.get('hand_size', [0.090, 0.063, 0.13])
hand_size = kwargs.get('hand_size', [0.113, 0.063, 0.13])
left_size = kwargs.get('left_size', [0.006, 0.037, 0.086])
right_size = kwargs.get('right_size', [0.006, 0.037, 0.086])
@@ -124,6 +135,7 @@ def refine_grasp_pose(
frist_sign = False
y_n = y_p = False
left_last = right_last = False
hand_num = left_num = right_num = 0
while hand_num < 20 and left_num < 10 and right_num < 10:
collision_code = collision_detector(
@@ -132,86 +144,139 @@ def refine_grasp_pose(
if collision_code == 0:
if y_p and not y_n:
refine[1] += 0.005
refine[1] += 0.004
y_p = y_n = False
left_last = True
right_last = False
continue
if not y_p and y_n:
refine[1] -= 0.005
refine[1] -= 0.004
y_p = y_n = False
left_last = False
right_last = True
continue
if search_mode:
break
position = target_position + (expect_orientation @ refine.T).T
rmat = tfs.affines.compose(np.squeeze(np.asarray(position)), expect_orientation, [1, 1, 1])
rmat = tfs.affines.compose(
np.squeeze(np.asarray(position)), expect_orientation, [1, 1, 1]
)
return rmat, True
if collision_code == 6:
return 1202, False
if collision_code == 1:
refine[2] -= 0.01
refine[2] -= 0.008
hand_num += 1
print("z + 0.01")
print("z + 0.008")
continue
elif collision_code == 2:
if not frist_sign:
print("z + 0.005")
refine[2] -= 0.005
if collision_code == 2:
if frist_sign:
print("z + 0.004")
refine[2] -= 0.004
frist_sign = True
continue
refine[0] -= 0.005
refine[0] -= 0.002
hand_num += 1
print("x - 0.005")
print("x - 0.002")
continue
elif collision_code == 3:
if not frist_sign:
print("z + 0.005")
refine[2] -= 0.005
if collision_code == 3:
if frist_sign:
print("z + 0.004")
refine[2] -= 0.004
frist_sign = True
continue
refine[0] += 0.005
refine[0] += 0.002
hand_num += 1
print("x + 0.005")
print("x + 0.002")
continue
elif collision_code == 4:
if collision_code == 4:
y_p = True
y_n = False
refine[1] += 0.005
refine[1] += 0.004
left_num += 1
print("y + 0.005")
print("y + 0.004")
continue
elif collision_code == 5:
if collision_code == 5:
y_p = False
y_n = True
refine[1] -= 0.005
refine[1] -= 0.004
right_num += 1
print("y - 0.005")
print("y - 0.004")
continue
else:
return 1202, False
if search_mode:
right_num = left_num = 0
if left_last and not right_last:
y_min = refine[1]
while left_num < 10:
left_num += 1
refine[1] += 0.004
collision_code = collision_detector(
points, refine, volume=[left_volume, right_volume], search_mode=True, **kwargs
)
if collision_code:
refine[1] = (refine[1] - 0.004 + y_min) / 2
break
else:
refine[1] = y_min + 0.2
print(f"left_num = {left_num}")
elif not left_last and right_last:
y_max = refine[1]
while right_num < 10:
right_num += 1
refine[1] -= 0.004
collision_code = collision_detector(
points, refine, volume=[left_volume, right_volume], search_mode=True, **kwargs
)
if collision_code:
refine[1] = (refine[1] + 0.004 + y_max) / 2
break
else:
refine[1] = y_max - 0.2
print(f"right_num = {right_num}")
elif not left_last and not right_last:
y_cur = refine[1]
while left_num < 6:
left_num += 1
refine[1] += 0.004
collision_code = collision_detector(
points, refine, volume=[left_volume, right_volume], search_mode=True, **kwargs
)
if collision_code:
y_max = refine[1] - 0.004
break
else:
y_max = refine[1] + 0.024
print(f"left_num = {left_num}")
refine[1] = y_cur
while right_num < 6:
right_num += 1
refine[1] -= 0.004
collision_code = collision_detector(
points, refine, volume=[left_volume, right_volume], search_mode=True, **kwargs
)
if collision_code:
refine[1] = (refine[1] + 0.004 + y_max) / 2
break
else:
refine[1] = (y_cur - 0.024 + y_max) / 2
print(f"right_num = {right_num}")
position = target_position + (expect_orientation @ refine.T).T
rmat = tfs.affines.compose(
np.squeeze(np.asarray(position)),
expect_orientation,
[1, 1, 1]
)
return rmat, True
return 1202, False
# def draw(pcd, R, T):
# point = T
# x, y, z = R[:, 0], R[:, 1], R[:, 2]
#
# points = [
# [0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1],
# point, point + x, point + y, point + z
#
# ] # 画点：原点、第一主成分、第二主成分
# lines = [
# [0, 1], [0, 2], [0, 3],
# [4, 5], [4, 6], [4, 7]
# ] # 画出三点之间两两连线
# colors = [
# [1, 0, 0], [0, 1, 0], [0, 0, 1],
# [1, 0, 0], [0, 1, 0], [0, 0, 1]
# ]
# line_set = o3d.geometry.LineSet(points=o3d.utility.Vector3dVector(points), lines=o3d.utility.Vector2iVector(lines))
# line_set.colors = o3d.utility.Vector3dVector(colors)
#
# o3d.visualization.draw_geometries([pcd, line_set])