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