import os import re import time import signal 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 init_realsense, get_closest_red_point import sys # 把.so所在目录加入 Python 路径 sys.path.append("/home/lgv/cmvr/cmvr-es/cmake-build-debug/example") # 导入模块 from robot_wrapper import Robot def get_pose(joint_positions, base_link="PELVIS_S", target_link="R_FINGER_TIP", exec_path="/home/lgv/cmvr/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/lgv/cmvr/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") # robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") # js = robot.getJointQ('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) # ========== Ctrl+C 处理 ========== def signal_handler(sig, frame): print("\n收到 Ctrl+C，准备退出...") raise SystemExit signal.signal(signal.SIGINT, signal_handler) def rt_to_transform(R, t): """拼接旋转矩阵和平移向量为齐次矩阵""" T = np.eye(4) T[:3, :3] = R T[:3, 3] = np.squeeze(t) return T def transform_point(T, P): """用齐次变换矩阵 T (4x4) 把点 P(3,) 转换到新坐标系""" P_h = np.append(P, 1) # [x, y, z, 1] P_new = T @ P_h return P_new[:3] def compute_point_in_base(T_base_ee, R_ee_cam, t_ee_cam, P_cam_target): """ 已知: T_base_ee : 基座->末端 (4x4) R_ee_cam : 末端->相机的旋转 (3x3) t_ee_cam : 末端->相机的平移 (3,) P_cam_target : 目标点在相机下的坐标 (3,) 返回: P_base_target : 目标点在基座下的坐标 (3,) """ # 拼接 T_ee_cam T_ee_cam = rt_to_transform(R_ee_cam, t_ee_cam) # 得到相机在 base 下的位姿 T_base_cam = T_base_ee @ T_ee_cam # 把目标点从相机系变换到 base 系 P_base_target = transform_point(T_base_cam, P_cam_target) return P_base_target if __name__ == "__main__": pipeline, align = init_realsense() try: # 初始化机器人 robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") # 移动到初始关节位姿 init_joint = [-0.304106 , 1.30538 , 1.4465 ,1.93647 , -2.84955 ,-0.116586 ,0.123911] # init_joint = [0 ,0 , 0 ,0 , 0 ,0 ,0] robot.moveJ("right", init_joint) js = robot.getJointQ('right') print("当前关节角:", js) # 获取末端在基座下的位姿 # T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP") T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP") print("末端位姿 T_base_ee:\n", T_base_ee) # T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP") # print("末端位姿 T_base_ee:\n", T_base_ee) # # T_base_ee = get_pose(js, base_link="R_WRIST_R_S", target_link="R_FINGER_TIP") # print("末端位姿 T_base_ee:\n", T_base_ee) # while True: # time.sleep(1) # 获取红点在相机坐标系下的坐标 P_cam_target = get_closest_red_point(pipeline, align, vis_path="red_point.png") if P_cam_target is None: raise RuntimeError("未检测到红点，无法计算目标点") print("红点在相机坐标系下:", P_cam_target) # P_cam_target = np.array([0.0716422, 0.0482324, 0.325]) # "--frame-id", # "R_FINGER_TIP", # "--child-frame-id", # "camera_color_optical_frame", # "--x", # "-0.132714", # "--y", # "0.00161894", # "--z", # "0.0760576", # "--qx", # "0.512312", # "--qy", # "-0.503306", # "--qz", # "0.493563", # "--qw", # "-0.490525", # # "--roll", # # "0.18103", # # "--pitch", # # "1.60289", # # "--yaw", # # "-1.76387", # 手眼标定结果：四元数和平移 # q_ee_cam = np.array([0.519375,-0.502778, 0.4866,-0.490596]) # t_ee_cam = np.array([-0.0815577, 0.0321285, 0.0746921]) q_ee_cam = np.array([0.512312, -0.503306, 0.493563,-0.490525]) t_ee_cam = np.array([-0.124714, -0.02281894, 0.0800576]) R_ee_cam = R.from_quat(q_ee_cam).as_matrix() print("末端->相机旋转矩阵 R_ee_cam:\n", R_ee_cam) # 构造相机相对于 末端的变换矩阵 T_ee_cam = np.eye(4) T_ee_cam[:3, :3] = R_ee_cam T_ee_cam[:3, 3] = t_ee_cam # 相机相对于 基座的 变换矩阵 T_base_cam = T_base_ee @ T_ee_cam # 相机下红点 -> 基座下红点 P_base_target = np.append(P_cam_target, 1) # 齐次坐标 P_base_target = (T_base_cam @ P_base_target)[:3] print("红点在基座坐标系下:", P_base_target) # while True: # time.sleep(1) # ----------------- IK 求解 ----------------- # 目标位姿 为 相机 相对于 基座的 姿态 # R_base_target = T_base_ee[:3, :3] @ R_ee_cam R_base_target = T_base_ee[:3, :3] rot = R.from_matrix(R_base_target) target_euler = rot.as_euler('xyz', degrees=True) d = rot.as_euler('xyz', degrees=False) target_rx, target_ry, target_rz = target_euler quat = rot.as_quat() print("目标旋转欧拉角 (XYZ 度):", target_euler) print("目标旋转欧拉角 (XYZ 弧度):", d) print("目标旋转四元数 [x,y,z,w]:", quat) # while True: # time.sleep(10) # IK 求解 ok, lq, rq = solve_ik(*P_base_target, target_rx, target_ry, target_rz, base_link="PELVIS_S", target_link="R_FINGER_TIP") if not ok: raise RuntimeError("IK 求解失败") # 移动到目标点 robot.moveJ("right", rq) print("已到达目标点，按 Ctrl+C 回到初始位姿...") while True: time.sleep(1) except Exception as e: print("捕获异常:", e) finally: # 回到初始位姿 robot.moveJ("right", [-0.05804541534555635, 1.460164607187404, 1.458934384971377, 0.29042831678163805, -1.498103103566999, 0.039690803641003906, -0.08653132466712826]) pipeline.stop() align = None # if __name__ == "__main__": # # try: # # 初始化机器人 # robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") # js = robot.getJointQ('right') # # js = np.zeros(7) # # js[6] = 0.8 # # 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") # # print(T_base_cam - T_base_ee) # T_ee1_ee2 = get_pose(js, base_link="R_FINGER_TIP", target_link="R_WRIST_R_S") # print(T_ee1_ee2) # # # T_ee2_ee1 = get_pose(js, base_link="R_WRIST_R_S", target_link="R_FINGER_TIP") # print(T_ee2_ee1) # # # # robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364]) # # # robot.moveJ("right", [-0.344938, 0.935147, 2.27031,1.68959, -2.32841,0.460145, 0.300996]) # # js = robot.getJointQ('right') # # print("js = ") # # print(js) # # # print("已到达目标点，按 Ctrl+C 回到初始位姿...") # while True: # time.sleep(1) # except SystemExit: # print("安全退出程序...") # # # # finally: # # robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364])