SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
f47a93c21f
cmvr-es/python/vision_servo/follow_target.py

473 lines
18 KiB
Python
Raw Normal View History

feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
import os
feat:add touch api
2025-10-09 16:31:21 +08:00
import re
update vision servo
2025-08-23 15:51:07 +08:00
import time
import signal
feat:add touch api
2025-10-09 16:31:21 +08:00
import cv2
import subprocess
update vision servo
2025-08-23 15:51:07 +08:00
import numpy as np
feat:add touch api
2025-10-09 16:31:21 +08:00
import pyrealsense2 as rs
update vision servo
2025-08-23 15:51:07 +08:00
from scipy.spatial.transform import Rotation as R
feat:add touch api
2025-10-09 16:31:21 +08:00
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)
feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
feat:add touch api
2025-10-09 16:31:21 +08:00
# --- 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])
feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
feat:add touch api
2025-10-09 16:31:21 +08:00
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 处理 ==========
feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
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):
feat:add touch api
2025-10-09 16:31:21 +08:00
"""
已知:
T_base_ee : 基座->末端 (4x4)
R_ee_cam : 末端->相机的旋转 (3x3)
t_ee_cam : 末端->相机的平移 (3,)
P_cam_target : 目标点在相机下的坐标 (3,)
返回:
P_base_target : 目标点在基座下的坐标 (3,)
"""
# 拼接 T_ee_cam
feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
T_ee_cam = rt_to_transform(R_ee_cam, t_ee_cam)
feat:add touch api
2025-10-09 16:31:21 +08:00
# 得到相机在 base 下的位姿
feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
T_base_cam = T_base_ee @ T_ee_cam
feat:add touch api
2025-10-09 16:31:21 +08:00
# 把目标点从相机系变换到 base 系
P_base_target = transform_point(T_base_cam, P_cam_target)
update urdf
2025-08-25 10:29:39 +08:00
feat:add touch api
2025-10-09 16:31:21 +08:00
return P_base_target
update vision servo
2025-08-23 15:51:07 +08:00
feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
feat:add touch api
2025-10-09 16:31:21 +08:00
# if __name__ == "__main__":
feat:add get pose rpc service
2025-11-03 11:44:52 +08:00
# pipeline, align = init_realsense()
feat:add touch api
2025-10-09 16:31:21 +08:00
# try:
# # 初始化机器人
# robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01")
feat:add get pose rpc service
2025-11-03 11:44:52 +08:00
#
# # 移动到初始关节位姿
#
# 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)
#
feat:add touch api
2025-10-09 16:31:21 +08:00
# js = robot.getJointQ('right')
feat:add get pose rpc service
2025-11-03 11:44:52 +08:00
# 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])
feat:add touch api
2025-10-09 16:31:21 +08:00
#
feat:add get pose rpc service
2025-11-03 11:44:52 +08:00
# # "--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",
feat:add touch api
2025-10-09 16:31:21 +08:00
#
feat:add get pose rpc service
2025-11-03 11:44:52 +08:00
# # 手眼标定结果：四元数和平移
# # q_ee_cam = np.array([0.519375,-0.502778, 0.4866,-0.490596])
# # t_ee_cam = np.array([-0.0815577, 0.0321285, 0.0746921])
feat:add touch api
2025-10-09 16:31:21 +08:00
#
feat:add get pose rpc service
2025-11-03 11:44:52 +08:00
# 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)
feat:add touch api
2025-10-09 16:31:21 +08:00
#
#
feat:add get pose rpc service
2025-11-03 11:44:52 +08:00
# # 构造相机相对于 末端的变换矩阵
# T_ee_cam = np.eye(4)
# T_ee_cam[:3, :3] = R_ee_cam
# T_ee_cam[:3, 3] = t_ee_cam
feat:add touch api
2025-10-09 16:31:21 +08:00
#
feat:add get pose rpc service
2025-11-03 11:44:52 +08:00
# # 相机相对于 基座的 变换矩阵
# T_base_cam = T_base_ee @ T_ee_cam
feat:add touch api
2025-10-09 16:31:21 +08:00
#
feat:add get pose rpc service
2025-11-03 11:44:52 +08:00
# # 相机下红点 -> 基座下红点
# P_base_target = np.append(P_cam_target, 1) # 齐次坐标
# P_base_target = (T_base_cam @ P_base_target)[:3]
# print("红点在基座坐标系下:", P_base_target)
feat:add touch api
2025-10-09 16:31:21 +08:00
#
#
feat:add get pose rpc service
2025-11-03 11:44:52 +08:00
# # 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)
feat:add touch api
2025-10-09 16:31:21 +08:00
# print("已到达目标点，按 Ctrl+C 回到初始位姿...")
feat:add get pose rpc service
2025-11-03 11:44:52 +08:00
#
feat:add touch api
2025-10-09 16:31:21 +08:00
# while True:
# time.sleep(1)
feat:add get pose rpc service
2025-11-03 11:44:52 +08:00
#
# 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__":
# 初始化机器人
robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01")
# js = robot.getJointQ('right')
js = np.zeros(7)
js = np.array([-0.054383226063586795, 1.4797969033848364, -1.4685445467885516, -0.03790717127687202, -0.2797606954361809, 0.1916896945079446, -0.17680932146902423])
# 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_ee)
Reference in New Issue Copy Permalink