""" MMK2 SDK 使用示例 """ import time import numpy as np import cv2 import logging # 设置日志 logging.basicConfig(level=logging.INFO, format="%(asctime)s - %(levelname)s - %(message)s") logger = logging.getLogger(__name__) # 导入新的SDK from mmk2_sdk import MMK2Robot, RobotMode # 全局变量存储机器人IP ROBOT_IP = None def get_robot_ip(): """获取用户输入的机器人IP地址""" global ROBOT_IP if ROBOT_IP is not None: return ROBOT_IP while True: try: ip = input("请输入机器人IP地址: ").strip() if not ip: print("❌ IP地址不能为空，请重新输入") continue # 简单的IP格式验证 parts = ip.split(".") if len(parts) == 4 and all(part.isdigit() and 0 <= int(part) <= 255 for part in parts): ROBOT_IP = ip return ip else: print("❌ IP地址格式不正确，请重新输入") except KeyboardInterrupt: print("\n程序被用户中断") return None except Exception as e: print(f"❌ 输入错误: {e}") def basic_usage_example(): """基本使用示例""" logger.info("=== MMK2 SDK 基本使用示例 ===") # 获取机器人IP robot_ip = get_robot_ip() if not robot_ip: return False # 初始化机器人 robot = MMK2Robot(ip=robot_ip, mode=RobotMode.REAL) # 检查连接 if not robot.is_connected(): logger.error("机器人连接失败") return False logger.info("✅ 机器人连接成功") print("-" * 50) joint_states = robot.get_joint_states() if not joint_states: print("无法获取机器人状态") return print("MMK2 Robot Status (机器人状态):") print(f"Mode: {robot._mode.value}") print(f"IP: {robot._ip}") print(f"Port: {robot._port}") print(f"Connected: {robot.is_connected()}") print() print("Joint States positions (关节位置):") print(f"left_arm : {joint_states.q[0:6]}") print(f"left_arm_eef : {joint_states.q[6:7]}") print(f"right_arm : {joint_states.q[7:13]}") print(f"right_arm_eef : {joint_states.q[13:14]}") print(f"spine : {joint_states.q[14:15]}") print(f"head : {joint_states.q[15:17]}") print(f"base : {joint_states.q[17:20]}") print() print("Joint States velocities (关节速度):") print(f"left_arm : {joint_states.dq[0:6]}") print(f"left_arm_eef : {joint_states.dq[6:7]}") print(f"right_arm : {joint_states.dq[7:13]}") print(f"right_arm_eef : {joint_states.dq[13:14]}") print(f"spine : {joint_states.dq[14:15]}") print(f"head : {joint_states.dq[15:17]}") print(f"base : {joint_states.dq[17:20]}") print() print("Joint States efforts (关节力矩):") print(f"left_arm : {joint_states.tau[0:6]}") print(f"left_arm_eef : {joint_states.tau[6:7]}") print(f"right_arm : {joint_states.tau[7:13]}") print(f"right_arm_eef : {joint_states.tau[13:14]}") print(f"spine : {joint_states.tau[14:15]}") print(f"head : {joint_states.tau[15:17]}") print(f"base : {joint_states.tau[17:20]}") print() # 获取末端位姿 left_pos, left_quat = robot.get_arm_pose("left") right_pos, right_quat = robot.get_arm_pose("right") print("ARM End Effector Poses (机械臂末端位姿):") print(f"left_arm_eef_pos : {left_pos}") print(f"left_arm_eef_quat : {left_quat}") print(f"right_arm_eef_pos : {right_pos}") print(f"right_arm_eef_quat : {right_quat}") print() return True def arm_control_example(): """机械臂控制示例""" logger.info("=== 机械臂控制示例 ===") # 获取机器人IP robot_ip = get_robot_ip() if not robot_ip: return False robot = MMK2Robot(ip=robot_ip, mode=RobotMode.REAL) if not robot.is_connected(): logger.error("机器人连接失败") return False # 1. 关节控制 logger.info("1. 关节控制示例") left_joints = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] success = robot.move_arm_joints("left", left_joints, block=True) logger.info(f"左臂关节控制: {'成功' if success else '失败'}") right_joints = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] success = robot.move_arm_joints("right", right_joints, block=True) logger.info(f"右臂关节控制: {'成功' if success else '失败'}") # 双臂关节控制 logger.info("双臂关节控制示例") left_joints = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] right_joints = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] success = robot.move_arm_joints("all", [left_joints, right_joints], block=True) logger.info(f"双臂关节控制: {'成功' if success else '失败'}") # 2. 位姿控制 logger.info("2. 位姿控制示例") # 获取当前位姿并稍微移动 current_pos, current_quat = robot.get_arm_pose("left") target_pose = list(current_pos) + list(current_quat) target_pose[0] += 0.05 # X方向移动5cm success = robot.move_arm_pose("left", target_pose, block=True) logger.info(f"左臂位姿控制: {'成功' if success else '失败'}") current_pos, current_quat = robot.get_arm_pose("right") target_pose = list(current_pos) + list(current_quat) target_pose[0] += 0.05 # X方向移动5cm success = robot.move_arm_pose("right", target_pose, block=True) logger.info(f"右臂位姿控制: {'成功' if success else '失败'}") # 双臂位姿控制 logger.info("双臂位姿控制示例") left_pos, left_quat = robot.get_arm_pose("left") left_target_pose = list(left_pos) + list(left_quat) left_target_pose[0] += 0.05 # X方向移动5cm right_pos, right_quat = robot.get_arm_pose("right") right_target_pose = list(right_pos) + list(right_quat) right_target_pose[0] += 0.05 # X方向移动5cm success = robot.move_arm_pose("all", [left_target_pose, right_target_pose], block=True) logger.info(f"双臂位姿控制: {'成功' if success else '失败'}") # 3. 夹爪控制 logger.info("3. 夹爪控制示例") success = robot.control_gripper("left", 0.5, block=True) logger.info(f"左夹爪控制: {'成功' if success else '失败'}") success = robot.control_gripper("right", 0.5, block=True) logger.info(f"右夹爪控制: {'成功' if success else '失败'}") # 双臂夹爪控制 logger.info("双臂夹爪控制示例") success = robot.control_gripper("all", 0.5, block=True) logger.info(f"双臂夹爪控制: {'成功' if success else '失败'}") # 4. 相对运动 logger.info("4. 相对运动示例") delta_pose = [0.02, 0.0, 0.0, 0.0, 0.0, 0.0] # X方向移动2cm success = robot.move_arm_relative("left", delta_pose, block=True) logger.info(f"左臂相对运动: {'成功' if success else '失败'}") delta_pose = [0.02, 0.0, 0.0, 0.0, 0.0, 0.0] # X方向移动2cm success = robot.move_arm_relative("right", delta_pose, block=True) logger.info(f"右臂相对运动: {'成功' if success else '失败'}") # 双臂相对运动（通过位姿控制实现） logger.info("双臂相对运动示例") left_pos, left_quat = robot.get_arm_pose("left") left_target_pose = list(left_pos) + list(left_quat) left_target_pose[0] += 0.02 # X方向移动2cm right_pos, right_quat = robot.get_arm_pose("right") right_target_pose = list(right_pos) + list(right_quat) right_target_pose[0] += 0.02 # X方向移动2cm success = robot.move_arm_pose("all", [left_target_pose, right_target_pose], block=True) logger.info(f"双臂相对运动: {'成功' if success else '失败'}") # 重置到起始姿态 robot.reset_to_start("all") logger.info("双臂重置到起始姿态") # 重置到零点姿态 robot.reset_to_zero("all") logger.info("双臂重置到零点姿态") return True def base_control_example(): """底盘控制示例""" logger.info("=== 底盘控制示例 ===") # 获取机器人IP robot_ip = get_robot_ip() if not robot_ip: return False robot = MMK2Robot(ip=robot_ip, mode=RobotMode.REAL) if not robot.is_connected(): logger.error("机器人连接失败") return False # 获取当前底盘位姿 current_pose = robot.get_base_pose() logger.info(f"当前底盘位姿: {current_pose}") # 1. 前进后退 logger.info("1. 前进后退示例") success = robot.move_forward(0.1, block=True) logger.info(f"前进0.1m: {'成功' if success else '失败'}") success = robot.move_backward(0.1, block=True) logger.info(f"后退0.1m: {'成功' if success else '失败'}") # 2. 转向 logger.info("2. 转向示例") success = robot.turn_left(np.pi / 4, block=True) logger.info(f"左转45度: {'成功' if success else '失败'}") success = robot.turn_right(np.pi / 4, block=True) logger.info(f"右转45度: {'成功' if success else '失败'}") # 3. 侧向移动 logger.info("3. 侧向移动示例") success = robot.lateral_move(0.05, block=True) logger.info(f"右移0.05m: {'成功' if success else '失败'}") success = robot.lateral_move(-0.05, block=True) logger.info(f"左移0.05m: {'成功' if success else '失败'}") # 4. 移动到指定位姿 logger.info("4. 移动到指定位姿示例") target_x = current_pose[0] + 0.1 target_y = current_pose[1] + 0.1 target_theta = current_pose[2] + 0.1 success = robot.move_base(target_x, target_y, target_theta, block=True) logger.info(f"移动到({target_x:.2f}, {target_y:.2f}, {target_theta:.2f}): {'成功' if success else '失败'}") return True def select_rgb_profile() -> str: """选择RGB分辨率配置，返回形如 "W,H,FPS" 的字符串。""" options = { "1": ("1280x720 @ 30fps", "1280,720,30"), "2": ("1280x720 @ 15fps", "1280,720,15"), "3": ("640x480 @ 30fps", "640,480,30"), "4": ("640x480 @ 15fps", "640,480,15"), } print("\n请选择相机RGB输出分辨率:") for k, (label, _) in options.items(): print(f"{k}. {label}") while True: sel = input("请输入序号 (1-4，默认1): ").strip() or "1" if sel in options: return options[sel][1] print("❌ 无效选择，请输入 1-4 之间的数字") def select_depth_profile() -> str: """选择深度分辨率配置，返回形如 "W,H,FPS" 的字符串。""" options = { "1": ("640x480 @ 30fps", "640,480,30"), "2": ("640x480 @ 15fps", "640,480,15"), "3": ("424x240 @ 30fps", "424,240,30"), "4": ("424x240 @ 15fps", "424,240,15"), } print("\n请选择相机深度输出分辨率:") for k, (label, _) in options.items(): print(f"{k}. {label}") while True: sel = input("请输入序号 (1-4，默认1): ").strip() or "1" if sel in options: return options[sel][1] print("❌ 无效选择，请输入 1-4 之间的数字") def select_camera_type(camera_name: str) -> str: """选择相机类型，返回 "REALSENSE" 或 "USB"。""" print(f"\n请选择{camera_name}的相机类型:") print("1. REALSENSE") print("2. USB") while True: sel = input("请输入序号 (1-2，默认1): ").strip() or "1" if sel == "1": return "REALSENSE" elif sel == "2": return "USB" print("❌ 无效选择，请输入 1 或 2") def get_camera_sn(camera_name: str) -> str: """获取用户输入的相机SN号""" while True: try: sn = input(f"请输入{camera_name}的SN号 (提示：香橙派中docker内输入 rs-enumerate-devices | grep Serial 查看): ").strip() if not sn: print("❌ SN号不能为空，请重新输入") continue return sn except KeyboardInterrupt: print("\n程序被用户中断") return None except Exception as e: print(f"❌ 输入错误: {e}") continue def ask_yes_no(question: str) -> bool: """询问用户是否选择""" while True: try: answer = input(f"{question} (y/n): ").strip().lower() if answer in ["y", "yes", "是"]: return True elif answer in ["n", "no", "否"]: return False else: print("❌ 请输入 y/n 或 是/否") except KeyboardInterrupt: print("\n程序被用户中断") return False except Exception as e: print(f"❌ 输入错误: {e}") def camera_example(): logger.info("=== 相机使用示例（带可视化） ===") # 获取机器人IP robot_ip = get_robot_ip() if not robot_ip: return False robot = MMK2Robot(ip=robot_ip) if not robot.is_connected(): logger.error("机器人连接失败") return False # 交互式配置相机参数 logger.info("开始配置相机参数...") camera_config = {} cameras_to_start = [] depth_enabled = {} rgb_profile = select_rgb_profile() depth_profile = select_depth_profile() # 询问头部相机 if ask_yes_no("是否要启动头部相机 (head_camera)？"): head_sn = get_camera_sn("头部相机") if head_sn: # 询问是否启用深度 enable_depth = ask_yes_no("是否启用头部相机的深度功能？") depth_enabled["head_camera"] = enable_depth camera_config["head_camera"] = { "camera_type": "REALSENSE", "serial_no": f"'{head_sn}'", "rgb_camera.color_profile": rgb_profile, "enable_depth": "true" if enable_depth else "false", "depth_module.depth_profile": depth_profile, "align_depth.enable": "true" if enable_depth else "false", } cameras_to_start.append("head_camera") logger.info(f"✅ 头部相机配置完成 (深度: {'启用' if enable_depth else '禁用'})") # 询问左臂相机 if ask_yes_no("是否要启动左臂相机 (left_camera)？"): # 选择相机类型 left_camera_type = select_camera_type("左臂相机") if left_camera_type == "REALSENSE": # REALSENSE 相机需要输入 SN 和配置深度 left_sn = get_camera_sn("左臂相机") if left_sn: # 询问是否启用深度 enable_depth = ask_yes_no("是否启用左臂相机的深度功能？") depth_enabled["left_camera"] = enable_depth camera_config["left_camera"] = { "camera_type": "REALSENSE", "serial_no": f"'{left_sn}'", "rgb_camera.color_profile": rgb_profile, "enable_depth": "true" if enable_depth else "false", "align_depth.enable": "true" if enable_depth else "false", "depth_module.depth_profile": depth_profile, } cameras_to_start.append("left_camera") logger.info(f"✅ 左臂相机配置完成 (REALSENSE, 深度: {'启用' if enable_depth else '禁用'})") elif left_camera_type == "USB": # USB 相机使用固定配置，无需用户输入 camera_config["left_camera"] = { "camera_type": "USB", "video_device": "/dev/left_camera", "image_width": "640", "image_height": "480", "framerate": "25", } depth_enabled["left_camera"] = False # USB 相机不支持深度 cameras_to_start.append("left_camera") logger.info("✅ 左臂相机配置完成 (USB, 分辨率: 640x480@25fps)") # 询问右臂相机 if ask_yes_no("是否要启动右臂相机 (right_camera)？"): # 选择相机类型 right_camera_type = select_camera_type("右臂相机") if right_camera_type == "REALSENSE": # REALSENSE 相机需要输入 SN 和配置深度 right_sn = get_camera_sn("右臂相机") if right_sn: # 询问是否启用深度 enable_depth = ask_yes_no("是否启用右臂相机的深度功能？") depth_enabled["right_camera"] = enable_depth camera_config["right_camera"] = { "camera_type": "REALSENSE", "serial_no": f"'{right_sn}'", "rgb_camera.color_profile": rgb_profile, "enable_depth": "true" if enable_depth else "false", "align_depth.enable": "true" if enable_depth else "false", "depth_module.depth_profile": depth_profile, } cameras_to_start.append("right_camera") logger.info(f"✅ 右臂相机配置完成 (REALSENSE, 深度: {'启用' if enable_depth else '禁用'})") elif right_camera_type == "USB": # USB 相机使用固定配置，无需用户输入 camera_config["right_camera"] = { "camera_type": "USB", "video_device": "/dev/right_camera", "image_width": "640", "image_height": "480", "framerate": "25", } depth_enabled["right_camera"] = False # USB 相机不支持深度 cameras_to_start.append("right_camera") logger.info("✅ 右臂相机配置完成 (USB, 分辨率: 640x480@25fps)") # 检查是否至少配置了一个相机 if not camera_config: logger.warning("没有配置任何相机，退出示例") return False # 设置相机配置 robot.camera.set_camera_config(camera_config) logger.info(f"相机配置已设置，将启动: {cameras_to_start}") logger.info("开始相机流，按 'q' 键退出...") try: # 启动相机流 robot.camera.start_stream(cameras_to_start) # 初始化帧计数和FPS计算 frame_counts = {"head_camera": 0, "left_camera": 0, "right_camera": 0} last_times = {"head_camera": 0.0, "left_camera": 0.0, "right_camera": 0.0} start_times = {"head_camera": None, "left_camera": None, "right_camera": None} fps_values = {"head_camera": 0.0, "left_camera": 0.0, "right_camera": 0.0} avg_fps_values = {"head_camera": 0.0, "left_camera": 0.0, "right_camera": 0.0} while True: current_time = time.time() # 获取头部相机帧（如果配置了） if "head_camera" in cameras_to_start: frame = robot.camera.get_head_camera_frame() if frame: frame_counts["head_camera"] += 1 # 初始化开始时间（第一帧） if start_times["head_camera"] is None: start_times["head_camera"] = current_time # 计算实时FPS（相邻两帧的时间差） if last_times["head_camera"] > 0: time_diff = current_time - last_times["head_camera"] if time_diff > 0: fps_values["head_camera"] = 1.0 / time_diff # 计算平均FPS（累计帧数 / 累计时间） elapsed_time = current_time - start_times["head_camera"] if elapsed_time > 0: avg_fps_values["head_camera"] = frame_counts["head_camera"] / elapsed_time last_times["head_camera"] = current_time # 显示RGB图像 rgb_image = frame.get("rgb") if rgb_image is not None: # 在图像上绘制帧计数和FPS display_image = rgb_image.copy() frame_count = frame_counts["head_camera"] fps = fps_values["head_camera"] avg_fps = avg_fps_values["head_camera"] # 设置文本属性 font = cv2.FONT_HERSHEY_SIMPLEX font_scale = 0.7 color = (0, 0, 255) # 红色 thickness = 2 # 绘制帧计数、实时FPS和平均FPS info_text = f"Frame: {frame_count} | FPS: {fps:.1f} | Avg FPS: {avg_fps:.1f}" cv2.putText(display_image, info_text, (10, 30), font, font_scale, color, thickness) cv2.imshow("Head Camera RGB", display_image) if frame_counts["head_camera"] % 30 == 0: # 每30帧打印一次信息 logger.info(f"头部相机RGB帧: Frame: {frame_count} | FPS: {fps:.1f} | Avg FPS: {avg_fps:.1f}") # 显示深度图像（如果启用了深度功能） if depth_enabled.get("head_camera", False): depth_image = frame.get("depth") if depth_image is not None: # 将深度图像转换为可显示格式 depth_display = cv2.applyColorMap( cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET ) # 在深度图像上绘制帧计数和FPS frame_count = frame_counts["head_camera"] fps = fps_values["head_camera"] avg_fps = avg_fps_values["head_camera"] # 设置文本属性 font = cv2.FONT_HERSHEY_SIMPLEX font_scale = 0.7 color = (0, 0, 255) # 红色 thickness = 2 # 绘制帧计数、实时FPS和平均FPS info_text = f"Frame: {frame_count} | FPS: {fps:.1f} | Avg FPS: {avg_fps:.1f}" cv2.putText(depth_display, info_text, (10, 30), font, font_scale, color, thickness) cv2.imshow("Head Camera Depth", depth_display) if frame_counts["head_camera"] % 30 == 0: # 每30帧打印一次信息 logger.info(f"头部相机深度帧: Frame: {frame_count} | FPS: {fps:.1f} | Avg FPS: {avg_fps:.1f}") # 获取左臂相机帧（如果配置了） if "left_camera" in cameras_to_start: left_frame = robot.camera.get_left_camera_frame() if left_frame: frame_counts["left_camera"] += 1 # 初始化开始时间（第一帧） if start_times["left_camera"] is None: start_times["left_camera"] = current_time # 计算实时FPS（相邻两帧的时间差） if last_times["left_camera"] > 0: time_diff = current_time - last_times["left_camera"] if time_diff > 0: fps_values["left_camera"] = 1.0 / time_diff # 计算平均FPS（累计帧数 / 累计时间） elapsed_time = current_time - start_times["left_camera"] if elapsed_time > 0: avg_fps_values["left_camera"] = frame_counts["left_camera"] / elapsed_time last_times["left_camera"] = current_time # 显示RGB图像 left_rgb = left_frame.get("rgb") if left_rgb is not None: # 在图像上绘制帧计数和FPS display_image = left_rgb.copy() frame_count = frame_counts["left_camera"] fps = fps_values["left_camera"] avg_fps = avg_fps_values["left_camera"] # 设置文本属性 font = cv2.FONT_HERSHEY_SIMPLEX font_scale = 0.7 color = (0, 0, 255) # 红色 thickness = 2 # 绘制帧计数、实时FPS和平均FPS info_text = f"Frame: {frame_count} | FPS: {fps:.1f} | Avg FPS: {avg_fps:.1f}" cv2.putText(display_image, info_text, (10, 30), font, font_scale, color, thickness) cv2.imshow("Left Camera RGB", display_image) if frame_counts["left_camera"] % 30 == 0: # 每30帧打印一次信息 logger.info(f"左臂相机RGB帧: Frame: {frame_count} | FPS: {fps:.1f} | Avg FPS: {avg_fps:.1f}") # 显示深度图像（如果启用了深度功能） if depth_enabled.get("left_camera", False): left_depth = left_frame.get("depth") if left_depth is not None: depth_display = cv2.applyColorMap( cv2.convertScaleAbs(left_depth, alpha=0.03), cv2.COLORMAP_JET ) # 在深度图像上绘制帧计数和FPS frame_count = frame_counts["left_camera"] fps = fps_values["left_camera"] avg_fps = avg_fps_values["left_camera"] # 设置文本属性 font = cv2.FONT_HERSHEY_SIMPLEX font_scale = 0.7 color = (0, 0, 255) # 红色 thickness = 2 # 绘制帧计数、实时FPS和平均FPS info_text = f"Frame: {frame_count} | FPS: {fps:.1f} | Avg FPS: {avg_fps:.1f}" cv2.putText(depth_display, info_text, (10, 30), font, font_scale, color, thickness) cv2.imshow("Left Camera Depth", depth_display) if frame_counts["left_camera"] % 30 == 0: # 每30帧打印一次信息 logger.info(f"左臂相机深度帧: Frame: {frame_count} | FPS: {fps:.1f} | Avg FPS: {avg_fps:.1f}") # 获取右臂相机帧（如果配置了） if "right_camera" in cameras_to_start: right_frame = robot.camera.get_right_camera_frame() if right_frame: frame_counts["right_camera"] += 1 # 初始化开始时间（第一帧） if start_times["right_camera"] is None: start_times["right_camera"] = current_time # 计算实时FPS（相邻两帧的时间差） if last_times["right_camera"] > 0: time_diff = current_time - last_times["right_camera"] if time_diff > 0: fps_values["right_camera"] = 1.0 / time_diff # 计算平均FPS（累计帧数 / 累计时间） elapsed_time = current_time - start_times["right_camera"] if elapsed_time > 0: avg_fps_values["right_camera"] = frame_counts["right_camera"] / elapsed_time last_times["right_camera"] = current_time # 显示RGB图像 right_rgb = right_frame.get("rgb") if right_rgb is not None: # 在图像上绘制帧计数和FPS display_image = right_rgb.copy() frame_count = frame_counts["right_camera"] fps = fps_values["right_camera"] avg_fps = avg_fps_values["right_camera"] # 设置文本属性 font = cv2.FONT_HERSHEY_SIMPLEX font_scale = 0.7 color = (0, 0, 255) # 红色 thickness = 2 # 绘制帧计数、实时FPS和平均FPS info_text = f"Frame: {frame_count} | FPS: {fps:.1f} | Avg FPS: {avg_fps:.1f}" cv2.putText(display_image, info_text, (10, 30), font, font_scale, color, thickness) cv2.imshow("Right Camera RGB", display_image) if frame_counts["right_camera"] % 30 == 0: # 每30帧打印一次信息 logger.info(f"右臂相机RGB帧: Frame: {frame_count} | FPS: {fps:.1f} | Avg FPS: {avg_fps:.1f}") # 显示深度图像（如果启用了深度功能） if depth_enabled.get("right_camera", False): right_depth = right_frame.get("depth") if right_depth is not None: depth_display = cv2.applyColorMap( cv2.convertScaleAbs(right_depth, alpha=0.03), cv2.COLORMAP_JET ) # 在深度图像上绘制帧计数和FPS frame_count = frame_counts["right_camera"] fps = fps_values["right_camera"] avg_fps = avg_fps_values["right_camera"] # 设置文本属性 font = cv2.FONT_HERSHEY_SIMPLEX font_scale = 0.7 color = (0, 0, 255) # 红色 thickness = 2 # 绘制帧计数、实时FPS和平均FPS info_text = f"Frame: {frame_count} | FPS: {fps:.1f} | Avg FPS: {avg_fps:.1f}" cv2.putText(depth_display, info_text, (10, 30), font, font_scale, color, thickness) cv2.imshow("Right Camera Depth", depth_display) if frame_counts["right_camera"] % 30 == 0: # 每30帧打印一次信息 logger.info(f"右臂相机深度帧: Frame: {frame_count} | FPS: {fps:.1f} | Avg FPS: {avg_fps:.1f}") # 检查退出条件 if cv2.waitKey(1) & 0xFF == ord("q"): break except KeyboardInterrupt: logger.info("用户中断相机流") except Exception as e: logger.error(f"相机示例异常: {e}") return False finally: cv2.destroyAllWindows() robot.camera.stop_stream() logger.info("相机流已停止") return True def head_control_example(): """头部控制示例""" logger.info("=== 头部控制示例 ===") # 头部角度范围定义 HEAD_YAW_MIN = -0.5 # 头部偏航最小值 HEAD_YAW_MAX = 0.5 # 头部偏航最大值 HEAD_PITCH_MIN = -1.18 # 头部俯仰最小值，往下低头 HEAD_PITCH_MAX = 0.16 # 头部俯仰最大值，往上抬头 # 获取机器人IP robot_ip = get_robot_ip() if not robot_ip: return False robot = MMK2Robot(ip=robot_ip, mode=RobotMode.REAL) if not robot.is_connected(): logger.error("机器人连接失败") return False logger.info(f"头部角度范围: 偏航[{HEAD_YAW_MIN:.2f}, {HEAD_YAW_MAX:.2f}], 俯仰[{HEAD_PITCH_MIN:.2f}, {HEAD_PITCH_MAX:.2f}]") # 1. 头部左右转动 logger.info("1. 头部左右转动示例") success = robot.set_head(0.1, 0.0, block=True) # 右转 logger.info(f"头部右转 (0.1, 0.0): {'成功' if success else '失败'}") success = robot.set_head(-0.1, 0.0, block=True) # 左转 logger.info(f"头部左转 (-0.1, 0.0): {'成功' if success else '失败'}") # 2. 头部上下俯仰 logger.info("2. 头部上下俯仰示例") success = robot.set_head(0.0, 0.1, block=True) # 向上抬头 logger.info(f"头部上仰 (0.0, 0.1): {'成功' if success else '失败'}") success = robot.set_head(0.0, -0.1, block=True) # 向下低头 logger.info(f"头部下俯 (0.0, -0.1): {'成功' if success else '失败'}") # 3. 极限位置测试 logger.info("3. 极限位置测试") success = robot.set_head(HEAD_YAW_MAX, 0.0, block=True) # 最大右转 logger.info(f"最大右转 ({HEAD_YAW_MAX}, 0.0): {'成功' if success else '失败'}") success = robot.set_head(HEAD_YAW_MIN, 0.0, block=True) # 最大左转 logger.info(f"最大左转 ({HEAD_YAW_MIN}, 0.0): {'成功' if success else '失败'}") success = robot.set_head(0.0, HEAD_PITCH_MAX, block=True) # 最大上仰 logger.info(f"最大上仰 (0.0, {HEAD_PITCH_MAX}): {'成功' if success else '失败'}") success = robot.set_head(0.0, HEAD_PITCH_MIN, block=True) # 最大下俯 logger.info(f"最大下俯 (0.0, {HEAD_PITCH_MIN}): {'成功' if success else '失败'}") # 4. 复合运动 logger.info("4. 复合运动示例") success = robot.set_head(0.3, 0.1, block=True) # 右转+上仰 logger.info(f"头部复合运动 (0.3, 0.1): {'成功' if success else '失败'}") # 5. 回到零位 success = robot.set_head(0.0, 0.0, block=True) logger.info(f"头部回到零位 (0.0, 0.0): {'成功' if success else '失败'}") return True def spine_control_example(): """升降控制示例""" logger.info("=== 升降控制示例 ===") # 升降台高度范围定义 SPINE_HEIGHT_MIN = -0.04 # 升降最小值，给负的时候在实际上是往上走 SPINE_HEIGHT_MAX = 0.87 # 升降最大值，给正的实际上是往下走 # 获取机器人IP robot_ip = get_robot_ip() if not robot_ip: return False robot = MMK2Robot(ip=robot_ip, mode=RobotMode.REAL) if not robot.is_connected(): logger.error("机器人连接失败") return False logger.info(f"升降台高度范围: [{SPINE_HEIGHT_MIN:.3f}, {SPINE_HEIGHT_MAX:.3f}]") logger.info("注意：负值实际上是往上走，正值实际上是往下走") # 获取当前升降高度 current_spine = robot.get_joint_states() if current_spine and len(current_spine.q) > 16: current_height = current_spine.q[16] # 脊柱关节位置 logger.info(f"当前升降高度: {current_height:.3f}m") else: current_height = 0.0 logger.info("无法获取当前升降高度，使用默认值") # 1. 向上运动（负值） logger.info("1. 向上运动 1cm 示例（输入 -0.01）") target_height = max(current_height - 0.01, SPINE_HEIGHT_MIN) success = robot.set_spine(target_height, block=True) logger.info(f"向上运动到 {target_height:.3f}m: {'成功' if success else '失败'}") # 2. 向下运动（正值） logger.info("2. 向下运动 10cm 示例（输入 0.1）") target_height = min(current_height + 0.1, SPINE_HEIGHT_MAX) success = robot.set_spine(target_height, block=True) logger.info(f"向下运动到 {target_height:.3f}m: {'成功' if success else '失败'}") # 3. 极限位置测试 logger.info("3. 极限位置测试") success = robot.set_spine(SPINE_HEIGHT_MIN, block=True) # 最高位置 logger.info(f"最高位置 {SPINE_HEIGHT_MIN:.3f}m: {'成功' if success else '失败'}") success = robot.set_spine(SPINE_HEIGHT_MAX, block=True) # 最低位置 logger.info(f"最低位置 {SPINE_HEIGHT_MAX:.3f}m: {'成功' if success else '失败'}") # 4. 连续升降运动 logger.info("4. 连续升降运动示例") heights = [0.2, 0.6, 0.4, 0.1] # 在有效范围内的测试高度 for i, height in enumerate(heights): success = robot.set_spine(height, block=True) logger.info(f"第{i+1}次升降到 {height:.3f}m: {'成功' if success else '失败'}") time.sleep(1) # 等待1秒 # 5. 回到零位 success = robot.set_spine(0.0, block=True) logger.info(f"回到零位 0.0m: {'成功' if success else '失败'}") return True def tf_example(): """TF获取示例""" logger.info("=== TF获取示例 ===") # 获取机器人IP robot_ip = get_robot_ip() if not robot_ip: return False robot = MMK2Robot(ip=robot_ip, mode=RobotMode.REAL) if not robot.is_connected(): logger.error("机器人连接失败") return False try: # 定义要获取的TF关系 tf_frames = [ ("base_link", "head_camera_link"), # 从base_link到head_camera_link的变换 ] logger.info("正在获取TF信息...") # 发送TF请求 response = robot._client.get_tf(tf_frames) logger.info(f"成功获取TF信息，包含 {len(response)} 个变换:") # 打印所有TF变换 for i, pose in enumerate(response): logger.info(f"变换 {i+1}:") logger.info(f" 目标坐标系: {tf_frames[i][0]}") logger.info(f" 参考坐标系: {tf_frames[i][1]}") # 位置信息 position = pose.position logger.info(f" 位置: x={position.x:.4f}, y={position.y:.4f}, z={position.z:.4f}") # 旋转信息（四元数） orientation = pose.orientation logger.info( f" 旋转: x={orientation.x:.4f}, y={orientation.y:.4f}, z={orientation.z:.4f}, w={orientation.w:.4f}" ) logger.info("") return True except Exception as e: logger.error(f"获取TF信息时发生错误: {e}") return False def error_handling_example(): """错误处理示例""" logger.info("=== 错误处理示例 ===") # 获取机器人IP robot_ip = get_robot_ip() if not robot_ip: return False robot = MMK2Robot(ip=robot_ip, mode=RobotMode.REAL) if not robot.is_connected(): logger.error("机器人连接失败") return False # 测试无效参数 logger.info("测试无效参数...") success = robot.move_arm_joints("invalid_arm", [0, 0, 0, 0, 0, 0]) logger.info(f"无效机械臂参数: {'成功' if success else '失败'}") # 检查错误信息 error = robot.get_last_error() if error: logger.info(f"捕获到错误: {error}") # 清除错误 robot.clear_errors() logger.info("错误已清除") # 检查错误状态 has_errors = robot.has_errors() logger.info(f"是否有错误: {has_errors}") # 获取错误摘要 error_summary = robot.get_error_summary() logger.info(f"错误摘要: {error_summary}") return True def status_checking_example(): """状态检查示例""" logger.info("=== 状态检查示例 ===") # 获取机器人IP robot_ip = get_robot_ip() if not robot_ip: return False robot = MMK2Robot(ip=robot_ip, mode=RobotMode.REAL) if not robot.is_connected(): logger.error("机器人连接失败") return False # 控制机械臂 target_joints = [0.0, 0.0, 0.324, 0.0, 0.724, 0.0] robot.move_arm_joints("left", target_joints, block=False) # 检查关节状态 is_ok, max_error = robot.check_arm_joints("left", target_joints) logger.info(f"关节状态检查: 到位={is_ok}, 最大误差={max_error:.3f}") # 控制位姿 current_pos, current_quat = robot.get_arm_pose("left") target_pose = list(current_pos) + list(current_quat) target_pose[0] += 0.05 robot.move_arm_pose("left", target_pose, block=False) # 检查位姿状态 is_ok, max_error = robot.check_arm_pose("left", target_pose) logger.info(f"位姿状态检查: 到位={is_ok}, 最大误差={max_error:.3f}") # 控制底盘 current_base = robot.get_base_pose() target_base = [current_base[0] + 0.1, current_base[1], current_base[2]] robot.move_base(target_base[0], target_base[1], target_base[2], block=False) # 检查底盘状态 is_ok, max_error = robot.check_base_pose(target_base) logger.info(f"底盘状态检查: 到位={is_ok}, 最大误差={max_error:.3f}") # 控制脊柱 current_spine_state = robot.get_joint_states() if current_spine_state and len(current_spine_state.q) > 14: current_spine = current_spine_state.q[14] target_spine = current_spine + 0.1 else: target_spine = 0.1 robot.set_spine(target_spine, block=False) time.sleep(1) # 检查脊柱状态 is_ok, max_error = robot.check_spine_joints(target_spine) logger.info(f"脊柱状态检查: 到位={is_ok}, 最大误差={max_error:.3f}") return True def waypoints_control_example(): """轨迹点控制示例""" logger.info("=== 轨迹点控制示例 ===") # 获取机器人IP robot_ip = get_robot_ip() if not robot_ip: return False robot = MMK2Robot(ip=robot_ip, mode=RobotMode.REAL) if not robot.is_connected(): logger.error("机器人连接失败") return False # 1. 关节轨迹控制示例 logger.info("1. 关节轨迹控制示例") robot.reset_to_start("all") # 基于当前位置生成轨迹点 left_arm_joints = [ [0.0, 0.0, 0.324, 0.0, 0.724, 0.0], # 起始位置 [0.0, 0.0, 0.324, 0.0, 0.724, 0.5], # 中间位置1 [0.1, 0.1, 0.324, 0.0, 0.724, 0.3], # 中间位置2 [0.0, 0.0, 0.324, 0.0, 0.724, 0.0], # 回到起始位置 ] right_arm_joints = [ [0.0, 0.0, 0.324, 0.0, -0.724, 0.0], # 起始位置 [0.0, 0.0, 0.324, 0.0, -0.724, -0.5], # 中间位置1 [-0.1, -0.1, 0.324, 0.0, -0.724, -0.3], # 中间位置2 [0.0, 0.0, 0.324, 0.0, -0.724, 0.0], # 回到起始位置 ] # 测试左臂关节轨迹 success = robot.move_arm_joint_waypoints("left", left_arm_joints, block=True) logger.info(f"左臂关节轨迹控制: {'成功' if success else '失败'}") # 测试右臂关节轨迹 success = robot.move_arm_joint_waypoints("right", right_arm_joints, block=True) logger.info(f"右臂关节轨迹控制: {'成功' if success else '失败'}") # 2. 位姿轨迹控制示例 logger.info("2. 位姿轨迹控制示例") left_arm_poses = [ [0.5, 0.22, 1.13, -0.584, 0.394, 0.095, 0.700], # 起始位姿 [0.5, 0.27, 1.13, -0.584, 0.394, 0.095, 0.700], # 中间位姿1 [0.55, 0.25, 1.15, -0.584, 0.394, 0.095, 0.700], # 中间位姿2 [0.5, 0.22, 1.13, -0.584, 0.394, 0.095, 0.700], # 回到起始位姿 ] right_arm_poses = [ [0.5, -0.22, 1.13, 0.584, 0.394, -0.095, 0.700], # 起始位姿 [0.5, -0.27, 1.13, 0.584, 0.394, -0.095, 0.700], # 中间位姿1 [0.55, -0.25, 1.15, 0.584, 0.394, -0.095, 0.700], # 中间位姿2 [0.5, -0.22, 1.13, 0.584, 0.394, -0.095, 0.700], # 回到起始位姿 ] # 测试左臂位姿轨迹 success = robot.move_arm_pose_waypoints("left", left_arm_poses, block=True) logger.info(f"左臂位姿轨迹控制: {'成功' if success else '失败'}") # 测试右臂位姿轨迹 success = robot.move_arm_pose_waypoints("right", right_arm_poses, block=True) logger.info(f"右臂位姿轨迹控制: {'成功' if success else '失败'}") logger.info("3. 双臂协调轨迹示例") # 双臂同时执行不同的轨迹 left_coord_joints = [ [0.0, 0.0, 0.324, 0.0, 0.724, 0.0], [0.2, 0.0, 0.324, 0.0, 0.724, 0.0], [0.2, 0.2, 0.324, 0.0, 0.724, 0.0], [0.0, 0.2, 0.324, 0.0, 0.724, 0.0], [0.0, 0.0, 0.324, 0.0, 0.724, 0.0], ] right_coord_joints = [ [0.0, 0.0, 0.324, 0.0, -0.724, 0.0], [-0.2, 0.0, 0.324, 0.0, -0.724, 0.0], [-0.2, -0.2, 0.324, 0.0, -0.724, 0.0], [0.0, -0.2, 0.324, 0.0, -0.724, 0.0], [0.0, 0.0, 0.324, 0.0, -0.724, 0.0], ] # 分别执行双臂轨迹（依次执行） logger.info("3.1. 双臂依次执行轨迹示例") success_left = robot.move_arm_joint_waypoints("left", left_coord_joints, block=True) success_right = robot.move_arm_joint_waypoints("right", right_coord_joints, block=True) logger.info(f"双臂依次执行轨迹: 左臂={'成功' if success_left else '失败'}, 右臂={'成功' if success_right else '失败'}") # 双臂同时执行轨迹（使用双臂接口） logger.info("3.2. 双臂同时执行轨迹示例") success = robot.move_arm_joint_waypoints("all", [left_coord_joints, right_coord_joints], block=True) logger.info(f"双臂同时执行轨迹: {'成功' if success else '失败'}") # 双臂同时执行位姿轨迹 logger.info("3.3. 双臂同时执行位姿轨迹示例") success = robot.move_arm_pose_waypoints("all", [left_arm_poses, right_arm_poses], block=True) logger.info(f"双臂同时执行位姿轨迹: {'成功' if success else '失败'}") # 重置到起始姿态 robot.reset_to_start("all") logger.info("双臂重置到起始姿态") return True def show_menu(): """显示菜单""" print("\n" + "=" * 60) print("🤖 MMK2 SDK 使用示例") print("=" * 60) print("请选择要运行的示例:") print("1. 基本使用示例 - 连接测试、状态获取") print("2. 机械臂控制示例 - 关节控制、位姿控制、夹爪控制") print("3. 底盘控制示例 - 前进后退、转向、侧移") print("4. 头部控制示例 - 头部左右转动、上下俯仰") print("5. 升降控制示例 - 脊柱升降运动") print("6. 相机使用示例 - 图像获取和可视化显示") print("7. TF获取示例 - 坐标系变换信息") print("8. 错误处理示例 - 错误检测和处理") print("9. 状态检查示例 - 闭环控制验证") print("10. 连续轨迹点控制示例 - 关节轨迹、位姿轨迹") print("0. 退出程序") print("=" * 60) def run_example(choice): """运行选定的示例""" examples = { "1": ("基本使用示例", basic_usage_example), "2": ("机械臂控制示例", arm_control_example), "3": ("底盘控制示例", base_control_example), "4": ("头部控制示例", head_control_example), "5": ("升降控制示例", spine_control_example), "6": ("相机使用示例", camera_example), "7": ("TF获取示例", tf_example), "8": ("错误处理示例", error_handling_example), "9": ("状态检查示例", status_checking_example), "10": ("连续轨迹点控制示例", waypoints_control_example), } if choice not in examples: print("❌ 无效选择，请重新输入") return False name, example_func = examples[choice] print(f"\n🚀 开始运行: {name}") print("-" * 50) try: success = example_func() if success: print(f"✅ {name} 完成") else: print(f"❌ {name} 失败") return success except KeyboardInterrupt: print(f"\n⚠️ {name} 被用户中断") return False except Exception as e: print(f"❌ {name} 异常: {e}") return False def main(): print("欢迎使用 MMK2 SDK 示例程序!") while True: show_menu() try: choice = input("请输入选择 (0-10): ").strip() if choice == "0": print("👋 感谢使用，再见!") break elif choice in ["1", "2", "3", "4", "5", "6", "7", "8", "9", "10"]: success = run_example(choice) if not success: print("示例运行失败，请检查机器人连接和配置") input("\n按回车键继续...") else: print("❌ 无效选择，请输入 0-10 之间的数字") except KeyboardInterrupt: print("\n\n👋 程序被用户中断，再见!") break except Exception as e: print(f"❌ 程序异常: {e}") break if __name__ == "__main__": main()