Merge branch 'dev' of http://192.168.1.100:18088/smart_bench/cmvr-es
This commit is contained in:
commit
e8cb8bb2b2
@ -83,12 +83,12 @@
|
|||||||
</BioHead >
|
</BioHead >
|
||||||
|
|
||||||
<Microphone>
|
<Microphone>
|
||||||
<!-- <ffmpegMicPhone id="mic1" alsa="hw:0" channels="2" sampleRate="44100" volume="80"/>-->
|
<ffmpegMicPhone id="mic1" alsa="hw:0" channels="2" sampleRate="44100" volume="80"/>
|
||||||
<!-- <ffmpegMicPhone id="mic2" alsa="hw:1" channels="2" sampleRate="44100" volume="80"/>-->
|
<!-- <ffmpegMicPhone id="mic2" alsa="hw:1" channels="2" sampleRate="44100" volume="80"/>-->
|
||||||
</Microphone>
|
</Microphone>
|
||||||
|
|
||||||
<Speaker>
|
<Speaker>
|
||||||
<!-- <ffmpegSpeaker id="spk1" serial="" alas="hw:0,0" channels="2" sampleRate="44100" softResample="1" latency="500000" volume="80"/>-->
|
<ffmpegSpeaker id="spk1" serial="" alas="hw:0,0" channels="2" sampleRate="44100" softResample="1" latency="500000" volume="80"/>
|
||||||
</Speaker>
|
</Speaker>
|
||||||
|
|
||||||
<Canbus>
|
<Canbus>
|
||||||
|
|||||||
@ -559,7 +559,37 @@
|
|||||||
<visual>
|
<visual>
|
||||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
<geometry>
|
<geometry>
|
||||||
<sphere radius="0.002"/>
|
<sphere radius="0.004"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0 1 1 1"/> <!-- 绿色 -->
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.005"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="R_FINGER_TIP_FIXED" type="fixed">
|
||||||
|
<origin xyz="0.006 -0.2851 -0.01" rpy="0 0 -1.5707963267"/>
|
||||||
|
<parent link="R_WRIST_R_S"/>
|
||||||
|
<child link="R_FINGER_TIP"/>
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="R_CAM">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
<mass value="0"/>
|
||||||
|
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.004"/>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material name="">
|
<material name="">
|
||||||
<color rgba="0 1 0 1"/> <!-- 绿色 -->
|
<color rgba="0 1 0 1"/> <!-- 绿色 -->
|
||||||
@ -573,10 +603,11 @@
|
|||||||
</collision>
|
</collision>
|
||||||
</link>
|
</link>
|
||||||
|
|
||||||
<joint name="R_FINGER_TIP_FIXED" type="fixed">
|
<joint name="R_CAM_FIXED" type="fixed">
|
||||||
<origin xyz="-0.01212 -0.17655 0.07506" rpy="-1.5707963267948966 0 3.141592653589793"/>
|
<origin xyz="-0.01212 -0.17655 0.07506" rpy="-1.5707963267 0 3.1415926535"/>
|
||||||
<parent link="R_WRIST_R_S"/>
|
<parent link="R_WRIST_R_S"/>
|
||||||
<child link="R_FINGER_TIP"/>
|
<child link="R_CAM"/>
|
||||||
<axis xyz="0 0 1"/>
|
<axis xyz="0 0 1"/>
|
||||||
</joint>
|
</joint>
|
||||||
|
|
||||||
</robot>
|
</robot>
|
||||||
|
|||||||
65
python/hand_eye_calibration/check_precesion.py
Normal file
65
python/hand_eye_calibration/check_precesion.py
Normal file
@ -0,0 +1,65 @@
|
|||||||
|
import numpy as np
|
||||||
|
import pandas as pd
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
|
||||||
|
def get_T_cp():
|
||||||
|
pass
|
||||||
|
|
||||||
|
# ---------- 用户给出的 T_ee->cam ----------
|
||||||
|
# R_ec = np.array([[ 0.92095 , 0.33864198, 0.19280227],
|
||||||
|
# [-0.2465026 , 0.88946636, -0.38481952],
|
||||||
|
# [-0.30180718, 0.30687328, 0.90263017]])
|
||||||
|
# t_ec = np.array([[-0.00596568],
|
||||||
|
# [-0.61356453],
|
||||||
|
# [ 0.02675161]])
|
||||||
|
|
||||||
|
R_ec = np.array([[ 1 , 0, 0],
|
||||||
|
[0 , 1, 0],
|
||||||
|
[0, 0, 1]])
|
||||||
|
t_ec = np.array([[-0.035],
|
||||||
|
[-0.18],
|
||||||
|
[ 0.08]])
|
||||||
|
|
||||||
|
T_ec = np.eye(4)
|
||||||
|
T_ec[:3, :3] = R_ec
|
||||||
|
T_ec[:3, 3] = t_ec.ravel()
|
||||||
|
|
||||||
|
# ---------- 读取 RobotToolPose.csv ----------
|
||||||
|
csv_path = Path("RobotToolPose.csv")
|
||||||
|
if not csv_path.exists():
|
||||||
|
raise FileNotFoundError("RobotToolPose.csv 未上传到工作目录")
|
||||||
|
|
||||||
|
mat = np.loadtxt(csv_path, delimiter=',') # 3 × (4*N)
|
||||||
|
if mat.shape[0] != 3 or mat.shape[1] % 4 != 0:
|
||||||
|
raise ValueError("RobotToolPose.csv 格式应为 3×(4*N)")
|
||||||
|
|
||||||
|
N = mat.shape[1] // 4
|
||||||
|
T_list = []
|
||||||
|
for i in range(N):
|
||||||
|
block = mat[:, 4*i:4*i+4] # 3×4
|
||||||
|
T = np.eye(4)
|
||||||
|
T[:3,:4] = block
|
||||||
|
T_list.append(T)
|
||||||
|
|
||||||
|
# ---------- 计算 T_base->cam = T_base->ee * T_ee->cam ----------
|
||||||
|
T_bc_list = [T_be @ T_ec for T_be in T_list]
|
||||||
|
|
||||||
|
# 简单检查：平移范数 & 旋转角度（相对于第一帧）
|
||||||
|
def rot_to_axis_angle(R):
|
||||||
|
angle = np.arccos(max(min((np.trace(R)-1)/2,1),-1))
|
||||||
|
return angle
|
||||||
|
|
||||||
|
trans_norms = [np.linalg.norm(T[:3,3]) for T in T_bc_list]
|
||||||
|
rot_angles = [rot_to_axis_angle(T[:3,:3] @ T_bc_list[0][:3,:3].T) for T in T_bc_list]
|
||||||
|
|
||||||
|
df = pd.DataFrame({
|
||||||
|
'frame': np.arange(N),
|
||||||
|
'tx': [T[0,3] for T in T_bc_list],
|
||||||
|
'ty': [T[1,3] for T in T_bc_list],
|
||||||
|
'tz': [T[2,3] for T in T_bc_list],
|
||||||
|
'||t|| (m)': trans_norms,
|
||||||
|
'Δθ w.r.t #0 (rad)': rot_angles
|
||||||
|
})
|
||||||
|
|
||||||
|
print(df)
|
||||||
@ -4,15 +4,17 @@
|
|||||||
眼在手上 用采集到的图片信息和机械臂位姿信息计算 相机坐标系相对于机械臂末端坐标系的 旋转矩阵和平移向量
|
眼在手上 用采集到的图片信息和机械臂位姿信息计算 相机坐标系相对于机械臂末端坐标系的 旋转矩阵和平移向量
|
||||||
A2^{-1}*A1*X=X*B2*B1^{−1}
|
A2^{-1}*A1*X=X*B2*B1^{−1}
|
||||||
"""
|
"""
|
||||||
|
|
||||||
import os
|
import os
|
||||||
import logging
|
import logging
|
||||||
|
|
||||||
|
|
||||||
import yaml
|
import yaml
|
||||||
|
|
||||||
import cv2
|
import cv2
|
||||||
import numpy as np
|
import numpy as np
|
||||||
from scipy.spatial.transform import Rotation as R
|
from scipy.spatial.transform import Rotation as R
|
||||||
|
|
||||||
|
|
||||||
from libs.auxiliary import find_latest_data_folder
|
from libs.auxiliary import find_latest_data_folder
|
||||||
from libs.log_setting import CommonLog
|
from libs.log_setting import CommonLog
|
||||||
|
|
||||||
@ -46,12 +48,15 @@ def func():
|
|||||||
|
|
||||||
# 获取标定板角点的位置
|
# 获取标定板角点的位置
|
||||||
objp = np.zeros((XX * YY, 3), np.float32)
|
objp = np.zeros((XX * YY, 3), np.float32)
|
||||||
|
|
||||||
objp[:, :2] = np.mgrid[0:XX, 0:YY].T.reshape(-1, 2) # 将世界坐标系建在标定板上，所有点的Z坐标全部为0，所以只需要赋值x和y
|
objp[:, :2] = np.mgrid[0:XX, 0:YY].T.reshape(-1, 2) # 将世界坐标系建在标定板上，所有点的Z坐标全部为0，所以只需要赋值x和y
|
||||||
objp = L*objp
|
objp = L*objp
|
||||||
|
|
||||||
|
|
||||||
obj_points = [] # 存储3D点
|
obj_points = [] # 存储3D点
|
||||||
img_points = [] # 存储2D点
|
img_points = [] # 存储2D点
|
||||||
|
|
||||||
|
|
||||||
images_num = [f for f in os.listdir(images_path) if f.endswith('.jpg')]
|
images_num = [f for f in os.listdir(images_path) if f.endswith('.jpg')]
|
||||||
|
|
||||||
for i in range(1, len(images_num) + 1): #标定好的图片在images_path路径下，从0.jpg到x.jpg
|
for i in range(1, len(images_num) + 1): #标定好的图片在images_path路径下，从0.jpg到x.jpg
|
||||||
@ -62,6 +67,7 @@ def func():
|
|||||||
|
|
||||||
logger_.info(f'读 {image_file}')
|
logger_.info(f'读 {image_file}')
|
||||||
|
|
||||||
|
|
||||||
img = cv2.imread(image_file)
|
img = cv2.imread(image_file)
|
||||||
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
|
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
|
||||||
|
|
||||||
@ -70,6 +76,7 @@ def func():
|
|||||||
|
|
||||||
if ret:
|
if ret:
|
||||||
|
|
||||||
|
|
||||||
obj_points.append(objp)
|
obj_points.append(objp)
|
||||||
|
|
||||||
corners2 = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), criteria) # 在原角点的基础上寻找亚像素角点
|
corners2 = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), criteria) # 在原角点的基础上寻找亚像素角点
|
||||||
@ -130,4 +137,3 @@ if __name__ == '__main__':
|
|||||||
logger_.info(f"平移向量是:\n { translation_vector}")
|
logger_.info(f"平移向量是:\n { translation_vector}")
|
||||||
|
|
||||||
logger_.info(f"四元数是：\n { quaternion}")
|
logger_.info(f"四元数是：\n { quaternion}")
|
||||||
|
|
||||||
|
|||||||
5
python/hand_eye_calibration/config/config.yaml
Normal file
5
python/hand_eye_calibration/config/config.yaml
Normal file
@ -0,0 +1,5 @@
|
|||||||
|
# 棋盘格参数
|
||||||
|
checkerboard_args:
|
||||||
|
XX : 11 #标定板的中长度对应的角点的个数
|
||||||
|
YY : 8 #标定板的中宽度对应的角点的个数
|
||||||
|
L : 0.015 #标定板一格的长度 单位为米
|
||||||
151
python/hand_eye_calibration/data_collect.py
Normal file
151
python/hand_eye_calibration/data_collect.py
Normal file
@ -0,0 +1,151 @@
|
|||||||
|
import os
|
||||||
|
import time
|
||||||
|
import pyrealsense2 as rs
|
||||||
|
import numpy as np
|
||||||
|
import cv2
|
||||||
|
|
||||||
|
# =============================
|
||||||
|
# 1. 关节点位定义（20组，每组7个）
|
||||||
|
# =============================
|
||||||
|
# JOINT_POSITIONS = [
|
||||||
|
# [-0.1756037771702, 1.1800090074539, 1.8522456884384, 1.5761110782623, -2.7331359386444, 0.2563314139843, -0.0606026798487],
|
||||||
|
# [-0.1923076510429, 1.1099290847778, 1.8832376003265, 1.7514026165009, -2.6219944953918, 0.1934144645929, 0.2889180481434],
|
||||||
|
# [0.2704087197781, 0.9557962417603, 1.7136577367783, 1.6711657047272, -2.5147924423218, 0.0584972538054, 0.783962905407],
|
||||||
|
# [0.383531242609, 0.7108500599861, 1.7350907325745, 1.8691725730896, -2.2797479629517, -0.1232690215111, 1.2245988845825],
|
||||||
|
# [0.3922453224659, 0.7737803459167, 1.8068689107895, 1.6336191892624, -2.1118586063385, -0.0338453501463, 1.1426001787186],
|
||||||
|
# [0.2443187087774, 0.7818641066551, 1.9731577634811, 1.5233805179596, -1.872123837471, 0.0405574627221, 1.1054704189301],
|
||||||
|
# [0.1276659220457, 0.9156659245491, 2.0081288814545, 1.4481414556503, -1.8519257307053, 0.2532368600368, 0.8394842743874],
|
||||||
|
# [0.1861536949873, 1.0141937732697, 1.8355123996735, 1.1459348201752, -2.2625010013580, 0.5969776511192, 0.2645072638988],
|
||||||
|
# [-0.0192848723382, 0.9415585398674, 1.8732609748840, 1.1809715032578, -2.6499755382538, 0.6354467868805, -0.2003904730082],
|
||||||
|
# [-0.1747295111418, 1.0453623533249, 1.8939573764801, 1.4446462392807, -2.6536157131195, 0.5147834420204, -0.1335265636444],
|
||||||
|
# [-0.0191576723009, 1.0618592500687, 1.8731262683868, 1.4747705459595, -2.4312825202942, 0.3830471336842, 0.2654716968536],
|
||||||
|
# [0.1436645090580, 0.8961057662964, 1.9278227090836, 1.5483351945877, -2.1520030498505, 0.2300240248442, 0.7717508077621],
|
||||||
|
# [-0.2458887547255, 1.3182551860809, 1.8035455942154, 1.7302685976028, -2.8433899879456, 0.0447540767491, -0.0295310281217],
|
||||||
|
# [-0.1628440171480, 1.1346882581711, 1.7724473476410, 1.8686875104904, -2.8621222972870, 0.0011371960863, 0.3351055085659],
|
||||||
|
# [0.2227641940117, 0.9846931695938, 1.5547094345093, 1.9238842725754, -2.8571910858154, -0.1052865162492, 0.8961741328239],
|
||||||
|
# [0.4523145258427, 0.7738097310066, 1.5341449975967, 1.9425786733627, -2.5897336006165, -0.1147941574454, 1.2599183320999],
|
||||||
|
# [0.6323996782303, 0.7300637960434, 1.3266047239304, 2.0460503101349, -2.7566838264465, -0.1049704179168, 1.4412622451782],
|
||||||
|
# [0.6435400247574, 0.7136778831482, 1.3097128868103, 2.0483627319336, -2.7511403560638, -0.1215622797608, 1.5219746828079],
|
||||||
|
# [0.0131992585957, 1.1347451210022, 1.4958723783493, 1.7922105789185, -3.1400320529938, -0.2474720925093, 0.4145260155201],
|
||||||
|
# [-0.1823624074459, 1.4246475696564, 1.7136197090149, 1.6952623128891, -2.9659118652344, -0.1499162465334, -0.0364842526615],
|
||||||
|
# ]
|
||||||
|
|
||||||
|
# JOINT_POSITIONS = [
|
||||||
|
# [-0.718823, 1.40293, 2.16063, 1.88231, -2.7615, 0.489431, -0.26], # 0.05 -0.3 -0.2
|
||||||
|
# [-0.708813, 1.12828, 2.50833, 1.80323, -2.31185, 0.655554, 0.0481596], # 0.05 -0.4 -0.12
|
||||||
|
# [ -0.741438, 1.25175, 2.32353, 1.87691, -2.52151, 0.551262, -0.127302], # 0.05 -0.35 -0.16
|
||||||
|
# [-0.450394, 1.09129, 2.19947, 1.44129, -2.64341, 0.614236, -0.26], # 0.1 -0.4 -0.21
|
||||||
|
# [-0.594676, 1.36527, 1.89605, 1.89043, -2.83642, 0.234229, -0.204878], # 0.1 -0.25 -0.21
|
||||||
|
# [ -0.741438, 1.25175, 2.32353, 1.87691, -2.52151, 0.551262, -0.127302], # 0.1 -0.33 -0.15
|
||||||
|
# [-0.390978, 1.16001, 2.05936, 1.90062, -2.5732, 0.280691, 0.125318], # 0.15 -0.31 -0.14
|
||||||
|
# [-0.373488, 1.2204, 1.93236, 1.6772, -2.73648, 0.297523, -0.140674], # 0.15 -0.3 -0.21
|
||||||
|
# [-0.401033, 1.34807, 1.77306, 1.81898, -2.87362, 0.135286, -0.111775], # 0.15 -0.23 -0.21
|
||||||
|
# [ -0.175812, 1.17802, 1.85193, 1.57614, -2.73311, 0.256294, -0.0606398], # 0.2 -0.3 -0.2
|
||||||
|
# [-0.373488, 1.2204, 1.93236, 1.6772, -2.73648, 0.297523, -0.140674], # 0.2 -0.25 -0.17
|
||||||
|
# [-0.0984266, 1.07668, 1.88658, 1.2654, -2.72499, 0.414404, -0.234536], # 0.2 -0.36 -0.23
|
||||||
|
# [0.0871982, 1.09615, 1.7789, 1.19035, -2.75303, 0.346485, -0.173718], # 0.25 -0.34 -0.22
|
||||||
|
# [ 0.0222405, 1.18277, 1.74292, 1.51783, -2.75674, 0.178517, 0.0381994], # 0.25 -0.28 -0.18
|
||||||
|
# [0.047511, 1.19839, 1.72007, 1.29276, -2.81499, 0.234448, -0.163403], # 0.25 -0.29 -0.23
|
||||||
|
# ]
|
||||||
|
|
||||||
|
JOINT_POSITIONS = [
|
||||||
|
[1.1838487386703, 0.4235305488110, 1.2719281911850, 1.5996315479279, -1.8642479181290, -0.1892226040363, 1.5996874570847], # 0.05 -0.3 -0.2
|
||||||
|
[1.1144028902054, 0.9934917092323, 1.2339128255844, 1.9597420692444, -2.7524161338806, -0.1010338962078, 2.0266277790070], # 0.05 -0.4 -0.12
|
||||||
|
[0.7585971355438, 1.2986438274384, 1.5511839389801, 1.9102549552917, -3.1083328723907,0.3383120596409, 1.7495325803757], # 0.05 -0.35 -0.16
|
||||||
|
[0.7717612981796, -0.0227999277413, 1.2341349124908, 0.8974148035049, -1.1484123468399, 1.0799195766449, 1.3140604496002], # 0.1 -0.4 -0.21
|
||||||
|
[0.3262319564819, 1.4255702495575, 1.0224531888962, 1.6128582954407, -3.1161243915558, -1.0476765632629, 0.5188082456589], # 0.1 -0.25 -0.21
|
||||||
|
[0.6811909079552, -0.0465415082872, 0.6306891441345, 0.7774662375450, -0.9128333926201, 1.0805308818817, 0.7373644113541], # 0.1 -0.33 -0.15
|
||||||
|
[0.5894140601158, 0.5544267892838, 0.1245087385178, 0.6875356435776, -1.0255894660950, 1.0803401470184, -0.3253187835217], # 0.15 -0.31 -0.14
|
||||||
|
[-0.0615737587214, 0.5200612545013, 2.2596797943115, 1.9721800088882, -1.8687928915024, -0.0344338789582, 1.2385385036469], # 0.15 -0.3 -0.21
|
||||||
|
[0.9264683723450, 1.2653641700745, 1.7594407796860, 1.5681478977203, -2.9171006679535, 0.1166879013181, 1.7105128765106], # 0.15 -0.23 -0.21
|
||||||
|
[1.0020719766617, 0.4507283568382, 1.0405201911926, 1.0608046054840, -1.5450072288513, 0.1164857149124, 1.1358253955841], # 0.2 -0.3 -0.2
|
||||||
|
[0.7424476146698, 0.0144095467404, 0.9496489167213, 0.8356217741966, -1.2565077543259, 1.0145307779312, 0.8847945928574], # 0.2 -0.25 -0.17
|
||||||
|
[0.2278160750866, 0.7444448471069, 0.9402200579643, 0.5896779298782, -1.5535788536072, 1.0801521539688, -0.3112177252769], # 0.2 -0.36 -0.23
|
||||||
|
[-0.0866613686085, 1.3698191642761, 1.1545568704605, 1.6992814540863, -2.7753651142120, -0.8176611065865, -0.1862865835428], # 0.25 -0.34 -0.22
|
||||||
|
[0.2748872637749, 1.5703777074814, 1.0753053426743, 1.0457334518433, -2.9064140319824, -0.9773108959198, -0.3075242042542], # 0.25 -0.28 -0.18
|
||||||
|
[0.2410523593426, 0.9616132378578, 1.6866004467010, 1.9817161560059, -2.7480125427246, 0.0709219276905, 1.1297777891159],
|
||||||
|
[0.5065582394600, 0.8918085694313, 2.1731114387512, 1.3742331266403, -2.4524819850922, 1.0801626443863, 1.1498324871063],
|
||||||
|
[0.5651561021805, 0.7603342533112, 1.1436395645142, 1.9550926685333, -1.9175499677658, -1.0519500970840, 0.6535356044769],
|
||||||
|
[-0.3106776177883, 0.8671860694885, 2.9035902023315, 1.7579817771912, -1.5784481763840, 1.0808564424515, 1.4593633413315],
|
||||||
|
[0.0202996153384, -0.2583181858063, 1.7621537446976, 1.8765900135040, -1.8188692331314, -0.0442187041044, 0.7810990214348], # 0.25 -0.29 -0.23
|
||||||
|
]
|
||||||
|
|
||||||
|
input_i = int(input("set index: "))
|
||||||
|
# =============================
|
||||||
|
# 2. 初始化RealSense
|
||||||
|
# =============================
|
||||||
|
TARGET_SERIAL = "243122075614"
|
||||||
|
ctx = rs.context()
|
||||||
|
devices = ctx.query_devices()
|
||||||
|
if len(devices) == 0:
|
||||||
|
raise RuntimeError("未检测到任何 RealSense 相机，请检查连接")
|
||||||
|
|
||||||
|
selected_device = None
|
||||||
|
for dev in devices:
|
||||||
|
if dev.get_info(rs.camera_info.serial_number) == TARGET_SERIAL:
|
||||||
|
selected_device = dev
|
||||||
|
break
|
||||||
|
if selected_device is None:
|
||||||
|
raise RuntimeError(f"未找到序列号为 {TARGET_SERIAL} 的 RealSense 相机")
|
||||||
|
|
||||||
|
pipeline = rs.pipeline()
|
||||||
|
config = rs.config()
|
||||||
|
config.enable_device(TARGET_SERIAL)
|
||||||
|
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
|
||||||
|
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
|
||||||
|
pipeline.start(config)
|
||||||
|
|
||||||
|
align = rs.align(rs.stream.color)
|
||||||
|
|
||||||
|
# =============================
|
||||||
|
# 3. 数据采集流程
|
||||||
|
# =============================
|
||||||
|
output_joint_file = "data/20250804-1331/joints_record.txt"
|
||||||
|
|
||||||
|
with open(output_joint_file, "a") as f:
|
||||||
|
# for idx, q in enumerate(JOINT_POSITIONS, start=1):
|
||||||
|
idx = input_i
|
||||||
|
q = JOINT_POSITIONS[input_i]
|
||||||
|
print(f"\n===== 采集点 {idx} =====")
|
||||||
|
cmd = "moveJ one right " + " ".join(str(v) for v in q)
|
||||||
|
os.system(cmd)
|
||||||
|
print("运动完成，等待1秒...")
|
||||||
|
time.sleep(1)
|
||||||
|
|
||||||
|
# 获取关节角度输出
|
||||||
|
tmp_file = "./data/tmp_joint_output.txt"
|
||||||
|
os.system(f"getJoint > {tmp_file}")
|
||||||
|
with open(tmp_file, "r") as tf:
|
||||||
|
lines = tf.readlines()
|
||||||
|
joint_output = lines[-1].strip() if lines else ""
|
||||||
|
print("getJoint 输出：\n", joint_output)
|
||||||
|
|
||||||
|
# 保存关节数据
|
||||||
|
f.write(f"{idx} {joint_output}\n")
|
||||||
|
f.flush()
|
||||||
|
|
||||||
|
# 捕获对齐后的彩色图像
|
||||||
|
for i in range(10):
|
||||||
|
try:
|
||||||
|
frames = pipeline.wait_for_frames(timeout_ms=5000)
|
||||||
|
if frames:
|
||||||
|
break
|
||||||
|
except RuntimeError:
|
||||||
|
print("⚠️ 读取帧超时，重试中...")
|
||||||
|
time.sleep(1)
|
||||||
|
frames = pipeline.wait_for_frames()
|
||||||
|
aligned_frames = align.process(frames)
|
||||||
|
color_frame = aligned_frames.get_color_frame()
|
||||||
|
|
||||||
|
if not color_frame:
|
||||||
|
print("未获取彩色图像，跳过")
|
||||||
|
exit(-1)
|
||||||
|
|
||||||
|
color_image = np.asanyarray(color_frame.get_data())
|
||||||
|
img_path = os.path.join(os.path.dirname(output_joint_file), f"img_{idx}.png")
|
||||||
|
cv2.imwrite(img_path, color_image)
|
||||||
|
print(f"保存图像 {img_path}")
|
||||||
|
|
||||||
|
# 停止RealSense
|
||||||
|
pipeline.stop()
|
||||||
|
print("\n=== 数据采集完成 ===")
|
||||||
79
python/math/rpy2rotation.py
Normal file
79
python/math/rpy2rotation.py
Normal file
@ -0,0 +1,79 @@
|
|||||||
|
import numpy as np
|
||||||
|
|
||||||
|
def rpy_to_matrix(roll, pitch, yaw, degrees=False):
|
||||||
|
"""
|
||||||
|
URDF: R = Rz(yaw) @ Ry(pitch) @ Rx(roll)
|
||||||
|
roll, pitch, yaw 默认为弧度；degrees=True 时按角度输入。
|
||||||
|
"""
|
||||||
|
if degrees:
|
||||||
|
roll, pitch, yaw = np.deg2rad([roll, pitch, yaw])
|
||||||
|
|
||||||
|
cr, sr = np.cos(roll), np.sin(roll)
|
||||||
|
cp, sp = np.cos(pitch), np.sin(pitch)
|
||||||
|
cy, sy = np.cos(yaw), np.sin(yaw)
|
||||||
|
|
||||||
|
Rx = np.array([[1, 0, 0],
|
||||||
|
[0, cr, -sr],
|
||||||
|
[0, sr, cr]], dtype=float)
|
||||||
|
Ry = np.array([[ cp, 0, sp],
|
||||||
|
[ 0, 1, 0],
|
||||||
|
[-sp, 0, cp]], dtype=float)
|
||||||
|
Rz = np.array([[cy, -sy, 0],
|
||||||
|
[sy, cy, 0],
|
||||||
|
[ 0, 0, 1]], dtype=float)
|
||||||
|
|
||||||
|
return Rz @ Ry @ Rx
|
||||||
|
|
||||||
|
|
||||||
|
def matrix_to_rpy(R, degrees=False, eps=1e-9):
|
||||||
|
"""
|
||||||
|
从旋转矩阵恢复 URDF 的 (roll, pitch, yaw)，满足 R = Rz(yaw)·Ry(pitch)·Rx(roll)。
|
||||||
|
返回弧度；degrees=True 时返回角度。
|
||||||
|
含万向节锁处理。
|
||||||
|
"""
|
||||||
|
R = np.asarray(R, dtype=float)
|
||||||
|
assert R.shape == (3, 3)
|
||||||
|
|
||||||
|
# 可选：小幅正交化以抑制数值误差（不想要可注释掉）
|
||||||
|
# 使用极分解的简化：R ≈ U*V^T
|
||||||
|
U, _, Vt = np.linalg.svd(R)
|
||||||
|
R = U @ Vt
|
||||||
|
|
||||||
|
# R[2,0] = -sin(pitch)
|
||||||
|
sp_neg = R[2, 0]
|
||||||
|
sp_neg = np.clip(sp_neg, -1.0, 1.0)
|
||||||
|
|
||||||
|
# 非万向节锁：|sp_neg| < 1
|
||||||
|
if abs(sp_neg) < 1.0 - eps:
|
||||||
|
pitch = np.arcsin(-sp_neg) # pitch
|
||||||
|
roll = np.arctan2(R[2, 1], R[2, 2]) # roll
|
||||||
|
yaw = np.arctan2(R[1, 0], R[0, 0]) # yaw
|
||||||
|
else:
|
||||||
|
# 万向节锁：|sp_neg| ≈ 1，此时 yaw 与 roll 耦合
|
||||||
|
# 令 yaw = 0，通过 R 的其它项恢复 roll
|
||||||
|
pitch = np.pi/2 if sp_neg < 0 else -np.pi/2
|
||||||
|
yaw = 0.0
|
||||||
|
# 当 pitch = ±pi/2 时，R[0,1] 与 R[1,1] 携带 roll 信息
|
||||||
|
# 推导自 R = Rz(yaw)·Ry(±pi/2)·Rx(roll)
|
||||||
|
roll = np.arctan2(-R[0, 1] if sp_neg < 0 else R[0, 1],
|
||||||
|
R[1, 1])
|
||||||
|
|
||||||
|
if degrees:
|
||||||
|
return tuple(np.rad2deg([roll, pitch, yaw]))
|
||||||
|
return (roll, pitch, yaw)
|
||||||
|
|
||||||
|
|
||||||
|
# ---------- 简单自检 ----------
|
||||||
|
if __name__ == "__main__":
|
||||||
|
# rpy = (0.3, -0.6, 1.2) # roll, pitch, yaw (rad)
|
||||||
|
# R = rpy_to_matrix(*rpy)
|
||||||
|
# rpy_back = matrix_to_rpy(R)
|
||||||
|
# print("R:\n", R)
|
||||||
|
# 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],
|
||||||
|
[0, 0, 1]
|
||||||
|
])
|
||||||
|
print(matrix_to_rpy(R))
|
||||||
@ -4,7 +4,7 @@ find_package(protobuf REQUIRED)
|
|||||||
|
|
||||||
add_library(canbus SHARED
|
add_library(canbus SHARED
|
||||||
${CMAKE_CURRENT_SOURCE_DIR}/can_client/socket/socket_can_client_raw.cc
|
${CMAKE_CURRENT_SOURCE_DIR}/can_client/socket/socket_can_client_raw.cc
|
||||||
${CMAKE_CURRENT_SOURCE_DIR}/can_client/pcan/pcan_client.cc
|
# ${CMAKE_CURRENT_SOURCE_DIR}/can_client/pcan/pcan_client.cc
|
||||||
${CMAKE_CURRENT_SOURCE_DIR}/common/byte.cc)
|
${CMAKE_CURRENT_SOURCE_DIR}/common/byte.cc)
|
||||||
|
|
||||||
target_include_directories(canbus PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
target_include_directories(canbus PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||||
@ -13,9 +13,11 @@ add_library(cmvr_es::device::canbus ALIAS canbus)
|
|||||||
|
|
||||||
target_link_libraries(canbus
|
target_link_libraries(canbus
|
||||||
PRIVATE
|
PRIVATE
|
||||||
|
proto-objects
|
||||||
|
gRPC::grpc++
|
||||||
protobuf::libprotobuf
|
protobuf::libprotobuf
|
||||||
glog::glog
|
glog::glog
|
||||||
PUBLIC pcanbasic
|
# PUBLIC pcanbasic
|
||||||
)
|
)
|
||||||
|
|
||||||
# --------------------------------------------------------
|
# --------------------------------------------------------
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user