290 lines
12 KiB
Python
290 lines
12 KiB
Python
|
import os
|
|||
|
|
import re
|
|||
|
|
import cv2
|
|||
|
|
import subprocess
|
|||
|
|
import numpy as np
|
|||
|
|
import pyrealsense2 as rs
|
|||
|
|
from scipy.spatial.transform import Rotation as R
|
|||
|
|
from get_chessboard_position import start_pipeline, get_aligned_frames, stop_pipeline
|
|||
|
|
|
|||
|
|
|
|||
|
|
def get_joint_position(side):
|
|||
|
|
if side not in ("left", "right"):
|
|||
|
|
raise ValueError("side 必须是 'left' 或 'right'")
|
|||
|
|
|
|||
|
|
res = subprocess.run( ["getJoint"], stdout=subprocess.PIPE, stderr=subprocess.STDOUT, check=True, text=True)
|
|||
|
|
|
|||
|
|
pattern = rf"^{side}\s*:\s*(.+)$"
|
|||
|
|
for line in res.stdout.splitlines():
|
|||
|
|
m = re.match(pattern, line.strip())
|
|||
|
|
if m:
|
|||
|
|
try:
|
|||
|
|
return [float(x) for x in m.group(1).split()]
|
|||
|
|
except ValueError:
|
|||
|
|
raise RuntimeError(f"{side} 行格式解析失败: {line}")
|
|||
|
|
raise RuntimeError(f"未在 getJoint 输出中找到 '{side}:' 行")
|
|||
|
|
|
|||
|
|
|
|||
|
|
def get_pose(joint_positions, base_link="PELVIS_S", target_link="R_FINGER_TIP", exec_path="/home/xtkuang/projects/cmvr-es/cmake-build-debug/example/solve_fk"):
|
|||
|
|
# ----------- 1. 构造命令行参数 -----------
|
|||
|
|
joint_str = [str(j) for j in joint_positions]
|
|||
|
|
cmd = [exec_path, base_link, target_link, *joint_str]
|
|||
|
|
|
|||
|
|
# ----------- 2. 运行并捕获输出 -----------
|
|||
|
|
res = subprocess.run(cmd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, check=True, text=True)
|
|||
|
|
|
|||
|
|
# ----------- 3. 解析 4×4 变换矩阵 -----------
|
|||
|
|
# 定位起始行（含 “变换矩阵T” 或 “T:”）
|
|||
|
|
lines = res.stdout.splitlines()
|
|||
|
|
start_idx = next((i for i, l in enumerate(lines) if re.search(r"变换矩阵|T\s*:", l)), None)
|
|||
|
|
|
|||
|
|
if start_idx is None or start_idx + 4 >= len(lines):
|
|||
|
|
raise RuntimeError("未在 solve_fk 输出中找到 4×4 变换矩阵:\n" + res.stdout)
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
mat = np.array([[float(x) for x in lines[start_idx + 1 + r].split()] for r in range(4)], dtype=np.float64)
|
|||
|
|
if mat.shape != (4, 4):
|
|||
|
|
raise ValueError
|
|||
|
|
except Exception:
|
|||
|
|
raise RuntimeError("矩阵解析失败，原始输出:\n" + res.stdout)
|
|||
|
|
return mat
|
|||
|
|
|
|||
|
|
|
|||
|
|
def solve_ik(x, y, z, rx, ry, rz, base_link="PELVIS_S", target_link="R_WRIST_R_S",
|
|||
|
|
exe_path= "/home/xtkuang/projects/cmvr-es/cmake-build-debug/example/solve_ik"):
|
|||
|
|
cmd = [exe_path, base_link, target_link, f"{x}", f"{y}", f"{z}", f"{rx}", f"{ry}", f"{rz}"]
|
|||
|
|
print(" ".join(cmd))
|
|||
|
|
res = subprocess.run(cmd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, text=True, check=True)
|
|||
|
|
lines = res.stdout.splitlines()
|
|||
|
|
|
|||
|
|
success_line = next((l for l in lines if "[IK Solve]" in l), "")
|
|||
|
|
success = "success=true" in success_line.lower()
|
|||
|
|
|
|||
|
|
if not success:
|
|||
|
|
return False, None, None
|
|||
|
|
|
|||
|
|
def _parse(tag):
|
|||
|
|
pats = rf"^{tag}\s*:\s*(.+)$"
|
|||
|
|
for l in lines:
|
|||
|
|
m = re.match(pats, l.strip())
|
|||
|
|
if m:
|
|||
|
|
return [float(v) for v in m.group(1).split()]
|
|||
|
|
raise RuntimeError(f"success=true 但未找到 {tag}: 行！\n{res.stdout}")
|
|||
|
|
|
|||
|
|
left_q = _parse("left")
|
|||
|
|
right_q = _parse("right")
|
|||
|
|
return True, left_q, right_q
|
|||
|
|
|
|||
|
|
|
|||
|
|
def moveJ(q, side='right'):
|
|||
|
|
cmd = ["moveJ" ,"one" , f"{side}", f"{q[0]}", f"{q[1]}", f"{q[2]}", f"{q[3]}", f"{q[4]}", f"{q[5]}", f"{q[6]}"]
|
|||
|
|
res = subprocess.run(cmd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, text=True, check=True)
|
|||
|
|
print("movej success")
|
|||
|
|
|
|||
|
|
|
|||
|
|
def rotation_to_degree(mat):
|
|||
|
|
pitch = -np.arcsin(mat[2, 0])
|
|||
|
|
if np.abs(np.cos(pitch)) > 1e-6: # 非奇异
|
|||
|
|
roll = np.arctan2(mat[2, 1], mat[2, 2])
|
|||
|
|
yaw = np.arctan2(mat[1, 0], mat[0, 0])
|
|||
|
|
else: # gimbal lock
|
|||
|
|
roll = 0.0
|
|||
|
|
yaw = np.arctan2(-mat[0, 1], mat[1, 1])
|
|||
|
|
return yaw, pitch, roll
|
|||
|
|
|
|||
|
|
def black_point_position(color_frame, depth_frame, depth_intr, vis_path=None, min_radius=10, avg_window=3):
|
|||
|
|
"""
|
|||
|
|
检测白底黑圆的圆心坐标，返回相机系 (X, Y, Z) [m]
|
|||
|
|
|
|||
|
|
参数
|
|||
|
|
----
|
|||
|
|
color_frame / depth_frame : 对齐后的 RealSense frame
|
|||
|
|
depth_intr : 深度流 intrinsics (rs.intrinsics)
|
|||
|
|
vis_path : 若给定，则保存可视化图片
|
|||
|
|
min_radius : HoughCircles/轮廓的最小半径，像素
|
|||
|
|
avg_window : 深度均值窗口半径（像素）
|
|||
|
|
"""
|
|||
|
|
color_img = np.asanyarray(color_frame.get_data()).copy()
|
|||
|
|
gray = cv2.cvtColor(color_img, cv2.COLOR_BGR2GRAY)
|
|||
|
|
|
|||
|
|
# 1. 二值化（寻找黑色区域）
|
|||
|
|
# Otsu 自动阈值 + 取反 => 黑圆为白，背景为黑
|
|||
|
|
_, mask = cv2.threshold(gray, 0, 255, cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU)
|
|||
|
|
|
|||
|
|
# 2. 轮廓检测，取面积最大的圆形候选
|
|||
|
|
cnts, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
|||
|
|
if not cnts:
|
|||
|
|
raise RuntimeError("未检测到任何黑色区域")
|
|||
|
|
|
|||
|
|
# 取最大面积
|
|||
|
|
cnt = max(cnts, key=cv2.contourArea)
|
|||
|
|
(u, v), radius = cv2.minEnclosingCircle(cnt)
|
|||
|
|
|
|||
|
|
if radius < min_radius:
|
|||
|
|
raise RuntimeError(f"检测到圆半径过小 ({radius:.1f}px)，请检查 min_radius 设置或图像质量")
|
|||
|
|
|
|||
|
|
# 3. 取邻域深度中值
|
|||
|
|
width, height = depth_frame.get_width(), depth_frame.get_height()
|
|||
|
|
depths = [
|
|||
|
|
depth_frame.get_distance(int(round(u + du)), int(round(v + dv)))
|
|||
|
|
for du in range(-avg_window, avg_window + 1)
|
|||
|
|
for dv in range(-avg_window, avg_window + 1)
|
|||
|
|
if 0 <= int(round(u + du)) < width and
|
|||
|
|
0 <= int(round(v + dv)) < height
|
|||
|
|
]
|
|||
|
|
depths = [d for d in depths if d > 0]
|
|||
|
|
if not depths:
|
|||
|
|
raise RuntimeError("圆心处深度无效 (0)")
|
|||
|
|
|
|||
|
|
depth = float(np.median(depths))
|
|||
|
|
|
|||
|
|
# 4. 像素 -> 相机坐标
|
|||
|
|
x, y, z = rs.rs2_deproject_pixel_to_point(
|
|||
|
|
depth_intr, [u, v], depth
|
|||
|
|
)
|
|||
|
|
pos = np.array([x, y, z], dtype=np.float32)
|
|||
|
|
|
|||
|
|
# 5. 可视化
|
|||
|
|
if vis_path is not None:
|
|||
|
|
cx, cy = depth_intr.ppx, depth_intr.ppy
|
|||
|
|
cv2.drawMarker(color_img, (int(cx), int(cy)), (0, 255, 0), markerType=cv2.MARKER_CROSS, markerSize=20, thickness=2)
|
|||
|
|
cv2.circle(color_img, (int(u), int(v)), int(radius), (0, 0, 255), 2)
|
|||
|
|
cv2.circle(color_img, (int(u), int(v)), 5, (0, 0, 255), -1)
|
|||
|
|
cv2.imwrite(vis_path, color_img)
|
|||
|
|
print(f"✓ 已保存标记图到 {vis_path}")
|
|||
|
|
|
|||
|
|
return pos
|
|||
|
|
|
|||
|
|
|
|||
|
|
def red_point_position(color_frame, depth_frame, depth_intr, vis_path=None, min_radius=3, avg_window=3):
|
|||
|
|
"""
|
|||
|
|
检测白底红圆的圆心坐标，返回相机系 (X,Y,Z) [m]
|
|||
|
|
"""
|
|||
|
|
# --- 1. 取彩色帧 ----
|
|||
|
|
color_img = np.asanyarray(color_frame.get_data()).copy()
|
|||
|
|
hsv = cv2.cvtColor(color_img, cv2.COLOR_BGR2HSV)
|
|||
|
|
|
|||
|
|
# --- 2. 阈值分割：红色有两个 Hue 区间 (0-10)∪(170-180) ---
|
|||
|
|
lower_red1 = np.array([0, 100, 100])
|
|||
|
|
upper_red1 = np.array([10, 255, 255])
|
|||
|
|
lower_red2 = np.array([170, 100, 100])
|
|||
|
|
upper_red2 = np.array([180, 255, 255])
|
|||
|
|
|
|||
|
|
mask1 = cv2.inRange(hsv, lower_red1, upper_red1)
|
|||
|
|
mask2 = cv2.inRange(hsv, lower_red2, upper_red2)
|
|||
|
|
mask = cv2.bitwise_or(mask1, mask2)
|
|||
|
|
|
|||
|
|
# 可选：形态学开闭运算去噪
|
|||
|
|
kernel = np.ones((5,5), np.uint8)
|
|||
|
|
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
|
|||
|
|
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
|
|||
|
|
|
|||
|
|
# --- 3. 轮廓取最大圆 ---
|
|||
|
|
cnts, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
|||
|
|
if not cnts:
|
|||
|
|
cv2.imwrite(vis_path, color_img)
|
|||
|
|
raise RuntimeError("未检测到任何红色区域")
|
|||
|
|
|
|||
|
|
cnt = max(cnts, key=cv2.contourArea)
|
|||
|
|
(u, v), radius = cv2.minEnclosingCircle(cnt)
|
|||
|
|
if radius < min_radius:
|
|||
|
|
raise RuntimeError(f"检测到圆半径过小 ({radius:.1f}px)，请检查 min_radius 或图像质量")
|
|||
|
|
|
|||
|
|
# --- 4. 深度中值 ---
|
|||
|
|
width, height = depth_frame.get_width(), depth_frame.get_height()
|
|||
|
|
depths = [
|
|||
|
|
depth_frame.get_distance(int(round(u + du)), int(round(v + dv)))
|
|||
|
|
for du in range(-avg_window, avg_window + 1)
|
|||
|
|
for dv in range(-avg_window, avg_window + 1)
|
|||
|
|
if 0 <= int(round(u + du)) < width and
|
|||
|
|
0 <= int(round(v + dv)) < height
|
|||
|
|
]
|
|||
|
|
depths = [d for d in depths if d > 0]
|
|||
|
|
if not depths:
|
|||
|
|
raise RuntimeError("圆心处深度无效 (0)")
|
|||
|
|
depth = float(np.median(depths))
|
|||
|
|
|
|||
|
|
# --- 5. 反投影到 3-D ---
|
|||
|
|
x, y, z = rs.rs2_deproject_pixel_to_point(depth_intr, [u, v], depth)
|
|||
|
|
pos = np.array([x, y, z], dtype=np.float32)
|
|||
|
|
|
|||
|
|
# --- 6. 可视化保存 ---
|
|||
|
|
if vis_path is not None:
|
|||
|
|
cx, cy = depth_intr.ppx, depth_intr.ppy
|
|||
|
|
cv2.drawMarker(color_img, (int(cx), int(cy)), (0, 255, 0), markerType=cv2.MARKER_CROSS, markerSize=20, thickness=2)
|
|||
|
|
cv2.circle(color_img, (int(u), int(v)), int(radius), (255, 0, 0), 2) # 蓝圈标红圆
|
|||
|
|
cv2.circle(color_img, (int(u), int(v)), 5, (255, 0, 0), -1)
|
|||
|
|
cv2.imwrite(vis_path, color_img)
|
|||
|
|
print(f"✓ 已保存标记图到 {vis_path}")
|
|||
|
|
|
|||
|
|
return pos
|
|||
|
|
|
|||
|
|
|
|||
|
|
if __name__ == "__main__":
|
|||
|
|
pipeline, align, depth_intr = start_pipeline("243122075614")
|
|||
|
|
js = get_joint_position('right')
|
|||
|
|
T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_WRIST_R_S")
|
|||
|
|
T_base_cam = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP")
|
|||
|
|
P_base_cam = T_base_cam[:3, 3]
|
|||
|
|
|
|||
|
|
color_f, depth_f = get_aligned_frames(pipeline, align)
|
|||
|
|
# P_cam_target = black_point_position(color_f, depth_f, depth_intr, vis_path="black_point.png")
|
|||
|
|
P_cam_target = red_point_position(color_f, depth_f, depth_intr, vis_path="red_point.png")
|
|||
|
|
|
|||
|
|
P_base_target = np.array([
|
|||
|
|
P_base_cam[0] + P_cam_target[2],
|
|||
|
|
P_base_cam[1] + P_cam_target[0],
|
|||
|
|
P_base_cam[2] - P_cam_target[1],
|
|||
|
|
])
|
|||
|
|
print("P_base_target:", P_base_target)
|
|||
|
|
|
|||
|
|
P_base_tool = P_base_target - np.array([0.45, 0, 0])
|
|||
|
|
T_base_tool = T_base_ee
|
|||
|
|
T_base_tool[:3, 3] = P_base_tool
|
|||
|
|
|
|||
|
|
print("T_base_tool:", T_base_tool)
|
|||
|
|
|
|||
|
|
target_x, target_y, target_z = T_base_tool[0, 3], T_base_tool[1, 3], T_base_tool[2, 3]
|
|||
|
|
target_rx, target_ry, target_rz =R.from_matrix(T_base_tool[:3, :3]).as_euler('xyz', degrees=True)
|
|||
|
|
|
|||
|
|
ok, lq, rq = solve_ik(target_x, target_y, target_z, target_rx, target_ry, target_rz)
|
|||
|
|
if not ok:
|
|||
|
|
print("solve ik failed")
|
|||
|
|
exit(-1)
|
|||
|
|
moveJ(rq)
|
|||
|
|
|
|||
|
|
# try:
|
|||
|
|
# while True:
|
|||
|
|
# js = get_joint_position('right')
|
|||
|
|
# T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_WRIST_R_S")
|
|||
|
|
# T_base_cam = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP")
|
|||
|
|
# P_base_cam = T_base_cam[:3, 3]
|
|||
|
|
#
|
|||
|
|
# color_f, depth_f = get_aligned_frames(pipeline, align)
|
|||
|
|
# # P_cam_target = black_point_position(color_f, depth_f, depth_intr, vis_path="black_point.png")
|
|||
|
|
# P_cam_target = red_point_position(color_f, depth_f, depth_intr, vis_path="red_point.png")
|
|||
|
|
#
|
|||
|
|
# P_base_target = np.array([
|
|||
|
|
# P_base_cam[0] + P_cam_target[2],
|
|||
|
|
# P_base_cam[1] + P_cam_target[0],
|
|||
|
|
# P_base_cam[2] - P_cam_target[1],
|
|||
|
|
# ])
|
|||
|
|
# print("P_base_target:", P_base_target)
|
|||
|
|
#
|
|||
|
|
# P_base_tool = P_base_target - np.array([0.3, 0, 0])
|
|||
|
|
# T_base_tool = T_base_ee
|
|||
|
|
# T_base_tool[:3, 3] = P_base_tool
|
|||
|
|
#
|
|||
|
|
# print("T_base_tool:", T_base_tool)
|
|||
|
|
#
|
|||
|
|
# target_x, target_y, target_z = T_base_tool[0, 3], T_base_tool[1, 3], T_base_tool[2, 3]
|
|||
|
|
# target_rx, target_ry, target_rz =R.from_matrix(T_base_tool[:3, :3]).as_euler('xyz', degrees=True)
|
|||
|
|
#
|
|||
|
|
# ok, lq, rq = solve_ik(target_x, target_y, target_z, target_rx, target_ry, target_rz)
|
|||
|
|
# if not ok:
|
|||
|
|
# print("solve ik failed")
|
|||
|
|
# exit(-1)
|
|||
|
|
# # moveJ(rq)
|
|||
|
|
# except Exception as e:
|
|||
|
|
# print(e)
|