Skip to content
Merged
Show file tree
Hide file tree
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
37 changes: 37 additions & 0 deletions src/Mujoco_manrun/config/gait_params.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,37 @@
gait_config:
SLOW:
freq: 0.3
amp: 0.3
coupling: 0.3
speed_freq_gain: 0.2
speed_amp_gain: 0.1
com_z_offset: 0.02
NORMAL:
freq: 0.5
amp: 0.4
coupling: 0.2
speed_freq_gain: 0.4
speed_amp_gain: 0.2
com_z_offset: 0.0
TROT:
freq: 0.8
amp: 0.5
coupling: 0.25
speed_freq_gain: 0.5
speed_amp_gain: 0.3
com_z_offset: -0.01
STEP_IN_PLACE:
freq: 0.4
amp: 0.2
coupling: 0.3
speed_freq_gain: 0.0
speed_amp_gain: 0.0
com_z_offset: 0.01
lock_torso: true

sim_params:
init_wait_time: 4.0
sim_duration: 120.0
foot_contact_threshold: 1.5
com_safety_threshold: 0.6
speed_reduction_factor: 0.5
4 changes: 4 additions & 0 deletions src/Mujoco_manrun/launch/mujoco_manrun.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
<launch>
<rosparam command="load" file="$(find mujoco_manrun)/config/gait_params.yaml" />
<node name="mujoco_manrun_node" pkg="mujoco_manrun" type="main.py" output="screen" />
</launch>
184 changes: 184 additions & 0 deletions src/Mujoco_manrun/scripts/humanoid_stabilizer.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,184 @@
import sys
import os
SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__))
sys.path.insert(0, SCRIPT_DIR)

import mujoco
import numpy as np
import time
from cpg_oscillator import CPGOscillator
from sensor_simulator import SensorSimulator
from utils import quat_to_euler_xyz, clip_value
from keyboard_handler import KeyboardInputHandler # 新增导入

class HumanoidStabilizer:
def __init__(self, model_path):
# 加载Mujoco模型
self.model = mujoco.MjModel.from_xml_path(model_path)
self.data = mujoco.MjData(self.model)

# 初始化CPG振荡器(步态生成)
self.cpg = CPGOscillator()

# 初始化传感器模拟
self.sensor = SensorSimulator(self.model, self.data)

# 控制参数
self.velocity = 0.0 # 前进速度
self.turn_rate = 0.0 # 转向速率
self.gait_mode = "NORMAL" # 默认步态
self.turn_angle = 0.0 # 新增转向角度参数
self.walk_speed = 0.5 # 新增行走速度参数
self.state = "STAND" # 新增状态参数
self.enable_sensor_simulation = True # 传感器模拟开关

# ROS相关初始化(兼容处理)
self.has_ros = False
self.ros_handler = None

def _update_control(self):
"""更新机器人控制指令(融合键盘/ROS输入)"""
# 1. 从CPG生成步态关节指令(修复原代码中cpg.generate_gait不存在的问题)
# 这里使用基础的CPG输出作为关节目标(实际需根据模型关节结构调整)
dt = 0.005 # 与仿真步长一致
cpg_output = self.cpg.update(dt, speed_factor=self.walk_speed, turn_factor=self.turn_rate)

# 示例关节映射(需根据实际模型关节名称调整)
joint_targets = {
"left_hip": cpg_output,
"right_hip": -cpg_output,
"left_knee": -cpg_output * 0.8,
"right_knee": cpg_output * 0.8
}

# 2. 设置关节控制指令
for joint_name, target_pos in joint_targets.items():
try:
joint_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, joint_name)
self.data.ctrl[joint_id] = target_pos
except:
continue

# 3. 限制控制指令范围
self.data.ctrl = clip_value(self.data.ctrl, -1.5, 1.5)

def set_gait_mode(self, mode):
"""设置步态模式(扩展支持更多模式)"""
valid_modes = ["NORMAL", "SLOW", "FAST", "TROT", "STEP_IN_PLACE"]
self.gait_mode = mode if mode in valid_modes else "NORMAL"
# 根据步态模式调整CPG参数
gait_params = {
"SLOW": {"freq": 0.3, "amp": 0.3},
"NORMAL": {"freq": 0.5, "amp": 0.4},
"FAST": {"freq": 0.7, "amp": 0.5},
"TROT": {"freq": 0.8, "amp": 0.6},
"STEP_IN_PLACE": {"freq": 0.4, "amp": 0.3}
}
self.cpg.base_freq = gait_params[self.gait_mode]["freq"]
self.cpg.base_amp = gait_params[self.gait_mode]["amp"]

def set_velocity(self, v):
"""设置前进速度(-1.0 ~ 1.0)"""
self.velocity = clip_value(v, -1.0, 1.0, "速度")

def set_turn_rate(self, tr):
"""设置转向速率(-1.0 ~ 1.0)"""
self.turn_rate = clip_value(tr, -1.0, 1.0, "转向速率")

def set_turn_angle(self, ta):
"""设置转向角度"""
self.turn_angle = clip_value(ta, -np.pi/4, np.pi/4, "转向角度")

def set_walk_speed(self, ws):
"""设置行走速度"""
self.walk_speed = clip_value(ws, 0.1, 1.0, "行走速度")

def set_state(self, state):
"""设置机器人状态"""
valid_states = ["STAND", "WALK", "STOP", "EMERGENCY"]
self.state = state if state in valid_states else "STAND"
if self.state == "EMERGENCY":
self.data.ctrl[:] = 0 # 紧急停止时归零控制

def print_sensor_data(self):
"""打印传感器数据"""
self.sensor.print_sensor_data()

def simulate(self):
"""主仿真循环:控制+可视化"""
# 启动键盘监听线程
keyboard_handler = KeyboardInputHandler(self)
keyboard_handler.start()

# ========== 兼容新旧Mujoco版本的Viewer启动 ==========
try:
# 新版Mujoco(2.3.0+)
import mujoco.viewer
viewer = mujoco.viewer.launch_passive(self.model, self.data)
use_new_viewer = True
viewer_running = True # 新增状态标记
except (ImportError, AttributeError):
# 旧版Mujoco(2.1.0-)或mujoco-py
import mujoco.glfw as glfw
glfw.init()
window = glfw.create_window(1280, 720, "Humanoid Simulation", None, None)
glfw.make_context_current(window)
glfw.swap_interval(1)
viewer = mujoco.MjViewer(window)
viewer.set_model(self.model)
use_new_viewer = False

# 主仿真循环
print("仿真启动!按H查看控制帮助")
while True:
# 退出条件(修复新版Viewer判断)
if use_new_viewer:
try:
viewer.sync() # 尝试同步,失败则表示窗口已关闭
except:
viewer_running = False
if not viewer_running:
break
else:
if glfw.window_should_close(window):
break

# 状态机控制
if self.state == "STOP":
self.velocity = 0
self.turn_rate = 0

# 2. 更新控制指令(融合ROS/键盘)
self._update_control()

# 3. 更新传感器数据
self.sensor.get_sensor_data(self.gait_mode)

# 4. 执行一步仿真
mujoco.mj_step(self.model, self.data)

# 5. 更新可视化
if not use_new_viewer:
viewer.render()
glfw.swap_buffers(window)
glfw.poll_events()

# 6. 控制仿真频率(200Hz)
time.sleep(0.005)

# 清理资源
keyboard_handler.running = False # 停止键盘线程
keyboard_handler.join()
if not use_new_viewer:
glfw.destroy_window(window)
glfw.terminate()
print("仿真结束!")

# 测试代码(单独运行时)
if __name__ == "__main__":
model_path = os.path.join(SCRIPT_DIR, "../models/humanoid.xml")
# 检查备用路径
if not os.path.exists(model_path):
model_path = os.path.join(SCRIPT_DIR, "models/humanoid.xml")
stabilizer = HumanoidStabilizer(model_path)
stabilizer.simulate()
85 changes: 85 additions & 0 deletions src/Mujoco_manrun/scripts/keyboard_handler.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,85 @@
import threading
import time
import sys
import numpy as np

class KeyboardInputHandler(threading.Thread):
"""键盘输入监听线程(独立模块,仅处理按键指令)"""
def __init__(self, stabilizer):
super().__init__(daemon=True)
self.stabilizer = stabilizer
self.running = True

def run(self):
"""打印控制说明并启动监听"""
self._print_help()
while self.running:
try:
key = self._get_key_non_blocking()
if key:
self._handle_key(key)
time.sleep(0.01)
except:
continue

def _print_help(self):
"""打印控制指令说明"""
print("\n===== 控制指令说明 =====")
print("w: 开始行走 | s: 停止行走 | e: 紧急停止 | r: 恢复站立")
print("a: 左转 | d: 右转 | 空格: 原地转向 | z: 减速 | x: 加速")
print("m: 传感器模拟开关 | p: 打印传感器数据")
print("1: 慢走 | 2: 正常走 | 3: 小跑 | 4: 原地踏步")
print("========================\n")

def _get_key_non_blocking(self):
"""非阻塞获取键盘输入(跨平台)"""
if sys.platform == "win32":
import msvcrt
if msvcrt.kbhit():
return msvcrt.getch().decode('utf-8').lower()
else:
import select
if sys.stdin in select.select([sys.stdin], [], [], 0)[0]:
return sys.stdin.read(1).lower()
return None

def _handle_key(self, key):
"""处理按键指令"""
if key == 'w':
current_gait = self.stabilizer.gait_mode
self.stabilizer.set_state("WALK")
self.stabilizer.set_gait_mode(current_gait)
print(f"[指令] 行走 | 步态: {current_gait} | 速度: {self.stabilizer.walk_speed:.2f}")
elif key == 's':
self.stabilizer.set_state("STOP")
print("[指令] 停止行走")
elif key == 'e':
self.stabilizer.set_state("EMERGENCY")
print("[指令] 紧急停止")
elif key == 'r':
self.stabilizer.set_state("STAND")
print("[指令] 恢复站立")
elif key == 'a':
self.stabilizer.set_turn_angle(self.stabilizer.turn_angle + 0.05)
print(f"[指令] 左转 | 转向: {self.stabilizer.turn_angle:.2f}rad")
elif key == 'd':
self.stabilizer.set_turn_angle(self.stabilizer.turn_angle - 0.05)
print(f"[指令] 右转 | 转向: {self.stabilizer.turn_angle:.2f}rad")
elif key == ' ':
self.stabilizer.set_turn_angle(0.2 if self.stabilizer.turn_angle <= 0 else -0.2)
print(f"[指令] 原地转向 | 转向: {self.stabilizer.turn_angle:.2f}rad")
elif key == 'z':
self.stabilizer.set_walk_speed(self.stabilizer.walk_speed - 0.1)
print(f"[指令] 减速 | 速度: {self.stabilizer.walk_speed:.2f}")
elif key == 'x':
self.stabilizer.set_walk_speed(self.stabilizer.walk_speed + 0.1)
print(f"[指令] 加速 | 速度: {self.stabilizer.walk_speed:.2f}")
elif key == 'm':
self.stabilizer.enable_sensor_simulation = not self.stabilizer.enable_sensor_simulation
print(f"[指令] 传感器模拟{'开启' if self.stabilizer.enable_sensor_simulation else '关闭'}")
elif key == 'p':
self.stabilizer.print_sensor_data()
elif key in ['1', '2', '3', '4']:
gait_map = {'1': 'SLOW', '2': 'NORMAL', '3': 'TROT', '4': 'STEP_IN_PLACE'}
self.stabilizer.set_gait_mode(gait_map[key])
print(f"[指令] 切换为{self.stabilizer.gait_mode}模式")
45 changes: 45 additions & 0 deletions src/Mujoco_manrun/scripts/main.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,45 @@
import sys
import os
# 强制添加源码目录到Python路径(解决ROS编译后导入问题)
SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__))
sys.path.insert(0, SCRIPT_DIR)

def main():
"""主函数:初始化+启动仿真"""
# 1. 检查模型路径
model_path = os.path.join(SCRIPT_DIR, "../models/humanoid.xml")
if not os.path.exists(model_path):
# 备用路径(ROS编译后)
model_path = os.path.join(SCRIPT_DIR, "models/humanoid.xml")
if not os.path.exists(model_path):
print(f"错误:找不到模型文件 {model_path}")
sys.exit(1)
print(f"加载模型:{model_path}")

# 2. 导入核心模块(修复后)
try:
from humanoid_stabilizer import HumanoidStabilizer
from ros_handler import ROSHandler
except ImportError as e:
print(f"导入模块失败:{e}")
sys.exit(1)

# 3. 初始化机器人控制器
stabilizer = HumanoidStabilizer(model_path)

# 4. 启动ROS监听线程
ros_handler = ROSHandler(stabilizer)
ros_handler.start()

# 5. 启动仿真
try:
stabilizer.simulate()
except KeyboardInterrupt:
print("用户中断仿真")
finally:
# 停止ROS线程
ros_handler.stop()
ros_handler.join()

if __name__ == "__main__":
main()
Loading
Loading