我有一个UR5 CB3机械臂，需要训练它来捡西瓜。为此，我安装了一个摄像头，用于确定西瓜相对于 TCP（或末端执行器/抓握“手”）的位置。该 TCP 坐标系称为 K1。我还知道 TCP 相对于基础坐标系的位置和方向。该基础坐标系称为 K0。 为了简化问题，我删除了所有用于确定西瓜位置的代码，只插入 K1 坐标系中给出的任意坐标，例如 [ 0 , 0 , 0.1] 。现在我想计算这些坐标在K0坐标系中的值。从数学上讲，这应该是一个相当简单的代码，我现在将使用 [roll,pitch,yaw] 来计算旋转矩阵 rot_mat_K1_to_K0，然后使用 rot_mat_K1_to_K0 与 K1 系统中指向 W 的向量进行矩阵乘法。然后，我将在 K1 坐标中给定的从 K0 到 K1 的向量与现在从 K1 到 W 的旋转向量以简单的向量加法相加。 然后我可以将这个最终向量发送到我的机器人以移动到那里。
但是在下面的Python代码中，我遇到了机器人移动的问题，但方向完全不相关。至少错误是可重复的，因此手臂总是以相同但错误的方向移动。
以下代码称为“raw_motion_tester_6.py”：
from time import sleep
import numpy as np
from scipy.spatial import transform as tf
import rtde_receive
import rtde_control
print("Environment Ready")
# RTDE configuration
ROBOT_IP = "192.168.1.102" # IP address of your UR5 CB3 robot
FREQUENCY = 125 # Frequency of data retrieval in Hz
# Establish RTDE connections
rtde_receiver = rtde_receive.RTDEReceiveInterface(ROBOT_IP)
rtde_controler = rtde_control.RTDEControlInterface(ROBOT_IP)
# Define the variables to retrieve from RTDE
variables = ["actual_TCP_pose"] #What does this do?
# Start data synchronization
rtde_receiver.startFileRecording("recorded_data.csv")
####################################################################
# Define the movement distance in the K1 coordinate system
move_distance_x = 0.0 # Move X.X m along the x-axis of K1
move_distance_y = 0.0 # Move X.X m along the y-axis of K1
move_distance_z = -0.1 # Move X.X m along the z-axis of K1
####################################################################
try:
while True:
# Receive and parse RTDE data
tcp_position = rtde_receiver.getActualTCPPose()
# Matrix transformation
origin_K1_wrt_K0 = tcp_position[:3] # Origin of the TCP w.r.t. the Robot Base
angles_K1_wrt_K0 = tcp_position[3:6] # Orientation of the origin of the TCP w.r.t. the Robot Base
# Create a rotation matrix based on the angles with scipy transform
rot_K0_to_K1 = tf.Rotation.from_rotvec(angles_K1_wrt_K0) # rotvec or euler should not make a difference
rot_mat_K0_to_K1 = rot_K0_to_K1.as_matrix()
print(f"Rotation Matrix K0 to K1: {rot_mat_K0_to_K1}")
rot_mat_K1_to_K0 = rot_mat_K0_to_K1.T
# Create the movement vector in the K1 coordinate system
move_vector_K1 = np.array([move_distance_x, move_distance_y, move_distance_z])
# Convert the movement vector from K1 to K0 coordinate system
move_vector_K0 = rot_mat_K1_to_K0 @ move_vector_K1
print(f"Move Vector K0: {move_vector_K0}", f"Shape of Move Vector K0: {move_vector_K0.shape}")
# Calculate the new TCP position in the K0 coordinate system
new_tcp_position_K0 = origin_K1_wrt_K0 + move_vector_K0
# Move the robot to the new TCP position
rtde_controler.moveL(new_tcp_position_K0.tolist() + [tcp_position[3], tcp_position[4], tcp_position[5]], 0.1, 0.2)
# sleep(1)
break
except KeyboardInterrupt:
# Stop file recording and disconnect on keyboard interrupt
rtde_receiver.stopFileRecording()
rtde_receiver.disconnect()
rtde_controler.disconnect()
finally:
print("Disconnected from the UR5 CB3 robot")
简化版本，没有旋转矩阵，只有平移，称为“raw_motion_tester_7.py”，工作得很好，有旋转和平移 TCP，所以假设问题出在原始数学部分的某个地方是合乎逻辑的代码。
from time import sleep
import numpy as np
from scipy.spatial import transform as tf
import rtde_receive
import rtde_control
from math import pi
print("Environment Ready")
# RTDE configuration
ROBOT_IP = "192.168.1.102" # IP address of your UR5 CB3 robot
FREQUENCY = 125 # Frequency of data retrieval in Hz
# Establish RTDE connections
rtde_receiver = rtde_receive.RTDEReceiveInterface(ROBOT_IP)
rtde_controler = rtde_control.RTDEControlInterface(ROBOT_IP)
# Define the variables to retrieve from RTDE
variables = ["actual_TCP_pose"] #What does this do?
# Start data synchronization
rtde_receiver.startFileRecording("recorded_data.csv")
####################################################################
# Define the movement distance in the K1 coordinate system
move_distance_x = 0.0 # Move X.X m along the x-axis of K1
move_distance_y = -0.0 # Move X.X m along the y-axis of K1
move_distance_z = -0.0 # Move X.X m along the z-axis of K1
rotate_tcp_x = 0 # Rotate X.X rad around the x-axis of K0
rotate_tcp_y = 0 # Rotate X.X rad around the y-axis of K0
rotate_tcp_z = 0 # Rotate X.X rad around the z-axis of K0
####################################################################
try:
while True:
# Receive and parse RTDE data
tcp_position = rtde_receiver.getActualTCPPose()
# Matrix transformation
origin_K1_wrt_K0 = tcp_position[:3] # Origin of the TCP w.r.t. the Robot Base
angles_K1_wrt_K0 = tcp_position[3:6] # Orientation of the origin of the TCP w.r.t. the Robot Base
# Create the movement vector in the K0 coordinate system
move_vector_K0 = np.array([move_distance_x, move_distance_y, move_distance_z])
# Calculate the new TCP position in the K0 coordinate system
new_tcp_position_K0 = origin_K1_wrt_K0 + move_vector_K0
# Move the robot to the new TCP position
rtde_controler.moveL(new_tcp_position_K0.tolist() + [tcp_position[3]+rotate_tcp_x, tcp_position[4]+rotate_tcp_y, tcp_position[5]+rotate_tcp_z], 0.1, 0.2)
# sleep(1)
break
except KeyboardInterrupt:
# Stop file recording and disconnect on keyboard interrupt
rtde_receiver.stopFileRecording()
rtde_receiver.disconnect()
rtde_controler.disconnect()
finally:
print("Disconnected from the UR5 CB3 robot")
为了了解 rtde_receiver.getActualTCPPose() 实际获取的数据，我将其保存到一个 txt 文件中：“TCP Position: [0.17477931598731355, 0.5513537036720785, 0.524021777855706, 231, -1.6571432341558983, 1.3242450163108708]" 前三个是坐标米，最后三个是以拉德为单位的方向。该文档将该函数描述为“工具的实际笛卡尔坐标：(x,y,z,rx,ry,rz)，其中 rx、ry 和 rz 是工具方向的旋转向量表示”。
如上所述，我编写了一个更简单的代码版本，以排除基本的连接问题或类似问题。我也非常确定我正确地计算了旋转矩阵。我调查了我使用的库，据我所知，我正在使用所有库，尤其是 scipy 的
rot_K0_to_K1 = tf.Rotation.from_rotvec(angles_K1_wrt_K0)编辑：我忘记添加未连接到机器人时运行的代码版本。 该数据包含 UR5 的任意但真实的位置数据。我希望这能让我更容易理解我的问题。
from time import sleep
import numpy as np
from scipy.spatial import transform as tf
print("Environment Ready")
####################################################################
# Define the movement distance in the K1 coordinate system
move_distance_x = 0.0 # Move X.X m along the x-axis of K1
move_distance_y = 0.0 # Move X.X m along the y-axis of K1
move_distance_z = -0.1 # Move X.X m along the z-axis of K1
####################################################################
# Pseudo TCP Position for test purposes
tcp_position = [0.17477931598731355, 0.5513537036720785, 0.524021777855706, 0.4299023783620231, -1.6571432341558983, 1.3242450163108708] # This is an arbitrary location of the robot, the first three are the xyz coordinates of the TCP in the baseframe, and the last three are the rotation along the xyz axis
# Matrix transformation
origin_K1_wrt_K0 = tcp_position[:3] # Origin of the TCP w.r.t. the Robot Base
angles_K1_wrt_K0 = tcp_position[3:6] # Orientation of the origin of the TCP w.r.t. the Robot Base
# Create a rotation matrix based on the angles with scipy transform
rot_K0_to_K1 = tf.Rotation.from_rotvec(angles_K1_wrt_K0) # rotvec or euler should not make a difference
rot_mat_K0_to_K1 = rot_K0_to_K1.as_matrix()
print(f"Rotation Matrix K0 to K1: {rot_mat_K0_to_K1}")
rot_mat_K1_to_K0 = rot_mat_K0_to_K1.T
# Create the movement vector in the K1 coordinate system
move_vector_K1 = np.array([move_distance_x, move_distance_y, move_distance_z])
# Convert the movement vector from K1 to K0 coordinate system
move_vector_K0 = rot_mat_K1_to_K0 @ move_vector_K1
print(f"Move Vector K0: {move_vector_K0}", f"Shape of Move Vector K0: {move_vector_K0.shape}")
# Calculate the new TCP position in the K0 coordinate system
new_tcp_position_K0 = origin_K1_wrt_K0 + move_vector_K0
# Move the robot to the new TCP position
new_complete_tcp_description = new_tcp_position_K0.tolist() + [tcp_position[3], tcp_position[4], tcp_position[5]]
print("new_complete_tcp_description: ", new_complete_tcp_description)
# sleep(1)
好吧，伙计们，我解决了。在我开始使用这个机器人之前，TCP 位置和方向似乎校准错误。校准是在机器人本身上完成的，而不是在我的代码中完成的，所以我无法直接找到它。所以我的动作实际上是正确的，但是坐标系K1并不是我期望的那样...... 好吧，我希望有人在像我一样浪费时间寻找错误之前看到这一点。我们再次看到系统调试是多么重要......