import math
class GaitGenerator:
def __init__(self, gait_type='walk', freq=1.0):
self.gait_type = gait_type
self.freq = freq
self.T = 1.0 / freq
self.gait_phases = {
'walk': {'RF': 0.25, 'LB': 0.50, 'RB': 0.75},
'trot': {'RF': 0.50, 'LB': 0.50, 'RB': 0.00},
'pace': {'RF': 0.00, 'LB': 0.50, 'RB': 0.50},
'bound': {'RF': 0.50, 'LB': 0.00, 'RB': 0.50},
}
self.swing_ratio = {
'walk': 0.25,
'trot': 0.50,
'pace': 0.50,
'bound': 0.50,
}
def leg_phase(self, leg_name, t):
phases = self.gait_phases[self.gait_type]
swing_r = self.swing_ratio[self.gait_type]
if leg_name == 'LF':
offset = 0
else:
offset = phases[leg_name]
phase_t = (t / self.T + offset) % 1.0
return 1 if phase_t < swing_r else 0
def support_pattern(self, t):
legs = ['LF', 'RF', 'LB', 'RB']
pattern = {}
for leg in legs:
pattern[leg] = 'stance' if self.leg_phase(leg, t) == 0 else 'swing'
return pattern
def n_support_legs(self, t):
p = self.support_pattern(t)
return sum(1 for v in p.values() if v == 'stance')
def gait_sequence(self, n_steps=20):
dt = self.T / n_steps
sequence = []
for i in range(n_steps):
t = i * dt
p = self.support_pattern(t)
n = self.n_support_legs(t)
sequence.append((t, p, n))
return sequence
print("=" * 60)
print(" Gait Simulation: Walk / Trot / Pace / Bound")
print("=" * 60)
for gait in ['walk', 'trot', 'pace', 'bound']:
gen = GaitGenerator(gait_type=gait, freq=1.0)
print(f"\n [{gait.upper()}] T={gen.T:.2f}s, swing_ratio={gen.swing_ratio[gait]:.0%}")
print(f" {'Time':>6s} {'LF':>6s} {'RF':>6s} {'LB':>6s} {'RB':>6s} Support")
print(" " + "-" * 50)
seq = gen.gait_sequence(8)
for t, pattern, n in seq:
states = []
for leg in ['LF', 'RF', 'LB', 'RB']:
s = pattern[leg]
states.append(' S' if s == 'stance' else ' W')
print(f" {t:6.3f}s {' '.join(states)} {n}")
print("\n" + "=" * 60)
print(" Gait Stability Comparison")
print("=" * 60)
class StabilityAnalysis:
def __init__(self, body_L=0.4, body_W=0.2):
self.legs = {
'LF': ( body_L/2, body_W/2),
'RF': ( body_L/2, -body_W/2),
'LB': (-body_L/2, body_W/2),
'RB': (-body_L/2, -body_W/2),
}
def support_polygon_area(self, support_legs):
if len(support_legs) < 3:
return 0.0
pts = [self.legs[l] for l in support_legs]
cx = sum(p[0] for p in pts) / len(pts)
cy = sum(p[1] for p in pts) / len(pts)
pts.sort(key=lambda p: math.atan2(p[1]-cy, p[0]-cx))
n = len(pts)
area = 0
for i in range(n):
j = (i+1) % n
area += pts[i][0]*pts[j][1]
area -= pts[j][0]*pts[i][1]
return abs(area) / 2
sa = StabilityAnalysis()
for gait in ['walk', 'trot', 'pace', 'bound']:
gen = GaitGenerator(gait_type=gait, freq=1.0)
min_support = 4
min_area = float('inf')
for i in range(100):
t = i * gen.T / 100
pattern = gen.support_pattern(t)
support = [l for l, s in pattern.items() if s == 'stance']
n = len(support)
area = sa.support_polygon_area(support)
min_support = min(min_support, n)
if n >= 3:
min_area = min(min_area, area)
else:
min_area = 0
stab = "HIGH (static)" if min_support >= 3 else "LOW (need dynamic)"
print(f" {gait:6s}: min_support={min_support}, min_area={min_area*1e4:.1f}cm2, stability={stab}")
print()
print(" OK - Gait simulation complete")
仿真结果：
============================================================
Gait Simulation: Walk / Trot / Pace / Bound
============================================================
[WALK] T=1.00s, swing_ratio=25%
Time LF RF LB RB Support
--------------------------------------------------
0.000s W S S S 3
0.125s W S S S 3
0.250s S S S W 3
0.375s S S S W 3
0.500s S S W S 3
0.625s S S W S 3
0.750s S W S S 3
0.875s S W S S 3
[TROT] T=1.00s, swing_ratio=50%
Time LF RF LB RB Support
--------------------------------------------------
0.000s W S S W 2
0.125s W S S W 2
0.250s W S S W 2
0.375s W S S W 2
0.500s S W W S 2
0.625s S W W S 2
0.750s S W W S 2
0.875s S W W S 2
[PACE] T=1.00s, swing_ratio=50%
Time LF RF LB RB Support
--------------------------------------------------
0.000s W W S S 2
0.125s W W S S 2
0.250s W W S S 2
0.375s W W S S 2
0.500s S S W W 2
0.625s S S W W 2
0.750s S S W W 2
0.875s S S W W 2
[BOUND] T=1.00s, swing_ratio=50%
Time LF RF LB RB Support
--------------------------------------------------
0.000s W S W S 2
0.125s W S W S 2
0.250s W S W S 2
0.375s W S W S 2
0.500s S W S W 2
0.625s S W S W 2
0.750s S W S W 2
0.875s S W S W 2
============================================================
Gait Stability Comparison
============================================================
walk : min_support=3, min_area=400.0cm2, stability=HIGH (static)
trot : min_support=2, min_area=0.0cm2, stability=LOW (need dynamic)
pace : min_support=2, min_area=0.0cm2, stability=LOW (need dynamic)
bound : min_support=2, min_area=0.0cm2, stability=LOW (need dynamic)
OK - Gait simulation complete