import math, random
class UnstructuredTerrainWalker:
def __init__(self, mass=6.2):
self.mass = mass
self.g = 9.81
def generate_random_terrain(self, length=2.0, resolution=0.05):
terrain = []
h = 0
for i in range(int(length/resolution) + 1):
x = i * resolution
h += random.gauss(0, 0.01)
h = max(-0.05, min(0.1, h))
# Add some obstacles
if random.random() < 0.1:
h += random.choice([0.03, -0.02, 0.05])
h = max(-0.05, min(0.15, h))
terrain.append((x, h))
return terrain
def find_safe_foothold(self, terrain, target_x, search_range=0.1):
candidates = []
for x, h in terrain:
if abs(x - target_x) <= search_range:
candidates.append((x, h))
if not candidates:
return (target_x, 0)
# Score: prefer flat and close
best = None
best_score = -1
for x, h in candidates:
# Compute local slope
nearby = [(nx, nh) for nx, nh in terrain if abs(nx - x) < 0.1]
if len(nearby) >= 2:
slopes = []
for i in range(len(nearby)-1):
dx = nearby[i+1][0] - nearby[i][0]
dh = nearby[i+1][1] - nearby[i][1]
if dx > 0:
slopes.append(abs(dh/dx))
avg_slope = sum(slopes)/len(slopes) if slopes else 0
else:
avg_slope = 0
flat_score = max(0, 1 - avg_slope/0.5)
dist = abs(x - target_x)
dist_score = max(0, 1 - dist/search_range)
score = 0.7 * flat_score + 0.3 * dist_score
if score > best_score:
best_score = score
best = (x, h)
return best if best else (target_x, 0)
def simulate_adaptive_walk(self, terrain, n_steps=10, step_length=0.1):
positions = []
x = 0
z = terrain[0][1] if terrain else 0
for step in range(n_steps):
target_x = x + step_length
fh = self.find_safe_foothold(terrain, target_x)
x, z = fh
positions.append((step, x, z))
return positions
random.seed(42)
utw = UnstructuredTerrainWalker()
print("=" * 55)
print(" Unstructured Terrain Walking Simulation")
print("=" * 55)
# Generate and analyze terrain
terrain = utw.generate_random_terrain(length=2.0)
print(f"\n [Generated Terrain: {len(terrain)} points]")
for i in range(0, len(terrain), 4):
x, h = terrain[i]
bar = '#' * max(0, int((h + 0.05) * 200))
print(f" x={x:.2f}m h={h:+.3f}m {bar}")
# Foothold search
print(f"\n [Safe Foothold Search]")
for target_x in [0.1, 0.3, 0.5, 0.8, 1.2]:
fh = utw.find_safe_foothold(terrain, target_x)
print(f" target_x={target_x:.1f}m -> foothold ({fh[0]:.3f}, {fh[1]:.4f})")
# Adaptive walk
print(f"\n [Adaptive Walk - 10 steps]")
positions = utw.simulate_adaptive_walk(terrain, n_steps=10)
for step, x, z in positions:
dz = z - (positions[step-1][2] if step > 0 else 0)
print(f" Step {step}: x={x:.3f}m z={z:.4f}m dz={dz*1000:.1f}mm")
print()
print(" OK - Unstructured terrain simulation complete")
仿真结果：
=======================================================
Unstructured Terrain Walking Simulation
=======================================================
[Generated Terrain: 41 points]
x=0.00m h=-0.001m #########
x=0.20m h=-0.035m ##
x=0.40m h=-0.050m
x=0.60m h=-0.033m ###
x=0.80m h=-0.034m ###
x=1.00m h=-0.043m #
x=1.20m h=-0.007m ########
x=1.40m h=-0.004m #########
x=1.60m h=+0.016m #############
x=1.80m h=+0.054m ####################
x=2.00m h=+0.073m ########################
[Safe Foothold Search]
target_x=0.1m -> foothold (0.050, -0.0032)
target_x=0.3m -> foothold (0.300, -0.0489)
target_x=0.5m -> foothold (0.500, -0.0418)
target_x=0.8m -> foothold (0.800, -0.0339)
target_x=1.2m -> foothold (1.200, -0.0069)
[Adaptive Walk - 10 steps]
Step 0: x=0.050m z=-0.0032m dz=-3.2mm
Step 1: x=0.200m z=-0.0351m dz=-31.9mm
Step 2: x=0.300m z=-0.0489m dz=-13.8mm
Step 3: x=0.400m z=-0.0500m dz=-1.1mm
Step 4: x=0.500m z=-0.0418m dz=8.2mm
Step 5: x=0.600m z=-0.0326m dz=9.2mm
Step 6: x=0.700m z=-0.0500m dz=-17.4mm
Step 7: x=0.800m z=-0.0339m dz=16.1mm
Step 8: x=0.900m z=-0.0500m dz=-16.1mm
Step 9: x=1.000m z=-0.0431m dz=6.9mm
OK - Unstructured terrain simulation complete