From caf2da33272e8218743ece50d9be4896704743a7 Mon Sep 17 00:00:00 2001 From: hjw <1576345902@qq.com> Date: Thu, 26 Sep 2024 08:30:58 +0000 Subject: [PATCH 1/3] =?UTF-8?q?=E6=9B=B4=E6=96=B0=20Trace/com=5Fpose.txt?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Trace/com_pose.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Trace/com_pose.txt b/Trace/com_pose.txt index b897cea..28c50d0 100644 --- a/Trace/com_pose.txt +++ b/Trace/com_pose.txt @@ -1,4 +1,4 @@ -0.101852 -0.994553 -0.0221425 42.7527 --0.990341 -0.0992653 -0.0968082 30.2341 -0.0940829 0.0317887 -0.995057 80.0869 +-0.000714604 0.999379 -0.0352319 -19.4992 +-0.992053 -0.00514141 -0.125718 28.7299 +-0.125821 0.0348621 0.99144 -107.48 0 0 0 1 \ No newline at end of file From bd6576130b6fa8b12dfa676346bd3bfae467cff3 Mon Sep 17 00:00:00 2001 From: hjw <1576345902@qq.com> Date: Thu, 26 Sep 2024 08:31:17 +0000 Subject: [PATCH 2/3] =?UTF-8?q?=E6=9B=B4=E6=96=B0=20Trace/handeye=5Fcalibr?= =?UTF-8?q?ation.py?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Trace/handeye_calibration.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Trace/handeye_calibration.py b/Trace/handeye_calibration.py index b2f5626..4e2d7d9 100644 --- a/Trace/handeye_calibration.py +++ b/Trace/handeye_calibration.py @@ -3,7 +3,7 @@ from scipy.spatial.transform import Rotation as R def vec2rpy(normal,long_edge_direction): # 将法向量的反方向作为机械臂末端执行器的新Z轴 - z_axis = -normal / np.linalg.norm(normal) # 归一化并取反向作为Z轴 + z_axis = normal / np.linalg.norm(normal) # 归一化并取反向作为Z轴 x_axis = long_edge_direction/np.linalg.norm(long_edge_direction) x_axis = x_axis-np.dot(x_axis,z_axis)*z_axis From 40d91cb11c87e375cb6a37b6b4f65a5b9265f14b Mon Sep 17 00:00:00 2001 From: hjw <1576345902@qq.com> Date: Thu, 26 Sep 2024 08:32:26 +0000 Subject: [PATCH 3/3] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=20Trace/vec=5Fchange.py?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Trace/vec_change.py | 112 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 112 insertions(+) create mode 100644 Trace/vec_change.py diff --git a/Trace/vec_change.py b/Trace/vec_change.py new file mode 100644 index 0000000..3bd79cf --- /dev/null +++ b/Trace/vec_change.py @@ -0,0 +1,112 @@ +import numpy as np +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D + +def plot_coordinate_system(ax, T, name, color, labels): + """绘制坐标系""" + origin = T[:3, 3] + x_axis = origin + T[:3, 0] * 300 # X 轴 + y_axis = origin + T[:3, 1] * 300 # Y 轴 + z_axis = origin + T[:3, 2] * 300 # Z 轴 + + # 绘制原点 + ax.scatter(*origin, color=color, s=100) + + # 绘制轴线 + ax.quiver(*origin, *(x_axis - origin), color='r', length=1, arrow_length_ratio=0.2, linewidth=2) + ax.quiver(*origin, *(y_axis - origin), color='g', length=1, arrow_length_ratio=0.2, linewidth=2) + ax.quiver(*origin, *(z_axis - origin), color='b', length=1, arrow_length_ratio=0.2, linewidth=2) + + # 标注坐标系名称 + ax.text(*x_axis, f'{labels[0]}', color='r', fontsize=12) + ax.text(*y_axis, f'{labels[1]}', color='g', fontsize=12) + ax.text(*z_axis, f'{labels[2]}', color='b', fontsize=12) + +# A 到 B 的齐次转换矩阵 (工具到基坐标系) +T_AB = np.array([[-9.36910568e-01,-4.37100341e-03, 3.49541818e-01, 5.04226000e+02], + [-5.82144893e-03, 9.99978253e-01, -3.09911034e-03, 2.62300000e+00], + [-3.49520671e-01, -4.93842907e-03, -9.36915638e-01, 5.23709000e+02], + [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]) + +# B 到 C 的齐次转换矩阵 (相机到工具) +T_BC = np.loadtxt('./cam_pose.txt', delimiter=' ') + +# 计算 A 到 C 的齐次转换矩阵 +T_AC = T_AB @ T_BC + +# 输入四个角点的空间坐标 (相机坐标系下) +corner_points_camera = np.array([ + [-0.07010334103611927, -0.007624093814835717, 0.638128259308727], + [0.03136683809654154, 0.003582437967995489, 0.6478950644491274], + [0.02505911529466708, 0.04860494901872052, 0.625793874394482], + [-0.07246355234803807, 0.03687307395179221, 0.6171704935761987] +]) + +# 将角点从相机坐标系转换到法兰坐标系 +corner_points_flange = np.dot(T_BC[:3, :3], corner_points_camera.T).T + T_BC[:3, 3] + +# 将角点从法兰坐标系转换到基坐标系 +corner_points_base = np.dot(T_AB[:3, :3], corner_points_flange.T).T + T_AB[:3, 3] + +# 计算每两个相邻角点之间的边向量 +edges = np.array([corner_points_base[i] - corner_points_base[i - 1] for i in range(len(corner_points_base))]) + +# 计算每条边的长度 +edge_lengths = np.linalg.norm(edges, axis=1) + +# 找到最长的边 +max_edge_idx = np.argmax(edge_lengths) +long_edge_direction = edges[max_edge_idx] / edge_lengths[max_edge_idx] # 单位化方向向量 + +# 假设法向量 (a, b, c) 在相机坐标系下 +normal_vector_camera = np.array([-0.1305,0.38402,0.91404, 0]) # 最后一个元素为0，因为它是方向矢量 + +# 将法向量从相机坐标系转换到法兰坐标系 +normal_vector_flange = T_BC @ normal_vector_camera + +# 将法向量从法兰坐标系转换到基坐标系 +normal_vector_base = T_AB @ normal_vector_flange + +# 创建 3D 图形对象 +fig = plt.figure() +ax = fig.add_subplot(111, projection='3d') + +# 设置绘图区域的范围 +ax.set_xlim([-1000, 1000]) +ax.set_ylim([-1000, 1000]) +ax.set_zlim([-1000, 1000]) + +# 绘制基坐标系 O +plot_coordinate_system(ax, np.eye(4), 'O', 'k', ['x', 'y', 'z']) + +# 绘制法兰坐标系 B +plot_coordinate_system(ax, T_AB, 'B', 'm', ["x'", "y'", "z'"]) + +# 绘制相机坐标系 C +plot_coordinate_system(ax, T_AC, 'C', 'b', ["x''", "y''", "z''"]) + +# 绘制长边方向向量 (基坐标系下) +origin = np.zeros(3) # 基坐标系的原点 +long_edge_endpoint = long_edge_direction * 300 +ax.quiver(*origin, *(long_edge_endpoint), color='orange', length=1, arrow_length_ratio=0.2, linewidth=2) +ax.text(*long_edge_endpoint, 'Long Edge', color='orange', fontsize=12) + +# 绘制法向量 (基坐标系下) +normal_vector_endpoint = normal_vector_base[:3] * 300 +ax.quiver(*origin, *(normal_vector_endpoint), color='purple', length=1, arrow_length_ratio=0.2, linewidth=2) +ax.text(*normal_vector_endpoint, 'Normal Vector', color='purple', fontsize=12) + +# 在基坐标系下绘制四个角点和边 +ax.scatter(corner_points_base[:, 0], corner_points_base[:, 1], corner_points_base[:, 2], color='b', s=50, label='Corners') +for i in range(len(corner_points_base)): + ax.plot([corner_points_base[i - 1, 0], corner_points_base[i, 0]], + [corner_points_base[i - 1, 1], corner_points_base[i, 1]], + [corner_points_base[i - 1, 2], corner_points_base[i, 2]], 'k--') + +# 设置标签 +ax.set_xlabel('X') +ax.set_ylabel('Y') +ax.set_zlabel('Z') + +# 显示图形 +plt.show()