stability

Center-of-mass, ZMP, support polygon, and tip-over margin metrics.

Stability metrics for walking mechanism evaluation.

Provides center-of-mass (CoM) tracking, zero-moment-point (ZMP) approximation, support polygon computation, and tip-over margin for planar walkers.

These metrics run on top of pymunk simulation data extracted from DynamicLinkage and are collected into StabilityTimeSeries for post-simulation analysis.

Example:

from leggedsnake.stability import compute_stability_snapshot, StabilityTimeSeries

snapshots = []
prev = None
for t in range(steps):
    world.update()
    snap = compute_stability_snapshot(dl, prev, t * dt, dt, gravity_y)
    snapshots.append(snap)
    prev = snap
series = StabilityTimeSeries(snapshots)
print(series.summary_metrics())

class leggedsnake.stability.StabilitySnapshot(time: float, com: tuple[float, float], com_velocity: tuple[float, float], zmp_x: float, support_polygon: list[tuple[float, float]], tip_over_margin: float, body_angle: float, ground_reaction_force: float = 0.0, peak_contact_force: float = 0.0)

Bases: object

Single-timestep stability measurement.

__init__(time: float, com: tuple[float, float], com_velocity: tuple[float, float], zmp_x: float, support_polygon: list[tuple[float, float]], tip_over_margin: float, body_angle: float, ground_reaction_force: float = 0.0, peak_contact_force: float = 0.0) → None

body_angle: float

com: tuple[float, float]

com_velocity: tuple[float, float]

ground_reaction_force: float: Sum of contact-force magnitudes between the linkage and the ground (Newtons) — zero when no foot is touching.

peak_contact_force: float: Largest single-contact force magnitude this step.

support_polygon: list[tuple[float, float]]

time: float

tip_over_margin: float

zmp_x: float

class leggedsnake.stability.StabilityTimeSeries(snapshots: list[~leggedsnake.stability.StabilitySnapshot] = <factory>)

Bases: object

Full stability history across a simulation run.

Variables:: snapshots (list[StabilitySnapshot]) – Chronologically ordered stability measurements.

__init__(snapshots: list[~leggedsnake.stability.StabilitySnapshot] = <factory>) → None

property angular_stability: float: RMS body angle — lower means more upright.

property com_trajectory: list[tuple[float, float]]: Center-of-mass (x, y) at each timestep.

property mean_ground_reaction_force: float

Mean summed foot-ground contact force across the run (N).

Averages over every step — steps with no foot contact count as zero. A well-footed walker’s mean should approximate the weight of the walker.

property mean_speed: float: Mean forward (x-axis) CoM speed across the run.

property mean_tip_over_margin: float: Average tip-over margin across the simulation.

property min_tip_over_margin: float: Worst-case tip-over margin (most unstable instant).

property peak_contact_force: float

Largest single foot-ground contact force across the run (N).

Useful for spotting impulsive spikes that would shatter a real mechanism even when the summed force looks benign.

property peak_ground_reaction_force: float: Maximum summed foot-ground contact force across the run (N).

snapshots: list[StabilitySnapshot]

property speed_variance: float

Population variance of the forward CoM speed.

Captures how steady the walker’s progress is under the current terrain — a smooth gait on flat ground has near-zero variance, jerky gaits or rough terrain raise it.

summary_metrics() → dict[str, float]: Flat dictionary of all scalar stability metrics.

property zmp_excursion: float: Maximum deviation of ZMP from the support polygon center.

leggedsnake.stability.approximate_zmp(com: tuple[float, float], com_velocity: tuple[float, float], prev_velocity: tuple[float, float], dt: float, gravity: float) → float

Zero-moment-point x-coordinate via inverted-pendulum model.

Uses the linear inverted pendulum approximation:

x_zmp = x_com - (z_com * a_x) / (g + a_z)

where a is the CoM acceleration computed by finite difference from consecutive velocity readings, and g is the magnitude of gravitational acceleration (positive downward).

Parameters:

com (tuple[float, float]) – Current center-of-mass position.
com_velocity (tuple[float, float]) – Current CoM velocity.
prev_velocity (tuple[float, float]) – CoM velocity at the previous timestep.
dt (float) – Timestep duration (seconds).
gravity (float) – Gravitational acceleration magnitude (positive, e.g. 9.81).

Returns:

ZMP x-coordinate.

Return type:

float

leggedsnake.stability.compute_com(linkage: DynamicLinkage) → tuple[float, float]

Mass-weighted center of mass of all rigid bodies.

Parameters:: linkage (DynamicLinkage) – The physics-enabled linkage.
Returns:: (x, y) center of mass in world coordinates.
Return type:: tuple[float, float]

leggedsnake.stability.compute_com_velocity(linkage: DynamicLinkage) → tuple[float, float]

Mass-weighted center-of-mass velocity.

Parameters:: linkage (DynamicLinkage) – The physics-enabled linkage.
Returns:: (vx, vy) CoM velocity.
Return type:: tuple[float, float]

leggedsnake.stability.compute_stability_snapshot(linkage: DynamicLinkage, prev_snapshot: StabilitySnapshot | None, time: float, dt: float, gravity: float, foot_ids: list[str] | None = None, ground_threshold: float = 0.1, static_body: pm.Body | None = None) → StabilitySnapshot

Assemble a full stability snapshot from current physics state.

Parameters:

linkage (DynamicLinkage) – The physics-enabled linkage (call after world.update()).
prev_snapshot (StabilitySnapshot | None) – Previous snapshot (for velocity finite-differencing). If None, ZMP defaults to CoM x-position.
time (float) – Current simulation time.
dt (float) – Physics timestep.
gravity (float) – Gravitational acceleration magnitude (positive).
foot_ids (list[str] | None) – Node IDs of feet for support polygon computation.
ground_threshold (float) – Maximum y-coordinate for ground contact detection.
static_body (pm.Body | None) – If provided, sample foot–ground contact forces via sample_ground_reaction_force() and populate the ground_reaction_force and peak_contact_force fields. Pass world.space.static_body to enable.

Return type:

StabilitySnapshot

leggedsnake.stability.compute_tip_over_margin(com: tuple[float, float], support_polygon: list[tuple[float, float]]) → float

Signed distance from CoM projection to nearest support boundary.

For a 2D planar walker, the “support polygon” is typically a line segment on the x-axis. The margin is the minimum horizontal distance from the CoM x-projection to the support boundary edges.

Positive means the CoM is inside the support; negative means tipping.

Parameters:

com (tuple[float, float]) – Center-of-mass position.
support_polygon (list[tuple[float, float]]) – Ground contact points (sorted by x).

Returns:

Signed tip-over margin. Positive = stable, negative = tipping.

Return type:

float

leggedsnake.stability.get_support_polygon(linkage: DynamicLinkage, foot_ids: list[str] | None = None, ground_threshold: float = 0.1) → list[tuple[float, float]]

Convex hull of foot positions near ground level.

Parameters:

linkage (DynamicLinkage) – The physics-enabled linkage.
foot_ids (list[str] | None) – Node IDs of feet. If None, uses all joints.
ground_threshold (float) – Maximum y-coordinate for a foot to be considered in ground contact.

Returns:

Convex hull vertices of the support polygon (may be empty, a single point, or a line segment for 2D walkers).

Return type:

list[tuple[float, float]]

leggedsnake.stability.sample_ground_reaction_force(linkage: DynamicLinkage, static_body: pm.Body, dt: float) → tuple[float, float]

Sample foot–ground contact forces right after a physics step.

Iterates active pymunk arbiters on every linkage body and sums the impulse magnitude of arbiters that involve the space’s static body (the ground / road). Call after space.step(dt); arbiters are only valid then.

Parameters:

linkage (DynamicLinkage) – The linkage whose contacts to sample.
static_body (pm.Body) – The pymunk static body that owns all ground segments — typically world.space.static_body.
dt (float) – The timestep just taken. Used to convert accumulated impulse to an average force (F = J / dt). Must be > 0.

Returns:

(total_force, peak_force) — sum of all foot-ground contact forces this step, and the largest single-contact force. Both in the same units as pymunk mass × m / s² (usually Newtons).

Return type:

tuple[float, float]