From 0d81acc715188b1d01d6928524abc82885111828 Mon Sep 17 00:00:00 2001 From: wangadsa <1377468276@qq.com> Date: Tue, 23 Sep 2025 09:06:41 +0800 Subject: [PATCH 1/5] =?UTF-8?q?=E5=AE=8C=E6=88=90=E9=80=89=E9=A2=98?= =?UTF-8?q?=EF=BC=8C=E5=B9=B6=E8=BF=9B=E8=A1=8C=E9=A6=96=E6=AC=A1=E6=8F=90?= =?UTF-8?q?=E4=BA=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/drone/README.md | 1 + src/drone/main.py | 0 2 files changed, 1 insertion(+) create mode 100644 src/drone/README.md create mode 100644 src/drone/main.py diff --git a/src/drone/README.md b/src/drone/README.md new file mode 100644 index 0000000000..44ed492a75 --- /dev/null +++ b/src/drone/README.md @@ -0,0 +1 @@ +无人机 \ No newline at end of file diff --git a/src/drone/main.py b/src/drone/main.py new file mode 100644 index 0000000000..e69de29bb2 From 4652d014f443c8d3513a640c9aef791d08a2c09e Mon Sep 17 00:00:00 2001 From: wangadsa <1377468276@qq.com> Date: Mon, 24 Nov 2025 09:25:26 +0800 Subject: [PATCH 2/5] =?UTF-8?q?=E5=AE=8C=E6=88=90=E6=97=A0=E4=BA=BA?= =?UTF-8?q?=E6=9C=BA=E8=B7=9F=E9=9A=8F=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/drone/main.py | 107 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 107 insertions(+) diff --git a/src/drone/main.py b/src/drone/main.py index e69de29bb2..17ff59b983 100644 --- a/src/drone/main.py +++ b/src/drone/main.py @@ -0,0 +1,107 @@ +from djitellopy import Tello +import cv2 +import numpy as np +import time + +# 跟随参数 +FOLLOW_DISTANCE = 100 # 目标在画面中的期望宽度(像素) +FOLLOW_HEIGHT = 50 # 目标在画面中的期望高度(像素) +MAX_SPEED = 30 # 最大飞行速度(cm/s) + +# 初始化 Tello 无人机 +tello = Tello() +tello.connect() +print(f"电池电量: {tello.get_battery()}%") + +# 启动摄像头 +tello.streamon() +cap = cv2.VideoCapture("udp://@0.0.0.0:11111") # Tello 摄像头流地址 + + +# 目标检测(简化:用颜色识别,可替换为 YOLO 识别行人/车辆) +def detect_target(frame): + # 转换为 HSV 颜色空间(识别蓝色目标,可根据实际目标调整) + hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) + lower_blue = np.array([100, 120, 70]) + upper_blue = np.array([140, 255, 255]) + mask = cv2.inRange(hsv, lower_blue, upper_blue) + + # 查找轮廓 + contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + if contours: + # 取最大轮廓(假设最大的是目标) + max_contour = max(contours, key=cv2.contourArea) + x, y, w, h = cv2.boundingRect(max_contour) + return (x, y, w, h) # 目标坐标和尺寸 + return None + + +# 跟随控制逻辑 +def follow_target(target, frame): + h, w, _ = frame.shape + if target: + x, y, tw, th = target + # 计算目标中心 + target_center_x = x + tw // 2 + target_center_y = y + th // 2 + # 计算画面中心 + frame_center_x = w // 2 + frame_center_y = h // 2 + + # 水平方向控制(左右) + dx = target_center_x - frame_center_x + left_right_speed = -dx * 0.3 # 比例控制 + left_right_speed = np.clip(left_right_speed, -MAX_SPEED, MAX_SPEED) + + # 垂直方向控制(上下) + dy = target_center_y - frame_center_y + up_down_speed = dy * 0.3 + up_down_speed = np.clip(up_down_speed, -MAX_SPEED, MAX_SPEED) + + # 前后方向控制(距离) + distance_error = tw - FOLLOW_DISTANCE + forward_backward_speed = -distance_error * 0.2 + forward_backward_speed = np.clip(forward_backward_speed, -MAX_SPEED, MAX_SPEED) + + # 发布速度指令 + tello.send_rc_control( + int(left_right_speed), + int(forward_backward_speed), + int(up_down_speed), + 0 # 偏航速度(0 = 不旋转) + ) + + # 绘制目标框 + cv2.rectangle(frame, (x, y), (x + tw, y + th), (0, 255, 0), 2) + cv2.circle(frame, (target_center_x, target_center_y), 5, (0, 0, 255), -1) + else: + # 未检测到目标,悬停 + tello.send_rc_control(0, 0, 0, 0) + + +# 主循环 +tello.takeoff() # 起飞 +time.sleep(2) # 悬停2秒 +try: + while True: + ret, frame = cap.read() + if not ret: + break + # 检测目标 + target = detect_target(frame) + # 跟随控制 + follow_target(target, frame) + # 显示画面 + cv2.imshow("Tello Follow", frame) + # 按 'q' 退出 + if cv2.waitKey(1) & 0xFF == ord('q'): + break +except KeyboardInterrupt: + pass +finally: + # 降落并清理资源 + tello.land() + tello.streamoff() + cap.release() + cv2.destroyAllWindows() + tello.end() \ No newline at end of file From 8a268185af2f1c47089002d2109bb1cb7caae813 Mon Sep 17 00:00:00 2001 From: wangadsa <1377468276@qq.com> Date: Mon, 22 Dec 2025 08:29:08 +0800 Subject: [PATCH 3/5] =?UTF-8?q?=E9=87=8D=E6=96=B0=E9=80=89=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/{drone => Robot_arm_grasp}/README.md | 0 src/Robot_arm_grasp/main.py | 0 src/drone/main.py | 107 ----------------------- 3 files changed, 107 deletions(-) rename src/{drone => Robot_arm_grasp}/README.md (100%) create mode 100644 src/Robot_arm_grasp/main.py delete mode 100644 src/drone/main.py diff --git a/src/drone/README.md b/src/Robot_arm_grasp/README.md similarity index 100% rename from src/drone/README.md rename to src/Robot_arm_grasp/README.md diff --git a/src/Robot_arm_grasp/main.py b/src/Robot_arm_grasp/main.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/drone/main.py b/src/drone/main.py deleted file mode 100644 index 17ff59b983..0000000000 --- a/src/drone/main.py +++ /dev/null @@ -1,107 +0,0 @@ -from djitellopy import Tello -import cv2 -import numpy as np -import time - -# 跟随参数 -FOLLOW_DISTANCE = 100 # 目标在画面中的期望宽度(像素) -FOLLOW_HEIGHT = 50 # 目标在画面中的期望高度(像素) -MAX_SPEED = 30 # 最大飞行速度(cm/s) - -# 初始化 Tello 无人机 -tello = Tello() -tello.connect() -print(f"电池电量: {tello.get_battery()}%") - -# 启动摄像头 -tello.streamon() -cap = cv2.VideoCapture("udp://@0.0.0.0:11111") # Tello 摄像头流地址 - - -# 目标检测(简化:用颜色识别,可替换为 YOLO 识别行人/车辆) -def detect_target(frame): - # 转换为 HSV 颜色空间(识别蓝色目标,可根据实际目标调整) - hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) - lower_blue = np.array([100, 120, 70]) - upper_blue = np.array([140, 255, 255]) - mask = cv2.inRange(hsv, lower_blue, upper_blue) - - # 查找轮廓 - contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - if contours: - # 取最大轮廓(假设最大的是目标) - max_contour = max(contours, key=cv2.contourArea) - x, y, w, h = cv2.boundingRect(max_contour) - return (x, y, w, h) # 目标坐标和尺寸 - return None - - -# 跟随控制逻辑 -def follow_target(target, frame): - h, w, _ = frame.shape - if target: - x, y, tw, th = target - # 计算目标中心 - target_center_x = x + tw // 2 - target_center_y = y + th // 2 - # 计算画面中心 - frame_center_x = w // 2 - frame_center_y = h // 2 - - # 水平方向控制(左右) - dx = target_center_x - frame_center_x - left_right_speed = -dx * 0.3 # 比例控制 - left_right_speed = np.clip(left_right_speed, -MAX_SPEED, MAX_SPEED) - - # 垂直方向控制(上下) - dy = target_center_y - frame_center_y - up_down_speed = dy * 0.3 - up_down_speed = np.clip(up_down_speed, -MAX_SPEED, MAX_SPEED) - - # 前后方向控制(距离) - distance_error = tw - FOLLOW_DISTANCE - forward_backward_speed = -distance_error * 0.2 - forward_backward_speed = np.clip(forward_backward_speed, -MAX_SPEED, MAX_SPEED) - - # 发布速度指令 - tello.send_rc_control( - int(left_right_speed), - int(forward_backward_speed), - int(up_down_speed), - 0 # 偏航速度(0 = 不旋转) - ) - - # 绘制目标框 - cv2.rectangle(frame, (x, y), (x + tw, y + th), (0, 255, 0), 2) - cv2.circle(frame, (target_center_x, target_center_y), 5, (0, 0, 255), -1) - else: - # 未检测到目标,悬停 - tello.send_rc_control(0, 0, 0, 0) - - -# 主循环 -tello.takeoff() # 起飞 -time.sleep(2) # 悬停2秒 -try: - while True: - ret, frame = cap.read() - if not ret: - break - # 检测目标 - target = detect_target(frame) - # 跟随控制 - follow_target(target, frame) - # 显示画面 - cv2.imshow("Tello Follow", frame) - # 按 'q' 退出 - if cv2.waitKey(1) & 0xFF == ord('q'): - break -except KeyboardInterrupt: - pass -finally: - # 降落并清理资源 - tello.land() - tello.streamoff() - cap.release() - cv2.destroyAllWindows() - tello.end() \ No newline at end of file From 339a5749664072db7eb72461180409f82535d570 Mon Sep 17 00:00:00 2001 From: wangadsa <1377468276@qq.com> Date: Mon, 22 Dec 2025 09:36:43 +0800 Subject: [PATCH 4/5] =?UTF-8?q?=E6=9C=BA=E6=A2=B0=E8=87=82=E6=8A=93?= =?UTF-8?q?=E5=8F=96=E4=BB=BF=E7=9C=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Robot_arm_grasp/main.py | 299 ++++++++++++++++++++++++++++++++++++ 1 file changed, 299 insertions(+) diff --git a/src/Robot_arm_grasp/main.py b/src/Robot_arm_grasp/main.py index e69de29bb2..e7992b0a43 100644 --- a/src/Robot_arm_grasp/main.py +++ b/src/Robot_arm_grasp/main.py @@ -0,0 +1,299 @@ +import cv2 +import numpy as np +import pygame +import math +import threading +import time + +# ====================== 全局配置参数(可根据需求微调)===================== +# ---------------------- 视觉识别参数(真实摄像头)---------------------- +TARGET_COLOR_LOWER = np.array([0, 120, 70]) # 红色HSV下限(可适配自己的红色目标) +TARGET_COLOR_UPPER = np.array([10, 255, 255]) # 红色HSV上限 +TARGET_COLOR_LOWER2 = np.array([170, 120, 70]) # 红色补集(解决红色跨0度的问题) +TARGET_COLOR_UPPER2 = np.array([180, 255, 255]) +MIN_CONTOUR_AREA = 300 # 最小轮廓面积(过滤小噪声) +CAMERA_INDEX = 0 # 摄像头索引(0为默认,若没反应改1) +VISION_WIDTH = 640 # 摄像头画面宽度 +VISION_HEIGHT = 480 # 摄像头画面高度 + +# ---------------------- 仿真窗口与机械臂参数 ---------------------- +SIM_WIDTH = 800 # 仿真窗口宽度 +SIM_HEIGHT = 600 # 仿真窗口高度 +ARM_BASE_POS = (400, 500)# 机械臂基座在仿真窗口的位置 +# 机械臂各段长度(4段,可调整) +ARM_SEGMENT_LENGTHS = [100, 80, 60, 40] +GRIPPER_SIZE = 20 # 夹爪大小 +# 仿真目标物体初始位置(红色小球) +TARGET_OBJ_INIT_POS = (500, 300) +TARGET_OBJ_RADIUS = 15 + +# ---------------------- 全局变量(线程间通信)---------------------- +target_pixel_pos = None # 视觉识别的目标像素坐标 +grasp_trigger = False # 抓取触发标志 +is_camera_available = True # 摄像头是否可用 + +# ====================== 1. 视觉识别线程(独立运行,不阻塞仿真)===================== +def vision_recognition_thread(): + """处理真实摄像头的红色目标检测,输出目标像素坐标""" + global target_pixel_pos, is_camera_available + # 初始化摄像头 + cap = cv2.VideoCapture(CAMERA_INDEX) + cap.set(cv2.CAP_PROP_FRAME_WIDTH, VISION_WIDTH) + cap.set(cv2.CAP_PROP_FRAME_HEIGHT, VISION_HEIGHT) + + # 检查摄像头是否可用 + if not cap.isOpened(): + print(f"\n⚠️ 警告:无法打开摄像头(索引:{CAMERA_INDEX})") + print("💡 解决方案:1. 检查摄像头连接 2. 更换CAMERA_INDEX为1 3. 程序将使用模拟目标") + is_camera_available = False + # 模拟目标位置(摄像头画面中心) + while True: + target_pixel_pos = (VISION_WIDTH // 2, VISION_HEIGHT // 2) + time.sleep(0.05) + # 按q退出时终止线程 + if cv2.waitKey(1) & 0xFF == ord('q'): + break + return + + print("\n✅ 摄像头初始化成功!") + print("🎯 正在检测红色目标...") + + while True: + # 读取摄像头帧 + ret, frame = cap.read() + if not ret: + time.sleep(0.05) + continue + + # ---------------------- 目标检测核心逻辑 ---------------------- + # 1. 转HSV颜色空间(便于颜色筛选) + hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) + # 2. 生成红色掩码(合并两个区间,解决红色跨0度问题) + mask1 = cv2.inRange(hsv, TARGET_COLOR_LOWER, TARGET_COLOR_UPPER) + mask2 = cv2.inRange(hsv, TARGET_COLOR_LOWER2, TARGET_COLOR_UPPER2) + mask = cv2.bitwise_or(mask1, mask2) + # 3. 形态学操作(去噪声) + kernel = np.ones((5, 5), np.uint8) + mask = cv2.erode(mask, kernel, iterations=1) # 腐蚀 + mask = cv2.dilate(mask, kernel, iterations=1) # 膨胀 + # 4. 查找轮廓 + contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + + # 初始化目标位置 + target_pixel_pos = None + if contours: + # 取面积最大的轮廓(认为是目标) + max_contour = max(contours, key=cv2.contourArea) + if cv2.contourArea(max_contour) > MIN_CONTOUR_AREA: + # 计算轮廓中心 + M = cv2.moments(max_contour) + if M["m00"] != 0: + cX = int(M["m10"] / M["m00"]) + cY = int(M["m01"] / M["m00"]) + target_pixel_pos = (cX, cY) + # 绘制轮廓和中心标记 + cv2.drawContours(frame, [max_contour], -1, (0, 255, 0), 2) + cv2.circle(frame, (cX, cY), 5, (0, 0, 255), -1) + cv2.putText(frame, f"Target ({cX},{cY})", (cX-50, cY-10), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1) + + # ---------------------- 绘制提示信息 ---------------------- + if target_pixel_pos: + cv2.putText(frame, "✅ Target Found", (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) + else: + cv2.putText(frame, "❌ Target Lost", (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2) + + # 显示画面 + cv2.imshow("🤖 真实摄像头 - 红色目标检测", frame) + + # 按q键退出 + if cv2.waitKey(1) & 0xFF == ord('q'): + cap.release() + cv2.destroyAllWindows() + break + + # 释放资源 + cap.release() + cv2.destroyAllWindows() + +# ====================== 2. 机械臂仿真类(封装运动和绘制逻辑)===================== +class ArmSimulation: + def __init__(self, base_pos, segment_lengths): + """初始化机械臂""" + self.base_pos = np.array(base_pos, dtype=float) # 基座位置 + self.segment_lengths = segment_lengths # 各段长度 + self.num_segments = len(segment_lengths) # 段数(关节数) + self.joint_angles = [0.0 for _ in range(self.num_segments)] # 各关节角度(初始为0) + self.end_pos = self.calculate_end_position() # 末端执行器位置 + self.gripper_closed = False # 夹爪状态:False=张开,True=闭合 + + def calculate_end_position(self): + """正运动学:根据关节角度计算末端执行器位置""" + pos = self.base_pos.copy() + current_angle = 0.0 # 累计角度 + for i in range(self.num_segments): + current_angle += self.joint_angles[i] + # 计算当前段的位移(坐标系:y轴向下为正,需调整角度) + dx = self.segment_lengths[i] * math.cos(current_angle - math.pi/2) + dy = self.segment_lengths[i] * math.sin(current_angle - math.pi/2) + pos += np.array([dx, dy]) + return pos + + def move_to_target(self, target_pos, step=0.01): + """逆运动学简化版:移动末端执行器到目标位置""" + current_pos = self.calculate_end_position() + # 距离大于5像素时继续移动(精度阈值) + while np.linalg.norm(current_pos - target_pos) > 5: + # 计算方向向量(从当前位置到目标位置) + dir_vec = target_pos - current_pos + dir_vec = dir_vec / np.linalg.norm(dir_vec) # 归一化 + # 调整各关节角度(比例控制,简单有效) + for i in range(self.num_segments): + self.joint_angles[i] += dir_vec[0] * step - dir_vec[1] * step + # 更新末端位置 + current_pos = self.calculate_end_position() + # 模拟运动速度(暂停10ms) + pygame.time.wait(10) + self.end_pos = current_pos + + def close_gripper(self): + """闭合夹爪""" + self.gripper_closed = True + pygame.time.wait(300) # 模拟夹爪闭合时间 + + def open_gripper(self): + """张开夹爪""" + self.gripper_closed = False + pygame.time.wait(300) + + def draw(self, screen): + """在PyGame窗口中绘制机械臂""" + # 1. 绘制基座 + pygame.draw.circle(screen, (50, 50, 150), tuple(map(int, self.base_pos)), 15) + # 2. 绘制机械臂各段和关节 + pos = self.base_pos.copy() + current_angle = 0.0 + for i in range(self.num_segments): + current_angle += self.joint_angles[i] + # 计算当前段的终点位置 + dx = self.segment_lengths[i] * math.cos(current_angle - math.pi/2) + dy = self.segment_lengths[i] * math.sin(current_angle - math.pi/2) + new_pos = pos + np.array([dx, dy]) + # 绘制段(线条) + pygame.draw.line(screen, (150, 150, 150), tuple(map(int, pos)), tuple(map(int, new_pos)), 8) + # 绘制关节(小圆) + pygame.draw.circle(screen, (100, 100, 100), tuple(map(int, new_pos)), 8) + pos = new_pos + # 3. 绘制末端执行器(夹爪) + if self.gripper_closed: + # 闭合状态:三角形 + pygame.draw.polygon(screen, (200, 50, 50), [ + (pos[0]-GRIPPER_SIZE//2, pos[1]), + (pos[0], pos[1]-GRIPPER_SIZE//2), + (pos[0]+GRIPPER_SIZE//2, pos[1]) + ]) + else: + # 张开状态:两条竖线+一条横线 + pygame.draw.line(screen, (200, 50, 50), (pos[0]-GRIPPER_SIZE, pos[1]), (pos[0]+GRIPPER_SIZE, pos[1]), 4) + pygame.draw.line(screen, (200, 50, 50), (pos[0]-GRIPPER_SIZE//2, pos[1]-GRIPPER_SIZE//2), + (pos[0]-GRIPPER_SIZE//2, pos[1]+GRIPPER_SIZE//2), 4) + pygame.draw.line(screen, (200, 50, 50), (pos[0]+GRIPPER_SIZE//2, pos[1]-GRIPPER_SIZE//2), + (pos[0]+GRIPPER_SIZE//2, pos[1]+GRIPPER_SIZE//2), 4) + +# ====================== 3. 主程序(仿真窗口+抓取逻辑)===================== +def main(): + global grasp_trigger, target_pixel_pos + # 1. 启动视觉识别线程(守护线程,主程序退出时自动终止) + vision_thread = threading.Thread(target=vision_recognition_thread, daemon=True) + vision_thread.start() + time.sleep(1) # 等待线程初始化完成 + + # 2. 初始化PyGame + pygame.init() + screen = pygame.display.set_mode((SIM_WIDTH, SIM_HEIGHT)) + pygame.display.set_caption("🤖 机械臂抓取仿真(开箱即用)") + clock = pygame.time.Clock() + + # 3. 创建机械臂和仿真目标物体 + arm = ArmSimulation(ARM_BASE_POS, ARM_SEGMENT_LENGTHS) + target_obj_pos = np.array(TARGET_OBJ_INIT_POS, dtype=float) + target_obj_color = (255, 0, 0) # 初始红色:未抓取 + target_obj_grabbed = False # 目标是否被抓取 + + # 4. 打印操作说明 + print("\n" + "="*50) + print("操作说明:") + print(" 🎮 按【空格键】:执行抓取操作") + print(" 🎮 按【q键】:退出程序") + print("="*50 + "\n") + + # 主循环 + running = True + while running: + # 填充背景色 + screen.fill((240, 240, 240)) + + # ---------------------- 处理PyGame事件 ---------------------- + for event in pygame.event.get(): + if event.type == pygame.QUIT: + running = False + if event.type == pygame.KEYDOWN: + if event.key == pygame.K_q: + running = False + if event.key == pygame.K_SPACE: + grasp_trigger = True # 触发抓取 + + # ---------------------- 绘制仿真元素 ---------------------- + # 1. 绘制仿真目标物体 + pygame.draw.circle(screen, target_obj_color, tuple(map(int, target_obj_pos)), TARGET_OBJ_RADIUS) + # 2. 绘制机械臂 + arm.draw(screen) + + # ---------------------- 抓取逻辑处理 ---------------------- + if grasp_trigger: + grasp_trigger = False + if target_pixel_pos is not None: + print("\n📢 开始执行抓取流程...") + # 步骤1:将视觉像素坐标映射到仿真窗口坐标 + # 映射公式:将摄像头的像素坐标(0~640, 0~480)转换为仿真窗口的机械臂运动范围 + sim_x = ARM_BASE_POS[0] + (target_pixel_pos[0] - VISION_WIDTH//2) * 0.5 + sim_y = ARM_BASE_POS[1] - (target_pixel_pos[1] - VISION_HEIGHT//2) * 0.5 + target_sim_pos = np.array([sim_x, sim_y], dtype=float) + print(f"🔍 视觉像素坐标:{target_pixel_pos} → 仿真坐标:({int(sim_x)}, {int(sim_y)})") + + # 步骤2:移动机械臂到目标位置 + print("🤖 机械臂正在移动到目标位置...") + arm.move_to_target(target_sim_pos) + + # 步骤3:闭合夹爪 + print("🤖 夹爪闭合,抓取目标...") + arm.close_gripper() + + # 步骤4:更新目标物体状态(模拟被抓取) + target_obj_pos = arm.end_pos + np.array([0, -20]) # 目标随夹爪移动 + target_obj_color = (0, 255, 0) # 绿色:已抓取 + target_obj_grabbed = True + + print("✅ 抓取流程完成!") + else: + print("❌ 未检测到目标,无法执行抓取!") + + # ---------------------- 实时更新目标物体位置(若被抓取)---------------------- + if target_obj_grabbed: + target_obj_pos = arm.end_pos + np.array([0, -20]) + + # 更新屏幕显示 + pygame.display.flip() + # 控制帧率(60帧/秒) + clock.tick(60) + + # 退出程序 + pygame.quit() + cv2.destroyAllWindows() + print("\n👋 程序已正常退出!") + +# ====================== 程序入口 ====================== +if __name__ == "__main__": + main() \ No newline at end of file From b7ba400c7dda465c92a250f122f671c141a3940d Mon Sep 17 00:00:00 2001 From: wangadsa <1377468276@qq.com> Date: Mon, 22 Dec 2025 11:12:53 +0800 Subject: [PATCH 5/5] =?UTF-8?q?=E8=87=AA=E5=8A=A8=E6=89=A7=E8=A1=8C?= =?UTF-8?q?=E6=9C=BA=E6=A2=B0=E8=87=82=E7=A7=BB=E5=8A=A8=E5=92=8C=E6=8A=93?= =?UTF-8?q?=E5=8F=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Robot_arm_grasp/main.py | 411 +++++++++--------------------- src/Robot_arm_grasp/robot_arm.xml | 0 2 files changed, 121 insertions(+), 290 deletions(-) create mode 100644 src/Robot_arm_grasp/robot_arm.xml diff --git a/src/Robot_arm_grasp/main.py b/src/Robot_arm_grasp/main.py index e7992b0a43..690a69e2e4 100644 --- a/src/Robot_arm_grasp/main.py +++ b/src/Robot_arm_grasp/main.py @@ -1,299 +1,130 @@ -import cv2 import numpy as np -import pygame import math -import threading import time - -# ====================== 全局配置参数(可根据需求微调)===================== -# ---------------------- 视觉识别参数(真实摄像头)---------------------- -TARGET_COLOR_LOWER = np.array([0, 120, 70]) # 红色HSV下限(可适配自己的红色目标) -TARGET_COLOR_UPPER = np.array([10, 255, 255]) # 红色HSV上限 -TARGET_COLOR_LOWER2 = np.array([170, 120, 70]) # 红色补集(解决红色跨0度的问题) -TARGET_COLOR_UPPER2 = np.array([180, 255, 255]) -MIN_CONTOUR_AREA = 300 # 最小轮廓面积(过滤小噪声) -CAMERA_INDEX = 0 # 摄像头索引(0为默认,若没反应改1) -VISION_WIDTH = 640 # 摄像头画面宽度 -VISION_HEIGHT = 480 # 摄像头画面高度 - -# ---------------------- 仿真窗口与机械臂参数 ---------------------- -SIM_WIDTH = 800 # 仿真窗口宽度 -SIM_HEIGHT = 600 # 仿真窗口高度 -ARM_BASE_POS = (400, 500)# 机械臂基座在仿真窗口的位置 -# 机械臂各段长度(4段,可调整) -ARM_SEGMENT_LENGTHS = [100, 80, 60, 40] -GRIPPER_SIZE = 20 # 夹爪大小 -# 仿真目标物体初始位置(红色小球) -TARGET_OBJ_INIT_POS = (500, 300) -TARGET_OBJ_RADIUS = 15 - -# ---------------------- 全局变量(线程间通信)---------------------- -target_pixel_pos = None # 视觉识别的目标像素坐标 -grasp_trigger = False # 抓取触发标志 -is_camera_available = True # 摄像头是否可用 - -# ====================== 1. 视觉识别线程(独立运行,不阻塞仿真)===================== -def vision_recognition_thread(): - """处理真实摄像头的红色目标检测,输出目标像素坐标""" - global target_pixel_pos, is_camera_available - # 初始化摄像头 - cap = cv2.VideoCapture(CAMERA_INDEX) - cap.set(cv2.CAP_PROP_FRAME_WIDTH, VISION_WIDTH) - cap.set(cv2.CAP_PROP_FRAME_HEIGHT, VISION_HEIGHT) - - # 检查摄像头是否可用 - if not cap.isOpened(): - print(f"\n⚠️ 警告:无法打开摄像头(索引:{CAMERA_INDEX})") - print("💡 解决方案:1. 检查摄像头连接 2. 更换CAMERA_INDEX为1 3. 程序将使用模拟目标") - is_camera_available = False - # 模拟目标位置(摄像头画面中心) - while True: - target_pixel_pos = (VISION_WIDTH // 2, VISION_HEIGHT // 2) - time.sleep(0.05) - # 按q退出时终止线程 - if cv2.waitKey(1) & 0xFF == ord('q'): - break - return - - print("\n✅ 摄像头初始化成功!") - print("🎯 正在检测红色目标...") - - while True: - # 读取摄像头帧 - ret, frame = cap.read() - if not ret: - time.sleep(0.05) - continue - - # ---------------------- 目标检测核心逻辑 ---------------------- - # 1. 转HSV颜色空间(便于颜色筛选) - hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) - # 2. 生成红色掩码(合并两个区间,解决红色跨0度问题) - mask1 = cv2.inRange(hsv, TARGET_COLOR_LOWER, TARGET_COLOR_UPPER) - mask2 = cv2.inRange(hsv, TARGET_COLOR_LOWER2, TARGET_COLOR_UPPER2) - mask = cv2.bitwise_or(mask1, mask2) - # 3. 形态学操作(去噪声) - kernel = np.ones((5, 5), np.uint8) - mask = cv2.erode(mask, kernel, iterations=1) # 腐蚀 - mask = cv2.dilate(mask, kernel, iterations=1) # 膨胀 - # 4. 查找轮廓 - contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - - # 初始化目标位置 - target_pixel_pos = None - if contours: - # 取面积最大的轮廓(认为是目标) - max_contour = max(contours, key=cv2.contourArea) - if cv2.contourArea(max_contour) > MIN_CONTOUR_AREA: - # 计算轮廓中心 - M = cv2.moments(max_contour) - if M["m00"] != 0: - cX = int(M["m10"] / M["m00"]) - cY = int(M["m01"] / M["m00"]) - target_pixel_pos = (cX, cY) - # 绘制轮廓和中心标记 - cv2.drawContours(frame, [max_contour], -1, (0, 255, 0), 2) - cv2.circle(frame, (cX, cY), 5, (0, 0, 255), -1) - cv2.putText(frame, f"Target ({cX},{cY})", (cX-50, cY-10), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1) - - # ---------------------- 绘制提示信息 ---------------------- - if target_pixel_pos: - cv2.putText(frame, "✅ Target Found", (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) - else: - cv2.putText(frame, "❌ Target Lost", (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2) - - # 显示画面 - cv2.imshow("🤖 真实摄像头 - 红色目标检测", frame) - - # 按q键退出 - if cv2.waitKey(1) & 0xFF == ord('q'): - cap.release() - cv2.destroyAllWindows() - break - - # 释放资源 - cap.release() - cv2.destroyAllWindows() - -# ====================== 2. 机械臂仿真类(封装运动和绘制逻辑)===================== -class ArmSimulation: - def __init__(self, base_pos, segment_lengths): - """初始化机械臂""" - self.base_pos = np.array(base_pos, dtype=float) # 基座位置 - self.segment_lengths = segment_lengths # 各段长度 - self.num_segments = len(segment_lengths) # 段数(关节数) - self.joint_angles = [0.0 for _ in range(self.num_segments)] # 各关节角度(初始为0) - self.end_pos = self.calculate_end_position() # 末端执行器位置 - self.gripper_closed = False # 夹爪状态:False=张开,True=闭合 - - def calculate_end_position(self): - """正运动学:根据关节角度计算末端执行器位置""" - pos = self.base_pos.copy() - current_angle = 0.0 # 累计角度 - for i in range(self.num_segments): - current_angle += self.joint_angles[i] - # 计算当前段的位移(坐标系:y轴向下为正,需调整角度) - dx = self.segment_lengths[i] * math.cos(current_angle - math.pi/2) - dy = self.segment_lengths[i] * math.sin(current_angle - math.pi/2) - pos += np.array([dx, dy]) - return pos - - def move_to_target(self, target_pos, step=0.01): - """逆运动学简化版:移动末端执行器到目标位置""" - current_pos = self.calculate_end_position() - # 距离大于5像素时继续移动(精度阈值) - while np.linalg.norm(current_pos - target_pos) > 5: - # 计算方向向量(从当前位置到目标位置) - dir_vec = target_pos - current_pos - dir_vec = dir_vec / np.linalg.norm(dir_vec) # 归一化 - # 调整各关节角度(比例控制,简单有效) - for i in range(self.num_segments): - self.joint_angles[i] += dir_vec[0] * step - dir_vec[1] * step - # 更新末端位置 - current_pos = self.calculate_end_position() - # 模拟运动速度(暂停10ms) - pygame.time.wait(10) - self.end_pos = current_pos +import mujoco as mj +from mujoco import viewer + +# ====================== 全局配置参数 ====================== +# MuJoCo模型路径 +MUJOCO_MODEL_PATH = "robot_arm.xml" +# 机械臂关节名 +JOINT_NAMES = ["joint0", "joint1", "joint2", "joint3"] +# 目标物体名 +TARGET_BODY_NAME = "target_ball" +# 仿真目标位置(可调整) +SIM_TARGET_POS = np.array([1.5, 1.0, 0.5]) +# 全局标志:是否已执行移动逻辑 +has_moved = False + +# ====================== MuJoCo机械臂控制类(适配原生UI)===================== +class MuJoCoArmController: + def __init__(self, model_path): + # 加载模型和数据 + self.model = mj.MjModel.from_xml_path(model_path) + self.data = mj.MjData(self.model) + # 获取关节和目标物体索引 + self.joint_ids = [mj.mj_name2id(self.model, mj.mjtObj.mjOBJ_JOINT, name) for name in JOINT_NAMES] + self.target_body_id = mj.mj_name2id(self.model, mj.mjtObj.mjOBJ_BODY, TARGET_BODY_NAME) + # 设置目标物体初始位置 + self.set_target_pos(SIM_TARGET_POS) + print("✅ MuJoCo机械臂控制器初始化完成") + + def set_target_pos(self, pos): + """设置目标物体位置""" + self.data.body(TARGET_BODY_NAME).xpos = pos + print(f"🎯 目标物体位置已设置为:{np.round(pos, 3)}") + + def get_joint_angles(self): + """获取当前关节角度""" + return [self.data.joint(name).qpos[0] for name in JOINT_NAMES] + + def set_joint_target(self, target_angles, kp=200, kd=10): + """PD控制关节到目标角度""" + current_angles = np.array(self.get_joint_angles()) + current_vel = np.array([self.data.joint(name).qvel[0] for name in JOINT_NAMES]) + torque = kp * (target_angles - current_angles) - kd * current_vel + for i, joint_id in enumerate(self.joint_ids): + self.data.ctrl[joint_id] = torque[i] + + def inverse_kinematics(self, target_pos): + """数值逆运动学求解关节角度(兼容版本,改用手动映射避免mj_inverse问题)""" + print("ℹ️ 使用兼容版逆运动学(坐标映射角度)") + # 手动坐标到角度的映射(可根据模型调整比例) + angle0 = target_pos[0] * np.pi / 4 # joint0:绕z轴旋转 + angle1 = target_pos[1] * np.pi / 4 # joint1:绕x轴旋转 + angle2 = -target_pos[1] * np.pi / 4 # joint2:绕x轴旋转 + angle3 = 0.0 # joint3:固定角度 + return np.array([angle0, angle1, angle2, angle3]) + + def move_to_target(self, target_pos): + """移动机械臂到目标位置""" + global has_moved + if has_moved: + return + print(f"\n📢 开始移动到目标位置:{np.round(target_pos, 3)}") + target_angles = self.inverse_kinematics(target_pos) + # 逐步逼近目标角度 + current_angles = np.array(self.get_joint_angles()) + step = 0.02 + while np.linalg.norm(current_angles - target_angles) > 0.02: + current_angles = np.clip(current_angles + step * np.sign(target_angles - current_angles), -np.pi, np.pi) + self.set_joint_target(current_angles) + mj.mj_step(self.model, self.data) + time.sleep(0.01) + self.set_joint_target(target_angles) + self.close_gripper() + has_moved = True + print("✅ 机械臂已到达目标位置并完成抓取") def close_gripper(self): - """闭合夹爪""" - self.gripper_closed = True - pygame.time.wait(300) # 模拟夹爪闭合时间 - - def open_gripper(self): - """张开夹爪""" - self.gripper_closed = False - pygame.time.wait(300) - - def draw(self, screen): - """在PyGame窗口中绘制机械臂""" - # 1. 绘制基座 - pygame.draw.circle(screen, (50, 50, 150), tuple(map(int, self.base_pos)), 15) - # 2. 绘制机械臂各段和关节 - pos = self.base_pos.copy() - current_angle = 0.0 - for i in range(self.num_segments): - current_angle += self.joint_angles[i] - # 计算当前段的终点位置 - dx = self.segment_lengths[i] * math.cos(current_angle - math.pi/2) - dy = self.segment_lengths[i] * math.sin(current_angle - math.pi/2) - new_pos = pos + np.array([dx, dy]) - # 绘制段(线条) - pygame.draw.line(screen, (150, 150, 150), tuple(map(int, pos)), tuple(map(int, new_pos)), 8) - # 绘制关节(小圆) - pygame.draw.circle(screen, (100, 100, 100), tuple(map(int, new_pos)), 8) - pos = new_pos - # 3. 绘制末端执行器(夹爪) - if self.gripper_closed: - # 闭合状态:三角形 - pygame.draw.polygon(screen, (200, 50, 50), [ - (pos[0]-GRIPPER_SIZE//2, pos[1]), - (pos[0], pos[1]-GRIPPER_SIZE//2), - (pos[0]+GRIPPER_SIZE//2, pos[1]) - ]) - else: - # 张开状态:两条竖线+一条横线 - pygame.draw.line(screen, (200, 50, 50), (pos[0]-GRIPPER_SIZE, pos[1]), (pos[0]+GRIPPER_SIZE, pos[1]), 4) - pygame.draw.line(screen, (200, 50, 50), (pos[0]-GRIPPER_SIZE//2, pos[1]-GRIPPER_SIZE//2), - (pos[0]-GRIPPER_SIZE//2, pos[1]+GRIPPER_SIZE//2), 4) - pygame.draw.line(screen, (200, 50, 50), (pos[0]+GRIPPER_SIZE//2, pos[1]-GRIPPER_SIZE//2), - (pos[0]+GRIPPER_SIZE//2, pos[1]+GRIPPER_SIZE//2), 4) - -# ====================== 3. 主程序(仿真窗口+抓取逻辑)===================== + """闭合夹爪(模拟抓取)""" + print("🤖 夹爪闭合,抓取目标") + # 目标物体随夹爪移动(兼容:检查arm4是否存在) + body_id = mj.mj_name2id(self.model, mj.mjtObj.mjOBJ_BODY, "arm4") + if body_id != -1: + gripper_pos = self.data.body("arm4").xpos + self.data.body(TARGET_BODY_NAME).xpos = gripper_pos + np.array([0, 0.1, 0]) + time.sleep(0.5) + +# ====================== 主程序(兼容所有MuJoCo版本,移除callback参数)===================== def main(): - global grasp_trigger, target_pixel_pos - # 1. 启动视觉识别线程(守护线程,主程序退出时自动终止) - vision_thread = threading.Thread(target=vision_recognition_thread, daemon=True) - vision_thread.start() - time.sleep(1) # 等待线程初始化完成 - - # 2. 初始化PyGame - pygame.init() - screen = pygame.display.set_mode((SIM_WIDTH, SIM_HEIGHT)) - pygame.display.set_caption("🤖 机械臂抓取仿真(开箱即用)") - clock = pygame.time.Clock() - - # 3. 创建机械臂和仿真目标物体 - arm = ArmSimulation(ARM_BASE_POS, ARM_SEGMENT_LENGTHS) - target_obj_pos = np.array(TARGET_OBJ_INIT_POS, dtype=float) - target_obj_color = (255, 0, 0) # 初始红色:未抓取 - target_obj_grabbed = False # 目标是否被抓取 - - # 4. 打印操作说明 - print("\n" + "="*50) - print("操作说明:") - print(" 🎮 按【空格键】:执行抓取操作") - print(" 🎮 按【q键】:退出程序") - print("="*50 + "\n") - - # 主循环 - running = True - while running: - # 填充背景色 - screen.fill((240, 240, 240)) - - # ---------------------- 处理PyGame事件 ---------------------- - for event in pygame.event.get(): - if event.type == pygame.QUIT: - running = False - if event.type == pygame.KEYDOWN: - if event.key == pygame.K_q: - running = False - if event.key == pygame.K_SPACE: - grasp_trigger = True # 触发抓取 - - # ---------------------- 绘制仿真元素 ---------------------- - # 1. 绘制仿真目标物体 - pygame.draw.circle(screen, target_obj_color, tuple(map(int, target_obj_pos)), TARGET_OBJ_RADIUS) - # 2. 绘制机械臂 - arm.draw(screen) - - # ---------------------- 抓取逻辑处理 ---------------------- - if grasp_trigger: - grasp_trigger = False - if target_pixel_pos is not None: - print("\n📢 开始执行抓取流程...") - # 步骤1:将视觉像素坐标映射到仿真窗口坐标 - # 映射公式:将摄像头的像素坐标(0~640, 0~480)转换为仿真窗口的机械臂运动范围 - sim_x = ARM_BASE_POS[0] + (target_pixel_pos[0] - VISION_WIDTH//2) * 0.5 - sim_y = ARM_BASE_POS[1] - (target_pixel_pos[1] - VISION_HEIGHT//2) * 0.5 - target_sim_pos = np.array([sim_x, sim_y], dtype=float) - print(f"🔍 视觉像素坐标:{target_pixel_pos} → 仿真坐标:({int(sim_x)}, {int(sim_y)})") - - # 步骤2:移动机械臂到目标位置 - print("🤖 机械臂正在移动到目标位置...") - arm.move_to_target(target_sim_pos) - - # 步骤3:闭合夹爪 - print("🤖 夹爪闭合,抓取目标...") - arm.close_gripper() - - # 步骤4:更新目标物体状态(模拟被抓取) - target_obj_pos = arm.end_pos + np.array([0, -20]) # 目标随夹爪移动 - target_obj_color = (0, 255, 0) # 绿色:已抓取 - target_obj_grabbed = True - - print("✅ 抓取流程完成!") - else: - print("❌ 未检测到目标,无法执行抓取!") - - # ---------------------- 实时更新目标物体位置(若被抓取)---------------------- - if target_obj_grabbed: - target_obj_pos = arm.end_pos + np.array([0, -20]) - - # 更新屏幕显示 - pygame.display.flip() - # 控制帧率(60帧/秒) - clock.tick(60) - - # 退出程序 - pygame.quit() - cv2.destroyAllWindows() - print("\n👋 程序已正常退出!") + global has_moved + # 初始化控制器 + arm_controller = MuJoCoArmController(MUJOCO_MODEL_PATH) + # 设置目标物体位置 + arm_controller.set_target_pos(SIM_TARGET_POS) + + # 打印操作说明 + print("\n=====================================") + print("📋 操作说明:") + print(" - 程序启动后会自动移动机械臂到目标位置并抓取") + print(" - 在Viewer中可通过鼠标调整视角(拖拽/滚轮)") + print(" - 按ESC键或关闭窗口退出") + print("=====================================\n") + + # 先执行机械臂移动逻辑(在启动Viewer前完成核心运动) + arm_controller.move_to_target(SIM_TARGET_POS) + + # 启动MuJoCo原生可视化Viewer(兼容版本:仅传入model和data,不使用callback) + # 方式1:使用viewer.launch(简单启动,部分版本支持) + try: + viewer.launch(arm_controller.model, arm_controller.data) + except Exception as e: + # 方式2:如果launch报错,改用手动循环(最兼容的方式) + print(f"⚠️ 简易启动失败,使用手动循环模式:{e}") + # 创建viewer实例 + v = viewer.Viewer(arm_controller.model, arm_controller.data) + while True: + # 持续步进到下一帧 + mj.mj_step(arm_controller.model, arm_controller.data) + # 更新viewer画面 + v.sync() + # 短暂休眠,控制帧率 + time.sleep(0.01) + # 检查是否关闭窗口 + if not v.is_running(): + break + v.close() -# ====================== 程序入口 ====================== if __name__ == "__main__": main() \ No newline at end of file diff --git a/src/Robot_arm_grasp/robot_arm.xml b/src/Robot_arm_grasp/robot_arm.xml new file mode 100644 index 0000000000..e69de29bb2