From c2b70306b8ec637312f7985c88f04d87a76a5955 Mon Sep 17 00:00:00 2001 From: gongjh0916 <2748130352@qq.com> Date: Fri, 19 Dec 2025 18:38:24 +0800 Subject: [PATCH 1/5] =?UTF-8?q?=E6=8F=90=E4=BA=A4=E9=80=89=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Mechanical_arm_grasping/README.md | 1 + src/Mechanical_arm_grasping/main.py | 0 2 files changed, 1 insertion(+) create mode 100644 src/Mechanical_arm_grasping/README.md create mode 100644 src/Mechanical_arm_grasping/main.py diff --git a/src/Mechanical_arm_grasping/README.md b/src/Mechanical_arm_grasping/README.md new file mode 100644 index 0000000000..e4f1315851 --- /dev/null +++ b/src/Mechanical_arm_grasping/README.md @@ -0,0 +1 @@ +机械臂抓取系统 \ No newline at end of file diff --git a/src/Mechanical_arm_grasping/main.py b/src/Mechanical_arm_grasping/main.py new file mode 100644 index 0000000000..e69de29bb2 From ef8b457878cba7016a6e7ae176e3cfa5d80ba1ed Mon Sep 17 00:00:00 2001 From: gongjh0916 <2748130352@qq.com> Date: Sun, 21 Dec 2025 21:46:25 +0800 Subject: [PATCH 2/5] =?UTF-8?q?=E6=B7=BB=E5=8A=A0.gitignore=EF=BC=8C?= =?UTF-8?q?=E5=BF=BD=E7=95=A5=E6=97=A0=E7=94=A8=E6=9C=AC=E5=9C=B0=E6=96=87?= =?UTF-8?q?=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Mechanical_arm_grasping/.gitgnore | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 src/Mechanical_arm_grasping/.gitgnore diff --git a/src/Mechanical_arm_grasping/.gitgnore b/src/Mechanical_arm_grasping/.gitgnore new file mode 100644 index 0000000000..e69de29bb2 From 7563e69fcedded95a65b5ce195ded4ea59a4e172 Mon Sep 17 00:00:00 2001 From: gongjh0916 <2748130352@qq.com> Date: Sun, 21 Dec 2025 21:51:36 +0800 Subject: [PATCH 3/5] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=88=9D=E5=A7=8B?= =?UTF-8?q?=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Mechanical_arm_grasping/.gitgnore | 0 src/Mechanical_arm_grasping/main.py | 28 +++++++++++++++++++++++++++ 2 files changed, 28 insertions(+) delete mode 100644 src/Mechanical_arm_grasping/.gitgnore diff --git a/src/Mechanical_arm_grasping/.gitgnore b/src/Mechanical_arm_grasping/.gitgnore deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/src/Mechanical_arm_grasping/main.py b/src/Mechanical_arm_grasping/main.py index e69de29bb2..ccc567aa29 100644 --- a/src/Mechanical_arm_grasping/main.py +++ b/src/Mechanical_arm_grasping/main.py @@ -0,0 +1,28 @@ +# 代码1:超声波传感器目标位置检测(简单版) +import time + +def ultrasonic_sensor_simulation(): + """模拟超声波传感器,返回目标与机械臂的距离(单位:cm)""" + # 随机生成合理距离(模拟真实传感器检测结果) + import random + target_distance = random.uniform(5, 50) # 目标距离5-50cm + print(f"【超声波传感器检测】目标距离:{target_distance:.2f} cm") + return target_distance + +def judge_target_position(distance): + """根据距离判断目标位置状态""" + if distance < 15: + return "近距离(可抓取)" + elif 15 <= distance <= 30: + return "中距离(需伸展)" + else: + return "远距离(超出范围)" + +# 主程序 +if __name__ == "__main__": + print("=== 机械臂目标检测(简单版)===") + for i in range(3): # 检测3次 + distance = ultrasonic_sensor_simulation() + position_state = judge_target_position(distance) + print(f"第{i+1}次检测结果:{position_state}\n") + time.sleep(1) # 间隔1秒检测 \ No newline at end of file From 4795836eb517401f38e9b4ca2db25a4252966a31 Mon Sep 17 00:00:00 2001 From: gongjh0916 <2748130352@qq.com> Date: Mon, 22 Dec 2025 18:14:52 +0800 Subject: [PATCH 4/5] =?UTF-8?q?=E6=8E=A7=E5=88=B6=E6=9C=BA=E6=A2=B0?= =?UTF-8?q?=E8=87=82=E5=8D=87=E9=99=8D=E6=A8=A1=E6=8B=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Mechanical_arm_grasping/main.py | 117 +++++++++++++++++++++------- 1 file changed, 90 insertions(+), 27 deletions(-) diff --git a/src/Mechanical_arm_grasping/main.py b/src/Mechanical_arm_grasping/main.py index ccc567aa29..33afe57051 100644 --- a/src/Mechanical_arm_grasping/main.py +++ b/src/Mechanical_arm_grasping/main.py @@ -1,28 +1,91 @@ -# 代码1:超声波传感器目标位置检测(简单版) -import time - -def ultrasonic_sensor_simulation(): - """模拟超声波传感器,返回目标与机械臂的距离(单位:cm)""" - # 随机生成合理距离(模拟真实传感器检测结果) - import random - target_distance = random.uniform(5, 50) # 目标距离5-50cm - print(f"【超声波传感器检测】目标距离:{target_distance:.2f} cm") - return target_distance - -def judge_target_position(distance): - """根据距离判断目标位置状态""" - if distance < 15: - return "近距离(可抓取)" - elif 15 <= distance <= 30: - return "中距离(需伸展)" +import pygame +import sys +import math # 导入Python内置math库,用于角度弧度转换 + +# 初始化pygame +pygame.init() + +# 窗口配置 +WINDOW_WIDTH = 400 +WINDOW_HEIGHT = 500 +screen = pygame.display.set_mode((WINDOW_WIDTH, WINDOW_HEIGHT)) +pygame.display.set_caption("机械臂关节升降模拟") + +# 颜色定义 +BLACK = (0, 0, 0) +WHITE = (255, 255, 255) +BLUE = (0, 128, 255) +RED = (255, 50, 50) + +# 机械臂参数 +BASE_X = WINDOW_WIDTH // 2 # 机械臂底座x坐标 +BASE_Y = WINDOW_HEIGHT - 50 # 机械臂底座y坐标 +JOINT_LENGTH = 200 # 关节臂长度(影响升降范围) +joint_angle = 90 # 关节初始角度(90度为垂直向下,0度为水平向左,180度为水平向右) +speed = 1 # 角度变化速度(控制升降快慢) + +def draw_arm(screen, base_x, base_y, angle, length): + """绘制机械臂模型""" + # 计算关节末端坐标(通过角度转换,使用math库的radians方法) + # 三角函数计算:y轴向下为正,需调整角度映射 + rad = math.radians(angle) # 修复:替换为math.radians + joint_end_x = base_x + length * math.cos(rad) # 修复:使用math.cos + joint_end_y = base_y - length * math.sin(rad) # 修复:使用math.sin,减号实现y轴向上为高度增加 + + # 绘制底座 + pygame.draw.circle(screen, BLUE, (base_x, base_y), 20) + # 绘制关节臂(底座到关节末端) + pygame.draw.line(screen, RED, (base_x, base_y), (joint_end_x, joint_end_y), 8) + # 绘制关节末端 + pygame.draw.circle(screen, BLACK, (int(joint_end_x), int(joint_end_y)), 10) + # 绘制高度提示文字 + height = int(BASE_Y - joint_end_y) # 计算当前高度(像素值,近似对应实际高度) + font = pygame.font.SysFont(None, 24) + text = font.render(f"关节高度:{height}px", True, BLACK) + screen.blit(text, (10, 10)) + +# 主循环 +clock = pygame.time.Clock() +is_running = True +move_direction = "up" # 初始运动方向:上升 + +while is_running: + # 控制帧率 + clock.tick(60) + # 填充背景色 + screen.fill(WHITE) + + # 事件处理(关闭窗口、按键控制) + for event in pygame.event.get(): + if event.type == pygame.QUIT: + is_running = False + # 按键控制:上箭头上升,下箭头下降 + if event.type == pygame.KEYDOWN: + if event.key == pygame.K_UP: + move_direction = "up" + if event.key == pygame.K_DOWN: + move_direction = "down" + + # 更新关节角度(控制升降) + if move_direction == "up": + # 角度增大:关节上升(最大角度170度,避免超出窗口) + if joint_angle < 170: + joint_angle += speed + else: + move_direction = "down" # 到达上限后自动下降 else: - return "远距离(超出范围)" - -# 主程序 -if __name__ == "__main__": - print("=== 机械臂目标检测(简单版)===") - for i in range(3): # 检测3次 - distance = ultrasonic_sensor_simulation() - position_state = judge_target_position(distance) - print(f"第{i+1}次检测结果:{position_state}\n") - time.sleep(1) # 间隔1秒检测 \ No newline at end of file + # 角度减小:关节下降(最小角度10度,避免超出窗口) + if joint_angle > 10: + joint_angle -= speed + else: + move_direction = "up" # 到达下限后自动上升 + + # 绘制机械臂 + draw_arm(screen, BASE_X, BASE_Y, joint_angle, JOINT_LENGTH) + + # 更新窗口显示 + pygame.display.flip() + +# 退出程序 +pygame.quit() +sys.exit() \ No newline at end of file From 0d1343a14572ebab2ef4e01eb395fe526393a31d Mon Sep 17 00:00:00 2001 From: gongjh0916 <2748130352@qq.com> Date: Mon, 22 Dec 2025 20:48:23 +0800 Subject: [PATCH 5/5] =?UTF-8?q?=E5=88=9D=E5=A7=8B=E6=9C=BA=E6=A2=B0?= =?UTF-8?q?=E8=87=82=E5=8D=87=E9=99=8D=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Mechanical_arm_grasping/main.py | 161 ++++++++++++++-------------- 1 file changed, 82 insertions(+), 79 deletions(-) diff --git a/src/Mechanical_arm_grasping/main.py b/src/Mechanical_arm_grasping/main.py index 33afe57051..683b104bbb 100644 --- a/src/Mechanical_arm_grasping/main.py +++ b/src/Mechanical_arm_grasping/main.py @@ -1,91 +1,94 @@ -import pygame -import sys -import math # 导入Python内置math库,用于角度弧度转换 +import pybullet as p +import pybullet_data +import time +import numpy as np -# 初始化pygame -pygame.init() -# 窗口配置 -WINDOW_WIDTH = 400 -WINDOW_HEIGHT = 500 -screen = pygame.display.set_mode((WINDOW_WIDTH, WINDOW_HEIGHT)) -pygame.display.set_caption("机械臂关节升降模拟") +class ArmElevatorControllerPyBullet: + def __init__(self): + # 连接PyBullet模拟器(GUI模式,显示界面) + self.physics_client = p.connect(p.GUI) + # 设置模型搜索路径(关键:确保能找到内置模型) + p.setAdditionalSearchPath(pybullet_data.getDataPath()) + # 关闭重力(避免机械臂倾倒,专注升降控制;若需要真实物理效果可开启) + p.setGravity(0, 0, 0) -# 颜色定义 -BLACK = (0, 0, 0) -WHITE = (255, 255, 255) -BLUE = (0, 128, 255) -RED = (255, 50, 50) + # 加载地面和KUKA IIWA机械臂(内置模型,必存在,无需额外配置) + self.plane_id = p.loadURDF("plane.urdf") # 加载地面 + # 机械臂初始位姿:坐标(0,0,0),姿态(无旋转) + self.arm_id = p.loadURDF( + "kuka_iiwa/model.urdf", + basePosition=[0, 0, 0], + baseOrientation=p.getQuaternionFromEuler([0, 0, 0]) + ) -# 机械臂参数 -BASE_X = WINDOW_WIDTH // 2 # 机械臂底座x坐标 -BASE_Y = WINDOW_HEIGHT - 50 # 机械臂底座y坐标 -JOINT_LENGTH = 200 # 关节臂长度(影响升降范围) -joint_angle = 90 # 关节初始角度(90度为垂直向下,0度为水平向左,180度为水平向右) -speed = 1 # 角度变化速度(控制升降快慢) + # 定义升降关节:选择KUKA IIWA的第1个关节(索引0,可实现垂直方向升降/旋转,适配升降逻辑) + self.elevator_joint_index = 0 + # 获取关节信息(限位、当前位置) + joint_info = p.getJointInfo(self.arm_id, self.elevator_joint_index) + self.joint_min = joint_info[8] # 关节运动下限 + self.joint_max = joint_info[9] # 关节运动上限 + self.current_pos = p.getJointState(self.arm_id, self.elevator_joint_index)[0] # 当前位置 -def draw_arm(screen, base_x, base_y, angle, length): - """绘制机械臂模型""" - # 计算关节末端坐标(通过角度转换,使用math库的radians方法) - # 三角函数计算:y轴向下为正,需调整角度映射 - rad = math.radians(angle) # 修复:替换为math.radians - joint_end_x = base_x + length * math.cos(rad) # 修复:使用math.cos - joint_end_y = base_y - length * math.sin(rad) # 修复:使用math.sin,减号实现y轴向上为高度增加 + # 打印关节初始化信息 + print(f"升降关节初始化完成:") + print(f"关节索引:{self.elevator_joint_index}") + print(f"当前位置:{self.current_pos:.3f}") + print(f"运动范围:[{self.joint_min:.3f}, {self.joint_max:.3f}]") - # 绘制底座 - pygame.draw.circle(screen, BLUE, (base_x, base_y), 20) - # 绘制关节臂(底座到关节末端) - pygame.draw.line(screen, RED, (base_x, base_y), (joint_end_x, joint_end_y), 8) - # 绘制关节末端 - pygame.draw.circle(screen, BLACK, (int(joint_end_x), int(joint_end_y)), 10) - # 绘制高度提示文字 - height = int(BASE_Y - joint_end_y) # 计算当前高度(像素值,近似对应实际高度) - font = pygame.font.SysFont(None, 24) - text = font.render(f"关节高度:{height}px", True, BLACK) - screen.blit(text, (10, 10)) + def move_elevator(self, target_pos, speed=0.05): + """ + 驱动升降关节运动到目标位置 + :param target_pos: 目标位置(需在关节限位范围内) + :param speed: 运动速度(正数,越小越慢) + """ + # 校验目标位置合法性 + if target_pos < self.joint_min or target_pos > self.joint_max: + raise ValueError(f"目标位置超出关节范围!允许范围:[{self.joint_min:.3f}, {self.joint_max:.3f}]") -# 主循环 -clock = pygame.time.Clock() -is_running = True -move_direction = "up" # 初始运动方向:上升 + print(f"\n开始升降运动:当前位置 {self.current_pos:.3f} → 目标位置 {target_pos:.3f}") + # 循环控制,直到接近目标位置(误差小于0.001) + while abs(self.current_pos - target_pos) > 0.001: + # 计算运动步长(根据目标位置判断升降方向) + step = speed if target_pos > self.current_pos else -speed + # 更新当前位置(防止超出限位) + self.current_pos = np.clip(self.current_pos + step, self.joint_min, self.joint_max) + # 发送位置指令给关节(位置控制模式) + p.setJointMotorControl2( + bodyUniqueId=self.arm_id, + jointIndex=self.elevator_joint_index, + controlMode=p.POSITION_CONTROL, + targetPosition=self.current_pos + ) + # 步进物理仿真(更新场景状态) + p.stepSimulation() + # 小幅延时,模拟真实运动节奏 + time.sleep(0.01) + # 获取模拟器中关节的实际位置(反馈同步) + self.current_pos = p.getJointState(self.arm_id, self.elevator_joint_index)[0] + # 实时刷新显示当前位置 + print(f"实时位置:{self.current_pos:.3f}", end='\r') -while is_running: - # 控制帧率 - clock.tick(60) - # 填充背景色 - screen.fill(WHITE) + print(f"\n升降运动完成!最终位置:{self.current_pos:.3f}") - # 事件处理(关闭窗口、按键控制) - for event in pygame.event.get(): - if event.type == pygame.QUIT: - is_running = False - # 按键控制:上箭头上升,下箭头下降 - if event.type == pygame.KEYDOWN: - if event.key == pygame.K_UP: - move_direction = "up" - if event.key == pygame.K_DOWN: - move_direction = "down" + def disconnect(self): + """断开与PyBullet模拟器的连接""" + p.disconnect(self.physics_client) + print("\n已断开与PyBullet模拟器的连接") - # 更新关节角度(控制升降) - if move_direction == "up": - # 角度增大:关节上升(最大角度170度,避免超出窗口) - if joint_angle < 170: - joint_angle += speed - else: - move_direction = "down" # 到达上限后自动下降 - else: - # 角度减小:关节下降(最小角度10度,避免超出窗口) - if joint_angle > 10: - joint_angle -= speed - else: - move_direction = "up" # 到达下限后自动上升 - # 绘制机械臂 - draw_arm(screen, BASE_X, BASE_Y, joint_angle, JOINT_LENGTH) +# ------------------- 主执行程序 ------------------- +if __name__ == "__main__": + # 1. 初始化机械臂升降控制器 + arm_controller = ArmElevatorControllerPyBullet() - # 更新窗口显示 - pygame.display.flip() - -# 退出程序 -pygame.quit() -sys.exit() \ No newline at end of file + try: + # 2. 执行升降动作序列 + arm_controller.move_elevator(target_pos=arm_controller.joint_max * 0.6, speed=0.03) # 上升(接近上限) + time.sleep(1) # 停顿1秒 + arm_controller.move_elevator(target_pos=arm_controller.joint_min * 0.6, speed=0.02) # 下降(接近下限) + time.sleep(1) # 停顿1秒 + arm_controller.move_elevator(target_pos=0) # 回到初始中间位置 + finally: + # 3. 无论是否出错,最终断开连接 + arm_controller.disconnect() \ No newline at end of file