mirror of
https://github.com/m1ngsama/robot_arm.git
synced 2026-03-25 19:53:49 +00:00
fix(voice_main): pass actual current_pos to execute_pick/execute_lift; add requirements.txt
- Closes #3: execute_pick and execute_lift now accept an optional current_pos parameter. Callers (execute_command) pass self.current_pos so move_line starts from the actual arm position rather than a hardcoded [120, 0, 60] default. Falls back to rest position when None. - Closes #4: add requirements.txt with correct package names. Note: README listed 'openai-whisper' but the code imports faster_whisper; requirements.txt uses the correct 'faster-whisper' package name. - Replace all `servo_buffer = []` assignments with `.clear()` to stay compatible with the deque introduced in PR #8 (#6 follow-up). - Partial #5: translate Chinese print/comment strings to English in execute_pick, execute_lift, and execute_command.
This commit is contained in:
parent
a7209c4f78
commit
9e672ce637
2 changed files with 95 additions and 61 deletions
33
requirements.txt
Normal file
33
requirements.txt
Normal file
|
|
@ -0,0 +1,33 @@
|
|||
# Robot Arm Voice Control System - Dependencies
|
||||
# Python 3.10+ required
|
||||
#
|
||||
# GPU acceleration (CUDA 11.8 / 12.x) is strongly recommended.
|
||||
# Install PyTorch with CUDA from: https://pytorch.org/get-started/locally/
|
||||
# Example (CUDA 12.1):
|
||||
# pip install torch torchvision torchaudio --index-url https://download.pytorch.org/whl/cu121
|
||||
|
||||
# --- Core scientific stack ---
|
||||
numpy>=1.24
|
||||
scipy>=1.11
|
||||
|
||||
# --- Serial communication ---
|
||||
pyserial>=3.5
|
||||
|
||||
# --- Computer vision ---
|
||||
opencv-python>=4.8
|
||||
ultralytics>=8.0 # YOLOv8
|
||||
|
||||
# --- Audio ---
|
||||
sounddevice>=0.4.6
|
||||
|
||||
# --- Speech recognition ---
|
||||
faster-whisper>=1.0.0 # NOTE: this is faster-whisper, NOT openai-whisper
|
||||
|
||||
# --- LLM inference ---
|
||||
torch>=2.0 # install with CUDA wheels from pytorch.org
|
||||
transformers>=4.40
|
||||
accelerate>=0.27
|
||||
peft>=0.10 # LoRA adapter loading
|
||||
|
||||
# --- Visualization (arm_main.py demo) ---
|
||||
matplotlib>=3.7
|
||||
|
|
@ -500,9 +500,17 @@ class AutoGraspSystem:
|
|||
return rx, ry
|
||||
return None
|
||||
|
||||
def execute_pick(self, rx, ry):
|
||||
"""执行抓取动作"""
|
||||
print(f"\n>>> 执行抓取: ({rx:.1f}, {ry:.1f})")
|
||||
def execute_pick(self, rx, ry, current_pos=None):
|
||||
"""Execute pick action: move to target (rx, ry) and grasp.
|
||||
|
||||
Args:
|
||||
rx, ry: robot-frame target coordinates (mm)
|
||||
current_pos: actual current end-effector position (mm, [x, y, z]).
|
||||
Falls back to default rest position if None.
|
||||
"""
|
||||
if current_pos is None:
|
||||
current_pos = np.array([120.0, 0.0, 60.0])
|
||||
|
||||
Z_HOVER = 120.0
|
||||
Z_GRAB = -15.0
|
||||
Z_AFTER_PICK = 50.0
|
||||
|
|
@ -511,67 +519,62 @@ class AutoGraspSystem:
|
|||
p_down = np.array([rx, ry, Z_GRAB])
|
||||
p_after = np.array([rx, ry, Z_AFTER_PICK])
|
||||
|
||||
# 张开夹爪
|
||||
print(f"\n[Pick] target=({rx:.1f}, {ry:.1f}), from={current_pos}")
|
||||
|
||||
self.arm.gripper_control(70)
|
||||
self.gripper_closed = False
|
||||
time.sleep(0.3)
|
||||
|
||||
# 【修复】清空缓冲区,确保新动作不受之前状态影响
|
||||
self.arm.servo_buffer = []
|
||||
self.arm.servo_buffer.clear()
|
||||
|
||||
# 【修复】使用 move_line 从当前位置移动到目标上方(更可靠)
|
||||
# 先获取当前位置的近似值
|
||||
current_approx = np.array([120.0, 0.0, 60.0]) # 默认休息位置
|
||||
self.arm.move_line(current_approx, p_up, p_start=-60, p_end=-60, duration=1.5)
|
||||
|
||||
# 下降抓取
|
||||
self.arm.move_line(current_pos, p_up, p_start=-60, p_end=-60, duration=1.5)
|
||||
self.arm.move_line(p_up, p_down, p_start=-60, p_end=-90, duration=1.5)
|
||||
|
||||
# 闭合夹爪
|
||||
self.arm.gripper_control(120)
|
||||
self.gripper_closed = True
|
||||
time.sleep(0.5)
|
||||
|
||||
# 抬起
|
||||
self.arm.move_line(p_down, p_after, p_start=-90, p_end=-60, duration=1.0)
|
||||
|
||||
return p_after
|
||||
|
||||
def execute_lift(self, rx, ry, height):
|
||||
"""执行抓取并抬起到指定高度"""
|
||||
print(f"\n>>> 执行抬起: 目标({rx:.1f}, {ry:.1f}), 高度{height}mm")
|
||||
def execute_lift(self, rx, ry, height, current_pos=None):
|
||||
"""Execute pick-and-lift: grasp target and raise by `height` mm.
|
||||
|
||||
Args:
|
||||
rx, ry: robot-frame target coordinates (mm)
|
||||
height: lift distance above grab point (mm)
|
||||
current_pos: actual current end-effector position (mm, [x, y, z]).
|
||||
Falls back to default rest position if None.
|
||||
"""
|
||||
if current_pos is None:
|
||||
current_pos = np.array([120.0, 0.0, 60.0])
|
||||
|
||||
Z_HOVER = 120.0
|
||||
Z_GRAB = -15.0
|
||||
|
||||
p_up = np.array([rx, ry, Z_HOVER])
|
||||
p_down = np.array([rx, ry, Z_GRAB])
|
||||
p_lifted = np.array([rx, ry, Z_GRAB + height])
|
||||
p_lifted[2] = np.clip(p_lifted[2], -20, 200)
|
||||
p_lifted = np.array([rx, ry, np.clip(Z_GRAB + height, -20, 200)])
|
||||
|
||||
print(f"\n[Lift] target=({rx:.1f}, {ry:.1f}), height={height}mm, from={current_pos}")
|
||||
|
||||
# 张开夹爪
|
||||
self.arm.gripper_control(70)
|
||||
self.gripper_closed = False
|
||||
time.sleep(0.3)
|
||||
|
||||
# 【修复】清空缓冲区
|
||||
self.arm.servo_buffer = []
|
||||
self.arm.servo_buffer.clear()
|
||||
|
||||
# 【修复】使用 move_line 移动到目标上方
|
||||
current_approx = np.array([120.0, 0.0, 60.0])
|
||||
self.arm.move_line(current_approx, p_up, p_start=-60, p_end=-60, duration=1.5)
|
||||
|
||||
# 下降抓取
|
||||
self.arm.move_line(current_pos, p_up, p_start=-60, p_end=-60, duration=1.5)
|
||||
self.arm.move_line(p_up, p_down, p_start=-60, p_end=-90, duration=1.5)
|
||||
|
||||
# 闭合夹爪
|
||||
self.arm.gripper_control(120)
|
||||
self.gripper_closed = True
|
||||
time.sleep(0.5)
|
||||
|
||||
# 抬起到指定高度
|
||||
self.arm.move_line(p_down, p_lifted, p_start=-90, p_end=-60, duration=1.5)
|
||||
|
||||
print(f"✓ 已抬起到高度: Z={p_lifted[2]:.1f}mm")
|
||||
print(f"[OK] Lifted to Z={p_lifted[2]:.1f}mm")
|
||||
return p_lifted
|
||||
|
||||
def execute_release(self):
|
||||
|
|
@ -598,7 +601,7 @@ class AutoGraspSystem:
|
|||
p_drop = np.array([current_pos[0], current_pos[1], Z_TABLE])
|
||||
|
||||
# 【修复】清空缓冲区
|
||||
self.arm.servo_buffer = []
|
||||
self.arm.servo_buffer.clear()
|
||||
|
||||
# 移动到放置点
|
||||
# 注意:如果当前已经在桌面高度以下,不要强行移动,或者先抬升?
|
||||
|
|
@ -624,7 +627,7 @@ class AutoGraspSystem:
|
|||
time.sleep(0.3)
|
||||
|
||||
# 【修复】清空缓冲区
|
||||
self.arm.servo_buffer = []
|
||||
self.arm.servo_buffer.clear()
|
||||
|
||||
# 【修复】使用 move_line 移动(更可靠)
|
||||
# 从一个安全的中间位置开始
|
||||
|
|
@ -757,12 +760,11 @@ class RobotApp:
|
|||
pos = self.grasp_sys.find_target(target)
|
||||
if pos:
|
||||
rx, ry = pos
|
||||
# 【修复】使用正确的方法名
|
||||
self.current_pos = self.grasp_sys.execute_lift(rx, ry, height)
|
||||
print(f"✓ 已抬起 {target},当前位置: {self.current_pos}")
|
||||
self.current_pos = self.grasp_sys.execute_lift(rx, ry, height, current_pos=self.current_pos)
|
||||
print(f"[OK] Lifted {target}, pos={self.current_pos}")
|
||||
else:
|
||||
print(f"✗ 未找到目标: {target}")
|
||||
print(f" 当前可见目标: {list(self.grasp_sys.detected_targets.keys())}")
|
||||
print(f"[Error] Target not found: {target}")
|
||||
print(f" Visible: {list(self.grasp_sys.detected_targets.keys())}")
|
||||
|
||||
elif action == "pick":
|
||||
target = cmd.get("target", "part")
|
||||
|
|
@ -770,12 +772,11 @@ class RobotApp:
|
|||
pos = self.grasp_sys.find_target(target)
|
||||
if pos:
|
||||
rx, ry = pos
|
||||
# 【修复】使用正确的方法名
|
||||
self.current_pos = self.grasp_sys.execute_pick(rx, ry)
|
||||
print(f"✓ 已抓取 {target},当前位置: {self.current_pos}")
|
||||
self.current_pos = self.grasp_sys.execute_pick(rx, ry, current_pos=self.current_pos)
|
||||
print(f"[OK] Picked {target}, pos={self.current_pos}")
|
||||
else:
|
||||
print(f"✗ 未找到目标: {target}")
|
||||
print(f" 当前可见目标: {list(self.grasp_sys.detected_targets.keys())}")
|
||||
print(f"[Error] Target not found: {target}")
|
||||
print(f" Visible: {list(self.grasp_sys.detected_targets.keys())}")
|
||||
|
||||
elif action == "move_inc":
|
||||
axis = cmd.get("axis", "z")
|
||||
|
|
@ -816,7 +817,7 @@ class RobotApp:
|
|||
dist = 30.0
|
||||
|
||||
# 清空缓冲,确保动作连贯
|
||||
self.grasp_sys.arm.servo_buffer = []
|
||||
self.grasp_sys.arm.servo_buffer.clear()
|
||||
|
||||
for i in range(3):
|
||||
# 向上
|
||||
|
|
@ -841,7 +842,7 @@ class RobotApp:
|
|||
dist = 30.0
|
||||
|
||||
# 清空缓冲
|
||||
self.grasp_sys.arm.servo_buffer = []
|
||||
self.grasp_sys.arm.servo_buffer.clear()
|
||||
|
||||
for i in range(3):
|
||||
# 向左 (y增加是左)
|
||||
|
|
|
|||
Loading…
Reference in a new issue