Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
50 changes: 50 additions & 0 deletions src/Mujoco_manrun/scripts/cpg_oscillator.py
Original file line number Diff line number Diff line change
@@ -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)])
29 changes: 29 additions & 0 deletions src/Mujoco_manrun/scripts/utils.py
Original file line number Diff line number Diff line change
@@ -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
Loading