import math
class GaitTrajectoryGenerator:
def __init__(self, step_length=0.1, step_height=0.05,
body_L=0.4, body_W=0.2):
self.step_length = step_length
self.step_height = step_height
self.body_L = body_L
self.body_W = body_W
self.hip_pos = {
'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 bezier_swing(self, t_norm, start, end, height):
x = start[0] + (end[0] - start[0]) * t_norm
y = start[1] + (end[1] - start[1]) * t_norm
z = 4 * height * t_norm * (1 - t_norm)
return (x, y, z)
def stance_trajectory(self, t_norm, start, end):
x = start[0] + (end[0] - start[0]) * t_norm
y = start[1] + (end[1] - start[1]) * t_norm
return (x, y, 0)
def generate_trot_trajectory(self, freq=2.0, n_frames=50):
T = 1.0 / freq
group1 = ['LF', 'RB']
group2 = ['RF', 'LB']
trajectories = {leg: [] for leg in self.hip_pos}
for i in range(n_frames):
t = i * T / n_frames
phase = (t / T) % 1.0
for leg in self.hip_pos:
hip = self.hip_pos[leg]
start_pos = (hip[0] - self.step_length/2, hip[1])
end_pos = (hip[0] + self.step_length/2, hip[1])
if leg in group1:
if phase < 0.5:
t_norm = phase / 0.5
traj = self.bezier_swing(t_norm, start_pos, end_pos, self.step_height)
else:
t_norm = (phase - 0.5) / 0.5
traj = self.stance_trajectory(t_norm, end_pos, start_pos)
else:
if phase < 0.5:
t_norm = phase / 0.5
traj = self.stance_trajectory(t_norm, end_pos, start_pos)
else:
t_norm = (phase - 0.5) / 0.5
traj = self.bezier_swing(t_norm, start_pos, end_pos, self.step_height)
trajectories[leg].append((t, traj))
return trajectories
def generate_walk_trajectory(self, freq=1.0, n_frames=40):
T = 1.0 / freq
swing_ratio = 0.25
leg_order = ['LF', 'RF', 'LB', 'RB']
phase_offsets = [0, 0.25, 0.50, 0.75]
trajectories = {leg: [] for leg in self.hip_pos}
for i in range(n_frames):
t = i * T / n_frames
phase = (t / T) % 1.0
for idx, leg in enumerate(leg_order):
hip = self.hip_pos[leg]
start_pos = (hip[0] - self.step_length/2, hip[1])
end_pos = (hip[0] + self.step_length/2, hip[1])
leg_phase = (phase - phase_offsets[idx]) % 1.0
if leg_phase < swing_ratio:
t_norm = leg_phase / swing_ratio
traj = self.bezier_swing(t_norm, start_pos, end_pos, self.step_height)
else:
t_norm = (leg_phase - swing_ratio) / (1 - swing_ratio)
traj = self.stance_trajectory(t_norm, end_pos, start_pos)
trajectories[leg].append((t, traj))
return trajectories
gen = GaitTrajectoryGenerator(step_length=0.1, step_height=0.05)
print("=" * 60)
print(" Gait Trajectory Generation Simulation")
print("=" * 60)
print(f" Step length: {gen.step_length*1000:.0f}mm")
print(f" Step height: {gen.step_height*1000:.0f}mm")
print(f"\n [Trot Trajectory (2Hz)]")
trot_traj = gen.generate_trot_trajectory(freq=2.0, n_frames=20)
for leg in ['LF', 'RB', 'RF', 'LB']:
print(f"\n {leg}:")
for t, (x, y, z) in trot_traj[leg][:10]:
state = "SWING" if z > 0.001 else "STANCE"
print(f" t={t:.3f}s x={x:.4f} y={y:.4f} z={z:.4f} [{state}]")
print(f"\n [Walk Trajectory (1Hz)]")
walk_traj = gen.generate_walk_trajectory(freq=1.0, n_frames=16)
for leg in ['LF', 'RF', 'LB', 'RB']:
print(f"\n {leg}:")
for t, (x, y, z) in walk_traj[leg][:8]:
state = "SWING" if z > 0.001 else "STANCE"
print(f" t={t:.3f}s x={x:.4f} y={y:.4f} z={z:.4f} [{state}]")
print(f"\n [Foot Speed Analysis - Trot]")
trot_traj_full = gen.generate_trot_trajectory(freq=2.0, n_frames=100)
for leg in ['LF']:
max_speed = 0
max_vz = 0
for i in range(1, len(trot_traj_full[leg])):
t0, (x0, y0, z0) = trot_traj_full[leg][i-1]
t1, (x1, y1, z1) = trot_traj_full[leg][i]
dt = t1 - t0
if dt > 0:
vx = (x1-x0)/dt
vz = (z1-z0)/dt
speed = math.sqrt(vx**2 + vz**2)
max_speed = max(max_speed, speed)
max_vz = max(max_vz, abs(vz))
print(f" {leg}: max_horiz_speed={max_speed:.3f}m/s, max_vert_speed={max_vz:.3f}m/s")
print(f"\n [Gait Parameter Comparison]")
params = [
("Walk-slow", 0.06, 0.03, 0.8),
("Walk-norm", 0.10, 0.05, 1.2),
("Trot-slow", 0.10, 0.04, 1.5),
("Trot-norm", 0.15, 0.06, 2.0),
("Trot-fast", 0.20, 0.08, 3.0),
]
print(f" {'Gait':>10s} {'Step_mm':>8s} {'Lift_mm':>8s} {'Freq_Hz':>8s} {'Speed_m/s':>8s}")
for name, sl, sh, freq in params:
speed = sl * freq * 2
print(f" {name:>10s} {sl*1000:8.0f} {sh*1000:8.0f} {freq:8.1f} {speed:8.3f}")
print()
print(" OK - Gait trajectory generation complete")
仿真结果：
============================================================
Gait Trajectory Generation Simulation
============================================================
Step length: 100mm
Step height: 50mm
[Trot Trajectory (2Hz)]
LF:
t=0.000s x=0.1500 y=0.1000 z=0.0000 [STANCE]
t=0.025s x=0.1600 y=0.1000 z=0.0180 [SWING]
t=0.050s x=0.1700 y=0.1000 z=0.0320 [SWING]
t=0.075s x=0.1800 y=0.1000 z=0.0420 [SWING]
t=0.100s x=0.1900 y=0.1000 z=0.0480 [SWING]
t=0.125s x=0.2000 y=0.1000 z=0.0500 [SWING]
t=0.150s x=0.2100 y=0.1000 z=0.0480 [SWING]
t=0.175s x=0.2200 y=0.1000 z=0.0420 [SWING]
t=0.200s x=0.2300 y=0.1000 z=0.0320 [SWING]
t=0.225s x=0.2400 y=0.1000 z=0.0180 [SWING]
RB:
t=0.000s x=-0.2500 y=-0.1000 z=0.0000 [STANCE]
t=0.025s x=-0.2400 y=-0.1000 z=0.0180 [SWING]
t=0.050s x=-0.2300 y=-0.1000 z=0.0320 [SWING]
t=0.075s x=-0.2200 y=-0.1000 z=0.0420 [SWING]
t=0.100s x=-0.2100 y=-0.1000 z=0.0480 [SWING]
t=0.125s x=-0.2000 y=-0.1000 z=0.0500 [SWING]
t=0.150s x=-0.1900 y=-0.1000 z=0.0480 [SWING]
t=0.175s x=-0.1800 y=-0.1000 z=0.0420 [SWING]
t=0.200s x=-0.1700 y=-0.1000 z=0.0320 [SWING]
t=0.225s x=-0.1600 y=-0.1000 z=0.0180 [SWING]
RF:
t=0.000s x=0.2500 y=-0.1000 z=0.0000 [STANCE]
t=0.025s x=0.2400 y=-0.1000 z=0.0000 [STANCE]
t=0.050s x=0.2300 y=-0.1000 z=0.0000 [STANCE]
t=0.075s x=0.2200 y=-0.1000 z=0.0000 [STANCE]
t=0.100s x=0.2100 y=-0.1000 z=0.0000 [STANCE]
t=0.125s x=0.2000 y=-0.1000 z=0.0000 [STANCE]
t=0.150s x=0.1900 y=-0.1000 z=0.0000 [STANCE]
t=0.175s x=0.1800 y=-0.1000 z=0.0000 [STANCE]
t=0.200s x=0.1700 y=-0.1000 z=0.0000 [STANCE]
t=0.225s x=0.1600 y=-0.1000 z=0.0000 [STANCE]
LB:
t=0.000s x=-0.1500 y=0.1000 z=0.0000 [STANCE]
t=0.025s x=-0.1600 y=0.1000 z=0.0000 [STANCE]
t=0.050s x=-0.1700 y=0.1000 z=0.0000 [STANCE]
t=0.075s x=-0.1800 y=0.1000 z=0.0000 [STANCE]
t=0.100s x=-0.1900 y=0.1000 z=0.0000 [STANCE]
t=0.125s x=-0.2000 y=0.1000 z=0.0000 [STANCE]
t=0.150s x=-0.2100 y=0.1000 z=0.0000 [STANCE]
t=0.175s x=-0.2200 y=0.1000 z=0.0000 [STANCE]
t=0.200s x=-0.2300 y=0.1000 z=0.0000 [STANCE]
t=0.225s x=-0.2400 y=0.1000 z=0.0000 [STANCE]
[Walk Trajectory (1Hz)]
LF:
t=0.000s x=0.1500 y=0.1000 z=0.0000 [STANCE]
t=0.062s x=0.1750 y=0.1000 z=0.0375 [SWING]
t=0.125s x=0.2000 y=0.1000 z=0.0500 [SWING]
t=0.188s x=0.2250 y=0.1000 z=0.0375 [SWING]
t=0.250s x=0.2500 y=0.1000 z=0.0000 [STANCE]
t=0.312s x=0.2417 y=0.1000 z=0.0000 [STANCE]
t=0.375s x=0.2333 y=0.1000 z=0.0000 [STANCE]
t=0.438s x=0.2250 y=0.1000 z=0.0000 [STANCE]
RF:
t=0.000s x=0.1833 y=-0.1000 z=0.0000 [STANCE]
t=0.062s x=0.1750 y=-0.1000 z=0.0000 [STANCE]
t=0.125s x=0.1667 y=-0.1000 z=0.0000 [STANCE]
t=0.188s x=0.1583 y=-0.1000 z=0.0000 [STANCE]
t=0.250s x=0.1500 y=-0.1000 z=0.0000 [STANCE]
t=0.312s x=0.1750 y=-0.1000 z=0.0375 [SWING]
t=0.375s x=0.2000 y=-0.1000 z=0.0500 [SWING]
t=0.438s x=0.2250 y=-0.1000 z=0.0375 [SWING]
LB:
t=0.000s x=-0.1833 y=0.1000 z=0.0000 [STANCE]
t=0.062s x=-0.1917 y=0.1000 z=0.0000 [STANCE]
t=0.125s x=-0.2000 y=0.1000 z=0.0000 [STANCE]
t=0.188s x=-0.2083 y=0.1000 z=0.0000 [STANCE]
t=0.250s x=-0.2167 y=0.1000 z=0.0000 [STANCE]
t=0.312s x=-0.2250 y=0.1000 z=0.0000 [STANCE]
t=0.375s x=-0.2333 y=0.1000 z=0.0000 [STANCE]
t=0.438s x=-0.2417 y=0.1000 z=0.0000 [STANCE]
RB:
t=0.000s x=-0.1500 y=-0.1000 z=0.0000 [STANCE]
t=0.062s x=-0.1583 y=-0.1000 z=0.0000 [STANCE]
t=0.125s x=-0.1667 y=-0.1000 z=0.0000 [STANCE]
t=0.188s x=-0.1750 y=-0.1000 z=0.0000 [STANCE]
t=0.250s x=-0.1833 y=-0.1000 z=0.0000 [STANCE]
t=0.312s x=-0.1917 y=-0.1000 z=0.0000 [STANCE]
t=0.375s x=-0.2000 y=-0.1000 z=0.0000 [STANCE]
t=0.438s x=-0.2083 y=-0.1000 z=0.0000 [STANCE]
[Foot Speed Analysis - Trot]
LF: max_horiz_speed=0.880m/s, max_vert_speed=0.784m/s
[Gait Parameter Comparison]
Gait Step_mm Lift_mm Freq_Hz Speed_m/s
Walk-slow 60 30 0.8 0.096
Walk-norm 100 50 1.2 0.240
Trot-slow 100 40 1.5 0.300
Trot-norm 150 60 2.0 0.600
Trot-fast 200 80 3.0 1.200
OK - Gait trajectory generation complete