代码由Qwen3-Max生成。
prompt：
设计一个读取UR3机械臂关节角度（两种格式输出：弧度和角度）和末端位姿的程序（3种格式：UR机械臂默认格式，欧拉角和旋转矩阵）。
1. 代码
ur3_state_reader.py
import rtde_receive import numpy as np from scipy.spatial.transform import Rotation # -- 全局配置 -- ROBOT_HOST = '192.168.1.101' # 请替换为你的 UR3 IP def get_robot_state(robot_host=ROBOT_HOST): """ 读取 UR3 机械臂当前状态，并以多种格式输出关节角和末端位姿。 返回: dict: 包含关节角（弧度/角度）和末端位姿（三种格式） """ try: rtde_r = rtde_receive.RTDEReceiveInterface(robot_host) # 1. 获取关节角度（弧度） q_rad = np.array(rtde_r.getActualQ()) # 单位：弧度 q_deg = np.rad2deg(q_rad) # 转换为角度 # 2. 获取末端位姿（UR 默认格式：[x, y, z, rx, ry, rz]） tcp_pose_ur = np.array(rtde_r.getActualTCPPose()) # 前3位位置(m)，后3位轴角向量(rad) # 3. 提取旋转部分（轴角向量） rotvec = tcp_pose_ur[3:] # 4. 转换为旋转矩阵 R = Rotation.from_rotvec(rotvec).as_matrix() # 5. 转换为欧拉角（默认顺序 'XYZ'，单位：度） euler_xyz_deg = Rotation.from_rotvec(rotvec).as_euler('xyz', degrees=True) # 关闭连接（RTDEReceiveInterface 通常不需要显式关闭，但可重用） # 注意：频繁创建/销毁 rtde_r 可能影响性能，建议在长期任务中复用实例 return { "joint_angles": { "radians": q_rad.tolist(), "degrees": q_deg.tolist() }, "tcp_pose": { "ur_format": tcp_pose_ur.tolist(), # [x, y, z, rx, ry, rz] "euler_xyz_degrees": euler_xyz_deg.tolist(), # [roll, pitch, yaw] in degrees "rotation_matrix": R.tolist() # 3x3 list of lists } } except Exception as e: print(f"❌ 读取机器人状态失败: {e}") return None def print_robot_state(state): """格式化打印机器人状态（用于调试或日志）""" if state is None: print("无法获取机器人状态。") return print("\n===== UR3 当前状态 =====") print("🔄 关节角度 (弧度):") for i, q in enumerate(state['joint_angles']['radians'], 1): print(f" q{i}: {q:.4f} rad") print("\n🔄 关节角度 (角度):") for i, q in enumerate(state['joint_angles']['degrees'], 1): print(f" q{i}: {q:.2f}°") print("\n📍 末端位姿 (UR 默认格式 [x,y,z,rx,ry,rz]):") pose = state['tcp_pose']['ur_format'] print(f" 位置 (m): [{pose[0]:.4f}, {pose[1]:.4f}, {pose[2]:.4f}]") print(f" 轴角 (rad): [{pose[3]:.4f}, {pose[4]:.4f}, {pose[5]:.4f}]") print("\n📍 末端位姿 (欧拉角 XYZ, 单位: 度):") euler = state['tcp_pose']['euler_xyz_degrees'] print(f" Roll (X): {euler[0]:.2f}°, Pitch (Y): {euler[1]:.2f}°, Yaw (Z): {euler[2]:.2f}°") print("\n📍 旋转矩阵 R = [r11 r12 r13; r21 r22 r23; r31 r32 r33]:") R = np.array(state['tcp_pose']['rotation_matrix']) print(R) if __name__ == '__main__': print("正在读取 UR3 机械臂当前状态...") state = get_robot_state() print_robot_state(state)2. 依赖库安装
pip install ur_rtde scipy numpy3. 输出
下面是两种不同TCP设置的输出。
(book_robot) robot@robot-cvai:~/workspace/book_robot/ur3$ python ur3_state_reader.py 正在读取 UR3 机械臂当前状态... ===== UR3 当前状态 ===== 🔄 关节角度 (弧度): q1: 1.5216 rad q2: -0.1532 rad q3: -1.9164 rad q4: 4.9550 rad q5: 4.5877 rad q6: -6.4484 rad 🔄 关节角度 (角度): q1: 87.18° q2: -8.78° q3: -109.80° q4: 283.90° q5: 262.85° q6: -369.47° 📍 末端位姿 (UR 默认格式 [x,y,z,rx,ry,rz]): 位置 (m): [0.0924, -0.2015, 0.4796] 轴角 (rad): [-0.0428, -1.7833, 2.2792] 📍 末端位姿 (欧拉角 XYZ, 单位: 度): Roll (X): -75.29°, Pitch (Y): -7.35°, Yaw (Z): 167.73° 📍 旋转矩阵 R = [r11 r12 r13; r21 r22 r23; r31 r32 r33]: [[-0.96913461 -0.1748315 -0.17381614] [ 0.21076242 -0.22179452 -0.95204327] [ 0.12789568 -0.959292 0.25179666]] (book_robot) robot@robot-cvai:~/workspace/book_robot/ur3$ python ur3_state_reader.py 正在读取 UR3 机械臂当前状态... ===== UR3 当前状态 ===== 🔄 关节角度 (弧度): q1: 1.5215 rad q2: -0.1532 rad q3: -1.9164 rad q4: 4.9550 rad q5: 4.5876 rad q6: -6.4485 rad 🔄 关节角度 (角度): q1: 87.18° q2: -8.78° q3: -109.80° q4: 283.90° q5: 262.85° q6: -369.47° 📍 末端位姿 (UR 默认格式 [x,y,z,rx,ry,rz]): 位置 (m): [0.0576, -0.3919, 0.5300] 轴角 (rad): [-0.0429, -1.7834, 2.2790] 📍 末端位姿 (欧拉角 XYZ, 单位: 度): Roll (X): -75.30°, Pitch (Y): -7.35°, Yaw (Z): 167.73° 📍 旋转矩阵 R = [r11 r12 r13; r21 r22 r23; r31 r32 r33]: [[-0.96911454 -0.17485003 -0.17390936] [ 0.21085012 -0.22172955 -0.95203899] [ 0.1279032 -0.95930364 0.25174848]] (book_robot) robot@robot-cvai:~/workspace/book_robot/ur3$
