import math
class SlopeWalker:
def __init__(self, mass=6.2, body_L=0.4, body_W=0.2, z_com=0.2, mu=0.6):
self.mass = mass
self.g = 9.81
self.body_L = body_L
self.body_W = body_W
self.z_com = z_com
self.mu = mu
def gravity_decomposition(self, slope_angle):
F_parallel = self.mass * self.g * math.sin(slope_angle)
F_normal = self.mass * self.g * math.cos(slope_angle)
return F_parallel, F_normal
def max_slope_angle(self):
return math.atan(self.mu)
def required_friction(self, slope_angle):
return math.tan(slope_angle)
def stability_on_slope(self, slope_angle, direction='uphill'):
theta = slope_angle
com_x_offset = self.z_com * math.sin(theta)
if direction == 'uphill':
com_eff_x = com_x_offset
else:
com_eff_x = -com_x_offset
margin_fwd = self.body_L/2 - com_eff_x
margin_back = self.body_L/2 + com_eff_x
return margin_fwd, margin_back
def simulate_slope_walking(self, slope_deg=15, duration=3.0, dt=0.01):
slope = math.radians(slope_deg)
F_par = self.mass * self.g * math.sin(slope)
F_norm = self.mass * self.g * math.cos(slope)
pos = 0
vel = 0
t = 0
history = []
step_count = 0
step_phase = 0
step_freq = 1.5
push_force = F_par * 1.2 # slightly more than gravity component
while t < duration:
step_phase = (t * step_freq) % 1.0
# Simplified: apply push during stance, coast during swing
if step_phase < 0.5:
acc = (push_force - F_par) / self.mass
else:
acc = -F_par / self.mass # no push during swing
vel += acc * dt
pos += vel * dt
if int(t*100) % 30 == 0:
history.append((t, pos, vel, F_par, F_norm))
t += dt
return history
sw = SlopeWalker()
print("=" * 55)
print(" Slope Walking Simulation")
print("=" * 55)
# Gravity decomposition
print("\n [Gravity Decomposition]")
for deg in [0, 5, 10, 15, 20, 25, 30]:
rad = math.radians(deg)
F_par, F_norm = sw.gravity_decomposition(rad)
print(f" {deg:2d}deg: F_parallel={F_par:.1f}N, F_normal={F_norm:.1f}N, "
f"ratio={F_par/F_norm:.3f}, need_mu={math.tan(rad):.3f}")
# Max slope
max_slope = sw.max_slope_angle()
print(f"\n [Max Slope with mu={sw.mu}]")
print(f" Max slope = {max_slope*180/math.pi:.1f}deg ({math.tan(max_slope):.3f})")
# Stability on slope
print(f"\n [Stability on Slope]")
for deg in [0, 10, 20, 30]:
rad = math.radians(deg)
mf, mb = sw.stability_on_slope(rad)
print(f" {deg:2d}deg: fwd_margin={mf*1000:.1f}mm, back_margin={mb*1000:.1f}mm "
f"{'STABLE' if mf > 0 and mb > 0 else 'UNSTABLE'}")
# Walking simulation
print(f"\n [Slope Walking (15deg)]")
hist = sw.simulate_slope_walking(slope_deg=15)
for t, pos, vel, Fp, Fn in hist:
print(f" t={t:.2f}s pos={pos:.3f}m vel={vel:.3f}m/s F_par={Fp:.1f}N")
print()
print(" OK - Slope walking simulation complete")
仿真结果：
=======================================================
Slope Walking Simulation
=======================================================
[Gravity Decomposition]
0deg: F_parallel=0.0N, F_normal=60.8N, ratio=0.000, need_mu=0.000
5deg: F_parallel=5.3N, F_normal=60.6N, ratio=0.087, need_mu=0.087
10deg: F_parallel=10.6N, F_normal=59.9N, ratio=0.176, need_mu=0.176
15deg: F_parallel=15.7N, F_normal=58.7N, ratio=0.268, need_mu=0.268
20deg: F_parallel=20.8N, F_normal=57.2N, ratio=0.364, need_mu=0.364
25deg: F_parallel=25.7N, F_normal=55.1N, ratio=0.466, need_mu=0.466
30deg: F_parallel=30.4N, F_normal=52.7N, ratio=0.577, need_mu=0.577
[Max Slope with mu=0.6]
Max slope = 31.0deg (0.600)
[Stability on Slope]
0deg: fwd_margin=200.0mm, back_margin=200.0mm STABLE
10deg: fwd_margin=165.3mm, back_margin=234.7mm STABLE
20deg: fwd_margin=131.6mm, back_margin=268.4mm STABLE
30deg: fwd_margin=100.0mm, back_margin=300.0mm STABLE
[Slope Walking (15deg)]
t=0.00s pos=0.000m vel=0.005m/s F_par=15.7N
t=0.30s pos=0.025m vel=0.157m/s F_par=15.7N
t=0.60s pos=-0.019m vel=-0.513m/s F_par=15.7N
t=0.90s pos=-0.200m vel=-0.543m/s F_par=15.7N
t=1.20s pos=-0.409m vel=-1.031m/s F_par=15.7N
t=1.50s pos=-0.790m vel=-1.275m/s F_par=15.7N
t=1.80s pos=-1.181m vel=-1.549m/s F_par=15.7N
t=2.11s pos=-1.763m vel=-1.970m/s F_par=15.7N
t=2.41s pos=-2.342m vel=-2.062m/s F_par=15.7N
t=2.71s pos=-3.074m vel=-2.671m/s F_par=15.7N
OK - Slope walking simulation complete