Skip to content
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
291 changes: 220 additions & 71 deletions src/Embodied_Human_Arm_Modeling/hand_animation_controller.py
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
import numpy as np
import time
import sys
import threading


class HandDemoMujoco3:
Expand All @@ -22,11 +23,6 @@ def __init__(self, model_path='left_hand.xml'):
print(f"📊 仿真时间步: {self.model.opt.timestep:.4f}秒")
print("=" * 60)

# 在 MuJoCo 3.x 中获取执行器名称的替代方法
print("📋 执行器信息:")
# 注意:MuJoCo 3.x 中获取执行器名称的方式不同
# 这里我们只显示数量,不尝试获取名称

# 创建预设姿态
self._create_preset_poses()

Expand All @@ -38,6 +34,23 @@ def __init__(self, model_path='left_hand.xml'):
self.start_values = None
self.target_values = None

# 添加暂停相关变量
self.paused = False
self.pause_start_time = 0
self.total_pause_time = 0
self.pause_lock = threading.Lock()

# 添加用户控制变量
self.manual_mode = False
self.should_exit = False

# 控制变量
self.should_toggle_pause = False
self.should_next_pose = False
self.should_prev_pose = False
self.should_toggle_mode = False
self.should_restart = False

print(f"🎭 创建了 {len(self.poses)} 种预设姿态")
print("=" * 60)

Expand All @@ -63,13 +76,6 @@ def _create_preset_poses(self):
'description': '所有手指完全握紧'
}

# 捏取
self.poses['捏取'] = {
'values': self._create_pose_pinch(),
'emoji': '🤏',
'description': '拇指和食指对捏'
}

# 圆柱体抓握
self.poses['圆柱体抓握'] = {
'values': self._create_pose_cylinder(),
Expand Down Expand Up @@ -119,11 +125,11 @@ def _create_pose_fist(self):
# 假设前20个执行器是:手腕(2) + 拇指(5) + 4个手指*3 + 小指额外(1)
values[:20] = [
0.0, 0.0, # 手腕
0.0, 1.57, 0.0, 0.5, 0.3, # 拇指
1.57, 1.57, 1.57, # 食指
1.57, 1.57, 1.57, # 中指
1.57, 1.57, 1.57, # 无名指
0.0, 1.57, 1.57, 1.57 # 小指
0.199, 0.354, 0.209, 0.698, 0.3, # 拇指
5, 5, 5, # 食指
5, 5, 5, # 中指
5, 5, 5, # 无名指
0.0, 5, 5, 5, # 小指
]
elif self.model.nu >= 10:
# 简化的握拳姿态
Expand All @@ -139,36 +145,6 @@ def _create_pose_fist(self):

return values

def _create_pose_pinch(self):
"""创建捏取姿态"""
values = np.zeros(self.model.nu)

if self.model.nu >= 20:
values[:20] = [
0.0, 0.0, # 手腕
0.5, 0.6, 0.0, 0.5, 0.8, # 拇指
0.2, 0.7, 0.7, # 食指
0.0, 0.2, 0.2, # 中指
0.0, 0.1, 0.1, # 无名指
0.0, 0.1, 0.1, 0.1 # 小指
]
elif self.model.nu >= 5:
# 简化的捏取:假设前5个执行器中,第2个是拇指,第3个是食指
for i in range(self.model.nu):
if i == 2: # 拇指
values[i] = 0.5
elif i == 3: # 食指
values[i] = 0.7
elif i >= 4: # 其他手指
values[i] = 0.2
else: # 手腕
values[i] = 0.0
else:
# 最小配置
for i in range(self.model.nu):
values[i] = 0.5 if i == 2 else 0.0

return values

def _create_pose_cylinder(self):
"""创建圆柱体抓握姿态"""
Expand Down Expand Up @@ -203,12 +179,12 @@ def _create_pose_scissors(self):

if self.model.nu >= 20:
values[:20] = [
0.0, 0.0, # 手腕
0.0, 10, # 手腕
0.2, 0.4, 0.0, 0.3, 0.2, # 拇指
0.0, 0.0, 0.0, # 食指
0.0, 0.0, 0.0, # 中指
0.7, 1.57, 1.57, # 无名指
0.0, 1.57, 1.57, 1.57 # 小指
0.7, 5, 5, # 无名指
0.0, 1, 5, 5 # 小指
]
elif self.model.nu >= 7:
# 简化的剪刀手:假设第3-4个是食指和中指,其他手指弯曲
Expand All @@ -233,11 +209,11 @@ def _create_pose_ok(self):
if self.model.nu >= 20:
values[:20] = [
0.0, 0.0, # 手腕
0.4, 0.6, 0.0, 0.5, 0.7, # 拇指
0.3, 0.7, 0.9, # 食指
0.0, 0.1, 0.2, # 中指
0.0, 0.1, 0.2, # 无名指
0.0, 0.1, 0.2, 0.2 # 小指
-0.178, 1.22, 0.134, 0.698, 0.361, # 拇指
0.2, 1.57, 1.3, # 食指
0.0, 2, 0.2, # 中指
0.0, 1, 0.2, # 无名指
0.0, -0.5, 0.2, 0.2 # 小指
]
elif self.model.nu >= 5:
# 简化的OK手势
Expand Down Expand Up @@ -266,9 +242,9 @@ def _create_pose_pointing(self):
0.0, 0.0, # 手腕
0.2, 0.3, 0.0, 0.2, 0.3, # 拇指
0.0, 0.0, 0.0, # 食指
1.57, 1.57, 1.57, # 中指
1.57, 1.57, 1.57, # 无名指
0.0, 1.57, 1.57, 1.57 # 小指
5, 5, 5, # 中指
5, 5, 5, # 无名指
0.0, 5, 5, 5, # 小指
]
elif self.model.nu >= 4:
# 简化的指点:假设第3个是食指
Expand Down Expand Up @@ -332,6 +308,114 @@ def update_animation(self):

return False

def toggle_pause(self):
"""切换暂停状态"""
with self.pause_lock:
if self.paused:
# 从暂停状态恢复
self.paused = False
pause_duration = time.time() - self.pause_start_time
self.total_pause_time += pause_duration
print(f"\n▶️ 演示继续 (暂停了 {pause_duration:.1f} 秒)")
else:
# 进入暂停状态
self.paused = True
self.pause_start_time = time.time()
print(f"\n⏸️ 演示暂停")

# 显示当前状态信息
current_pose = self.demo_sequence[self.current_pose_idx]
pose_info = self.poses[current_pose]
progress = (self.current_pose_idx + 1) / len(self.demo_sequence) * 100
print(f" 当前姿态: {current_pose} - {pose_info['description']}")
print(f" 进度: {progress:.1f}%")

def next_pose(self):
"""切换到下一个姿态"""
if not self.paused and not self.manual_mode:
# 只有在自动模式下才能手动切换
return False

with self.pause_lock:
if self.paused:
# 从暂停状态恢复但切换到下一个姿态
self.paused = False
pause_duration = time.time() - self.pause_start_time
self.total_pause_time += pause_duration

self.current_pose_idx = (self.current_pose_idx + 1) % len(self.demo_sequence)
next_pose = self.demo_sequence[self.current_pose_idx]
self.start_animation(next_pose)

print(f"\n⏭️ 切换到下一个姿态: {next_pose}")
return True

def previous_pose(self):
"""切换到上一个姿态"""
if not self.paused and not self.manual_mode:
# 只有在自动模式下才能手动切换
return False

with self.pause_lock:
if self.paused:
# 从暂停状态恢复但切换到上一个姿态
self.paused = False
pause_duration = time.time() - self.pause_start_time
self.total_pause_time += pause_duration

self.current_pose_idx = (self.current_pose_idx - 1) % len(self.demo_sequence)
previous_pose = self.demo_sequence[self.current_pose_idx]
self.start_animation(previous_pose)

print(f"\n⏮️ 切换到上一个姿态: {previous_pose}")
return True

def toggle_manual_mode(self):
"""切换手动模式"""
self.manual_mode = not self.manual_mode
if self.manual_mode:
print(f"\n🎮 切换到手动模式")
print(" 使用控制台命令控制")
else:
print(f"\n🤖 切换到自动模式")
return self.manual_mode

def print_controls(self):
"""打印控制说明"""
print("\n" + "=" * 60)
print("🎮 控制说明 (在终端中输入命令):")
print(" pause: 暂停/继续演示")
print(" next: 下一个姿态")
print(" prev: 上一个姿态")
print(" mode: 切换手动/自动模式")
print(" restart: 重新开始演示")
print(" help: 显示控制说明")
print(" quit: 退出演示")
print("=" * 60)

def process_command(self, command):
"""处理控制台命令"""
command = command.strip().lower()

if command == 'pause':
self.should_toggle_pause = True
elif command == 'next':
self.should_next_pose = True
elif command == 'prev':
self.should_prev_pose = True
elif command == 'mode':
self.should_toggle_mode = True
elif command == 'restart':
self.should_restart = True
elif command == 'help':
self.print_controls()
elif command == 'quit':
self.should_exit = True
print("\n👋 正在退出演示...")
elif command:
print(f"❌ 未知命令: {command}")
print("输入 'help' 查看可用命令")

def run_demo(self):
"""运行演示"""
print("\n" + "=" * 60)
Expand All @@ -340,15 +424,35 @@ def run_demo(self):
print(f"🎬 演示序列: {len(self.demo_sequence)} 个姿态")
print(f"⏱️ 每个姿态保持: 3.0秒")
print(f"🎥 动画过渡: {self.animation_duration}秒")
print("按 Ctrl+C 退出演示")
self.print_controls()
print("=" * 60)
print("\n💡 提示: 在终端中输入命令控制演示")
print("=" * 60)

# 设置初始姿态
initial_pose = self.demo_sequence[0]
self.data.ctrl[:] = self.poses[initial_pose]['values']

last_change = time.time()
hold_duration = 3.0 # 每个姿态保持3秒
hold_duration = 5 # 每个姿态保持3秒

# 启动一个线程来处理用户输入
import threading

def input_thread():
"""处理用户输入的线程"""
while not self.should_exit:
try:
command = input("\n> ").strip()
if command:
self.process_command(command)
except (EOFError, KeyboardInterrupt):
self.should_exit = True
break

# 启动输入线程
input_handler = threading.Thread(target=input_thread, daemon=True)
input_handler.start()

try:
with mujoco.viewer.launch_passive(self.model, self.data) as viewer:
Expand All @@ -366,19 +470,62 @@ def run_demo(self):
sys.stdout.write(f"进度: {0.0:5.1f}% - {pose_info['description']}")
sys.stdout.flush()

while viewer.is_running():
while viewer.is_running() and not self.should_exit:
current_time = time.time()

# 更新动画
self.update_animation()

# 检查是否需要切换到下一个姿态
if not self.animating and (current_time - last_change > hold_duration):
self.current_pose_idx = (self.current_pose_idx + 1) % len(self.demo_sequence)
next_pose = self.demo_sequence[self.current_pose_idx]

if self.start_animation(next_pose):
last_change = current_time
# 处理命令
if self.should_toggle_pause:
self.toggle_pause()
self.should_toggle_pause = False

if self.should_next_pose:
self.next_pose()
if not self.manual_mode:
self.toggle_manual_mode()
last_change = current_time
self.should_next_pose = False

if self.should_prev_pose:
self.previous_pose()
if not self.manual_mode:
self.toggle_manual_mode()
last_change = current_time
self.should_prev_pose = False

if self.should_toggle_mode:
self.toggle_manual_mode()
self.should_toggle_mode = False

if self.should_restart:
self.current_pose_idx = 0
self.start_animation(self.demo_sequence[0])
self.paused = False
self.total_pause_time = 0
last_change = current_time
print(f"\n🔄 重新开始演示")
self.should_restart = False

# 如果暂停,跳过更新
if self.paused:
viewer.sync()
time.sleep(0.01) # 减少CPU使用率
continue

# 在手动模式下,不自动切换姿态
if not self.manual_mode:
# 更新动画
self.update_animation()

# 检查是否需要切换到下一个姿态
if not self.animating and (current_time - last_change > hold_duration):
self.current_pose_idx = (self.current_pose_idx + 1) % len(self.demo_sequence)
next_pose = self.demo_sequence[self.current_pose_idx]

if self.start_animation(next_pose):
last_change = current_time
else:
# 手动模式下,只更新动画
self.update_animation()

# 运行仿真
mujoco.mj_step(self.model, self.data)
Expand All @@ -396,8 +543,10 @@ def run_demo(self):
import traceback
traceback.print_exc()
finally:
self.should_exit = True
print("\n" + "=" * 60)
print("🎉 演示结束")
print(f"⏱️ 总暂停时间: {self.total_pause_time:.1f}秒")
print("=" * 60)


Expand Down
Loading