update vision servo
This commit is contained in:
parent
e8cb8bb2b2
commit
97d0c59fae
@ -33,7 +33,7 @@
|
|||||||
<!-- <RightArm />-->
|
<!-- <RightArm />-->
|
||||||
<!-- <Neck/>-->
|
<!-- <Neck/>-->
|
||||||
<Humanoid id="hc01" dof="14"
|
<Humanoid id="hc01" dof="14"
|
||||||
urdf="/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf"
|
urdf="/home/xtkuang/projects/cmvr-es/config/robot_description/hc_description/dual_arm.urdf"
|
||||||
baseLink="PELVIS_S"
|
baseLink="PELVIS_S"
|
||||||
jointNames="L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R"
|
jointNames="L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R"
|
||||||
linkNames="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"
|
linkNames="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"
|
||||||
|
|||||||
@ -574,7 +574,7 @@
|
|||||||
</link>
|
</link>
|
||||||
|
|
||||||
<joint name="R_FINGER_TIP_FIXED" type="fixed">
|
<joint name="R_FINGER_TIP_FIXED" type="fixed">
|
||||||
<origin xyz="0.006 -0.2851 -0.01" rpy="0 0 -1.5707963267"/>
|
<origin xyz="-0.021 -0.2851 -0.01" rpy="0 0 -1.5707963267"/>
|
||||||
<parent link="R_WRIST_R_S"/>
|
<parent link="R_WRIST_R_S"/>
|
||||||
<child link="R_FINGER_TIP"/>
|
<child link="R_FINGER_TIP"/>
|
||||||
<axis xyz="0 0 1"/>
|
<axis xyz="0 0 1"/>
|
||||||
|
|||||||
@ -31,7 +31,7 @@ int main(int argc, char** argv) {
|
|||||||
std::vector<std::string> link_names = {
|
std::vector<std::string> link_names = {
|
||||||
"PELVIS_S",
|
"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",
|
"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<std::string> joint_names = {
|
std::vector<std::string> joint_names = {
|
||||||
"L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R",
|
"L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R",
|
||||||
|
|||||||
@ -48,7 +48,7 @@ int main(int argc, char **argv)
|
|||||||
std::vector<std::string> link_names = {
|
std::vector<std::string> link_names = {
|
||||||
"PELVIS_S",
|
"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",
|
"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<std::string> joint_names = {
|
std::vector<std::string> joint_names = {
|
||||||
|
|||||||
@ -72,8 +72,8 @@ if __name__ == "__main__":
|
|||||||
# print("rpy back:", rpy_back)
|
# print("rpy back:", rpy_back)
|
||||||
# print("max abs diff:", np.max(np.abs(np.array(rpy) - np.array(rpy_back))))
|
# print("max abs diff:", np.max(np.abs(np.array(rpy) - np.array(rpy_back))))
|
||||||
R = np.array([
|
R = np.array([
|
||||||
[-1, 0, 0],
|
|
||||||
[0, -1, 0],
|
[0, -1, 0],
|
||||||
|
[1, 0, 0],
|
||||||
[0, 0, 1]
|
[0, 0, 1]
|
||||||
])
|
])
|
||||||
print(matrix_to_rpy(R))
|
print(matrix_to_rpy(R))
|
||||||
@ -1,291 +1,20 @@
|
|||||||
import os
|
import os
|
||||||
import re
|
os.environ["GLOG_minloglevel"] = "1"
|
||||||
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
|
import sys
|
||||||
# 把.so所在目录加入 Python 路径
|
from utils import root_path
|
||||||
sys.path.append("/home/lgv/cmvr/cmvr-es/cmake-build-debug/example")
|
sys.path.append(os.path.join(root_path, "cmake-build-debug/example"))
|
||||||
|
|
||||||
# 导入模块
|
|
||||||
from robot_wrapper import Robot
|
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):
|
def signal_handler(sig, frame):
|
||||||
print("\n收到 Ctrl+C，准备退出...")
|
print("\n收到 Ctrl+C，准备退出...")
|
||||||
raise SystemExit
|
raise SystemExit
|
||||||
@ -306,110 +35,94 @@ def transform_point(T, P):
|
|||||||
return P_new[:3]
|
return P_new[:3]
|
||||||
|
|
||||||
def compute_point_in_base(T_base_ee, R_ee_cam, t_ee_cam, P_cam_target):
|
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)
|
T_ee_cam = rt_to_transform(R_ee_cam, t_ee_cam)
|
||||||
|
|
||||||
# 得到相机在 base 下的位姿
|
|
||||||
T_base_cam = T_base_ee @ T_ee_cam
|
T_base_cam = T_base_ee @ T_ee_cam
|
||||||
|
|
||||||
# 把目标点从相机系变换到 base 系
|
|
||||||
P_base_target = transform_point(T_base_cam, P_cam_target)
|
P_base_target = transform_point(T_base_cam, P_cam_target)
|
||||||
|
|
||||||
return P_base_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()
|
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:
|
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:
|
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:
|
except SystemExit:
|
||||||
print("安全退出程序...")
|
print("安全退出程序...")
|
||||||
|
|
||||||
finally:
|
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()
|
pipeline.stop()
|
||||||
align = None
|
|
||||||
depth_intr = None
|
|
||||||
|
|
||||||
#
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
|
|
||||||
|
|
||||||
# if __name__ == "__main__":
|
# if __name__ == "__main__":
|
||||||
#
|
# pipeline, align = init_realsense()
|
||||||
# try:
|
# try:
|
||||||
# # 初始化机器人
|
# # 初始化机器人
|
||||||
# robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01")
|
# robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01")
|
||||||
@ -420,6 +133,60 @@ if __name__ == "__main__":
|
|||||||
# js = robot.getJointQ('right')
|
# js = robot.getJointQ('right')
|
||||||
# print("js = ")
|
# print("js = ")
|
||||||
# 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 回到初始位姿...")
|
# print("已到达目标点，按 Ctrl+C 回到初始位姿...")
|
||||||
@ -430,3 +197,6 @@ if __name__ == "__main__":
|
|||||||
#
|
#
|
||||||
# finally:
|
# finally:
|
||||||
# robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364])
|
# robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364])
|
||||||
|
# pipeline.stop()
|
||||||
|
# align = None
|
||||||
|
# depth_intr = None
|
||||||
Binary file not shown.
|
Before Width: | Height: | Size: 1.2 MiB After Width: | Height: | Size: 1.1 MiB |
195
python/vision_servo/target_position.py
Normal file
195
python/vision_servo/target_position.py
Normal file
@ -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
|
||||||
48
python/vision_servo/utils.py
Normal file
48
python/vision_servo/utils.py
Normal file
@ -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
|
||||||
@ -13,8 +13,8 @@ add_library(cmvr_es::device::canbus ALIAS canbus)
|
|||||||
|
|
||||||
target_link_libraries(canbus
|
target_link_libraries(canbus
|
||||||
PRIVATE
|
PRIVATE
|
||||||
proto-objects
|
# proto-objects
|
||||||
gRPC::grpc++
|
# gRPC::grpc++
|
||||||
protobuf::libprotobuf
|
protobuf::libprotobuf
|
||||||
glog::glog
|
glog::glog
|
||||||
# PUBLIC pcanbasic
|
# PUBLIC pcanbasic
|
||||||
|
|||||||
@ -31,7 +31,9 @@ TEST(HumanoidRobotTest,MyRobotTest) {
|
|||||||
|
|
||||||
std::vector<std::vector<JointPoint>> traj;
|
std::vector<std::vector<JointPoint>> traj;
|
||||||
|
|
||||||
|
// robot->calibrateZeroQ("R_WRIST_P");
|
||||||
robot->calibrateZeroQ("R_WRIST_Y");
|
robot->calibrateZeroQ("R_WRIST_Y");
|
||||||
|
// robot->calibrateZeroQ("R_WRIST_R");
|
||||||
//
|
//
|
||||||
cmd = {
|
cmd = {
|
||||||
// {"L_SHOULDER_P", 0.0},
|
// {"L_SHOULDER_P", 0.0},
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user