diff --git a/config/cabin_robot.xml b/config/cabin_robot.xml index d229e483..b2bdaf53 100644 --- a/config/cabin_robot.xml +++ b/config/cabin_robot.xml @@ -33,7 +33,7 @@ - + diff --git a/example/solve_fk.cpp b/example/solve_fk.cpp index 423e01fd..ad52beda 100644 --- a/example/solve_fk.cpp +++ b/example/solve_fk.cpp @@ -31,7 +31,7 @@ int main(int argc, char** argv) { std::vector link_names = { "PELVIS_S", "L_SHOULDER_P_S", "L_SHOULDER_R_S", "L_SHOULDER_Y_S", "L_ELBOW_R_S", "L_WRIST_P_S", "L_WRIST_Y_S", "L_WRIST_R_S", - "R_SHOULDER_P_S", "R_SHOULDER_R_S", "R_SHOULDER_Y_S", "R_ELBOW_R_S", "R_WRIST_P_S", "R_WRIST_Y_S", "R_WRIST_R_S", "R_FINGER_TIP" + "R_SHOULDER_P_S", "R_SHOULDER_R_S", "R_SHOULDER_Y_S", "R_ELBOW_R_S", "R_WRIST_P_S", "R_WRIST_Y_S", "R_WRIST_R_S", "R_CAM", "R_FINGER_TIP" }; std::vector joint_names = { "L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R", diff --git a/example/solve_ik.cpp b/example/solve_ik.cpp index 794b6a20..87c16451 100644 --- a/example/solve_ik.cpp +++ b/example/solve_ik.cpp @@ -48,7 +48,7 @@ int main(int argc, char **argv) std::vector link_names = { "PELVIS_S", "L_SHOULDER_P_S", "L_SHOULDER_R_S", "L_SHOULDER_Y_S", "L_ELBOW_R_S", "L_WRIST_P_S", "L_WRIST_Y_S", "L_WRIST_R_S", - "R_SHOULDER_P_S", "R_SHOULDER_R_S", "R_SHOULDER_Y_S", "R_ELBOW_R_S", "R_WRIST_P_S", "R_WRIST_Y_S", "R_WRIST_R_S", "R_FINGER_TIP" + "R_SHOULDER_P_S", "R_SHOULDER_R_S", "R_SHOULDER_Y_S", "R_ELBOW_R_S", "R_WRIST_P_S", "R_WRIST_Y_S", "R_WRIST_R_S", "R_CAM", "R_FINGER_TIP" }; std::vector joint_names = { diff --git a/python/math/rpy2rotation.py b/python/math/rpy2rotation.py index 5039208b..075d3848 100644 --- a/python/math/rpy2rotation.py +++ b/python/math/rpy2rotation.py @@ -72,8 +72,8 @@ if __name__ == "__main__": # print("rpy back:", rpy_back) # print("max abs diff:", np.max(np.abs(np.array(rpy) - np.array(rpy_back)))) R = np.array([ - [-1, 0, 0], [0, -1, 0], + [1, 0, 0], [0, 0, 1] ]) print(matrix_to_rpy(R)) \ No newline at end of file diff --git a/python/vision_servo/follow_target.py b/python/vision_servo/follow_target.py index 39079b71..e66f4e1d 100644 --- a/python/vision_servo/follow_target.py +++ b/python/vision_servo/follow_target.py @@ -1,291 +1,20 @@ 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 +os.environ["GLOG_minloglevel"] = "1" import sys -# 把.so所在目录加入 Python 路径 -sys.path.append("/home/lgv/cmvr/cmvr-es/cmake-build-debug/example") - -# 导入模块 +from utils import root_path +sys.path.append(os.path.join(root_path, "cmake-build-debug/example")) from robot_wrapper import Robot +import time +import signal +import numpy as np +from scipy.spatial.transform import Rotation as R +from utils import solve_ik +from utils import get_pose +from target_position import init_realsense, get_closest_red_point - -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 @@ -306,110 +35,94 @@ def transform_point(T, P): 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 +IDEL_Q = [-0.05804541534555635, 1.460164607187404, 1.458934384971377, 0.29042831678163805, + -1.498103103566999, 0.039690803641003906, -0.08653132466712826] +INIT_POSE = [0.45, -0.25, -0.15, 0, 0, 0] +# FOLLOW_OFFSET = [-0.3, 0, 0] +# EE_LINK = "EE" +FOLLOW_OFFSET = [-0.05, 0, 0] +EE_LINK = "FINGER" + +def main(): + robot = Robot(os.path.join(root_path, "config/cabin_robot.xml"), "hc01") + current_q = robot.getJointQ('right') + diff = np.array(IDEL_Q) - np.array(current_q) + if np.max(np.abs(diff)) >= 0.01: + robot.moveJ("right", IDEL_Q) -if __name__ == "__main__": pipeline, align = init_realsense() + ok, _, init_rq = solve_ik(INIT_POSE, base_link="PELVIS_S", target_link="R_FINGER_TIP") + if not ok: + print("solve ik init_pose failed, init_pose:", INIT_POSE) + return + print("init q:", init_rq) + print("Robot movej: ", init_rq) + robot.moveJ("right", init_rq) try: - # 初始化机器人 - robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") - - # 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) - 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] - - print("T_base_ee =\n", T_base_ee) - # print("T_ee_cam =\n", T_ee_cam) - # print("T_base_cam =\n", T_base_cam) - - - P_cam_target = get_closest_red_point(pipeline, align, vis_path="red_point.png") - # P_cam_target = red_point_position(color_f, depth_f, depth_intr, vis_path="red_point.png") - print("P_cam_target (camera frame) =", P_cam_target) - - # T_cam_target = np.eye(4) - # T_cam_target[:3, 3] = P_cam_target - # T_base_target = T_base_cam @ T_cam_target - # P_base_target = T_base_target[:3, 3] - - # R_ee_cam = np.array([ - # [-1, 0, 0], - # [ 0, 0, -1], - # [ 0, -1, 0] - # ]) - # t_ee_cam = np.array([-0.01212, -0.17655, 0.07506]) - R_ee_cam = np.array( [[-0.99732744 , 0.02734315 , 0.0677519 ], - [-0.06713675 ,0.0228221 , -0.99748274], - [-0.02882056,-0.99936555, -0.02092538]]) - t_ee_cam = np.array([-0.00078482, -0.17419722, 0.06796275]) - # t_ee_cam = np.array([-0.05212, -0.17419722, 0.06796275]) - - # R_ee_cam = np.array( [[-0.99486349, 0.07846371 , 0.06395369], - # [-0.05884529 ,0.06577685, -0.9960977 ], - # [-0.08236419 ,-0.99474462 ,-0.06082177]]) - # - # t_ee_cam = np.array([-0.00797824, -0.17640328, 0.07459845]) - P_base_target = compute_point_in_base(T_base_ee, R_ee_cam, t_ee_cam, P_cam_target) - - print("P_base_target (base frame) =", P_base_target) - - # 位置：直接用 P_base_target - target_x, target_y, target_z = P_base_target - np.array([0.45, 0, 0]) - target_rx, target_ry, target_rz =R.from_matrix(T_base_ee[:3, :3]).as_euler('xyz', degrees=True) - - # 调 IK - ok, lq, rq = solve_ik(target_x, target_y, target_z, target_rx, target_ry, target_rz) - - - print("rq =", rq) - - if ok: - robot.moveJ("right", rq) - else: - print("solve ik failed") - - - print("已到达目标点，按 Ctrl+C 回到初始位姿...") while True: - time.sleep(1) + current_q = robot.getJointQ('right') + T_base_ee = get_pose(current_q, base_link="PELVIS_S", target_link="R_WRIST_R_S") + T_base_cam = get_pose(current_q, base_link="PELVIS_S", target_link="R_CAM") + T_base_tool = get_pose(current_q, base_link="PELVIS_S", target_link="R_FINGER_TIP") + P_cam_target = get_closest_red_point(pipeline, align, vis_path="red_point.png") + T_cam_target = np.eye(4) + T_cam_target[:3, 3] = P_cam_target + T_base_target = T_base_cam @ T_cam_target + P_base_target = T_base_target[:3, 3] + + desire_P_base_tool = P_base_target + FOLLOW_OFFSET + if EE_LINK == "EE": + desire_rpy_base_tool = R.from_matrix(T_base_ee[:3, :3]).as_euler('xyz', degrees=True) + desire_pose = list(desire_P_base_tool) + list(desire_rpy_base_tool) + ok, _, rq = solve_ik(desire_pose, base_link="PELVIS_S", target_link="R_WRIST_R_S") + elif EE_LINK == "CAM": + desire_rpy_base_tool = R.from_matrix(T_base_cam[:3, :3]).as_euler('xyz', degrees=True) + desire_pose = list(desire_P_base_tool) + list(desire_rpy_base_tool) + ok, _, rq = solve_ik(desire_pose, base_link="PELVIS_S", target_link="R_CAM") + elif EE_LINK == "FINGER": + desire_rpy_base_tool = R.from_matrix(T_base_tool[:3, :3]).as_euler('xyz', degrees=True) + desire_pose = list(desire_P_base_tool) + list(desire_rpy_base_tool) + ok, _, rq = solve_ik(desire_pose, base_link="PELVIS_S", target_link="R_FINGER_TIP") + else: + raise RuntimeError("undefined EE_LINK: ", EE_LINK) + + if not ok: + print("solve ik failed, pose=", desire_pose) + time.sleep(1) + else: + current_q = robot.getJointQ('right') + diff = np.array(rq) - np.array(current_q) + if np.max(diff) < 0.01: + continue + print("move position:", desire_P_base_tool) + print("Robot movej: ", rq) + input_data = input("move robot? type \"yes\" to move, or type \"no\" to skip \n") + if input_data == "yes": + robot.moveJ("right", rq) + time.sleep(10) + robot.moveJ("right", init_rq) + + print("Return to init pose, Robot movej: ", init_rq) + except SystemExit: print("安全退出程序...") finally: - robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364]) + robot.moveJ("right", IDEL_Q) pipeline.stop() - align = None - depth_intr = None -# + +if __name__ == '__main__': + main() + + # if __name__ == "__main__": -# +# pipeline, align = init_realsense() # try: # # 初始化机器人 # robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") @@ -420,6 +133,60 @@ if __name__ == "__main__": # js = robot.getJointQ('right') # print("js = ") # print(js) +# 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] +# +# print("T_base_ee =\n", T_base_ee) +# # print("T_ee_cam =\n", T_ee_cam) +# # print("T_base_cam =\n", T_base_cam) +# +# +# P_cam_target = get_closest_red_point(pipeline, align, vis_path="red_point.png") +# # P_cam_target = red_point_position(color_f, depth_f, depth_intr, vis_path="red_point.png") +# print("P_cam_target (camera frame) =", P_cam_target) +# +# # T_cam_target = np.eye(4) +# # T_cam_target[:3, 3] = P_cam_target +# # T_base_target = T_base_cam @ T_cam_target +# # P_base_target = T_base_target[:3, 3] +# +# # R_ee_cam = np.array([ +# # [-1, 0, 0], +# # [ 0, 0, -1], +# # [ 0, -1, 0] +# # ]) +# # t_ee_cam = np.array([-0.01212, -0.17655, 0.07506]) +# R_ee_cam = np.array( [[-0.99732744 , 0.02734315 , 0.0677519 ], +# [-0.06713675 ,0.0228221 , -0.99748274], +# [-0.02882056,-0.99936555, -0.02092538]]) +# t_ee_cam = np.array([-0.00078482, -0.17419722, 0.06796275]) +# # t_ee_cam = np.array([-0.05212, -0.17419722, 0.06796275]) +# +# # R_ee_cam = np.array( [[-0.99486349, 0.07846371 , 0.06395369], +# # [-0.05884529 ,0.06577685, -0.9960977 ], +# # [-0.08236419 ,-0.99474462 ,-0.06082177]]) +# # +# # t_ee_cam = np.array([-0.00797824, -0.17640328, 0.07459845]) +# P_base_target = compute_point_in_base(T_base_ee, R_ee_cam, t_ee_cam, P_cam_target) +# +# print("P_base_target (base frame) =", P_base_target) +# +# # 位置：直接用 P_base_target +# target_x, target_y, target_z = P_base_target - np.array([0.45, 0, 0]) +# target_rx, target_ry, target_rz =R.from_matrix(T_base_ee[:3, :3]).as_euler('xyz', degrees=True) +# +# # 调 IK +# ok, lq, rq = solve_ik(target_x, target_y, target_z, target_rx, target_ry, target_rz) +# +# +# print("rq =", rq) +# +# if ok: +# robot.moveJ("right", rq) +# else: +# print("solve ik failed") # # # print("已到达目标点，按 Ctrl+C 回到初始位姿...") @@ -430,3 +197,6 @@ if __name__ == "__main__": # # finally: # robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364]) +# pipeline.stop() +# align = None +# depth_intr = None \ No newline at end of file diff --git a/python/vision_servo/red_point.png b/python/vision_servo/red_point.png index e7a07ed5..81de0437 100644 Binary files a/python/vision_servo/red_point.png and b/python/vision_servo/red_point.png differ diff --git a/python/vision_servo/target_position.py b/python/vision_servo/target_position.py new file mode 100644 index 00000000..8e7e652e --- /dev/null +++ b/python/vision_servo/target_position.py @@ -0,0 +1,195 @@ +import cv2 +import numpy as np +import pyrealsense2 as rs + +def init_realsense(): + """ + 初始化 RealSense 管道并返回 pipeline 和 align 对象 + """ + pipeline = rs.pipeline() + config = rs.config() + config.enable_stream(rs.stream.depth, 848, 480, rs.format.z16, 30) + config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30) + profile = pipeline.start(config) + align = rs.align(rs.stream.color) + return pipeline, align + + +def get_closest_red_point(pipeline, align, vis_path = None,warmup=10): + """ + 获取最近红色圆点的 3D 坐标。 + 返回 (x, y, z) 或 None（如果没检测到）。 + """ + # 丢掉前几帧，让相机稳定 + for _ in range(warmup): + pipeline.wait_for_frames() + + frames = pipeline.wait_for_frames() + aligned_frames = align.process(frames) + + depth_frame = aligned_frames.get_depth_frame() + color_frame = aligned_frames.get_color_frame() + if not depth_frame or not color_frame: + return None + + depth_image = np.asanyarray(depth_frame.get_data()) + color_image = np.asanyarray(color_frame.get_data()) + depth_intrin = depth_frame.profile.as_video_stream_profile().intrinsics + + hsv = cv2.cvtColor(color_image, cv2.COLOR_BGR2HSV) + lower_red1 = np.array([0, 100, 100]) + upper_red1 = np.array([10, 255, 255]) + lower_red2 = np.array([160, 100, 100]) + upper_red2 = np.array([179, 255, 255]) + mask1 = cv2.inRange(hsv, lower_red1, upper_red1) + mask2 = cv2.inRange(hsv, lower_red2, upper_red2) + mask = cv2.bitwise_or(mask1, mask2) + + mask_blur = cv2.GaussianBlur(mask, (9, 9), 2) + circles = cv2.HoughCircles(mask_blur, cv2.HOUGH_GRADIENT, dp=1.2, minDist=20, + param1=50, param2=15, minRadius=5, maxRadius=50) + + closest_point = None + min_depth = float('inf') + + if circles is not None: + circles = np.uint16(np.around(circles)) + for i in circles[0, :]: + u, v, r = i + depth = depth_frame.get_distance(u, v) + if 0 < depth < min_depth: + min_depth = depth + closest_point = rs.rs2_deproject_pixel_to_point(depth_intrin, [u, v], depth) + + if vis_path is not None and closest_point is not None: + cv2.circle(color_image, (u, v), 6, (0,0,255), -1) + cv2.imwrite(vis_path, color_image) + return closest_point + + +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 \ No newline at end of file diff --git a/python/vision_servo/utils.py b/python/vision_servo/utils.py new file mode 100644 index 00000000..afb5646e --- /dev/null +++ b/python/vision_servo/utils.py @@ -0,0 +1,48 @@ +import os +import re +import subprocess +import numpy as np + +root_path = os.path.join(os.path.dirname(__file__), "../..") + +def get_pose(joint_positions, base_link="PELVIS_S", target_link="R_FINGER_TIP", + exec_path=os.path.join(root_path, "cmake-build-debug/example/solve_fk")): + joint_str = [str(j) for j in joint_positions] + cmd = [exec_path, base_link, target_link, *joint_str] + res = subprocess.run(cmd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, check=True, text=True) + 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(pose, base_link="PELVIS_S", target_link="R_WRIST_R_S", + exec_path=os.path.join(root_path, "cmake-build-debug/example/solve_ik")): + [x, y, z, rx, ry, rz] = pose + cmd = [exec_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 \ No newline at end of file diff --git a/src/devices/canbus/CMakeLists.txt b/src/devices/canbus/CMakeLists.txt index f9cf48ba..e7a72b00 100644 --- a/src/devices/canbus/CMakeLists.txt +++ b/src/devices/canbus/CMakeLists.txt @@ -13,8 +13,8 @@ add_library(cmvr_es::device::canbus ALIAS canbus) target_link_libraries(canbus PRIVATE - proto-objects - gRPC::grpc++ +# proto-objects +# gRPC::grpc++ protobuf::libprotobuf glog::glog # PUBLIC pcanbasic diff --git a/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp b/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp index fc1d8752..d3f39bf5 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp @@ -31,7 +31,9 @@ TEST(HumanoidRobotTest,MyRobotTest) { std::vector> traj; +// robot->calibrateZeroQ("R_WRIST_P"); robot->calibrateZeroQ("R_WRIST_Y"); +// robot->calibrateZeroQ("R_WRIST_R"); // cmd = { // {"L_SHOULDER_P", 0.0},