支持html5的网站北京网站设计有名 乐云践新

支持html5的网站,北京网站设计有名 乐云践新,做网站页面对PS切图,网站建设总费用代码由Qwen3-Max生成。prompt#xff1a;设计一个读取UR3机械臂关节角度#xff08;两种格式输出#xff1a;弧度和角度#xff09;和末端位姿的程序#xff08;3种格式#xff1a;UR机械臂默认格式#xff0c;欧拉角和旋转矩阵#xff09;。1. 代码ur3_state_reader.p…代码由Qwen3-Max生成。prompt设计一个读取UR3机械臂关节角度两种格式输出弧度和角度和末端位姿的程序3种格式UR机械臂默认格式欧拉角和旋转矩阵。1. 代码ur3_state_reader.pyimport 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_hostROBOT_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, degreesTrue) # 关闭连接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) robotrobot-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) robotrobot-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) robotrobot-cvai:~/workspace/book_robot/ur3$