🤖 静态稳定性

import math class StaticStability: def __init__(self, body_L=0.4, body_W=0.2, mass=6.2): self.mass = mass self.g = 9.81 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 convex_hull(self, points): pts = sorted(set(points)) if len(pts) <= 1: return pts lower = [] for p in pts: while len(lower) >= 2 and self.cross(lower[-2], lower[-1], p) <= 0: lower.pop() lower.append(p) upper = [] for p in reversed(pts): while len(upper) >= 2 and self.cross(upper[-2], upper[-1], p) <= 0: upper.pop() upper.append(p) return lower[:-1] + upper[:-1] @staticmethod def cross(O, A, B): return (A[0]-O[0])*(B[1]-O[1]) - (A[1]-O[1])*(B[0]-O[0]) def stability_margin(self, com, support_legs): pts = [self.legs[l] for l in support_legs] hull = self.convex_hull(pts) if len(hull) < 3: if len(hull) == 2: p1, p2 = hull dx, dy = p2[0]-p1[0], p2[1]-p1[1] seg_sq = dx*dx + dy*dy if seg_sq < 1e-10: return math.hypot(com[0]-p1[0], com[1]-p1[1]) t = max(0, min(1, ((com[0]-p1[0])*dx + (com[1]-p1[1])*dy)/seg_sq)) px, py = p1[0]+t*dx, p1[1]+t*dy return math.hypot(com[0]-px, com[1]-py) return 0 min_dist = float('inf') n = len(hull) for i in range(n): p1 = hull[i] p2 = hull[(i+1) % n] dx, dy = p2[0]-p1[0], p2[1]-p1[1] seg_sq = dx*dx + dy*dy t = max(0, min(1, ((com[0]-p1[0])*dx + (com[1]-p1[1])*dy)/seg_sq)) px, py = p1[0]+t*dx, p1[1]+t*dy d = math.hypot(com[0]-px, com[1]-py) min_dist = min(min_dist, d) return min_dist def is_inside_support(self, com, support_legs): return self.stability_margin(com, support_legs) > 0 def support_polygon_area(self, support_legs): pts = [self.legs[l] for l in support_legs] hull = self.convex_hull(pts) n = len(hull) if n < 3: return 0 area = 0 for i in range(n): j = (i+1) % n area += hull[i][0]*hull[j][1] - hull[j][0]*hull[i][1] return abs(area)/2 ss = StaticStability() print("=" * 60) print(" Static Stability Simulation") print("=" * 60) com = (0, 0) all_legs = ['LF', 'RF', 'LB', 'RB'] margin = ss.stability_margin(com, all_legs) area = ss.support_polygon_area(all_legs) print(f"\n [4-leg stance] CoM=({com[0]},{com[1]})") print(f" Stability margin: {margin*1000:.1f} mm") print(f" Support area: {area*1e4:.1f} cm2") print(f" Stable: {'YES' if ss.is_inside_support(com, all_legs) else 'NO'}") support_3 = ['RF', 'LB', 'RB'] for com_x in [0, 0.05, 0.1, 0.15, 0.2]: com = (com_x, 0) margin = ss.stability_margin(com, support_3) stable = ss.is_inside_support(com, support_3) s = "STABLE" if stable else "UNSTABLE" print(f" Lift LF, CoM=({com_x:.2f},0): margin={margin*1000:.1f}mm [{s}]") print(f"\n [Max Safe Offset Search]") for swing_leg in ['LF', 'RF', 'LB', 'RB']: support = [l for l in all_legs if l != swing_leg] max_fwd = 0 max_side = 0 for i in range(100): x = i * 0.005 if ss.is_inside_support((x, 0), support): max_fwd = x for i in range(100): y = i * 0.005 if ss.is_inside_support((0, y), support): max_side = y print(f" Lift {swing_leg}: fwd_max={max_fwd*1000:.0f}mm, side_max={max_side*1000:.0f}mm") print(f"\n [Walk Gait Stability]") walk_phases = [ (0.00, ['RF', 'LB', 'RB']), (0.25, ['LF', 'LB', 'RB']), (0.50, ['LF', 'RF', 'RB']), (0.75, ['LF', 'RF', 'LB']), ] for phase, support in walk_phases: swing = [l for l in all_legs if l not in support][0] margin = ss.stability_margin((0, 0), support) area = ss.support_polygon_area(support) print(f" Phase {phase:.2f} (lift {swing}): margin={margin*1000:.1f}mm, area={area*1e4:.1f}cm2") print(f"\n [CoM Offset Sensitivity]") com_offsets = [(0.05, 0), (0.10, 0), (0, 0.05), (0, 0.10)] for offset in com_offsets: margins = [] for phase, support in walk_phases: m = ss.stability_margin(offset, support) margins.append(m) min_m = min(margins) s = "STABLE" if min_m > 0 else "UNSTABLE" print(f" CoM offset=({offset[0]*1000:.0f},{offset[1]*1000:.0f})mm: min_margin={min_m*1000:.1f}mm [{s}]") print() print(" OK - Static stability simulation complete")