Skip to content
Merged
Changes from all commits
Commits
Show all changes
65 commits
Select commit Hold shift + click to select a range
d113f75
init project
qiqi678 Sep 8, 2025
4e99912
init project
qiqi678 Sep 8, 2025
df58c95
补充详细注释、结构化代码
qiqi678 Sep 9, 2025
b56ed65
Remove .vscode directory (personal editor config)
qiqi678 Sep 10, 2025
4a1042d
基础人形机器人模型
qiqi678 Sep 15, 2025
0c60433
修改文件名及主函数中循环模拟功能
qiqi678 Sep 22, 2025
6399701
删除不必要的日志文件
qiqi678 Sep 22, 2025
f305ac3
Merge branch 'main' into main
qiqi678 Sep 22, 2025
ed8cdcc
实现仿真机器人的弹跳功能
qiqi678 Sep 26, 2025
4e7b97a
实现仿真机器人的弹跳功能
qiqi678 Sep 26, 2025
556131f
删除重复的带空格文件夹 humanoid_motion control
qiqi678 Sep 26, 2025
9e558fe
增加一个Python文件
qiqi678 Sep 29, 2025
a2731a8
Save local changes of README.md
qiqi678 Sep 29, 2025
e0da6e2
Fix merge conflict in README.md with upstream/main
qiqi678 Sep 29, 2025
bd05420
Save local changes of README.md
qiqi678 Sep 29, 2025
1b7cbf2
添加随机动作策略函数
qiqi678 Oct 5, 2025
b5e696f
解决合并冲突,合并 upstream/main 分支内容
qiqi678 Oct 5, 2025
f06d238
重新提交添加随机动作策略函数
qiqi678 Oct 6, 2025
d13b322
添加一个有空隙的走廊形竞技场环境
qiqi678 Oct 10, 2025
0b816b3
通过删除run_walls.py解决合并冲突
qiqi678 Oct 10, 2025
2df51c9
恢复run_walls.py并准备解决冲突
qiqi678 Oct 10, 2025
88f9b94
Merge branch 'main' into qiqi678-main
qiqi678 Oct 10, 2025
38e1b8e
修改文件名
qiqi678 Oct 20, 2025
700b3c4
Merge branch 'qiqi678-main' of https://github.com/qiqi678/nn into qiq…
qiqi678 Oct 20, 2025
33bc1b9
删除重复的humanoid_motion_control模块
qiqi678 Oct 20, 2025
5ccfa00
删除 main 分支残留的旧目录 humanoid_motion_control
qiqi678 Oct 20, 2025
44070f8
解决冲突:保留新目录humanoid_Simulation_Environment_Establish,删除旧目录残留
qiqi678 Oct 20, 2025
c331e10
解决冲突:同步课程仓库motion_main.py到新目录
qiqi678 Oct 20, 2025
30b9bd7
删除FETCH_HEAD和git文件
qiqi678 Oct 20, 2025
2d86570
完善README.md说明文件
qiqi678 Oct 20, 2025
e2cda97
将main.py添加ROS节点并初始化代码
qiqi678 Oct 21, 2025
833b7d4
Merge branch 'main' into qiqi678-main
qiqi678 Oct 21, 2025
38d80f9
为main.py添加launch启动文件
qiqi678 Oct 27, 2025
544f1f9
Merge branch 'qiqi678-main' of https://github.com/qiqi678/nn into qiq…
qiqi678 Oct 27, 2025
1ea7ed7
为main.launch中路径添加注释
qiqi678 Oct 28, 2025
ce34b58
为main_humanoid.py添加ROS节点
qiqi678 Oct 28, 2025
b028a35
为main_humanoid.py文件添加详细注释
qiqi678 Nov 2, 2025
25ba501
Merge branch 'main' into qiqi678-main
qiqi678 Nov 2, 2025
272b96a
规范模块名
qiqi678 Nov 3, 2025
88864b4
Merge branch 'qiqi678-main' of https://github.com/qiqi678/nn into qiq…
qiqi678 Nov 3, 2025
196732c
Standardize filename case: track main.py (lowercase)
qiqi678 Nov 3, 2025
64287e6
Merge branch 'main' of https://github.com/OpenHUTB/nn into qiqi678-main
qiqi678 Nov 3, 2025
8df2fe7
Retain both Main.py and main.py from upstream (case-sensitive enabled)
qiqi678 Nov 3, 2025
15e1d51
为main_humanoid.py添加launch启动文件
qiqi678 Nov 4, 2025
e472647
优化main_humanoid.py,添加键盘控制功能
qiqi678 Nov 11, 2025
7857978
Merge branch 'main' into qiqi678-main
qiqi678 Nov 11, 2025
a696643
优化motion_main.py,重构带空隙的走廊环境构建逻辑
qiqi678 Nov 17, 2025
1ddbff5
Merge branch 'qiqi678-main' of https://github.com/qiqi678/nn into qiq…
qiqi678 Nov 18, 2025
f8a4241
恢复并合并本地button_control_vehicle文件夹的备份文件
qiqi678 Nov 18, 2025
4cbeaab
优化motion_main内容,重构空隙的走廊环境构建逻辑
qiqi678 Nov 18, 2025
9233641
恢复carla_manual_control.launch至原作者版本(哈希68c9df5)
qiqi678 Nov 18, 2025
418cea7
优化文件目录结构,确保以main.py为启动入口,将原各文件实现功能整合至main.py
qiqi678 Nov 24, 2025
2d20430
优化文件目录结构,确保以main.py为启动入口,将原各文件实现功能整合至main.py
qiqi678 Nov 24, 2025
adf9506
从课程仓库main分支恢复main_humanoid.py文件
qiqi678 Nov 24, 2025
6684b9f
删除旧的 main_humanoid.py 文件(优化目录结构后已不需要,并优化 main.py
qiqi678 Nov 25, 2025
1e91c2a
合并课程仓库main分支的更新,解决main.py冲突
qiqi678 Nov 25, 2025
fefd95c
添加无重力模式支持并优化键盘控制器
qiqi678 Dec 2, 2025
6214b84
Merge branch 'main' into qiqi678-main
qiqi678 Dec 2, 2025
f045055
优化main.py人形机器人控制逻辑
qiqi678 Dec 8, 2025
86fb13e
Merge branch 'qiqi678-main' of https://github.com/qiqi678/nn into qiq…
qiqi678 Dec 8, 2025
0305eb0
Merge branch 'main' into qiqi678-main
qiqi678 Dec 8, 2025
25d5bb6
集成深度学习控制器,优化步态自然度与无重力模式稳定性
qiqi678 Dec 12, 2025
756c044
完善README.md说明文件
qiqi678 Dec 13, 2025
e0937a4
增强监控模块+优化格式+新增衍生指标
qiqi678 Dec 20, 2025
f8790d9
Merge branch 'main' into qiqi678-main
qiqi678 Dec 20, 2025
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
243 changes: 205 additions & 38 deletions src/humanoid_simulation_environment_establish/main.py
Original file line number Diff line number Diff line change
Expand Up @@ -858,6 +858,70 @@ def _create_turning_only_action(self, turn_direction, dt=0.03):

return action

def _create_turning_only_action(self, turn_direction, dt=0.03):
"""创建仅转向动作(不产生腿部摆动,只在原地转向,目标转向45度)"""
action = np.zeros(self.action_dim)

if not self.actuator_indices:
return action

# 更新目标转向角度(每次按键设置目标为45度)
turn_velocity = 0.0
if turn_direction != 0:
# 计算转向误差
turn_error = self.target_turn_angle - self.current_turn_angle

# 如果接近目标角度,重置目标(允许连续转向)
if abs(turn_error) < 0.1: # 接近目标时,设置新的目标
self.target_turn_angle += turn_direction * self.turn_angle_per_step

# 计算转向速度(基于误差)
turn_velocity = np.clip(turn_error * 3.0, -self.turn_speed, self.turn_speed)

# 更新当前转向角度(模拟)
self.current_turn_angle += turn_velocity * dt
else:
# 没有转向指令时,逐渐减小转向角度
self.current_turn_angle *= 0.95
self.target_turn_angle = self.current_turn_angle # 同步目标角度

# 根据转向速度计算转向强度(归一化到-1到1)
if abs(turn_velocity) > 0.01:
normalized_turn = np.clip(turn_velocity / self.turn_speed, -1.0, 1.0)
else:
# 如果没有转向速度,直接使用方向(简化控制)
normalized_turn = turn_direction * 0.8 # 直接使用方向,强度0.8

# 原地转向:通过髋关节外展和躯干旋转实现
# 增大转向强度,使转向更明显
hip_turn_strength = 0.6 * normalized_turn # 从0.25增大到0.6
self._set_action(action, "hip_z_right", hip_turn_strength)
self._set_action(action, "hip_z_left", -hip_turn_strength)

# 躯干旋转辅助转向(主要转向来源,范围±45度)
abdomen_turn_strength = 0.8 * normalized_turn # 从0.15增大到0.8,充分利用±45度范围
self._set_action(action, "abdomen_z", abdomen_turn_strength)
self._set_action(action, "abdomen_x", 0.1 * normalized_turn)

# 躯干控制 - 更自然的轻微摆动
# 轻微前倾以辅助前进(减小前倾幅度,更自然)
abdomen_pitch = 0.08 * direction * gait_strength
# 添加轻微的上下摆动(配合步态,与腿部动作协调)
# 在支撑相时稍微下沉,在摆动相时稍微上升
abdomen_pitch += 0.02 * np.sin(phase + np.pi/4) * gait_strength
self._set_action(action, "abdomen_y", abdomen_pitch)

# 转向时允许侧倾(减小侧倾幅度)
self._set_action(action, "abdomen_x", 0.05 * turn_direction * gait_strength)

# 转向控制(减小转向幅度,更自然)
if turn_direction != 0:
self._set_action(action, "abdomen_z", 0.25 * turn_direction * gait_strength)
else:
self._set_action(action, "abdomen_z", 0.0)

return action

def _create_turning_only_action(self, turn_direction, dt=0.03):
"""创建仅转向动作(不产生腿部摆动,只在原地转向,目标转向45度)"""
action = np.zeros(self.action_dim)
Expand Down Expand Up @@ -1719,16 +1783,25 @@ def main():
# 将环境切换为“无重力”模式
env = GapCorridorEnvironment(corridor_length=100, corridor_width=10, use_gravity=False)

print("\n环境已初始化")
print(f"执行器数量: {env.model.nu}")
print(f"关节数量: {env.model.nq}")

controller = KeyboardController(env.model.nu, env.get_actuator_indices())
obs = env.reset()
total_reward = 0.0

print("\n启动MuJoCo交互式查看器...")
print("按 ESC 或关闭窗口退出程序")
print("\n" + "="*80)
print("🚀 环境初始化完成")
print("-"*80)
print(f" 执行器数量: {env.model.nu}")
print(f" 关节数量: {env.model.nq}")
print(f" 观测维度: {len(obs)}")
print(f" 重力模式: {'启用' if env.use_gravity else '禁用(无重力模式)'}")
print(f" 控制时间步: {env.control_timestep:.3f}s")
print(f" 物理时间步: {env.timestep:.3f}s")
print(f" 最大Episode步数: {env._max_episode_steps}")
print("="*80)

print("\n📺 启动MuJoCo交互式查看器...")
print(" 提示: 在查看器窗口中按键盘进行控制")
print(" 提示: 按 ESC 或关闭窗口退出程序")

try:
viewer_handle = mujoco.viewer.launch_passive(
Expand All @@ -1739,7 +1812,9 @@ def main():
show_right_ui=True
)

print("\n查看器已启动,开始仿真循环...")
print("\n✅ 查看器已启动,开始仿真循环...")
print(f" 状态报告将每100步输出一次")
print("")

step = 0
last_move_state = None # 记录上次移动状态,用于检测状态变化
Expand Down Expand Up @@ -1831,59 +1906,151 @@ def main():
if step % 100 == 0:
# 获取各个身体部位的位置
torso_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "torso")
torso_pos = env.data.xpos[torso_id] if torso_id >= 0 else None
torso_pos = env.data.xpos[torso_id].copy() if torso_id >= 0 else None

head_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "head")
head_pos = env.data.xpos[head_id] if head_id >= 0 else None
head_pos = env.data.xpos[head_id].copy() if head_id >= 0 else None

# 尝试获取左右脚位置(可能有不同的命名)
foot_right_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "foot_right")
if foot_right_id < 0:
foot_right_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "right_foot")
foot_right_pos = env.data.xpos[foot_right_id] if foot_right_id >= 0 else None
foot_right_pos = env.data.xpos[foot_right_id].copy() if foot_right_id >= 0 else None

foot_left_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "foot_left")
if foot_left_id < 0:
foot_left_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "left_foot")
foot_left_pos = env.data.xpos[foot_left_id] if foot_left_id >= 0 else None
foot_left_pos = env.data.xpos[foot_left_id].copy() if foot_left_id >= 0 else None

# 获取速度信息
linear_vel = np.zeros(6)
angular_vel = np.zeros(6)
if torso_id >= 0:
mujoco.mj_objectVelocity(env.model, env.data, mujoco.mjtObj.mjOBJ_BODY, torso_id, linear_vel, 0)
mujoco.mj_objectVelocity(env.model, env.data, mujoco.mjtObj.mjOBJ_BODY, torso_id, angular_vel, 1)
vx, vy, vz = linear_vel[0], linear_vel[1], linear_vel[2]
angular_vz = angular_vel[5] # 绕Z轴角速度(转向)
speed = np.sqrt(vx**2 + vy**2)

# 获取步态相位信息(如果有深度学习控制器)
gait_info = ""
if controller.deep_controller is not None:
gait_phase_deg = np.degrees(controller.deep_controller.gait_phase) % 360
gait_info = f"步态相位: {gait_phase_deg:.1f}°, 步频: {controller.deep_controller.gait_frequency:.2f}Hz"

# 获取动作统计信息
action_magnitude = np.max(np.abs(action))
action_mean = np.mean(np.abs(action))
action_std = np.std(action)

# 获取键盘控制状态
control_state = []
if controller.move_forward:
control_state.append("前进")
if controller.move_backward:
control_state.append("后退")
if controller.turn_left:
control_state.append("左转")
if controller.turn_right:
control_state.append("右转")
if not control_state:
control_state.append("静止")
control_str = "+".join(control_state) if control_state else "静止"

# 获取步态时间信息
step_time_info = f"步态时间: {controller.step_time:.2f}s"

# 尝试获取左右手位置
hand_right_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "hand_right")
if hand_right_id < 0:
hand_right_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "right_hand")
hand_right_pos = env.data.xpos[hand_right_id] if hand_right_id >= 0 else None
# 计算运行时间(模拟)
sim_time = step * env.control_timestep

hand_left_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "hand_left")
if hand_left_id < 0:
hand_left_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "left_hand")
hand_left_pos = env.data.xpos[hand_left_id] if hand_left_id >= 0 else None
# 获取奖励信息(当前步奖励和累计奖励)
recent_reward = reward # 当前步的奖励
avg_reward_per_step = total_reward / max(step, 1)

# 尝试获取骨盆位置
pelvis_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "pelvis")
pelvis_pos = env.data.xpos[pelvis_id] if pelvis_id >= 0 else None
# 打印分隔线
print("\n" + "="*80)
print(f"📊 状态报告 [Step {step} | 模拟时间: {sim_time:.2f}s]")
print("-"*80)

# 格式化输出
info_parts = []
# 控制状态
print(f"🎮 控制状态: {control_str:20s} | {step_time_info}")
if gait_info:
print(f"🚶 {gait_info}")

# 速度和运动信息
print(f"\n⚡ 运动信息:")
print(f" 线性速度: X={vx:+.3f} m/s, Y={vy:+.3f} m/s, Z={vz:+.3f} m/s")
print(f" 速度大小: {speed:.3f} m/s")
print(f" 角速度: Z轴旋转={np.degrees(angular_vz):+.2f} °/s")

# 位置信息(关键部位)
print(f"\n📍 身体部位位置:")
if torso_pos is not None:
info_parts.append(f"躯干={torso_pos}")
print(f" 躯干: X={torso_pos[0]:+.3f}, Y={torso_pos[1]:+.3f}, Z={torso_pos[2]:+.3f} m")
if head_pos is not None:
info_parts.append(f"头部={head_pos}")
if pelvis_pos is not None:
info_parts.append(f"骨盆={pelvis_pos}")
head_height = head_pos[2] - (torso_pos[2] if torso_pos is not None else 0)
print(f" 头部: X={head_pos[0]:+.3f}, Y={head_pos[1]:+.3f}, Z={head_pos[2]:+.3f} m (相对高度: {head_height:+.3f} m)")
if foot_right_pos is not None:
info_parts.append(f"右脚={foot_right_pos}")
print(f" 右脚: X={foot_right_pos[0]:+.3f}, Y={foot_right_pos[1]:+.3f}, Z={foot_right_pos[2]:+.3f} m")
if foot_left_pos is not None:
info_parts.append(f"左脚={foot_left_pos}")
if hand_right_pos is not None:
info_parts.append(f"右手={hand_right_pos}")
if hand_left_pos is not None:
info_parts.append(f"左手={hand_left_pos}")
print(f" 左脚: X={foot_left_pos[0]:+.3f}, Y={foot_left_pos[1]:+.3f}, Z={foot_left_pos[2]:+.3f} m")
# 计算步长(两脚间的X距离)
if foot_right_pos is not None:
step_length = abs(foot_right_pos[0] - foot_left_pos[0])
print(f" 步长: {step_length:.3f} m")

# 动作信息
print(f"\n🎯 动作统计:")
print(f" 动作维度: {len(action)}")
print(f" 最大幅度: {action_magnitude:.3f}")
print(f" 平均幅度: {action_mean:.3f}")
print(f" 标准差: {action_std:.3f}")

# 奖励信息
print(f"\n🏆 奖励信息:")
print(f" 当前步奖励: {recent_reward:+.4f}")
print(f" 累计奖励: {total_reward:+.4f}")
print(f" 平均奖励/步: {avg_reward_per_step:+.4f}")

# 深度学习信息(如果启用)
if controller.use_deep_learning and controller.deep_controller is not None:
buffer_size = len(controller.deep_controller.replay_buffer)
print(f"\n🧠 深度学习控制器:")
print(f" 经验缓冲区: {buffer_size}/{controller.deep_controller.replay_buffer.maxlen}")
print(f" 训练步数: {controller.deep_controller.step_count}")
print(f" 状态维度: {controller.deep_controller.state_dim}, 动作维度: {controller.deep_controller.action_dim}")

info_str = ", ".join(info_parts)
print(f"Step {step}: {info_str}, 累计奖励 = {total_reward:.2f}")
print("="*80 + "\n")

if done:
print(f"\nEpisode finished. Total reward: {total_reward:.2f}")
# 计算Episode统计信息
episode_duration = step * env.control_timestep
avg_reward_per_step = total_reward / max(step, 1)

# 获取最终位置信息
final_torso_id = mujoco.mj_name2id(env.model, mujoco.mjtObj.mjOBJ_BODY, "torso")

print("\n" + "="*80)
print("🎯 Episode 结束")
print("-"*80)
print(f" 总步数: {step}")
print(f" 持续时间: {episode_duration:.2f}s")
print(f" 累计奖励: {total_reward:+.4f}")
print(f" 平均奖励/步: {avg_reward_per_step:+.4f}")

# 获取最终位置信息
if final_torso_id >= 0:
final_torso_pos = env.data.xpos[final_torso_id]
print(f" 最终位置: X={final_torso_pos[0]:+.3f}, Y={final_torso_pos[1]:+.3f}, Z={final_torso_pos[2]:+.3f} m")
# 计算前进距离(从初始位置)
initial_pos = env._root_joint_qpos_start
if initial_pos is not None and (initial_pos + 2) < len(env.data.qpos):
initial_x = env.data.qpos[initial_pos]
distance_traveled = final_torso_pos[0] - initial_x
print(f" 前进距离: {distance_traveled:+.3f} m")

print("="*80 + "\n")

obs = env.reset()
total_reward = 0.0
step = 0
Expand Down
Loading