From 95e6753c1fd126177ff006f4012ebb81313aaa97 Mon Sep 17 00:00:00 2001
From: kakayishi <3311973426@qq.com>
Date: Sat, 20 Dec 2025 21:44:17 +0800
Subject: [PATCH 1/4] =?UTF-8?q?=E5=88=9D=E5=A7=8B=E5=8C=96ros=E8=8A=82?=
=?UTF-8?q?=E7=82=B9?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/Mujoco_manrun/CMakeLists.txt | 82 ++++++++
src/Mujoco_manrun/models/humanoid.xml | 267 ++++++++++++++++++++++++++
src/Mujoco_manrun/package.xml | 19 ++
src/Mujoco_manrun/scripts/__init__.py | 1 +
4 files changed, 369 insertions(+)
create mode 100644 src/Mujoco_manrun/CMakeLists.txt
create mode 100644 src/Mujoco_manrun/models/humanoid.xml
create mode 100644 src/Mujoco_manrun/package.xml
create mode 100644 src/Mujoco_manrun/scripts/__init__.py
diff --git a/src/Mujoco_manrun/CMakeLists.txt b/src/Mujoco_manrun/CMakeLists.txt
new file mode 100644
index 0000000000..ca69c45f0b
--- /dev/null
+++ b/src/Mujoco_manrun/CMakeLists.txt
@@ -0,0 +1,82 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(mujoco_manrun)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ rospy
+ geometry_msgs
+)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES mujoco_manrun
+# CATKIN_DEPENDS rospy geometry_msgs
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+ ${catkin_INCLUDE_DIRS}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+catkin_install_python(PROGRAMS
+ scripts/main.py
+ scripts/ros_handler.py
+ scripts/keyboard_handler.py
+ scripts/cpg_oscillator.py
+ scripts/humanoid_stabilizer.py
+ scripts/utils.py
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+install(DIRECTORY launch models
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_mujoco_manrun.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/src/Mujoco_manrun/models/humanoid.xml b/src/Mujoco_manrun/models/humanoid.xml
new file mode 100644
index 0000000000..a2c4a16fb5
--- /dev/null
+++ b/src/Mujoco_manrun/models/humanoid.xml
@@ -0,0 +1,267 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/Mujoco_manrun/package.xml b/src/Mujoco_manrun/package.xml
new file mode 100644
index 0000000000..65847b2ac2
--- /dev/null
+++ b/src/Mujoco_manrun/package.xml
@@ -0,0 +1,19 @@
+
+
+ mujoco_manrun
+ 0.0.0
+ The mujoco_manrun package
+
+ yskk
+ TODO
+
+ catkin
+ rospy
+ geometry_msgs
+ rospy
+ geometry_msgs
+
+
+ catkin
+
+
diff --git a/src/Mujoco_manrun/scripts/__init__.py b/src/Mujoco_manrun/scripts/__init__.py
new file mode 100644
index 0000000000..8fe5934ab3
--- /dev/null
+++ b/src/Mujoco_manrun/scripts/__init__.py
@@ -0,0 +1 @@
+#占位,让scripts成为合法的Python包
\ No newline at end of file
From 3fa348c66c87e3112f4187723e46275ed241748d Mon Sep 17 00:00:00 2001
From: kakayishi <3311973426@qq.com>
Date: Sun, 21 Dec 2025 15:14:12 +0800
Subject: [PATCH 2/4] =?UTF-8?q?=E6=96=B0=E5=A2=9E=E5=B7=A5=E5=85=B7?=
=?UTF-8?q?=E5=87=BD=E6=95=B0?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/Mujoco_manrun/scripts/cpg_oscillator.py | 50 +++++++++++++++++++++
src/Mujoco_manrun/scripts/utils.py | 29 ++++++++++++
2 files changed, 79 insertions(+)
create mode 100644 src/Mujoco_manrun/scripts/cpg_oscillator.py
create mode 100644 src/Mujoco_manrun/scripts/utils.py
diff --git a/src/Mujoco_manrun/scripts/cpg_oscillator.py b/src/Mujoco_manrun/scripts/cpg_oscillator.py
new file mode 100644
index 0000000000..d4c594bada
--- /dev/null
+++ b/src/Mujoco_manrun/scripts/cpg_oscillator.py
@@ -0,0 +1,50 @@
+import numpy as np
+
+class CPGOscillator:
+ """CPG中枢模式发生器(独立算法,无业务依赖)"""
+ def __init__(self, freq=0.5, amp=0.4, phase=0.0, coupling_strength=0.2):
+ self.base_freq = freq # 基础频率
+ self.base_amp = amp # 基础振幅
+ self.base_phase = phase # 初始相位
+ self.base_coupling = coupling_strength # 基础耦合强度
+
+ # 动态参数
+ self.freq = freq
+ self.amp = amp
+ self.phase = phase
+ self.coupling = coupling_strength
+ self.state = np.array([np.sin(phase), np.cos(phase)]) # 振荡器状态(x,y)
+
+ def update(self, dt, target_phase=0.0, speed_factor=1.0, turn_factor=0.0):
+ """
+ 更新CPG状态
+ :param dt: 时间步长
+ :param target_phase: 目标相位(用于耦合)
+ :param speed_factor: 速度系数(0~1)
+ :param turn_factor: 转向系数(-1~1)
+ :return: 关节目标偏移量
+ """
+ # 动态调整耦合强度:速度/转向越大,耦合越强
+ self.coupling = self.base_coupling * (1.0 + 0.5 * speed_factor + 0.8 * abs(turn_factor))
+ self.coupling = np.clip(self.coupling, 0.1, 0.5)
+
+ # 范德波尔振荡器方程(生物节律更自然)
+ mu = 1.0 # 非线性系数
+ x, y = self.state
+ dx = 2 * np.pi * self.freq * y + self.coupling * np.sin(target_phase - self.phase)
+ dy = 2 * np.pi * self.freq * (mu * (1 - x**2) * y - x)
+
+ # 积分更新状态
+ self.state += np.array([dx, dy]) * dt
+ self.phase = np.arctan2(self.state[0], self.state[1]) # 更新相位
+
+ # 返回当前输出
+ return self.amp * self.state[0]
+
+ def reset(self):
+ """重置CPG到初始状态"""
+ self.freq = self.base_freq
+ self.amp = self.base_amp
+ self.coupling = self.base_coupling
+ self.phase = self.base_phase
+ self.state = np.array([np.sin(self.phase), np.cos(self.phase)])
diff --git a/src/Mujoco_manrun/scripts/utils.py b/src/Mujoco_manrun/scripts/utils.py
new file mode 100644
index 0000000000..cb979f8409
--- /dev/null
+++ b/src/Mujoco_manrun/scripts/utils.py
@@ -0,0 +1,29 @@
+import numpy as np
+
+def quat_to_euler_xyz(quat):
+ """四元数转欧拉角(roll/pitch/yaw)"""
+ w, x, y, z = quat
+ sinr_cosp = 2 * (w * x + y * z)
+ cosr_cosp = 1 - 2 * (x * x + y * y)
+ roll = np.arctan2(sinr_cosp, cosr_cosp)
+
+ sinp = 2 * (w * y - z * x)
+ pitch = np.where(np.abs(sinp) >= 1, np.copysign(np.pi / 2, sinp), np.arcsin(sinp))
+
+ siny_cosp = 2 * (w * z + x * y)
+ cosy_cosp = 1 - 2 * (y * y + z * z)
+ yaw = np.arctan2(siny_cosp, cosy_cosp)
+
+ return np.array([roll, pitch, yaw])
+
+def normalize_angle(angle):
+ """角度归一化到[-π, π]"""
+ angle = np.mod(angle + np.pi, 2 * np.pi) - np.pi
+ return angle
+
+def clip_value(value, min_val, max_val, name="value"):
+ """带日志的数值限幅"""
+ clipped = np.clip(value, min_val, max_val)
+ if clipped != value:
+ print(f"[警告] {name}超出范围[{min_val}, {max_val}],已限幅为{clipped}")
+ return clipped
From 089b67e161c4251f0c0b121cca1ed9adfcb012f0 Mon Sep 17 00:00:00 2001
From: kakayishi <3311973426@qq.com>
Date: Sun, 21 Dec 2025 20:44:16 +0800
Subject: [PATCH 3/4] =?UTF-8?q?=E4=B8=8A=E4=BC=A0=E4=BA=86=E5=89=A9?=
=?UTF-8?q?=E4=BD=99=E7=9A=84=E4=BB=A3=E7=A0=81=E6=8B=86=E5=88=86=EF=BC=8C?=
=?UTF-8?q?=E4=BF=9D=E7=95=99main.py=E4=BD=9C=E4=B8=BA=E5=85=A5=E5=8F=A3?=
=?UTF-8?q?=E3=80=82?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/Mujoco_manrun/config/gait_params.yaml | 37 ++++
src/Mujoco_manrun/launch/mujoco_manrun.launch | 4 +
.../scripts/humanoid_stabilizer.py | 184 ++++++++++++++++++
src/Mujoco_manrun/scripts/keyboard_handler.py | 85 ++++++++
src/Mujoco_manrun/scripts/main.py | 45 +++++
src/Mujoco_manrun/scripts/ros_handler.py | 78 ++++++++
src/Mujoco_manrun/scripts/sensor_simulator.py | 153 +++++++++++++++
src/Mujoco_manrun/scripts/setup.py | 10 +
src/Mujoco_manrun/setup.py | 12 ++
9 files changed, 608 insertions(+)
create mode 100644 src/Mujoco_manrun/config/gait_params.yaml
create mode 100644 src/Mujoco_manrun/launch/mujoco_manrun.launch
create mode 100644 src/Mujoco_manrun/scripts/humanoid_stabilizer.py
create mode 100644 src/Mujoco_manrun/scripts/keyboard_handler.py
create mode 100644 src/Mujoco_manrun/scripts/main.py
create mode 100644 src/Mujoco_manrun/scripts/ros_handler.py
create mode 100644 src/Mujoco_manrun/scripts/sensor_simulator.py
create mode 100644 src/Mujoco_manrun/scripts/setup.py
create mode 100644 src/Mujoco_manrun/setup.py
diff --git a/src/Mujoco_manrun/config/gait_params.yaml b/src/Mujoco_manrun/config/gait_params.yaml
new file mode 100644
index 0000000000..f5107c5997
--- /dev/null
+++ b/src/Mujoco_manrun/config/gait_params.yaml
@@ -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
diff --git a/src/Mujoco_manrun/launch/mujoco_manrun.launch b/src/Mujoco_manrun/launch/mujoco_manrun.launch
new file mode 100644
index 0000000000..f141c4abd7
--- /dev/null
+++ b/src/Mujoco_manrun/launch/mujoco_manrun.launch
@@ -0,0 +1,4 @@
+
+
+
+
diff --git a/src/Mujoco_manrun/scripts/humanoid_stabilizer.py b/src/Mujoco_manrun/scripts/humanoid_stabilizer.py
new file mode 100644
index 0000000000..3c07b1991a
--- /dev/null
+++ b/src/Mujoco_manrun/scripts/humanoid_stabilizer.py
@@ -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()
diff --git a/src/Mujoco_manrun/scripts/keyboard_handler.py b/src/Mujoco_manrun/scripts/keyboard_handler.py
new file mode 100644
index 0000000000..55b8b57d78
--- /dev/null
+++ b/src/Mujoco_manrun/scripts/keyboard_handler.py
@@ -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}模式")
diff --git a/src/Mujoco_manrun/scripts/main.py b/src/Mujoco_manrun/scripts/main.py
new file mode 100644
index 0000000000..b07d87bc98
--- /dev/null
+++ b/src/Mujoco_manrun/scripts/main.py
@@ -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()
diff --git a/src/Mujoco_manrun/scripts/ros_handler.py b/src/Mujoco_manrun/scripts/ros_handler.py
new file mode 100644
index 0000000000..7a2c3e7b62
--- /dev/null
+++ b/src/Mujoco_manrun/scripts/ros_handler.py
@@ -0,0 +1,78 @@
+import sys
+import os
+SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__))
+sys.path.insert(0, SCRIPT_DIR)
+
+import threading
+import time
+
+class ROSHandler(threading.Thread):
+ def __init__(self, stabilizer):
+ super().__init__(daemon=True)
+ self.stabilizer = stabilizer
+ self.running = True
+ self.has_ros = False
+ self.rospy = None
+ self.Twist = None
+
+ # 尝试初始化ROS
+ try:
+ import rospy
+ from geometry_msgs.msg import Twist
+ self.rospy = rospy
+ self.Twist = Twist
+ self.has_ros = True
+
+ # 初始化ROS节点
+ if not self.rospy.core.is_initialized():
+ self.rospy.init_node('humanoid_cmd_vel_listener', anonymous=True)
+
+ # 订阅/cmd_vel话题
+ self.sub = self.rospy.Subscriber(
+ "/cmd_vel",
+ self.Twist,
+ self._cmd_vel_callback,
+ queue_size=1,
+ tcp_nodelay=True
+ )
+ print("[ROS提示] 已启动/cmd_vel话题监听")
+ except ImportError:
+ print("[ROS提示] 未检测到ROS环境,跳过/cmd_vel话题监听")
+ except Exception as e:
+ print(f"[ROS提示] ROS初始化失败:{e}")
+ self.has_ros = False
+
+ def _cmd_vel_callback(self, msg):
+ """处理/cmd_vel话题消息"""
+ # 提取速度和转向指令
+ linear_x = msg.linear.x # 前进/后退(-1~1)
+ angular_z = msg.angular.z # 左转/右转(-1~1)
+
+ # 设置到机器人控制器
+ self.stabilizer.set_velocity(linear_x)
+ self.stabilizer.set_turn_rate(angular_z)
+
+ # 打印日志
+ gait_mode = self.stabilizer.gait_mode
+ print(f"[ROS指令] 速度={linear_x:.2f} | 转向={angular_z:.2f} | 步态={gait_mode}")
+
+ def run(self):
+ """ROS监听线程主循环(修复spin_once错误)"""
+ if not self.has_ros:
+ return
+
+ # ROS 1兼容的周期监听(替代spin_once)
+ rate = self.rospy.Rate(100) # 100Hz刷新频率
+ while self.running and not self.rospy.is_shutdown():
+ try:
+ rate.sleep() # 非阻塞式等待,替代spin_once
+ except Exception as e:
+ print(f"[ROS提示] 监听线程异常:{e}")
+ break
+
+ def stop(self):
+ """停止监听线程"""
+ self.running = False
+ if self.has_ros:
+ self.rospy.signal_shutdown("Simulation stopped")
+ print("[ROS提示] 监听线程已停止")
diff --git a/src/Mujoco_manrun/scripts/sensor_simulator.py b/src/Mujoco_manrun/scripts/sensor_simulator.py
new file mode 100644
index 0000000000..667c83e99d
--- /dev/null
+++ b/src/Mujoco_manrun/scripts/sensor_simulator.py
@@ -0,0 +1,153 @@
+import numpy as np
+from collections import deque
+from utils import quat_to_euler_xyz
+
+class SensorSimulator:
+ """传感器模拟模块(IMU+足底力,独立可复用)"""
+ def __init__(self, model, data):
+ self.model = model
+ self.data = data
+
+ # 开关
+ self.enable_sensor_simulation = True
+
+ # IMU参数
+ self.imu_angle_noise = 0.01 # 欧拉角噪声(rad)
+ self.imu_vel_noise = 0.05 # 角速度噪声(rad/s)
+ self.imu_delay_frames = 2 # 延迟帧数
+
+ # 足底力参数
+ self.foot_force_noise = 0.3 # 力噪声(N)
+ self.foot_force_offset = 0.1 # 零漂
+ self.foot_contact_threshold = 1.5 # 接触判定阈值
+
+ # 缓存(模拟延迟)
+ self.imu_data_buffer = deque(maxlen=self.imu_delay_frames)
+ self.foot_data_buffer = deque(maxlen=self.imu_delay_frames)
+
+ # 存储当前数据
+ self.current_sensor_data = {}
+
+ def simulate_imu(self):
+ """模拟带噪声+延迟的IMU数据"""
+ # 真实数据
+ true_quat = self.data.qpos[3:7].astype(np.float64).copy()
+ true_euler = quat_to_euler_xyz(true_quat)
+ true_ang_vel = self.data.qvel[3:6].astype(np.float64).copy()
+
+ # 添加噪声
+ noisy_euler = true_euler + np.random.normal(0, self.imu_angle_noise, 3)
+ noisy_ang_vel = true_ang_vel + np.random.normal(0, self.imu_vel_noise, 3)
+
+ # 限幅
+ noisy_euler = np.clip(noisy_euler, -np.pi/2, np.pi/2)
+ noisy_ang_vel = np.clip(noisy_ang_vel, -5.0, 5.0)
+
+ # 缓存
+ self.imu_data_buffer.append({
+ "euler": noisy_euler,
+ "ang_vel": noisy_ang_vel,
+ "true_euler": true_euler,
+ "true_ang_vel": true_ang_vel
+ })
+
+ # 返回延迟后的数据
+ if len(self.imu_data_buffer) < self.imu_delay_frames:
+ return {"euler": true_euler, "ang_vel": true_ang_vel, "true_euler": true_euler, "true_ang_vel": true_ang_vel}
+ return self.imu_data_buffer[0]
+
+ def simulate_foot_force(self):
+ """模拟带噪声+零漂的足底力数据"""
+ # 真实接触力
+ left_force = self._get_foot_force("left")
+ right_force = self._get_foot_force("right")
+
+ # 添加噪声和零漂
+ noisy_left = left_force + np.random.normal(0, self.foot_force_noise) + self.foot_force_offset
+ noisy_right = right_force + np.random.normal(0, self.foot_force_noise) + self.foot_force_offset
+
+ # 限幅
+ noisy_left = max(0.0, noisy_left)
+ noisy_right = max(0.0, noisy_right)
+
+ # 接触判定
+ left_contact = 1 if noisy_left > self.foot_contact_threshold else 0
+ right_contact = 1 if noisy_right > self.foot_contact_threshold else 0
+
+ # 缓存
+ self.foot_data_buffer.append({
+ "left_force": noisy_left,
+ "right_force": noisy_right,
+ "left_contact": left_contact,
+ "right_contact": right_contact,
+ "true_left_force": left_force,
+ "true_right_force": right_force
+ })
+
+ # 返回延迟后的数据
+ if len(self.foot_data_buffer) < self.imu_delay_frames:
+ return {
+ "left_force": left_force, "right_force": right_force,
+ "left_contact": 1 if left_force > self.foot_contact_threshold else 0,
+ "right_contact": 1 if right_force > self.foot_contact_threshold else 0,
+ "true_left_force": left_force, "true_right_force": right_force
+ }
+ return self.foot_data_buffer[0]
+
+ def _get_foot_force(self, side):
+ """获取单只脚的真实接触力"""
+ import mujoco
+ foot_geoms = ["foot1_" + side, "foot2_" + side]
+ total_force = 0.0
+ for geom_name in foot_geoms:
+ geom_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_GEOM, geom_name)
+ force = np.zeros(6, dtype=np.float64)
+ mujoco.mj_contactForce(self.model, self.data, geom_id, force)
+ total_force += np.linalg.norm(force[:3])
+ return total_force
+
+ def get_sensor_data(self, gait_mode="NORMAL"):
+ """获取最终传感器数据(模拟/真实可切换)"""
+ if not self.enable_sensor_simulation:
+ # 关闭模拟:返回真实数据
+ imu_data = {
+ "euler": quat_to_euler_xyz(self.data.qpos[3:7]),
+ "ang_vel": self.data.qvel[3:6].copy(),
+ "true_euler": quat_to_euler_xyz(self.data.qpos[3:7]),
+ "true_ang_vel": self.data.qvel[3:6].copy()
+ }
+ foot_data = {
+ "left_force": self._get_foot_force("left"),
+ "right_force": self._get_foot_force("right"),
+ "left_contact": 1 if self._get_foot_force("left") > self.foot_contact_threshold else 0,
+ "right_contact": 1 if self._get_foot_force("right") > self.foot_contact_threshold else 0,
+ "true_left_force": self._get_foot_force("left"),
+ "true_right_force": self._get_foot_force("right")
+ }
+ else:
+ # 开启模拟:返回带噪声+延迟的数据
+ imu_data = self.simulate_imu()
+ foot_data = self.simulate_foot_force()
+
+ # 整合数据
+ self.current_sensor_data = {
+ "imu": imu_data,
+ "foot": foot_data,
+ "time": self.data.time,
+ "gait_mode": gait_mode
+ }
+ return self.current_sensor_data
+
+ def print_sensor_data(self):
+ """打印当前传感器数据"""
+ if not self.current_sensor_data:
+ print("[传感器] 暂无数据")
+ return
+ imu = self.current_sensor_data["imu"]
+ foot = self.current_sensor_data["foot"]
+ print("\n=== 传感器数据 ===")
+ print(f"时间: {self.current_sensor_data['time']:.2f}s | 模拟: {'开启' if self.enable_sensor_simulation else '关闭'} | 步态: {self.current_sensor_data['gait_mode']}")
+ print(f"IMU姿态: {imu['euler'][0]:.3f}/{imu['euler'][1]:.3f}/{imu['euler'][2]:.3f}rad (真实: {imu['true_euler'][0]:.3f}/{imu['true_euler'][1]:.3f}/{imu['true_euler'][2]:.3f})")
+ print(f"左脚力: {foot['left_force']:.2f}N (真实: {foot['true_left_force']:.2f}N) | 接触: {foot['left_contact']}")
+ print(f"右脚力: {foot['right_force']:.2f}N (真实: {foot['true_right_force']:.2f}N) | 接触: {foot['right_contact']}")
+ print("==================\n")
diff --git a/src/Mujoco_manrun/scripts/setup.py b/src/Mujoco_manrun/scripts/setup.py
new file mode 100644
index 0000000000..06c125f317
--- /dev/null
+++ b/src/Mujoco_manrun/scripts/setup.py
@@ -0,0 +1,10 @@
+from setuptools import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+# 获取ROS包配置
+d = generate_distutils_setup(
+ packages=['mujoco_manrun'], # 包名
+ package_dir={'': 'scripts'} # 源码目录
+)
+
+setup(**d)
diff --git a/src/Mujoco_manrun/setup.py b/src/Mujoco_manrun/setup.py
new file mode 100644
index 0000000000..6f5630e1a7
--- /dev/null
+++ b/src/Mujoco_manrun/setup.py
@@ -0,0 +1,12 @@
+## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+# fetch values from package.xml
+setup_args = generate_distutils_setup(
+ packages=['mujoco_manrun'],
+ package_dir={'': 'scripts'},
+)
+
+setup(**setup_args)
From af1d613d3a12a0276a473039d4c6922abddf04f5 Mon Sep 17 00:00:00 2001
From: kakayishi <3311973426@qq.com>
Date: Mon, 22 Dec 2025 09:43:47 +0800
Subject: [PATCH 4/4] =?UTF-8?q?=E5=AE=8C=E6=88=90ros=E5=8C=85=EF=BC=8C?=
=?UTF-8?q?=E8=A7=84=E8=8C=83=E4=BA=86=E6=96=87=E4=BB=B6=E7=9A=84=E4=BD=8D?=
=?UTF-8?q?=E7=BD=AE?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/Mujoco_manrun/CMakeLists.txt | 94 +++---
src/Mujoco_manrun/humanoid.xml | 267 ------------------
src/Mujoco_manrun/launch/mujoco_manrun.launch | 13 +-
src/Mujoco_manrun/{ => models}/g1_23dof.xml | 0
.../{ => models}/mimic_sites.xml | 0
src/Mujoco_manrun/{ => models}/scene.xml | 0
src/Mujoco_manrun/package.xml | 22 +-
src/Mujoco_manrun/setup.py | 10 +-
8 files changed, 70 insertions(+), 336 deletions(-)
delete mode 100644 src/Mujoco_manrun/humanoid.xml
rename src/Mujoco_manrun/{ => models}/g1_23dof.xml (100%)
rename src/Mujoco_manrun/{ => models}/mimic_sites.xml (100%)
rename src/Mujoco_manrun/{ => models}/scene.xml (100%)
diff --git a/src/Mujoco_manrun/CMakeLists.txt b/src/Mujoco_manrun/CMakeLists.txt
index ca69c45f0b..3defea3a09 100644
--- a/src/Mujoco_manrun/CMakeLists.txt
+++ b/src/Mujoco_manrun/CMakeLists.txt
@@ -1,82 +1,62 @@
cmake_minimum_required(VERSION 3.0.2)
project(mujoco_manrun)
-## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
+if($ENV{ROS_DISTRO} STREQUAL "noetic")
+ set(PYTHON_EXECUTABLE /usr/bin/python3)
+else()
+ set(PYTHON_EXECUTABLE /usr/bin/python2)
+endif()
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
- rospy
geometry_msgs
+ rospy
+ sensor_msgs
+ std_msgs
)
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
+catkin_python_setup()
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if your package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
-# INCLUDE_DIRS include
-# LIBRARIES mujoco_manrun
-# CATKIN_DEPENDS rospy geometry_msgs
-# DEPENDS system_lib
+ CATKIN_DEPENDS
+ geometry_msgs
+ rospy
+ sensor_msgs
+ std_msgs
)
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
include_directories(
-# include
${catkin_INCLUDE_DIRS}
)
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+install(DIRECTORY scripts/
+ DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}
+ FILES_MATCHING PATTERN "*.py"
+ PATTERN "__pycache__" EXCLUDE
+)
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-catkin_install_python(PROGRAMS
+catkin_install_python(PROGRAMS
scripts/main.py
- scripts/ros_handler.py
- scripts/keyboard_handler.py
- scripts/cpg_oscillator.py
- scripts/humanoid_stabilizer.py
- scripts/utils.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
-## Mark other files for installation (e.g. launch and bag files, etc.)
-install(DIRECTORY launch models
- DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+install(DIRECTORY models/
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/models
)
-#############
-## Testing ##
-#############
+install(DIRECTORY launch/
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+)
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_mujoco_manrun.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
+install(DIRECTORY config/
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config
+)
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
+if(CATKIN_ENABLE_TESTING)
+ if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/test/")
+ install(DIRECTORY test/
+ DESTINATION ${CATKIN_PACKAGE_TEST_DESTINATION}
+ FILES_MATCHING PATTERN "*.py"
+ )
+ catkin_add_nosetests(test)
+ endif()
+endif()
diff --git a/src/Mujoco_manrun/humanoid.xml b/src/Mujoco_manrun/humanoid.xml
deleted file mode 100644
index a2c4a16fb5..0000000000
--- a/src/Mujoco_manrun/humanoid.xml
+++ /dev/null
@@ -1,267 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/src/Mujoco_manrun/launch/mujoco_manrun.launch b/src/Mujoco_manrun/launch/mujoco_manrun.launch
index f141c4abd7..1ba87cd3aa 100644
--- a/src/Mujoco_manrun/launch/mujoco_manrun.launch
+++ b/src/Mujoco_manrun/launch/mujoco_manrun.launch
@@ -1,4 +1,15 @@
-
+
+
+
+
+
diff --git a/src/Mujoco_manrun/g1_23dof.xml b/src/Mujoco_manrun/models/g1_23dof.xml
similarity index 100%
rename from src/Mujoco_manrun/g1_23dof.xml
rename to src/Mujoco_manrun/models/g1_23dof.xml
diff --git a/src/Mujoco_manrun/mimic_sites.xml b/src/Mujoco_manrun/models/mimic_sites.xml
similarity index 100%
rename from src/Mujoco_manrun/mimic_sites.xml
rename to src/Mujoco_manrun/models/mimic_sites.xml
diff --git a/src/Mujoco_manrun/scene.xml b/src/Mujoco_manrun/models/scene.xml
similarity index 100%
rename from src/Mujoco_manrun/scene.xml
rename to src/Mujoco_manrun/models/scene.xml
diff --git a/src/Mujoco_manrun/package.xml b/src/Mujoco_manrun/package.xml
index 65847b2ac2..93cbfc91c3 100644
--- a/src/Mujoco_manrun/package.xml
+++ b/src/Mujoco_manrun/package.xml
@@ -1,18 +1,30 @@
mujoco_manrun
- 0.0.0
- The mujoco_manrun package
-
- yskk
- TODO
+ 1.0.0
+ ROS封装的人形机器人Mujoco仿真包,支持CPG步态控制、标准ROS接口通信
+ your_name
+ MIT
+
catkin
rospy
geometry_msgs
+ sensor_msgs
+ std_msgs
+ tf2_ros
+ std_srvs
+
+
rospy
geometry_msgs
+ sensor_msgs
+ std_msgs
+ tf2_ros
+ std_srvs
+ rviz
+
catkin
diff --git a/src/Mujoco_manrun/setup.py b/src/Mujoco_manrun/setup.py
index 6f5630e1a7..a027a9fdc9 100644
--- a/src/Mujoco_manrun/setup.py
+++ b/src/Mujoco_manrun/setup.py
@@ -1,12 +1,10 @@
-## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
-
-from distutils.core import setup
+from setuptools import setup
from catkin_pkg.python_setup import generate_distutils_setup
-# fetch values from package.xml
setup_args = generate_distutils_setup(
- packages=['mujoco_manrun'],
- package_dir={'': 'scripts'},
+ packages=[],
+ package_dir={"": "scripts"},
+ scripts=["scripts/main.py"]
)
setup(**setup_args)