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/2] =?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/2] =?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