根据优化的代码完善README.md文件

src/Neuro_Mujoco/README.md

@@ -44,10 +44,29 @@ python mujoco_utils.py visualize /path/to/your/model.xml --ros --policy /path/to
 
 # 目录自动加载（选目录内第一个模型文件）
 python mujoco_utils.py visualize /home/lan/桌面/nn/mujoco_menagerie/anybotics_anymal_b
+
+# 启动交互式模型选择菜单（支持PD控制、预设位置切换）
+python mujoco_utils.py menu
+
 1.2 交互说明
 窗口操作：鼠标拖拽旋转、滚轮缩放，按 ESC 键退出；
 关节状态：仅发布非自由关节（排除 mjJNT_FREE 类型）的位置 / 速度；
 策略映射：输出 [-1,1] 自动线性缩放至执行器 ctrlrange 范围，并强制裁剪至物理极限。
+菜单模式专属指令：
+- 预设位置切换：直接输入预设名称（如home/up/left，根据模型不同有所差异）
+- 自定义关节角度：set <关节号> <角度>（示例：set 0 0.5，单位rad）
+- 仿真控制：pause（暂停）/ resume（继续）/ exit（退出）
+- ROS键盘控制（需ROS环境）：在新终端运行 `python keyboard_control.py <关节数>`
+
+
+## 支持的预设模型
+工具内置4种常用机器人模型，可通过交互式菜单直接选择：
+1. Franka Panda（机械臂）：7关节，支持home/up/left/right预设位置
+2. UR5 机械臂：6关节，支持home/up/forward预设位置
+3. Franka Panda（带手爪）：8关节（含手爪），支持home/open_gripper/up_open预设位置
+4. Walker2d 机器人：6关节，支持stand/walk_left/walk_right预设位置
+
+
 ### 模型格式转换
 2.1 运行命令
 shell
@@ -103,6 +122,17 @@ class PolicyNetwork(nn.Module):
 输出：归一化控制指令（[-1,1]），自动映射至模型 actuator_ctrlrange 范围：
 python
 运行
+
+## PD控制器说明
+适用于模型精确位置控制，核心参数与逻辑：
+1. 控制参数：每个模型预配置KP（比例增益）和KD（微分增益），如Franka Panda默认KP=800.0、KD=60.0
+2. 控制逻辑：
+   ```python
+   # 核心PD计算
+   pos_error = 目标关节位置 - 当前关节位置
+   vel_error = -当前关节速度
+   关节力矩 = KP * pos_error + KD * vel_error
+
 ### 核心映射逻辑
 action = ctrl_min + (ctrl_max - ctrl_min) * (action + 1) / 2
 action = np.clip(action, ctrl_min, ctrl_max)  # 强制裁剪至物理极限

src/Neuro_Mujoco/cakin_ws/src/mujoco_ros/scripts/keyboard_control.py

@@ -0,0 +1,114 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import rospy
+from std_msgs.msg import Float32MultiArray
+from pynput import keyboard
+import sys
+import argparse
+
+# 全局变量
+pub = None
+delta_pos = None
+joint_num = 0
+step_size = 0.05  # 每次按键的角度增量（弧度）
+
+def on_press(key):
+    """键盘按下事件处理"""
+    global delta_pos
+    try:
+        # 将 delta_pos 重置为全零
+        delta_pos = [0.0] * joint_num
+
+        # 根据按键设置对应的关节增量
+        if key.char == '1':
+            delta_pos[0] = step_size
+        elif key.char == 'q':
+            delta_pos[0] = -step_size
+        elif key.char == '2':
+            delta_pos[1] = step_size
+        elif key.char == 'w':
+            delta_pos[1] = -step_size
+        elif key.char == '3' and joint_num > 2:
+            delta_pos[2] = step_size
+        elif key.char == 'e' and joint_num > 2:
+            delta_pos[2] = -step_size
+        elif key.char == '4' and joint_num > 3:
+            delta_pos[3] = step_size
+        elif key.char == 'r' and joint_num > 3:
+            delta_pos[3] = -step_size
+        elif key.char == '5' and joint_num > 4:
+            delta_pos[4] = step_size
+        elif key.char == 't' and joint_num > 4:
+            delta_pos[4] = -step_size
+        elif key.char == '6' and joint_num > 5:
+            delta_pos[5] = step_size
+        elif key.char == 'y' and joint_num > 5:
+            delta_pos[5] = -step_size
+        elif key.char == '7' and joint_num > 6:
+            delta_pos[6] = step_size
+        elif key.char == 'u' and joint_num > 6:
+            delta_pos[6] = -step_size
+        elif key.char == '8' and joint_num > 7:
+            delta_pos[7] = step_size
+        elif key.char == 'i' and joint_num > 7:
+            delta_pos[7] = -step_size
+
+        # 如果有增量，则发布消息
+        if any(d != 0 for d in delta_pos):
+            msg = Float32MultiArray(data=delta_pos)
+            pub.publish(msg)
+            rospy.loginfo(f"发布关节增量: {delta_pos}")
+
+    except AttributeError:
+        # 处理特殊按键（如方向键），这里我们忽略
+        pass
+
+def on_release(key):
+    """键盘释放事件处理"""
+    # 当按键释放时，发布一个全零的增量，让机器人停止在当前位置
+    if key == keyboard.Key.esc:
+        # 按下ESC键退出
+        rospy.loginfo("检测到ESC键，退出键盘控制...")
+        return False
+
+def main():
+    global pub, joint_num, delta_pos
+    rospy.init_node('keyboard_controller', anonymous=True)
+
+    # 使用 argparse 接收命令行参数
+    parser = argparse.ArgumentParser(description='ROS Keyboard Controller for MuJoCo')
+    parser.add_argument('joint_num', type=int, help='Number of joints to control (e.g., 7 for Franka)')
+    parser.add_argument('--step', type=float, default=0.05, help='Step size in radians for each key press.')
+    args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:]) # 处理ROS参数
+
+    joint_num = args.joint_num
+    step_size = args.step
+    delta_pos = [0.0] * joint_num
+
+    pub = rospy.Publisher('joint_position_delta', Float32MultiArray, queue_size=10)
+
+    rospy.loginfo("="*50)
+    rospy.loginfo("      ROS 键盘控制器已启动      ")
+    rospy.loginfo(f"控制关节数: {joint_num}, 步长: {step_size} rad")
+    rospy.loginfo("按键映射:")
+    rospy.loginfo("  关节1: '1' (增加) / 'q' (减少)")
+    rospy.loginfo("  关节2: '2' (增加) / 'w' (减少)")
+    rospy.loginfo("  关节3: '3' (增加) / 'e' (减少)")
+    rospy.loginfo("  关节4: '4' (增加) / 'r' (减少)")
+    rospy.loginfo("  关节5: '5' (增加) / 't' (减少)")
+    rospy.loginfo("  关节6: '6' (增加) / 'y' (减少)")
+    rospy.loginfo("  关节7: '7' (增加) / 'u' (减少)")
+    rospy.loginfo("  关节8: '8' (增加) / 'i' (减少)")
+    rospy.loginfo("  退出: 按 'ESC' 键")
+    rospy.loginfo("="*50)
+
+    # 启动键盘监听
+    with keyboard.Listener(on_press=on_press, on_release=on_release) as listener:
+        listener.join()
+
+if __name__ == '__main__':
+    try:
+        main()
+    except rospy.ROSInterruptException:
+        pass