#!/usr/bin/env python3
# Copyright (c) 2024-2026 Karana Dynamics Pty Ltd. All rights reserved.
#
# NOTICE TO USER:
#
# This source code and/or documentation (the "Licensed Materials") is
# the confidential and proprietary information of Karana Dynamics Inc.
# Use of these Licensed Materials is governed by the terms and conditions
# of a separate software license agreement between Karana Dynamics and the
# Licensee ("License Agreement"). Unless expressly permitted under that
# agreement, any reproduction, modification, distribution, or disclosure
# of the Licensed Materials, in whole or in part, to any third party
# without the prior written consent of Karana Dynamics is strictly prohibited.
#
# THE LICENSED MATERIALS ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND.
# KARANA DYNAMICS DISCLAIMS ALL WARRANTIES, EXPRESS OR IMPLIED, INCLUDING
# BUT NOT LIMITED TO WARRANTIES OF MERCHANTABILITY, NON-INFRINGEMENT, AND
# FITNESS FOR A PARTICULAR PURPOSE.
#
# IN NO EVENT SHALL KARANA DYNAMICS BE LIABLE FOR ANY DAMAGES WHATSOEVER,
# INCLUDING BUT NOT LIMITED TO LOSS OF PROFITS, DATA, OR USE, EVEN IF
# ADVISED OF THE POSSIBILITY OF SUCH DAMAGES, WHETHER IN CONTRACT, TORT,
# OR OTHERWISE ARISING OUT OF OR IN CONNECTION WITH THE LICENSED MATERIALS.
#
# U.S. Government End Users: The Licensed Materials are a "commercial item"
# as defined at 48 C.F.R. 2.101, and are provided to the U.S. Government
# only as a commercial end item under the terms of this license.
#
# Any use of the Licensed Materials in individual or commercial software must
# include, in the user documentation and internal source code comments,
# this Notice, Disclaimer, and U.S. Government Use Provision.
#!/usr/bin/env python3
"""Analyze, report, and safely edit kdFlex closed-loop mobility.
The module follows a staged workflow: discover coupled loop groups, inspect
their constraint Jacobians, explore connected closure-feasible motion, classify
coordinate participation, and only then apply an explicitly requested edit.
Read-only analysis and serialized-model mutation have separate class APIs.
"""
from __future__ import annotations
from dataclasses import dataclass, replace
from itertools import combinations
import math
from pathlib import Path
import tempfile
from typing import Any, Callable
import os
import click
import Karana.Core as kc
import Karana.Dynamics as kd
import Karana.Frame as kf
import numpy as np
from numpy.typing import NDArray
from scipy import linalg
from scipy.optimize import least_squares
from Karana.Dynamics.SOADyn_types import SubGraphDS
from Karana.KUtils.BasicPrefab import BasicPrefabDS
from Karana.Math.Ktyping import QuantityOrValue
from Karana.KUtils.kdpy_model_io import LoadedModel, _loadSubGraph, loadModel
INERT_SUBHINGE_TOLERANCE = 2.0e-7
[docs]
@dataclass(frozen=True)
class MobilityRequest:
"""Define the target nullity and admissible drivers for one loop group."""
anchor: str
target_mobility: int
drivers: tuple[str, ...]
allowed_components: tuple[str, ...]
[docs]
@dataclass(frozen=True)
class LoopGroup:
"""Identify constraint rows and velocity columns coupled into one loop."""
constraints: tuple[str, ...]
rows: tuple[int, ...]
columns: tuple[int, ...]
coordinate_ids: tuple[int, ...]
coordinate_names: tuple[str, ...]
[docs]
@dataclass(frozen=True)
class MobilityCandidate:
"""Describe one canonical cut-joint coordinate considered for insertion."""
constraint: str
component: str
subhinge_type: str
axis: tuple[float, float, float]
column: tuple[float, ...]
[docs]
@dataclass(frozen=True)
class MobilityChange:
"""Summarize an accepted connected-loop mobility augmentation."""
anchor: str
constraints: tuple[str, ...]
before_rank: int
before_nullity: int
target_mobility: int
additions: tuple[MobilityCandidate, ...]
predicted_rank: int
predicted_nullity: int
before_smallest_singular: float
predicted_smallest_singular: float
[docs]
@dataclass(frozen=True)
class GeometryChange:
"""Summarize one accepted or rejected weak-mode axis reconciliation."""
constraints: tuple[str, ...]
accepted: bool
before_rank: int
after_rank: int
before_nullity: int
after_nullity: int
before_weak_singular: float
after_weak_singular: float
axis_changes: tuple[tuple[str, float], ...]
validation_spans: tuple[tuple[str, float, float, str], ...] = ()
rejection_reason: str | None = None
[docs]
def mobilityRequests(
loop_options: dict[str, Any] | None,
) -> tuple[MobilityRequest, ...]:
"""Validate and merge repeatable four-field DOF requests by loop anchor.
Kclick returns one record per ``--add-dofs`` occurrence. Multiple records
may contribute drivers to the same loop, but target mobility and allowed
component families must agree. The result is canonical input for the
Jacobian-based recommendation phase.
"""
# Kclick records an omitted chained subcommand as an explicit None value.
loop_options = loop_options or {}
raw_records = loop_options.get("add_dofs", ())
records = raw_records if isinstance(raw_records, tuple) else ()
# Build parallel maps keyed by anchor so repeated records can contribute
# drivers while enforcing one target and component policy per loop.
targets: dict[str, int] = {}
drivers: dict[str, list[str]] = {}
allowed: dict[str, tuple[str, ...]] = {}
# Expand comma-delimited family selectors into canonical components once.
component_families = {
"pin": ("Rx", "Ry", "Rz"),
"linear": ("Tx", "Ty", "Tz"),
"rx": ("Rx",),
"ry": ("Ry",),
"rz": ("Rz",),
"tx": ("Tx",),
"ty": ("Ty",),
"tz": ("Tz",),
}
# Validate and canonicalize every record before constructing immutable
# MobilityRequest values.
for anchor, target_value, driver, allowed_value in records:
target_mobility = int(target_value)
components: list[str] = []
if allowed_value != "-":
selectors = allowed_value.split(",")
if any(not selector for selector in selectors):
raise click.UsageError(
f"loops --add-dofs has an empty allowed type for loop '{anchor}'"
)
for selector in selectors:
family = component_families.get(selector.lower())
if family is None:
raise click.UsageError(
f"unsupported loops --add-dofs allowed type '{selector}'"
)
for component in family:
if component not in components:
components.append(component)
component_tuple = tuple(components)
if anchor in targets:
if target_mobility != targets[anchor]:
raise click.UsageError(
f"loops --add-dofs entries for loop '{anchor}' have conflicting target mobility"
)
if component_tuple != allowed[anchor]:
raise click.UsageError(
f"loops --add-dofs entries for loop '{anchor}' have conflicting allowed types"
)
else:
targets[anchor] = target_mobility
drivers[anchor] = []
allowed[anchor] = component_tuple
if driver != "-":
if driver in drivers[anchor]:
raise click.UsageError(
f"mobility driver '{driver}' is duplicated for loop '{anchor}'"
)
drivers[anchor].append(driver)
# Freeze the validated mutable collections into analysis input values.
requests = []
for anchor, target_mobility in targets.items():
if len(drivers[anchor]) > target_mobility:
raise click.UsageError(
f"anchor '{anchor}' requests mobility {target_mobility} but has "
f"{len(drivers[anchor])} independent drivers"
)
requests.append(
MobilityRequest(
anchor,
target_mobility,
tuple(drivers[anchor]),
allowed[anchor],
)
)
return tuple(requests)
[docs]
def normalizedLoopOptions(options: dict[str, Any]) -> dict[str, Any]:
"""Return concrete loop defaults in place of Kclick omission sentinels.
Kclick distinguishes an uninvoked chained command from one whose options
hold ordinary false or empty values. Downstream loop code consumes stable
Python values and does not depend on sentinel identity.
"""
raw_options = options.get("loops")
if not isinstance(raw_options, dict):
return {}
# Kclick populates every field of an uninvoked chained command with its
# opaque UNSET sentinel, so accept only the concrete Click result types.
return {
"ranges_update": (
raw_options["ranges_update"]
if isinstance(raw_options.get("ranges_update"), tuple)
else ()
),
"optimize": (
raw_options["optimize"] if isinstance(raw_options.get("optimize"), bool) else False
),
"add_dofs": (
raw_options["add_dofs"] if isinstance(raw_options.get("add_dofs"), tuple) else ()
),
"remove_inert": (
raw_options["remove_inert"]
if isinstance(raw_options.get("remove_inert"), tuple)
else ()
),
"reduce_ball": (
raw_options["reduce_ball"] if isinstance(raw_options.get("reduce_ball"), tuple) else ()
),
"reduce_spherical": (
raw_options["reduce_spherical"]
if isinstance(raw_options.get("reduce_spherical"), tuple)
else ()
),
"mobility_check": (
raw_options["mobility_check"]
if isinstance(raw_options.get("mobility_check"), tuple)
else ()
),
"ranges_info": (
raw_options["ranges_info"]
if isinstance(raw_options.get("ranges_info"), bool)
else False
),
}
[docs]
def mobilityCheckSelections(
values: tuple[str, ...],
) -> tuple[tuple[str, int], ...] | None:
"""Parse repeatable ``SUBHINGE[/OFFSET]`` mobility-check selectors.
``None`` represents the special all-coordinate selection. A missing suffix
selects offset zero, while an explicit suffix must be a nonnegative integer.
"""
# ``all`` is an aggregate selector; combining it with named work would make
# report scope and progress ordering ambiguous.
if "all" in values:
if len(values) != 1:
raise click.UsageError("loops --mobility-check all cannot be combined with coordinates")
return None
# Preserve CLI order for progress reporting while rejecting duplicate work.
selections: list[tuple[str, int]] = []
for value in values:
name, separator, offset_text = value.rpartition("/")
if not separator:
selection = (value, 0)
else:
if not name or not offset_text.isdigit():
raise click.UsageError("loops --mobility-check expects SUBHINGE or SUBHINGE/OFFSET")
selection = (name, int(offset_text))
if selection in selections:
raise click.UsageError(f"loops --mobility-check coordinate is duplicated: {value}")
selections.append(selection)
return tuple(selections)
def _constraintRowsByName(subgraph: kd.SubGraph) -> dict[str, tuple[int, ...]]:
"""Map enabled cut-joint names to their six full-error row indices."""
constraints = subgraph.enabledConstraints(kd.BilateralConstraintType.CUTJOINT_LOOP)
result = {constraint.name(): [] for constraint in constraints}
constraint_names = {constraint.id(): constraint.name() for constraint in constraints}
for constraint in constraints:
del constraint
for row in range(6 * len(result)):
owner, _ = subgraph.constraintErrorAt(row)
try:
name = constraint_names.get(owner.id())
if name is not None:
result[name].append(row)
finally:
del owner
return {name: tuple(rows) for name, rows in result.items()}
[docs]
def discoverLoopGroups(
subgraph: kd.SubGraph,
*,
activity_tolerance: float = 1.0e-10,
) -> tuple[LoopGroup, ...]:
"""Discover connected cut-joint groups through shared active coordinates.
Two constraints belong to the same group when their Jacobian rows touch a
common active coordinate, including transitive connections. Grouping first
prevents kdpy from making an invalid independent decision for each cut
joint when several closures are algebraically coupled.
"""
# Infer coupling from shared active velocity columns rather than serialized
# topology alone; imported closures can share motion indirectly.
rows_by_name = _constraintRowsByName(subgraph)
if not rows_by_name:
return ()
matrix = np.asarray(kd.Algorithms.evalVelocityConstraintMatrix(subgraph, True), dtype=float)
graph_data = subgraph.graphCoordData()
try:
active_by_name: dict[str, set[int]] = {}
for name, rows in rows_by_name.items():
columns = np.flatnonzero(
np.linalg.norm(matrix[np.asarray(rows), :], axis=0) > activity_tolerance
)
coordinate_ids: set[int] = set()
for column in columns:
coordinate, _ = graph_data.coordAt(int(column))
coordinate_ids.add(coordinate.id())
del coordinate
active_by_name[name] = coordinate_ids
# Build connected components instead of assuming one cut joint is one
# independent loop. Shared coordinates couple their rank decisions.
remaining = set(rows_by_name)
groups: list[LoopGroup] = []
while remaining:
seed = min(remaining)
component = {seed}
component_coordinates = set(active_by_name[seed])
changed = True
while changed:
changed = False
for name in sorted(remaining - component):
if component_coordinates & active_by_name[name]:
component.add(name)
component_coordinates.update(active_by_name[name])
changed = True
remaining.difference_update(component)
# Freeze stable row, column, ID, and name inventories so later rank
# analysis does not depend on pybind wrapper lifetimes.
rows = tuple(sorted(row for name in component for row in rows_by_name[name]))
columns = tuple(
int(column)
for column in np.flatnonzero(
np.linalg.norm(matrix[np.asarray(rows), :], axis=0) > activity_tolerance
)
)
coordinate_ids = []
coordinate_names = []
for column in columns:
coordinate, _ = graph_data.coordAt(column)
coordinate_ids.append(coordinate.id())
coordinate_names.append(coordinate.name())
del coordinate
groups.append(
LoopGroup(
tuple(sorted(component)),
rows,
columns,
tuple(coordinate_ids),
tuple(coordinate_names),
)
)
return tuple(groups)
finally:
del graph_data
def _groupForAnchor(groups: tuple[LoopGroup, ...], anchor: str) -> LoopGroup:
"""Resolve a unique connected loop group containing a named coordinate."""
matches = [group for group in groups if anchor in group.coordinate_names]
if not matches:
raise click.UsageError(f"loops --add-dofs loop '{anchor}' is not active in an enabled loop")
if len(matches) != 1:
raise click.UsageError(f"loops --add-dofs loop '{anchor}' resolves to multiple loop groups")
return matches[0]
def _groupMatrix(subgraph: kd.SubGraph, group: LoopGroup) -> np.ndarray:
"""Evaluate the full constraint Jacobian restricted to one loop group."""
matrix = np.asarray(kd.Algorithms.evalVelocityConstraintMatrix(subgraph, True), dtype=float)
return matrix[np.ix_(group.rows, group.columns)]
def _treeDriverStates(
subgraph: kd.SubGraph,
driver_name: str,
*,
increment: float = 0.02,
closure_tolerance: float = 1.0e-8,
) -> tuple[np.ndarray, ...]:
"""Return closed witness states around one prescribed tree coordinate.
Existing solver freezes are restored before returning. If the driver is not
a scalar tree coordinate, or neither signed perturbation closes, the nominal
state is the only witness.
"""
tree_data = subgraph.treeCoordData()
graph_data = subgraph.graphCoordData()
solver = subgraph.cks()
# Capture the caller's configuration and characterize the weak mode before
# changing any physical axis.
nominal = np.asarray(subgraph.getQ(), dtype=float).copy()
# Preserve caller-owned freezes while this trial installs its own partition.
frozen_coordinates = _captureFrozenCoordinates(subgraph, solver)
target_column = None
q_index = None
try:
for column in range(tree_data.nU()):
coordinate, local_offset = tree_data.coordAt(column)
try:
if coordinate.name() == driver_name and local_offset == 0:
target_column = column
q_index = int(graph_data.coordOffsets(coordinate).Q)
break
finally:
del coordinate
if target_column is None or q_index is None:
return (nominal,)
matrix = np.asarray(
kd.Algorithms.evalVelocityConstraintMatrix(subgraph, False), dtype=float
)
pool = np.asarray([index for index in range(tree_data.nU()) if index != target_column])
_, triangular, pivots = linalg.qr(matrix[:, pool], mode="economic", pivoting=True)
diagonal = np.abs(np.diag(triangular))
scale = diagonal[0] if diagonal.size else 0.0
rank = int(np.count_nonzero(diagonal > 1.0e-9 * scale)) if scale else 0
if rank < _matrixRank(matrix):
return (nominal,)
dependent = set(pool[pivots[:rank]].tolist())
solver.clearFrozenCoords()
for column in range(tree_data.nU()):
if column not in dependent:
coordinate, local_offset = tree_data.coordAt(column)
# Package the provider and local offset as the Coordinate API type.
solver.freezeCoord((coordinate, local_offset))
del coordinate
states = [nominal]
for direction in (-1.0, 1.0):
trial = nominal.copy()
trial[q_index] += direction * increment
subgraph.setQ(trial)
if float(solver.solveQ()) <= closure_tolerance:
states.append(np.asarray(subgraph.getQ(), dtype=float).copy())
return tuple(states)
finally:
subgraph.setQ(nominal)
_restoreFrozenCoordinates(subgraph, solver, frozen_coordinates)
del solver
del graph_data
del tree_data
def _axisTangentBasis(axis: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
"""Return two orthonormal perturbation directions for a unit axis."""
trial = np.array([1.0, 0.0, 0.0])
if abs(float(np.dot(axis, trial))) > 0.8:
trial = np.array([0.0, 1.0, 0.0])
first = trial - np.dot(trial, axis) * axis
first /= np.linalg.norm(first)
return first, np.cross(axis, first)
def _axisAngle(first: np.ndarray, second: np.ndarray) -> float:
"""Return unsigned angular separation between two unit directions."""
return math.acos(float(np.clip(np.dot(first, second), -1.0, 1.0)))
def _weakModeProbeStates(
subgraph: kd.SubGraph,
group: LoopGroup,
weak_directions: np.ndarray,
*,
angular_distance: float = 0.05,
linear_distance: float = 0.005,
) -> tuple[np.ndarray, ...]:
"""Build finite predictor states along each weak velocity direction.
The source geometry can be unable to close away from its nominal pose, so
closed source poses are not a reliable optimization input. These states
integrate each near-null velocity direction through direct scalar Q charts.
They need not close before optimization: their purpose is to require the
candidate rank dependency to persist over a finite neighborhood instead of
occurring only at the nominal configuration.
"""
nominal = np.asarray(subgraph.getQ(), dtype=float).copy()
graph_data = subgraph.graphCoordData()
probes = [nominal]
try:
for direction in weak_directions:
delta = np.zeros_like(nominal)
scaled_components: list[tuple[int, float, float]] = []
for group_column, full_column in enumerate(group.columns):
coordinate, local_offset = graph_data.coordAt(full_column)
try:
# Only direct scalar charts can be integrated without a
# coordinate-specific position update rule.
if coordinate.nQ() != coordinate.nU() or coordinate.nQ() != 1:
continue
if not isinstance(coordinate, (kd.PinSubhinge, kd.LinearSubhinge)):
continue
q_index = int(graph_data.coordOffsets(coordinate).Q + local_offset)
distance = (
linear_distance
if isinstance(coordinate, kd.LinearSubhinge)
else angular_distance
)
scaled_components.append((q_index, float(direction[group_column]), distance))
finally:
del coordinate
scale_denominator = max(
(abs(value) / distance for _, value, distance in scaled_components),
default=0.0,
)
if scale_denominator == 0.0:
continue
scale = 1.0 / scale_denominator
for q_index, value, _ in scaled_components:
delta[q_index] += scale * value
# Symmetric probes prevent a correction that works on only one
# side of the nominal configuration from passing optimization.
probes.extend((nominal - delta, nominal + delta))
return tuple(probes)
finally:
del graph_data
def _validateFiniteGeometryMobility(
subgraph: kd.SubGraph,
group: LoopGroup,
drivers: tuple[str, ...],
target_rank: int,
*,
exact_relative_tolerance: float,
angular_distance: float = 0.05,
linear_distance: float = 0.005,
) -> tuple[
bool,
tuple[tuple[str, float, float, str], ...],
str | None,
]:
"""Require each selected driver to close through a finite two-sided range."""
nominal = np.asarray(subgraph.getQ(), dtype=float).copy()
validation_spans: list[tuple[str, float, float, str]] = []
if not drivers:
return False, (), "no direct physical driver is available for finite validation"
try:
for driver_name in drivers:
driver = _coordinateByName(subgraph, driver_name)
try:
if not isinstance(driver, (kd.PinSubhinge, kd.LinearSubhinge)):
return False, tuple(validation_spans), f"{driver_name} lacks a scalar axis"
distance = (
linear_distance if isinstance(driver, kd.LinearSubhinge) else angular_distance
)
unit = "m" if isinstance(driver, kd.LinearSubhinge) else "rad"
closed_states = [nominal.copy()]
measured = feasibleRangeLimit(
subgraph,
driver,
initial_step=0.25 * distance,
maximum_step=0.5 * distance,
maximum_distance=distance,
boundary_tolerance=distance * 1.0e-5,
sample_callback=lambda state: closed_states.append(state.copy()),
)
lower_distance = measured.current - measured.lower
upper_distance = measured.upper - measured.current
validation_spans.append((driver_name, lower_distance, upper_distance, unit))
required = distance * (1.0 - 2.0e-5)
if lower_distance < required or upper_distance < required:
return (
False,
tuple(validation_spans),
f"{driver_name} closes only -{lower_distance:.6g}/"
f"+{upper_distance:.6g} {unit}; requires +/-{distance:.6g} {unit}",
)
# A closed span is insufficient if the requested rank loss is
# present only at nominal. Check it at every retained witness.
for state in closed_states:
subgraph.setQ(state)
values = np.linalg.svd(_groupMatrix(subgraph, group), compute_uv=False)
if values.size <= target_rank or values[0] == 0.0:
return False, tuple(validation_spans), "finite rank validation is empty"
if values[target_rank] > exact_relative_tolerance * values[0]:
return (
False,
tuple(validation_spans),
f"{driver_name} loses the target rank over its finite span",
)
except (LockedLoopError, IndirectCoordinateChartError) as error:
return False, tuple(validation_spans), f"{driver_name}: {error}"
finally:
del driver
return True, tuple(validation_spans), None
finally:
subgraph.setQ(nominal)
[docs]
def optimizeLoopGeometry(
subgraph: kd.SubGraph,
group: LoopGroup,
*,
weak_relative_tolerance: float = 1.0e-3,
exact_relative_tolerance: float = 1.0e-9,
maximum_axis_parameter: float = 1.0e-3,
) -> GeometryChange | None:
"""Remove weak nonzero modes using bounded finite-motion axis corrections.
Small nonzero singular values often indicate nominally aligned hinge axes
that differ by modeling noise. Candidate axes are adjusted only within the
configured bound and evaluated at predictor poses along the weak modes. A
change is accepted only when the corrected mechanism then closes through a
finite two-sided range and retains the intended rank there; otherwise the
original geometry is restored.
"""
nominal = np.asarray(subgraph.getQ(), dtype=float).copy()
before_matrix = _groupMatrix(subgraph, group)
_, singular_values, right_transpose = np.linalg.svd(before_matrix, full_matrices=False)
if singular_values.size == 0 or singular_values[0] == 0.0:
return None
relative = singular_values / singular_values[0]
weak_indices = np.flatnonzero(
(relative > exact_relative_tolerance) & (relative < weak_relative_tolerance)
)
if weak_indices.size == 0:
return None
before_rank = int(np.count_nonzero(relative > exact_relative_tolerance))
target_rank = before_rank - int(weak_indices.size)
minor_size = target_rank + 1
if minor_size > min(before_matrix.shape):
return None
minor_count = math.comb(before_matrix.shape[0], minor_size) * math.comb(
before_matrix.shape[1], minor_size
)
if minor_count > 5000:
# Avoid an unexpectedly combinatorial command-line transformation.
return GeometryChange(
group.constraints,
False,
before_rank,
before_rank,
before_matrix.shape[1] - before_rank,
before_matrix.shape[1] - before_rank,
float(singular_values[weak_indices[0]]),
float(singular_values[weak_indices[0]]),
(),
)
# Select one likely physical driver per weak right-singular direction.
# Prefer physical tree coordinates as finite-motion witnesses because they
# can be prescribed independently during closure solves.
tree_data = subgraph.treeCoordData()
tree_names: set[str] = set()
try:
for column in range(tree_data.nU()):
coordinate, local_offset = tree_data.coordAt(column)
if local_offset == 0 and isinstance(coordinate, (kd.PinSubhinge, kd.LinearSubhinge)):
tree_names.add(coordinate.name())
del coordinate
finally:
del tree_data
drivers = []
for weak_index in weak_indices:
for column in np.argsort(-np.abs(right_transpose[weak_index, :])):
name = group.coordinate_names[int(column)]
if name in tree_names and name not in drivers:
drivers.append(name)
break
# Enforce the rank dependency away from nominal even when the imperfect
# source mechanism cannot provide a closed nonnominal starting pose.
states = list(
_weakModeProbeStates(
subgraph,
group,
right_transpose[weak_indices, :],
)
)
# Retain unique scalar axes participating in this connected group.
# Parameterize each unique scalar axis by two tangent components. This keeps
# optimization on the unit sphere without redundant radial variables.
subhinges = []
originals = []
bases = []
seen_names: set[str] = set()
try:
for name in group.coordinate_names:
if name in seen_names:
continue
try:
subhinge = _coordinateByName(subgraph, name)
except click.UsageError:
continue
subhinges.append(subhinge)
axis = np.asarray(subhinge.getUnitAxis(), dtype=float)
originals.append(axis)
bases.append(_axisTangentBasis(axis))
seen_names.add(name)
if not subhinges:
return None
def setAxes(parameters: np.ndarray) -> None:
"""Apply two tangent parameters to each participating unit axis."""
for index, subhinge in enumerate(subhinges):
first, second = bases[index]
axis = (
originals[index]
+ parameters[2 * index] * first
+ parameters[2 * index + 1] * second
)
subhinge.setUnitAxis(axis / np.linalg.norm(axis))
def localMatrix(parameters: np.ndarray, state: np.ndarray) -> np.ndarray:
"""Evaluate group geometry at one fixed generalized state."""
setAxes(parameters)
subgraph.setQ(state)
return _groupMatrix(subgraph, group)
row_subsets = tuple(combinations(range(before_matrix.shape[0]), minor_size))
column_subsets = tuple(combinations(range(before_matrix.shape[1]), minor_size))
def unscaledResiduals(parameters: np.ndarray) -> np.ndarray:
"""Return maximal minors that vanish at the requested lower rank."""
residuals = []
for state in states:
matrix = localMatrix(parameters, state) / singular_values[0]
residuals.extend(
np.linalg.det(matrix[np.ix_(rows, columns)])
for rows in row_subsets
for columns in column_subsets
)
return np.asarray(residuals)
zero = np.zeros(2 * len(subhinges))
# Normalize at the original geometry so optimizer tolerances remain
# meaningful across mechanisms with different raw minor magnitudes.
initial_residuals = unscaledResiduals(zero)
residual_scale = 1.0 / max(float(np.max(np.abs(initial_residuals))), 1.0e-12)
solution = least_squares(
lambda parameters: residual_scale * unscaledResiduals(parameters),
zero,
bounds=(-maximum_axis_parameter, maximum_axis_parameter),
xtol=1.0e-13,
ftol=1.0e-13,
gtol=1.0e-13,
max_nfev=300,
)
setAxes(solution.x)
axis_changes = tuple(
(
subhinge.name(),
_axisAngle(original, np.asarray(subhinge.getUnitAxis(), dtype=float)),
)
for subhinge, original in zip(subhinges, originals, strict=True)
)
worst_after = 0.0
after_rank = 0
for state in states:
matrix = localMatrix(solution.x, state)
values = np.linalg.svd(matrix, compute_uv=False)
worst_after = max(worst_after, float(values[target_rank]))
after_rank = max(after_rank, _matrixRank(matrix))
maximum_change = max((change for _, change in axis_changes), default=0.0)
rank_accepted = (
worst_after <= exact_relative_tolerance * singular_values[0]
and maximum_change <= math.sqrt(2.0) * maximum_axis_parameter * 1.001
)
validation_spans: tuple[tuple[str, float, float, str], ...] = ()
rejection_reason = None
accepted = rank_accepted
if rank_accepted:
accepted, validation_spans, rejection_reason = _validateFiniteGeometryMobility(
subgraph,
group,
tuple(drivers),
target_rank,
exact_relative_tolerance=exact_relative_tolerance,
)
else:
rejection_reason = "bounded optimization did not preserve the target finite rank"
if not accepted:
for subhinge, original in zip(subhinges, originals, strict=True):
subhinge.setUnitAxis(original)
after_rank = before_rank
worst_after = float(singular_values[weak_indices[0]])
axis_changes = ()
subgraph.setQ(nominal)
return GeometryChange(
group.constraints,
accepted,
before_rank,
after_rank,
before_matrix.shape[1] - before_rank,
before_matrix.shape[1] - after_rank,
float(singular_values[weak_indices[0]]),
worst_after,
axis_changes,
validation_spans,
rejection_reason,
)
finally:
subgraph.setQ(nominal)
for subhinge in subhinges:
del subhinge
def _matrixRank(matrix: np.ndarray, tolerance: float = 1.0e-9) -> int:
"""Return numerical rank relative to the matrix's leading singular value."""
singular_values = np.linalg.svd(matrix, compute_uv=False)
if singular_values.size == 0 or singular_values[0] == 0.0:
return 0
return int(np.count_nonzero(singular_values > tolerance * singular_values[0]))
def _coordinateByName(subgraph: kd.SubGraph, name: str) -> kd.PhysicalSubhinge:
"""Return a unique scalar graph subhinge by name."""
# Search packed velocity columns because a provider may occupy several local
# offsets; offset zero identifies a named scalar coordinate once.
graph_data = subgraph.graphCoordData()
result = None
seen: set[int] = set()
try:
for column in range(graph_data.nU()):
coordinate, local_offset = graph_data.coordAt(column)
if coordinate.name() == name and local_offset == 0:
if coordinate.id() in seen:
del coordinate
continue
if result is not None:
del coordinate
del result
raise click.UsageError(f"subhinge name '{name}' is ambiguous")
result = coordinate
seen.add(coordinate.id())
else:
del coordinate
if result is None:
raise click.UsageError(f"subhinge '{name}' was not found")
if not isinstance(result, (kd.PinSubhinge, kd.LinearSubhinge)):
del result
raise click.UsageError(f"subhinge '{name}' is not a scalar pin or slider")
return result
finally:
del graph_data
def _mobilityStates(
subgraph: kd.SubGraph,
drivers: tuple[str, ...],
*,
increment: float = 0.02,
closure_tolerance: float = 1.0e-8,
) -> tuple[np.ndarray, ...]:
"""Generate nominal and nearby closed states for requested driver names."""
nominal = np.asarray(subgraph.getQ(), dtype=float).copy()
states = [nominal]
for driver_name in drivers:
driver = _coordinateByName(subgraph, driver_name)
solver = subgraph.cks()
# Each driver trial starts from and returns to the caller's partition.
frozen_coordinates = _captureFrozenCoordinates(subgraph, solver)
try:
try:
solver, q_index = _configureLoopPartition(
subgraph, driver, 0, rank_tolerance=1.0e-9, solver=solver
)
except LockedLoopError:
# A currently locked target has no nonlinear neighboring state.
continue
for direction in (-1.0, 1.0):
trial = nominal.copy()
trial[q_index] += direction * increment
subgraph.setQ(trial)
residual = float(solver.solveQ())
if residual <= closure_tolerance:
states.append(np.asarray(subgraph.getQ(), dtype=float).copy())
finally:
subgraph.setQ(nominal)
_restoreFrozenCoordinates(subgraph, solver, frozen_coordinates)
del solver
del driver
return tuple(states)
def _mobilityCandidates(
group: LoopGroup,
rows_by_name: dict[str, tuple[int, ...]],
) -> tuple[MobilityCandidate, ...]:
"""Create canonical local twist candidates on every closure in a group."""
# A fixed local twist basis makes the combinatorial search reproducible and
# keeps accepted candidates directly serializable as pin or linear hinges.
definitions = (
("Rx", "PIN", (1.0, 0.0, 0.0), 0),
("Ry", "PIN", (0.0, 1.0, 0.0), 1),
("Rz", "PIN", (0.0, 0.0, 1.0), 2),
("Tx", "LINEAR", (1.0, 0.0, 0.0), 3),
("Ty", "LINEAR", (0.0, 1.0, 0.0), 4),
("Tz", "LINEAR", (0.0, 0.0, 1.0), 5),
)
row_offsets = {row: index for index, row in enumerate(group.rows)}
candidates = []
for constraint_name in group.constraints:
constraint_rows = rows_by_name[constraint_name]
if len(constraint_rows) != 6:
raise click.ClickException(
f"constraint '{constraint_name}' does not own six error rows"
)
for component, subhinge_type, axis, local_row in definitions:
column = np.zeros(len(group.rows))
column[row_offsets[constraint_rows[local_row]]] = 1.0
candidates.append(
MobilityCandidate(
constraint_name,
component,
subhinge_type,
axis,
tuple(float(value) for value in column),
)
)
return tuple(candidates)
def _driverProjectionRank(
matrix: np.ndarray,
driver_indices: tuple[int, ...],
rank_tolerance: float = 1.0e-9,
) -> tuple[int, int, int]:
"""Return matrix rank, nullity, and driver projection rank."""
_, singular_values, right_transpose = np.linalg.svd(matrix, full_matrices=True)
if singular_values.size and singular_values[0] > 0.0:
rank = int(np.count_nonzero(singular_values > rank_tolerance * singular_values[0]))
else:
rank = 0
nullspace = right_transpose[rank:, :].T
driver_rank = (
_matrixRank(nullspace[np.asarray(driver_indices), :], rank_tolerance)
if driver_indices
else 0
)
return rank, matrix.shape[1] - rank, driver_rank
[docs]
def recommendMobilityAdditions(
subgraph: kd.SubGraph,
group: LoopGroup,
request: MobilityRequest,
states: tuple[np.ndarray, ...],
) -> MobilityChange:
"""Select the smallest well-conditioned coordinate set meeting a request.
Candidate combinations are evaluated against every retained closed state.
A valid result reaches the requested nullity, preserves independent driver
projections, and avoids weaker singular modes than same-size alternatives.
"""
if any(driver not in group.coordinate_names for driver in request.drivers):
outside = next(driver for driver in request.drivers if driver not in group.coordinate_names)
raise click.UsageError(
f"mobility driver '{outside}' is outside anchor '{request.anchor}' loop group"
)
driver_indices = tuple(group.coordinate_names.index(driver) for driver in request.drivers)
nominal_matrix = _groupMatrix(subgraph, group)
before_rank = _matrixRank(nominal_matrix)
before_nullity = nominal_matrix.shape[1] - before_rank
before_singular = np.linalg.svd(nominal_matrix, compute_uv=False)
before_smallest = float(before_singular[before_rank - 1]) if before_rank else 0.0
if before_nullity >= request.target_mobility:
return MobilityChange(
request.anchor,
group.constraints,
before_rank,
before_nullity,
request.target_mobility,
(),
before_rank,
before_nullity,
before_smallest,
before_smallest,
)
rows_by_name = _constraintRowsByName(subgraph)
candidates = _mobilityCandidates(group, rows_by_name)
if request.allowed_components:
# An explicit allow-list restricts the otherwise unrestricted search.
candidates = tuple(
candidate
for candidate in candidates
if candidate.component in request.allowed_components
)
# Build one Jacobian per finite witness and restore the caller's state before
# any combinatorial search or possible exception leaves this phase.
saved_q = np.asarray(subgraph.getQ(), dtype=float).copy()
matrices = []
try:
for state in states:
subgraph.setQ(state)
matrices.append(_groupMatrix(subgraph, group))
finally:
subgraph.setQ(saved_q)
minimum_count = request.target_mobility - before_nullity
best = None
# Search cardinality first: conditioning breaks ties only among minimal sets,
# never in favor of unnecessary added coordinates.
for count in range(minimum_count, min(len(candidates), minimum_count + 4) + 1):
for selection in combinations(range(len(candidates)), count):
# Avoid redundant same-component additions to one constraint.
selected = tuple(candidates[index] for index in selection)
valid = True
worst_condition = np.inf
predicted_rank = 0
predicted_nullity = 0
for matrix in matrices:
augmented = np.column_stack(
(matrix, *(np.asarray(item.column) for item in selected))
)
rank, nullity, driver_rank = _driverProjectionRank(augmented, driver_indices)
if nullity < request.target_mobility or driver_rank < len(driver_indices):
valid = False
break
singular_values = np.linalg.svd(augmented, compute_uv=False)
retained = float(singular_values[rank - 1]) if rank else 0.0
worst_condition = min(worst_condition, retained)
predicted_rank = rank
predicted_nullity = nullity
if valid and (best is None or worst_condition > best[0]):
best = (
worst_condition,
selected,
predicted_rank,
predicted_nullity,
)
if best is not None:
break
if best is None:
restriction = (
f" within allowed components {', '.join(request.allowed_components)}"
if request.allowed_components
else ""
)
raise click.ClickException(
f"no tested cut-joint additions{restriction} satisfy mobility request "
f"'{request.anchor}'"
)
condition, selected, predicted_rank, predicted_nullity = best
return MobilityChange(
request.anchor,
group.constraints,
before_rank,
before_nullity,
request.target_mobility,
selected,
predicted_rank,
predicted_nullity,
before_smallest,
condition,
)
def _applyMobilityChanges(
model: SubGraphDS,
changes: tuple[MobilityChange, ...],
) -> None:
"""Add selected canonical subhinges to serialized cut-joint hinges."""
# Regroup the analysis audit by serialized owner; one connected-loop change
# may contribute additions to several cut joints.
additions_by_constraint: dict[str, list[MobilityCandidate]] = {}
for change in changes:
for addition in change.additions:
additions_by_constraint.setdefault(addition.constraint, []).append(addition)
found: set[str] = set()
for constraint in model.loop_constraints:
additions = additions_by_constraint.get(constraint.name)
if not additions:
continue
# Preserve all existing subhinges before appending selected freedoms.
subhinges = [item.model_copy(deep=True) for item in constraint.hinge.subhinge_params]
for addition in additions:
axis = np.asarray(addition.axis, dtype=float)
if addition.subhinge_type == "PIN":
subhinges.append(kd.PinSubhingeParams(unit_axis=axis, prescribed=False))
else:
subhinges.append(kd.LinearSubhingeParams(unit_axis=axis, prescribed=False))
is_ujoint = len(subhinges) == 2 and all(
isinstance(item, kd.PinSubhingeParams) for item in subhinges
)
constraint.hinge = kd.PhysicalHingeParams(
hinge_type=kd.HingeType.UJOINT if is_ujoint else kd.HingeType.CUSTOM,
subhinge_params=subhinges,
)
found.add(constraint.name)
missing = set(additions_by_constraint) - found
if missing:
raise click.ClickException(
f"could not modify cut-joint constraints: {', '.join(sorted(missing))}"
)
def _printMobilityReport(changes: tuple[MobilityChange, ...]) -> None:
"""Print an aligned summary of requested mobility changes."""
if not changes:
return
click.echo("Mobility additions:")
anchor_width = max(len(change.anchor) for change in changes)
for change in changes:
if not change.additions:
click.echo(
f" {change.anchor:<{anchor_width}} OK "
f"nullity={change.before_nullity} target={change.target_mobility}"
)
continue
additions = ", ".join(f"{item.constraint}:{item.component}" for item in change.additions)
click.echo(
f" {change.anchor:<{anchor_width}} "
f"nullity {change.before_nullity}->{change.predicted_nullity} "
f"rank {change.before_rank}->{change.predicted_rank} "
f"sigma {change.before_smallest_singular:.6g}->"
f"{change.predicted_smallest_singular:.6g} add {additions}"
)
def _printGeometryReport(changes: tuple[GeometryChange, ...]) -> None:
"""Print weak-mode geometry reconciliation results in aligned columns."""
if not changes:
click.echo("Loop geometry optimization: no weak nonzero modes found.")
return
click.echo("Loop geometry optimization:")
label_width = max(len(" + ".join(change.constraints)) for change in changes)
for change in changes:
label = " + ".join(change.constraints)
status = "UPDATED" if change.accepted else "UNCHANGED"
click.echo(
f" {label:<{label_width}} {status:<9} "
f"rank {change.before_rank}->{change.after_rank} "
f"nullity {change.before_nullity}->{change.after_nullity} "
f"weak sigma {change.before_weak_singular:.6g}->"
f"{change.after_weak_singular:.6g}"
)
for coordinate, angle in change.axis_changes:
click.echo(f" axis {coordinate:<48} delta={angle:.6g} rad")
for coordinate, lower, upper, unit in change.validation_spans:
click.echo(f" finite {coordinate:<46} -{lower:.6g}/+{upper:.6g} {unit}")
if change.rejection_reason is not None:
click.echo(f" rejected: {change.rejection_reason}")
[docs]
@dataclass(frozen=True)
class SerializedSubhingeRef:
"""Locate one serialized subhinge and the object that owns its hinge."""
name: str
owner: Any
owner_attribute: str
subhinge_index: int
def _serializedSubgraph(specification: Any) -> SubGraphDS:
"""Return the mutable SubGraphDS nested in a supported root model."""
if isinstance(specification, SubGraphDS):
return specification
if isinstance(specification, BasicPrefabDS):
return specification.params.subtree
raise click.UsageError("loop hinge simplification requires a SubGraph or BasicPrefab model")
def _bodyBranches(subgraph: SubGraphDS):
"""Yield every serialized body branch without retaining native handles."""
pending = list(reversed(subgraph.base_bodies))
while pending:
branch = pending.pop()
yield branch
pending.extend(reversed(branch.children))
def _serializedSubhinges(subgraph: SubGraphDS) -> dict[str, SerializedSubhingeRef]:
"""Index serialized physical and cutjoint subhinges by runtime name."""
# Keep lightweight parameter references rather than native subhinges so edit
# operations remain independent of temporary live-model lifetimes.
result = {}
def addHinge(owner: Any, attribute: str, prefix: str) -> None:
"""Add every parameter in one serialized hinge to the name index."""
hinge = getattr(owner, attribute)
for index, params in enumerate(hinge.subhinge_params):
type_name = type(params).__name__.removesuffix("Params")
name = f"{prefix}-{type_name}{index}"
result[name] = SerializedSubhingeRef(name, owner, attribute, index)
for branch in _bodyBranches(subgraph):
addHinge(branch.body.params, "hinge_params", branch.body.name)
for constraint in subgraph.loop_constraints:
# Scalar ConVel and other imported loop constraints do not own a
# physical cut-joint hinge and therefore contribute no subhinges.
if hasattr(constraint, "hinge"):
addHinge(constraint, "hinge", f"{constraint.name}_pframe")
return result
[docs]
def synchronizeSerializedRangeLimits(
subgraph: kd.SubGraph,
model_target: Any,
) -> None:
"""Copy live Q-coordinate range limits into a retained BasicPrefab specification.
SubGraph outputs serialize their live object directly. BasicPrefab loading,
however, retains the input DataStruct so that its wrapper, assets, and model
metadata survive output. Range analysis changes the instantiated
Multibody, so this synchronization step transfers each coordinate result back
to the matching serialized subhinge immediately before ``--to-model``.
"""
if not isinstance(model_target, BasicPrefabDS):
return
serialized = _serializedSubhinges(_serializedSubgraph(model_target))
graph_data = subgraph.graphCoordData()
visited: set[int] = set()
try:
for column in range(graph_data.nU()):
coordinate, _ = graph_data.coordAt(column)
try:
if coordinate.id() in visited:
continue
visited.add(coordinate.id())
if not isinstance(coordinate, kd.PhysicalSubhinge):
continue
reference = serialized.get(coordinate.name())
if reference is None:
continue
hinge = getattr(reference.owner, reference.owner_attribute)
parameters = hinge.subhinge_params[reference.subhinge_index]
# Copy the array so the serialized specification owns its data.
if 0:
parameters.range_limits = np.asarray(
coordinate.getRangeLimits(), dtype=float
).copy()
parameters.range_limits = _getVec(coordinate.getRangeLimits()).copy()
finally:
del coordinate
finally:
del graph_data
# Runtime prefab bindings are process-local IDs and must never leak into a
# portable model file. Some source prefabs resolve stale context IDs while
# loading, so clear both fields explicitly at serialization time.
model_target.context.parent_body_raw = None
model_target.context.model_manager_raw = None
def _replacementHinge(subhinges: list[Any]) -> kd.PhysicalHingeParams:
"""Choose the standard hinge type matching retained scalar subhinges."""
if len(subhinges) == 1 and isinstance(subhinges[0], kd.PinSubhingeParams):
hinge_type = kd.HingeType.REVOLUTE
elif len(subhinges) == 1 and isinstance(subhinges[0], kd.LinearSubhingeParams):
hinge_type = kd.HingeType.SLIDER
elif len(subhinges) == 2 and all(isinstance(item, kd.PinSubhingeParams) for item in subhinges):
hinge_type = kd.HingeType.UJOINT
elif len(subhinges) == 3 and all(isinstance(item, kd.PinSubhingeParams) for item in subhinges):
hinge_type = kd.HingeType.GIMBAL
else:
hinge_type = kd.HingeType.CUSTOM
return kd.PhysicalHingeParams(
hinge_type=hinge_type,
subhinge_params=subhinges,
)
def _validatedSelection(values: tuple[str, ...], option: str) -> set[str] | None:
"""Validate a repeatable name-or-all simplification selection."""
if not values:
return set()
if "all" in values:
if len(values) != 1:
raise click.UsageError(f"{option} all cannot be combined with names")
return None
if len(set(values)) != len(values):
raise click.UsageError(f"{option} subhinge names must be unique")
return set(values)
def _readSimplifiableSpecification(model_file: Path) -> Any:
"""Read a mutable SubGraph or BasicPrefab specification."""
try:
return BasicPrefabDS.fromFile(model_file, get_meta=True)
except Exception:
# Clear native identity maps before trying the other supported schema.
SubGraphDS.resetAllObjectsFromIds()
try:
return SubGraphDS.fromFile(model_file, get_meta=True)
except Exception as error:
raise click.UsageError(
"loop hinge simplification requires a SubGraph or BasicPrefab model"
) from error
def _analyzeSimplificationSpecification(
specification: Any,
*,
inert_tolerance: float = 2.0e-7,
status_output: list[LoopMobilityStatus] | None = None,
) -> tuple[list[CoordinateMobilityEvidence], list[BallReductionHint]]:
"""Collect complete inertness evidence and local BALL hints.
Serialized parameters cannot answer mobility questions by themselves, so a
temporary live multibody is constructed. Analysis is explicitly
non-mutating (``apply_limits=False``), and both native containers are
discarded before serialized editing begins, even if analysis fails.
"""
# Build analysis-only native containers from the current serialized edits;
# this avoids mixing evidence from an earlier live-model revision.
frames = kf.FrameContainer.create("kdpy_simplification_analysis_frames")
multibody = _serializedSubgraph(specification).toMultibody(frames)
evidence: list[CoordinateMobilityEvidence] = []
hints: list[BallReductionHint] = []
try:
multibody.ensureHealthy()
multibody.resetData()
_setLoopRangeLimits(
multibody,
selected_names=("all",),
evidence_output=evidence,
ball_hint_output=hints,
status_output=status_output,
inert_tolerance=inert_tolerance,
apply_limits=False,
)
return evidence, hints
finally:
kc.discard(multibody)
kc.discard(frames)
def _removeInertSubhinges(
specification: Any,
requested: tuple[str, ...],
) -> tuple[str, ...]:
"""Remove only requested scalar coordinates proven globally inert."""
# Resolve selection independently from evidence so explicit names receive a
# clear error instead of being silently ignored when proof is insufficient.
selection = _validatedSelection(requested, "loops --remove-inert")
if selection == set():
return ()
statuses: list[LoopMobilityStatus] = []
evidence, _ = _analyzeSimplificationSpecification(
specification,
status_output=statuses,
)
inert = {item.coordinate for item in evidence if item.classification == "INERT"}
references = _serializedSubhinges(_serializedSubgraph(specification))
scalar_inert = set()
for name in inert:
reference = references.get(name)
if reference is None:
continue
hinge = getattr(reference.owner, reference.owner_attribute)
params = hinge.subhinge_params[reference.subhinge_index]
if isinstance(params, (kd.PinSubhingeParams, kd.LinearSubhingeParams)):
scalar_inert.add(name)
targets = scalar_inert if selection is None else selection
missing = targets - scalar_inert
if missing:
status_by_name = {status.coordinate: status for status in statuses}
for name in sorted(missing):
status = status_by_name.get(name)
if status is None:
raise click.UsageError(
f"loops --remove-inert coordinate is not an active loop subhinge: {name}"
)
click.echo(
f"Rejected removal of {name}: measured range "
f"{status.feasible_range}; inertness threshold is "
f"{INERT_SUBHINGE_TOLERANCE:.1e} rad."
)
targets -= missing
# Apply edits only after every requested target has complete scalar evidence;
# validation failure therefore leaves the specification untouched.
changed = []
for name in sorted(targets):
reference = references.get(name)
if reference is None:
raise click.UsageError(f"cannot serialize inert subhinge '{name}'")
hinge = getattr(reference.owner, reference.owner_attribute)
params = hinge.subhinge_params[reference.subhinge_index]
if not isinstance(params, (kd.PinSubhingeParams, kd.LinearSubhingeParams)):
raise click.UsageError(f"'{name}' is not a removable scalar pin or linear subhinge")
retained = [
item.model_copy(deep=True)
for index, item in enumerate(hinge.subhinge_params)
if index != reference.subhinge_index
]
if not retained:
raise click.UsageError(
f"cannot remove sole subhinge '{name}'; replace its hinge with LOCKED"
)
setattr(reference.owner, reference.owner_attribute, _replacementHinge(retained))
changed.append(name)
return tuple(changed)
def _reduceBallSubhinges(
specification: Any,
requested: tuple[str, ...],
) -> tuple[str, ...]:
"""Replace selected reducible BALL coordinates through gimbal validation."""
selection = _validatedSelection(requested, "loops --reduce-ball")
if selection == set():
return ()
# Local SVD axes propose a fixed-axis chart but do not authorize deletion;
# the second full scan below supplies range-wide inertness evidence.
_, hints = _analyzeSimplificationSpecification(specification)
hints_by_name = {hint.coordinate: hint for hint in hints}
targets = set(hints_by_name) if selection is None else selection
missing = targets - set(hints_by_name)
if missing:
raise click.UsageError(
f"loops --reduce-ball lacks a reduction hint for: {', '.join(sorted(missing))}"
)
references = _serializedSubhinges(_serializedSubgraph(specification))
# Retain deep copies so candidates that fail global validation can restore
# their original axis parameterization exactly.
original_hinges = {}
for name in sorted(targets):
reference = references.get(name)
if reference is None:
raise click.UsageError(f"cannot serialize BALL subhinge '{name}'")
hinge = getattr(reference.owner, reference.owner_attribute)
if hinge.hinge_type != kd.HingeType.BALL:
raise click.UsageError(f"'{name}' does not belong to a BALL hinge")
original_hinges[name] = hinge.model_copy(deep=True)
axes = hints_by_name[name].axes
pins = [kd.PinSubhingeParams(unit_axis=np.asarray(axis), prescribed=False) for axis in axes]
setattr(
reference.owner,
reference.owner_attribute,
kd.PhysicalHingeParams(
hinge_type=kd.HingeType.GIMBAL,
subhinge_params=pins,
),
)
# Only complete range-wide evidence may remove the aligned gimbal pins.
# Rebuild and rescan provisional gimbals. Only pins proven inert over the
# connected range may be removed from the serialized hinge.
gimbal_evidence, _ = _analyzeSimplificationSpecification(specification)
inert_names = {item.coordinate for item in gimbal_evidence if item.classification == "INERT"}
references = _serializedSubhinges(_serializedSubgraph(specification))
changed = []
for name in sorted(targets):
reference = references[name.replace("SphericalSubhinge0", "PinSubhinge0")]
hinge = getattr(reference.owner, reference.owner_attribute)
prefix = name.removesuffix("SphericalSubhinge0")
retained = [
item.model_copy(deep=True)
for index, item in enumerate(hinge.subhinge_params)
if f"{prefix}PinSubhinge{index}" not in inert_names
]
if len(retained) not in (1, 2):
# Restore the BALL when fixed-axis evidence is incomplete or empty.
setattr(reference.owner, reference.owner_attribute, original_hinges[name])
continue
setattr(reference.owner, reference.owner_attribute, _replacementHinge(retained))
changed.append(name)
return tuple(changed)
def _reduceSphericalSubhinges(
specification: Any,
requested: tuple[str, ...],
) -> tuple[str, ...]:
"""Reduce selected spherical subhinges while preserving their parent hinges.
Parameters
----------
specification: Any
Mutable serialized SubGraph or BasicPrefab model.
requested: tuple[str, ...]
Spherical subhinge names, or ``all``.
Returns
-------
tuple[str, ...]
Names of spherical subhinges whose parent hinge was reduced.
"""
selection = _validatedSelection(requested, "loops --reduce-spherical")
if selection == set():
return ()
_, hints = _analyzeSimplificationSpecification(specification)
hints_by_name = {hint.coordinate: hint for hint in hints}
targets = set(hints_by_name) if selection is None else selection
missing = targets - set(hints_by_name)
if missing:
raise click.UsageError(
f"loops --reduce-spherical lacks a reduction hint for: {', '.join(sorted(missing))}"
)
references = _serializedSubhinges(_serializedSubgraph(specification))
original_hinges = {}
original_references = {}
original_indices: dict[str, int] = {}
for name in sorted(targets, key=lambda item: references[item].subhinge_index, reverse=True):
click.echo(f"Reducing spherical subhinge: {name}")
reference = references.get(name)
if reference is None:
raise click.UsageError(f"cannot serialize spherical subhinge '{name}'")
hinge = getattr(reference.owner, reference.owner_attribute)
original_hinges[name] = hinge.model_copy(deep=True)
original_references[name] = reference
spherical_index = reference.subhinge_index
original_indices[name] = spherical_index
axes = hints_by_name[name].axes
pins = [kd.PinSubhingeParams(unit_axis=np.asarray(axis), prescribed=False) for axis in axes]
retained = list(hinge.subhinge_params)
retained[spherical_index : spherical_index + 1] = pins
setattr(reference.owner, reference.owner_attribute, _replacementHinge(retained))
# Expanding an earlier spherical block shifts later blocks in the same
# parent hinge. Compute the provisional indices after all expansions so
# ``--reduce-spherical all`` remains correct for composite hinges.
provisional_pin_indices = {
name: tuple(
range(
index
+ 2
* sum(
other_index < index
and original_references[other].owner is reference.owner
and original_references[other].owner_attribute == reference.owner_attribute
for other, other_index in original_indices.items()
),
index
+ 2
* sum(
other_index < index
and original_references[other].owner is reference.owner
and original_references[other].owner_attribute == reference.owner_attribute
for other, other_index in original_indices.items()
)
+ 3,
)
)
for name, index in original_indices.items()
for reference in (original_references[name],)
}
# Explicit spherical reduction accepts the small numerical residuals that
# arise when a loop solver holds a nominally fixed axis at a tight but
# nonzero range. Strict scalar removal keeps its original tolerance.
gimbal_evidence, _ = _analyzeSimplificationSpecification(
specification,
inert_tolerance=1.0e-4,
)
inert_names = {item.coordinate for item in gimbal_evidence if item.classification == "INERT"}
changed = []
for name in sorted(targets):
reference = original_references[name]
original = original_hinges[name]
hinge = getattr(reference.owner, reference.owner_attribute)
prefix = name.rsplit("-SphericalSubhinge", 1)[0]
pin_indices = provisional_pin_indices[name]
retained = [
item.model_copy(deep=True)
for index, item in enumerate(hinge.subhinge_params)
if index not in pin_indices or f"{prefix}-PinSubhinge{index}" not in inert_names
]
if len(retained) == len(hinge.subhinge_params):
setattr(reference.owner, reference.owner_attribute, original)
click.echo(
f"Spherical subhinge unchanged: {name} "
"(no globally inert fixed-axis pin was proven)."
)
continue
setattr(reference.owner, reference.owner_attribute, _replacementHinge(retained))
changed.append(name)
click.echo(
f"Spherical subhinge changed: {name}; retained {len(retained)} parent-hinge subhinges."
)
return tuple(changed)
[docs]
def loadSimplifiedModel(
model_file: Path,
remove_inert: tuple[str, ...],
reduce_ball: tuple[str, ...],
reduce_spherical: tuple[str, ...],
) -> tuple[LoadedModel, tuple[str, ...], tuple[str, ...]]:
"""Apply validated hinge reductions and load the rebuilt root model."""
# Keep edits serialized until validation completes, then cross a temporary
# file boundary to rebuild clean runtime identity maps.
specification, meta = _readSimplifiableSpecification(model_file)
temporary_path = None
output_specification = None
try:
# Removal precedes hinge reductions so each later scan sees the topology
# that will actually be loaded and written.
removed = _removeInertSubhinges(specification, remove_inert)
reduced = _reduceBallSubhinges(specification, reduce_ball)
reduced += _reduceSphericalSubhinges(specification, reduce_spherical)
# A final full scan rejects topology changes that destroy continuation.
_analyzeSimplificationSpecification(specification)
with tempfile.NamedTemporaryFile(suffix=".yaml", delete=False) as temporary:
temporary_path = Path(temporary.name)
specification.toFile(temporary_path, meta=meta)
# Keep an untouched serialization target. Model loading binds and
# mutates its own BasicPrefabDS copy with runtime node references.
output_specification = specification.model_copy(deep=True)
finally:
del specification
SubGraphDS.resetAllObjectsFromIds()
try:
loaded = loadModel(temporary_path)
finally:
temporary_path.unlink(missing_ok=True)
return (
LoadedModel(
loaded.sim,
loaded.model_type,
loaded.state_target,
output_specification,
),
removed,
reduced,
)
def _printSimplificationReport(
removed: tuple[str, ...],
reduced: tuple[str, ...],
remove_requested: bool,
reduce_requested: bool,
reduce_spherical_requested: bool,
) -> None:
"""Report every range-validated hinge simplification."""
if removed:
click.echo("Removed globally inert subhinges:")
for name in removed:
click.echo(f" {name}")
if reduced:
click.echo("Reduced spherical subhinges:")
for name in reduced:
click.echo(f" {name}")
if remove_requested and not removed:
click.echo("No requested scalar subhinges had complete INERT evidence.")
if reduce_requested and not reduced:
click.echo("No requested BALL passed the complete fixed-axis reduction test.")
if reduce_spherical_requested and not reduced:
click.echo("No requested spherical subhinge passed the complete fixed-axis reduction test.")
[docs]
@dataclass(frozen=True)
class FeasibleRangeLimit:
"""Record one connected feasible interval and its evidence quality.
Limit flags distinguish a closure boundary from a configured search bound.
``worst_feasible_residual`` records the largest accepted pose error, while
``range_limits_supported`` states whether native limits can store the result.
"""
coordinate: str
current: float
lower: float
upper: float
lower_limited: bool
upper_limited: bool
worst_feasible_residual: float
old_limits: tuple[float, float] | None = None
dof_index: int = 0
range_limits_supported: bool = True
[docs]
@dataclass(frozen=True)
class CoordinateMobilityEvidence:
"""Summarize range-wide evidence for one candidate inert DOF.
Global inertness requires complete connected-range coverage and acceptable
participation and displacement at every retained closed state.
"""
coordinate: str
dof_index: int
classification: str
sampled_states: int
max_participation: float
max_displacement: float
complete_connected_range: bool
[docs]
@dataclass(frozen=True)
class BallReductionHint:
"""Describe a BALL coordinate's local active rotational subspace.
The axes are candidates for fixed-axis gimbal validation, not proof that a
rotational component can be removed.
"""
coordinate: str
participation_rank: int
coordinate_dofs: int
axes: tuple[tuple[float, float, float], ...]
[docs]
@dataclass(frozen=True)
class LoopMobilityStatus:
"""Provide one presentation-ready loop-coordinate classification record."""
coordinate: str
coordinate_type: str
dof_index: int
mobility: str
feasible_range: tuple[float, float] | None
details: str
[docs]
class LockedLoopError(ValueError):
"""Indicate that a loop coordinate cannot be retained as independent."""
[docs]
class ZeroLocalMobilityError(LockedLoopError):
"""Indicate that a coordinate is zero in every local nullspace vector."""
[docs]
class IndirectCoordinateChartError(LockedLoopError):
"""Indicate that scalar Q continuation cannot represent one velocity DOF."""
[docs]
class FrozenLoopCoordinateError(ValueError):
"""Indicate that a loop coordinate was frozen before range exploration."""
[docs]
class RangeLimitComputationError(RuntimeError):
"""Indicate that numerical closure did not establish a range limit."""
def _subhingeLabel(
subhinge: kd.PhysicalSubhinge,
dof_index: int | None = None,
) -> str:
"""Combine a physical subhinge's name and concrete type for reports.
Parameters
----------
subhinge: kd.PhysicalSubhinge
Physical coordinate provider to label.
dof_index: int | None
Optional local offset appended for multi-DOF providers.
Returns
-------
str
Human-readable coordinate/type label.
"""
# The pybind concrete class names distinguish pin and linear coordinates.
label = f"{subhinge.name()}/{type(subhinge).__name__}"
if dof_index is not None and subhinge.nU() > 1:
label += f"[{dof_index}]"
return label
def _finishRangeLimitProgress(measured: FeasibleRangeLimit) -> None:
"""Complete one pending range-limit progress line with its result.
Parameters
----------
measured: FeasibleRangeLimit
Completed range measurement to describe.
"""
if measured.old_limits is not None and np.allclose(
(measured.lower, measured.upper),
measured.old_limits,
rtol=1.0e-10,
atol=1.0e-12,
):
# Matching limits need only confirmation, while retaining their values.
old_lower, old_upper = measured.old_limits
click.echo(
f": OK; original limits "
f"[{_formatRangeEndpoint(old_lower)}, {_formatRangeEndpoint(old_upper)}]"
)
return
# ``hard`` marks a detected feasibility boundary; ``open`` marks only the
# configured exploration boundary and permits travel beyond the old value.
lower_kind = "hard" if measured.lower_limited else "open"
upper_kind = "hard" if measured.upper_limited else "open"
old_text = ""
if measured.old_limits is not None:
old_lower, old_upper = measured.old_limits
old_text = (
f"; old limits [{_formatRangeEndpoint(old_lower)}, {_formatRangeEndpoint(old_upper)}]"
)
result_kind = "new range" if measured.range_limits_supported else "range"
click.echo(
f": {result_kind} "
f"[{_formatRangeEndpoint(measured.lower)}, "
f"{_formatRangeEndpoint(measured.upper)}]"
f"{old_text} ({lower_kind}, {upper_kind})"
)
def _constraintRank(matrix: np.ndarray, relative_tolerance: float) -> int:
"""Return a scale-aware numerical rank for a constraint matrix.
Parameters
----------
matrix: np.ndarray
Matrix whose singular values are inspected.
relative_tolerance: float
Fraction of the largest singular value considered numerically
nonzero.
Returns
-------
int
Numerical matrix rank.
"""
singular_values = np.linalg.svd(matrix, compute_uv=False)
if singular_values.size == 0 or singular_values[0] == 0.0:
return 0
return int(np.count_nonzero(singular_values > relative_tolerance * singular_values[0]))
def _coordinateColumn(
coord_data: kd.CoordData,
subhinge: kd.PhysicalSubhinge,
dof_index: int,
) -> tuple[int, int]:
"""Return the packed velocity column for one local subhinge DOF.
Parameters
----------
coord_data: kd.CoordData
Graph coordinate mapping to search.
subhinge: kd.PhysicalSubhinge
Coordinate provider whose packed column is required.
dof_index: int
Local DOF offset within ``subhinge``.
Returns
-------
tuple[int, int]
Packed U column and confirmed local offset.
"""
for column in range(coord_data.nU()):
coordinate, local_offset = coord_data.coordAt(column)
try:
if coordinate.id() == subhinge.id() and local_offset == dof_index:
return column, int(local_offset)
finally:
# Release temporary native handles as soon as they are inspected.
del coordinate
raise ValueError(f"subhinge {subhinge.name()!r} is not in this SubGraph")
def _configureLoopPartition(
subgraph: kd.SubGraph,
subhinge: kd.PhysicalSubhinge,
dof_index: int,
rank_tolerance: float,
solver: kd.ConstraintKinematicsSolver | None = None,
) -> tuple[kd.ConstraintKinematicsSolver, int]:
"""Freeze an independent full-graph partition containing the target.
Parameters
----------
subgraph: kd.SubGraph
Graph whose constraint partition is configured.
subhinge: kd.PhysicalSubhinge
Coordinate provider retained as an independent target.
dof_index: int
Local target offset within ``subhinge``.
rank_tolerance: float
Relative threshold used for QR rank selection.
solver: kd.ConstraintKinematicsSolver | None
Optional existing CK solver to reconfigure.
Returns
-------
tuple[kd.ConstraintKinematicsSolver, int]
Configured CK solver and packed target Q index.
"""
graph_data = subgraph.graphCoordData()
try:
if not 0 <= dof_index < subhinge.nU():
raise IndexError(f"DOF index {dof_index} is outside {subhinge.name()!r}")
target_column, local_offset = _coordinateColumn(graph_data, subhinge, dof_index)
if subhinge.nQ() != subhinge.nU():
raise IndirectCoordinateChartError(
f"{subhinge.name()!r}[{dof_index}] does not have a direct position-coordinate chart"
)
# Full constraint rows and graph columns include cut-joint coordinates.
matrix = np.asarray(
kd.Algorithms.evalVelocityConstraintMatrix(subgraph, True),
dtype=float,
)
pool = np.asarray(
[index for index in range(graph_data.nU()) if index != target_column],
dtype=int,
)
_, triangular, pivots = linalg.qr(matrix[:, pool], mode="economic", pivoting=True)
diagonal = np.abs(np.diag(triangular))
scale = diagonal[0] if diagonal.size else 0.0
rank = int(np.count_nonzero(diagonal > rank_tolerance * scale) if scale > 0.0 else 0)
if rank < _constraintRank(matrix, rank_tolerance):
raise ZeroLocalMobilityError(
f"{subhinge.name()!r} cannot be retained as an independent "
"coordinate with the current constraint rank"
)
# QR selects the dependent columns; every other coordinate is prescribed.
dependent = set(pool[pivots[:rank]].tolist())
if solver is None:
solver = subgraph.cks()
solver.clearFrozenCoords()
for index in range(graph_data.nU()):
if index not in dependent:
coordinate, offset = graph_data.coordAt(index)
try:
# Package the provider and local offset as one Coordinate.
solver.freezeCoord((coordinate, offset))
finally:
del coordinate
offsets = graph_data.coordOffsets(subhinge)
return solver, int(offsets.Q + local_offset)
finally:
del graph_data
def _captureFrozenCoordinates(
subgraph: kd.SubGraph,
solver: kd.ConstraintKinematicsSolver,
) -> tuple[tuple[int, int, kd.CKFrozenCoordType], ...]:
"""Capture Q/U solver freezes as subhinge IDs and local indices.
Parameters
----------
subgraph: kd.SubGraph
Graph that owns the coordinates used by ``solver``.
solver: kd.ConstraintKinematicsSolver
Constraint-kinematics solver whose partition is captured.
Returns
-------
tuple[tuple[int, int, kd.CKFrozenCoordType], ...]
Stable coordinate-ID, local-offset, and freeze-type records suitable
for restoring the partition after a temporary range scan.
"""
# Native inventories use packed Q/U indices. Convert them to provider IDs and
# local offsets so later repartitioning cannot invalidate the snapshot.
frozen_q = {int(index) for index in solver.frozenCoords(True)}
frozen_u = {int(index) for index in solver.frozenCoords(False)}
graph_data = subgraph.graphCoordData()
frozen_types: dict[tuple[int, int], set[str]] = {}
seen_ids: set[int] = set()
try:
for column in range(graph_data.nU()):
coordinate, local_u = graph_data.coordAt(column)
try:
coordinate_id = coordinate.id()
if column in frozen_u:
frozen_types.setdefault((coordinate_id, local_u), set()).add("U")
if coordinate_id in seen_ids:
continue
seen_ids.add(coordinate_id)
q_offset = int(graph_data.coordOffsets(coordinate).Q)
for local_q in range(coordinate.nQ()):
if q_offset + local_q in frozen_q:
frozen_types.setdefault((coordinate_id, local_q), set()).add("Q")
finally:
del coordinate
finally:
del graph_data
# Collapse separate Q and U observations into the combined enum expected by
# freezeCoord during restoration.
result = []
for (coordinate_id, local_index), kinds in frozen_types.items():
if kinds == {"Q", "U"}:
frozen_type = kd.CKFrozenCoordType.Q_U
elif kinds == {"Q"}:
frozen_type = kd.CKFrozenCoordType.Q
else:
frozen_type = kd.CKFrozenCoordType.U
result.append((coordinate_id, local_index, frozen_type))
return tuple(result)
def _restoreFrozenCoordinates(
subgraph: kd.SubGraph,
solver: kd.ConstraintKinematicsSolver,
frozen_coordinates: tuple[tuple[int, int, kd.CKFrozenCoordType], ...],
) -> None:
"""Replace the temporary solver partition with an exact saved snapshot.
Parameters
----------
subgraph: kd.SubGraph
Graph used to resolve saved coordinate IDs.
solver: kd.ConstraintKinematicsSolver
Constraint-kinematics solver whose freezes are replaced.
frozen_coordinates: tuple[tuple[int, int, kd.CKFrozenCoordType], ...]
Records returned by :func:`_captureFrozenCoordinates` before
temporary changes.
"""
# Range continuation intentionally replaces the whole partition. Clearing
# that temporary state first avoids leaving a target freeze behind when Q
# and U partitions differ, then each caller-owned freeze is restored once.
solver.clearFrozenCoords()
# Store unresolved records separately from graph traversal. Saved records
# use stable native object IDs because packed Q/U indices can change when
# the solver rebuilds its dependent-coordinate partition.
pending = list(frozen_coordinates)
graph_data = subgraph.graphCoordData()
visited_ids: set[int] = set()
try:
# A physical coordinate may occupy several packed columns. Visit each
# provider once, then restore every saved local component belonging to
# that provider with its original Q, U, or combined freeze type.
for column in range(graph_data.nU()):
coordinate, _ = graph_data.coordAt(column)
try:
coordinate_id = coordinate.id()
if coordinate_id in visited_ids:
continue
visited_ids.add(coordinate_id)
matching = [item for item in pending if item[0] == coordinate_id]
for record in matching:
_, local_index, frozen_type = record
solver.freezeCoord((coordinate, local_index), frozen_type)
pending.remove(record)
finally:
del coordinate
finally:
del graph_data
# A remaining record means the graph changed while the scan was active or
# the snapshot referred to an object outside this SubGraph. Silently
# dropping it would leave the caller with a different solver partition.
if pending:
raise RuntimeError("could not resolve saved frozen coordinates")
def _targetWasFrozen(
subgraph: kd.SubGraph,
solver: kd.ConstraintKinematicsSolver,
subhinge: kd.PhysicalSubhinge,
dof_index: int,
) -> bool:
"""Return whether the target DOF has a pre-existing Q or U freeze.
Parameters
----------
subgraph: kd.SubGraph
Graph containing the target coordinate.
solver: kd.ConstraintKinematicsSolver
Solver whose current frozen-coordinate partition is inspected.
subhinge: kd.PhysicalSubhinge
Physical subhinge that provides the target coordinate.
dof_index: int
Local coordinate offset within ``subhinge``.
Returns
-------
bool
``True`` when either the target Q or U component is already frozen.
"""
# The CK solver reports packed indices, whereas this API receives a
# subhinge-local offset. Resolve both representations before inspecting the
# caller's partition so a Q-only or U-only freeze is treated consistently.
graph_data = subgraph.graphCoordData()
try:
u_index, _ = _coordinateColumn(graph_data, subhinge, dof_index)
q_index = int(graph_data.coordOffsets(subhinge).Q + dof_index)
finally:
del graph_data
return q_index in solver.frozenCoords(True) or u_index in solver.frozenCoords(False)
def _refineLoopBoundary(
subgraph: kd.SubGraph,
solver: kd.ConstraintKinematicsSolver,
q_index: int,
current: float,
direction: float,
feasible_distance: float,
infeasible_distance: float,
feasible_state: np.ndarray,
closure_tolerance: float,
boundary_tolerance: float,
sample_callback: Callable[[np.ndarray], None] | None = None,
) -> tuple[float, float]:
"""Bisect a feasibility transition while staying on its feasible side.
Parameters
----------
subgraph: kd.SubGraph
Graph whose Q state is varied during refinement.
solver: kd.ConstraintKinematicsSolver
CK solver used to restore closure after prescribing target Q.
q_index: int
Packed Q index of the frozen target coordinate.
current: float
Target-coordinate value at the nominal closed state.
direction: float
``-1`` for the lower endpoint or ``+1`` for the upper.
feasible_distance: float
Known feasible distance from ``current``.
infeasible_distance: float
Known infeasible distance from ``current``.
feasible_state: np.ndarray
Full closed Q state at ``feasible_distance``.
closure_tolerance: float
Largest ``solveQ()`` error accepted as closed.
boundary_tolerance: float
Desired width of the final distance bracket.
sample_callback: Callable[[np.ndarray], None] | None
Optional consumer for each accepted closed Q state.
Returns
-------
tuple[float, float]
The last demonstrated feasible distance and the largest accepted CK
error encountered while refining the boundary.
"""
# Distances are measured from the original coordinate value. ``lower`` is
# known to close and ``upper`` is known to exceed the closure tolerance;
# shrinking this bracket avoids mistaking the failed trial itself for the
# last usable coordinate value.
lower = feasible_distance
upper = infeasible_distance
state = feasible_state.copy()
worst_residual = 0.0
while upper - lower > boundary_tolerance:
# Always seed the midpoint from the most recent feasible configuration.
# A failed solve may alter dependent Q values, so using its resulting
# state would make later trials drift away from the connected branch.
midpoint = 0.5 * (lower + upper)
trial = state.copy()
trial[q_index] = current + direction * midpoint
subgraph.setQ(trial)
residual = float(solver.solveQ())
if residual <= closure_tolerance:
# The solver found a closed configuration at the midpoint. Advance
# the feasible side and retain the complete solved state as the
# seed for the next, more distant midpoint.
lower = midpoint
state = np.asarray(subgraph.getQ(), dtype=float).copy()
# Retain every accepted continuation state for range-wide analysis.
if sample_callback is not None:
sample_callback(state)
worst_residual = max(worst_residual, residual)
else:
# Pull the infeasible side inward, then explicitly reinstall the
# last accepted state because solveQ() is allowed to modify Q even
# when its final residual remains above tolerance.
upper = midpoint
subgraph.setQ(state)
# Returning the feasible side ensures the stored endpoint represents a
# configuration that was actually demonstrated to satisfy closure.
return lower, worst_residual
def _searchLoopDirection(
subgraph: kd.SubGraph,
solver: kd.ConstraintKinematicsSolver,
nominal: np.ndarray,
q_index: int,
current: float,
direction: float,
initial_step: float,
maximum_step: float,
maximum_distance: float,
closure_tolerance: float,
boundary_tolerance: float,
sample_callback: Callable[[np.ndarray], None] | None = None,
) -> tuple[float, bool, float]:
"""Continue from the nominal state to one local interval endpoint.
Parameters
----------
subgraph: kd.SubGraph
Graph whose target Q component is prescribed.
solver: kd.ConstraintKinematicsSolver
CK solver used to close each trial configuration.
nominal: np.ndarray
Full closed Q state from which this direction starts.
q_index: int
Packed Q index of the frozen target coordinate.
current: float
Target-coordinate value in ``nominal``.
direction: float
``-1`` for decreasing Q or ``+1`` for increasing Q.
initial_step: float
First distance increment away from ``current``.
maximum_step: float
Largest continuation increment permitted.
maximum_distance: float
Search horizon measured from ``current``.
closure_tolerance: float
Largest ``solveQ()`` error accepted as closed.
boundary_tolerance: float
Resolution used when refining a failed trial.
sample_callback: Callable[[np.ndarray], None] | None
Optional consumer for each accepted closed Q state.
Returns
-------
tuple[float, bool, float]
The explored distance, whether it ended at a refined hard boundary,
and the largest accepted CK error observed in this direction.
"""
# Each direction starts independently from the nominal closed pose. This
# prevents the negative scan's dependent-coordinate solution from choosing
# the assembly branch used by the positive scan.
subgraph.setQ(nominal)
distance = 0.0
step = initial_step
feasible_state = nominal.copy()
worst_residual = 0.0
while distance < maximum_distance:
# Propose a farther target coordinate while retaining all other Q values
# from the last closed solution. The frozen target survives solveQ();
# the CK solver changes only coordinates selected to restore closure.
candidate_distance = min(distance + step, maximum_distance)
trial = feasible_state.copy()
trial[q_index] = current + direction * candidate_distance
subgraph.setQ(trial)
residual = float(solver.solveQ())
feasible = residual <= closure_tolerance
if not feasible:
# The previous state and this failed target bracket the first loss
# of closure on the connected branch. Refine that bracket rather
# than reporting a step-size-dependent endpoint.
boundary, refined_residual = _refineLoopBoundary(
subgraph,
solver,
q_index,
current,
direction,
distance,
candidate_distance,
feasible_state,
closure_tolerance,
boundary_tolerance,
sample_callback,
)
return boundary, True, max(worst_residual, refined_residual)
# Promote the complete CK solution—not merely the prescribed target
# value—to the next seed so dependent coordinates follow continuously.
distance = candidate_distance
feasible_state = np.asarray(subgraph.getQ(), dtype=float).copy()
# Preserve the closed state before the next nonlinear trial changes it.
if sample_callback is not None:
sample_callback(feasible_state)
worst_residual = max(worst_residual, residual)
# Grow cautiously to avoid jumping to another nonlinear solution branch.
step = min(1.35 * step, maximum_step)
# No loss of closure was observed within the configured exploration
# distance. ``False`` tells the storage layer to encode this side as open.
return maximum_distance, False, worst_residual
def _projectionComponent(
subgraph: kd.SubGraph,
target_column: int,
activity_tolerance: float = 1.0e-10,
) -> tuple[np.ndarray, np.ndarray]:
"""Return the full constraint-coordinate component containing a target.
Unlike loop-edit grouping, range projection must include non-cut bilateral
constraint rows as well. A bipartite closure finds every row and column
connected through a nonzero Jacobian entry without pulling in unrelated
vehicle corners or mechanisms.
"""
matrix = np.asarray(kd.Algorithms.evalVelocityConstraintMatrix(subgraph, True), dtype=float)
active = np.abs(matrix) > activity_tolerance
columns = {int(target_column)}
rows: set[int] = set()
while True:
next_rows = set(np.flatnonzero(np.any(active[:, list(columns)], axis=1)))
next_columns = set(np.flatnonzero(np.any(active[list(next_rows), :], axis=0)))
if next_rows == rows and next_columns == columns:
break
rows = next_rows
columns = next_columns
return np.asarray(sorted(rows), dtype=int), np.asarray(sorted(columns), dtype=int)
[docs]
def feasibleRangeLimit(
subgraph: kd.SubGraph,
subhinge: kd.PhysicalSubhinge,
*,
dof_index: int = 0,
initial_step: float | None = None,
maximum_step: float | None = None,
maximum_distance: float | None = None,
closure_tolerance: float = 1.0e-9,
rank_tolerance: float = 1.0e-9,
boundary_tolerance: float = 1.0e-7,
sample_callback: Callable[[np.ndarray], None] | None = None,
) -> FeasibleRangeLimit:
"""Find the connected projected feasible interval around one subhinge DOF.
For every tested value of the selected coordinate, all remaining direct
position coordinates are optimization variables. Consequently the test
answers ``exists q_other: C(q) = 0`` and does not impose the artificial
path or frozen-coordinate partition used by the older continuation scan.
Scanning stops at the first infeasible value in each direction, retaining
only the connected interval containing the initial configuration.
"""
is_linear = subhinge.subhingeType() in (
kd.SubhingeType.LINEAR,
kd.SubhingeType.LINEAR3,
)
initial_step = initial_step or (0.0005 if is_linear else 0.01)
maximum_step = maximum_step or (0.005 if is_linear else 0.05)
# A complete revolution is required to distinguish a genuine angular
# boundary from the arbitrary +/-pi coordinate-chart seam.
maximum_distance = maximum_distance or (0.5 if is_linear else 2.0 * math.pi)
if not 0.0 < initial_step <= maximum_step <= maximum_distance:
raise ValueError("step sizes and maximum distance must be positive and ordered")
if not 0.0 < boundary_tolerance < initial_step:
raise ValueError("boundary tolerance must lie between zero and initial step")
# Preserve caller-visible mutable state. The finalizer restores generalized
# position and solver freezes after success or any failed continuation.
saved_q = np.asarray(subgraph.getQ(), dtype=float).copy()
solver = subgraph.cks()
if _targetWasFrozen(subgraph, solver, subhinge, dof_index):
del solver
raise FrozenLoopCoordinateError(f"{subhinge.name()!r}[{dof_index}] is already frozen")
try:
del rank_tolerance # Rank selection is intentionally absent here.
graph_data = subgraph.graphCoordData()
try:
if subhinge.nQ() != subhinge.nU():
raise IndirectCoordinateChartError(
f"{subhinge.name()!r}[{dof_index}] does not have a direct "
"position-coordinate chart"
)
target_column, local_offset = _coordinateColumn(graph_data, subhinge, dof_index)
q_index = int(graph_data.coordOffsets(subhinge).Q + local_offset)
# Restrict the optimization to the connected closure component.
# Coordinates outside this component do not occur in these rows and
# therefore cannot alter the projection result.
_, component_columns = _projectionComponent(subgraph, target_column)
# poseError() is the authoritative nonlinear residual vector. Its
# row packing can be shorter than the velocity matrix when imported
# scalar ConVel constraints are present, so evaluate all pose rows;
# unrelated components remain at their already closed nominal state.
residual_rows = np.arange(np.asarray(subgraph.poseError(), dtype=float).size, dtype=int)
group_q_indices: list[int] = []
visited_ids: set[int] = set()
for column in component_columns:
coordinate, _ = graph_data.coordAt(column)
try:
if coordinate.id() in visited_ids:
continue
visited_ids.add(coordinate.id())
offsets = graph_data.coordOffsets(coordinate)
group_q_indices.extend(
int(offsets.Q + local_q) for local_q in range(coordinate.nQ())
)
finally:
del coordinate
finally:
del graph_data
variable_indices = np.asarray(
[index for index in group_q_indices if index != q_index], dtype=int
)
jacobian_columns = np.asarray(
[column for column in component_columns if column != target_column],
dtype=int,
)
use_analytic_jacobian = variable_indices.size == jacobian_columns.size
# Prescribe target Q and solve only its connected closure component,
# trying retained states as seeds to stay on the same solution branch.
def solveProjected(
target_value: float,
seeds: tuple[np.ndarray, ...],
) -> tuple[np.ndarray | None, float]:
"""Minimize closure error with only the target coordinate fixed."""
best_state = None
best_residual = math.inf
for seed in seeds:
work_state = np.asarray(seed, dtype=float).copy()
work_state[q_index] = target_value
if variable_indices.size == 0:
# A one-coordinate component has nothing to optimize; its
# prescribed trial is either closed as-is or infeasible.
subgraph.setQ(work_state)
closure = np.asarray(subgraph.poseError(), dtype=float)[residual_rows]
value = float(np.linalg.norm(closure, ord=np.inf))
return (
work_state.copy() if value <= closure_tolerance else None,
value,
)
def residual(
values: np.ndarray,
trial: np.ndarray = work_state,
) -> np.ndarray:
"""Evaluate full cut-joint pose closure at one free state."""
trial[variable_indices] = values
trial[q_index] = target_value
subgraph.setQ(trial)
return np.asarray(subgraph.poseError(), dtype=float)[residual_rows]
def jacobian(
values: np.ndarray,
trial: np.ndarray = work_state,
) -> np.ndarray:
"""Return the pose gradient when Q and U charts align."""
trial[variable_indices] = values
trial[q_index] = target_value
subgraph.setQ(trial)
# Evaluate the full pose gradient only after installing this
# trial state, then restrict it to projected rows and variables.
matrix = np.asarray(
kd.Algorithms.evalConstraintPoseGradientMatrix(subgraph),
dtype=float,
)
return matrix[np.ix_(residual_rows, jacobian_columns)]
result = least_squares(
residual,
work_state[variable_indices],
# Fall back to numerical Q derivatives for imported blocks
# whose active U columns and position chart sizes differ.
jac=jacobian if use_analytic_jacobian else "2-point",
xtol=1.0e-12,
ftol=1.0e-12,
gtol=1.0e-12,
max_nfev=400,
)
candidate = work_state.copy()
candidate[variable_indices] = result.x
candidate[q_index] = target_value
subgraph.setQ(candidate)
closure = np.asarray(subgraph.poseError(), dtype=float)[residual_rows]
value = float(np.linalg.norm(closure, ord=np.inf))
if value < best_residual:
best_state = candidate.copy()
best_residual = value
if value <= closure_tolerance:
return candidate, value
return best_state if best_residual <= closure_tolerance else None, best_residual
# Positive and negative scans share projection policy but keep separate
# branch histories and boundary evidence.
def searchDirection(direction: float) -> tuple[float, bool, float]:
"""Scan one side of the nominal projection and refine its boundary."""
distance = 0.0
step = initial_step
witness = saved_q.copy()
accepted = [saved_q.copy()]
worst = 0.0
while distance < maximum_distance:
candidate_distance = min(distance + step, maximum_distance)
state, residual = solveProjected(
float(saved_q[q_index] + direction * candidate_distance),
(witness, saved_q, *accepted[-3:]),
)
if state is None:
feasible_distance = distance
infeasible_distance = candidate_distance
# Bisection uses every recent witness so a local optimizer
# failure is less likely to masquerade as a hard boundary.
while infeasible_distance - feasible_distance > boundary_tolerance:
midpoint = 0.5 * (feasible_distance + infeasible_distance)
refined, refined_residual = solveProjected(
float(saved_q[q_index] + direction * midpoint),
(witness, saved_q, *accepted[-3:]),
)
if refined is None:
infeasible_distance = midpoint
else:
feasible_distance = midpoint
witness = refined
accepted.append(refined.copy())
worst = max(worst, refined_residual)
if sample_callback is not None:
sample_callback(refined)
return feasible_distance, True, worst
distance = candidate_distance
witness = state
accepted.append(state.copy())
worst = max(worst, residual)
if sample_callback is not None:
sample_callback(state)
step = min(1.35 * step, maximum_step)
return maximum_distance, False, worst
current = float(saved_q[q_index])
negative, lower_limited, negative_residual = searchDirection(-1.0)
positive, upper_limited, positive_residual = searchDirection(1.0)
return FeasibleRangeLimit(
subhinge.name(),
current,
current - negative,
current + positive,
lower_limited,
upper_limited,
max(negative_residual, positive_residual),
dof_index=dof_index,
)
finally:
# No solver partition is changed; restore only the caller's state.
subgraph.setQ(saved_q)
del solver
def _sameRangeLimits(first: np.ndarray, second: np.ndarray) -> bool:
"""Return whether two stored endpoint pairs agree, including NaNs.
Parameters
----------
first: np.ndarray
First lower/upper endpoint pair.
second: np.ndarray
Second lower/upper endpoint pair.
Returns
-------
bool
``True`` when corresponding values match and NaNs occupy the same
positions.
"""
return bool(np.array_equal(first, second, equal_nan=True))
[docs]
def updateSubhingeRangeLimits(
subgraph: kd.SubGraph,
subhinge: kd.PhysicalSubhinge,
coordinate_offset: int,
) -> bool:
"""Measure and store one physical-subhinge coordinate's feasible range.
Finite values denote established feasibility boundaries, infinities denote
open search directions, and NaNs denote a failed numerical measurement.
Frozen coordinates and unsupported indirect charts are left unchanged.
Parameters
----------
subgraph: kd.SubGraph
Graph supplying closure constraints and its CK solver.
subhinge: kd.PhysicalSubhinge
Physical subhinge whose range-limit row is updated.
coordinate_offset: int
Local Q row to measure and store.
Returns
-------
bool
``True`` when the stored endpoint pair changed; otherwise ``False``.
"""
# Read the old row before running continuation so the return value describes
# an observable metadata change, not merely whether the calculation ran.
# Quantity-backed angular limits expose ``magnitude``; plain arrays do not.
stored_limits = subhinge.getRangeLimits()
old_limits = np.asarray(
getattr(stored_limits, "magnitude", stored_limits),
dtype=float,
)[coordinate_offset].copy()
try:
# Keep numerical exploration in feasibleRangeLimit(); this method owns
# only policy for skipped, failed, bounded, and open measurements.
measured = feasibleRangeLimit(
subgraph,
subhinge,
dof_index=coordinate_offset,
)
except (FrozenLoopCoordinateError, IndirectCoordinateChartError, LockedLoopError):
# A skipped coordinate retains its prior metadata because no new
# statement about its feasible excursion was established.
return False
except (FloatingPointError, RuntimeError, ValueError):
# The computation started but could not produce trustworthy endpoints.
# NaN explicitly invalidates stale computed limits so later inertness
# checks cannot treat an older finite interval as current evidence.
new_limits = np.full(2, np.nan)
else:
# Finite values represent boundaries found after a solveQ() transition.
# Reaching the search horizon without failure proves only that travel
# remains available, so encode that direction as mathematically open.
new_limits = np.asarray(
(
measured.lower if measured.lower_limited else -np.inf,
measured.upper if measured.upper_limited else np.inf,
),
dtype=float,
)
if _sameRangeLimits(old_limits, new_limits):
# Avoid notifying kdFlex variables when recomputation reproduces the
# exact metadata already stored, treating NaN pairs as equal here.
return False
# Publish the pair atomically so callers never observe one old endpoint and
# one endpoint from the new computation.
subhinge.setRangeLimits(new_limits, coordinate_offset)
return True
[docs]
def updateRangeLimits(
subgraph: kd.SubGraph,
selected_dofs: tuple[tuple[str, int], ...] | None = None,
) -> None:
"""Update feasible ranges for physical coordinates in a subgraph.
When ``selected_dofs`` is supplied, only the named coordinate offsets are
processed. Otherwise every physical hinge and cut-joint coordinate in the
graph is updated once.
Parameters
----------
subgraph: kd.SubGraph
Graph whose physical-subhinge range metadata is updated.
selected_dofs: tuple[tuple[str, int], ...] | None
Optional coordinate-name and local-offset pairs. ``None``
updates every physical coordinate in the graph.
"""
# Normalize selections once. Duplicate selectors are rejected because a
# repeated request should not run nonlinear continuation twice for one row.
if selected_dofs is not None and len(set(selected_dofs)) != len(selected_dofs):
raise ValueError("range-limit coordinate selections must be unique")
requested = set(selected_dofs) if selected_dofs is not None else None
graph_data = subgraph.graphCoordData()
try:
if requested is not None:
# Validate the entire selection before changing any stored limits.
available: dict[tuple[str, int], int] = {}
inventoried: set[int] = set()
for column in range(graph_data.nU()):
coordinate, _ = graph_data.coordAt(column)
try:
if coordinate.id() in inventoried or not isinstance(
coordinate, kd.PhysicalSubhinge
):
continue
inventoried.add(coordinate.id())
for offset in range(coordinate.nQ()):
key = (coordinate.name(), offset)
available[key] = available.get(key, 0) + 1
finally:
del coordinate
missing = requested - available.keys()
ambiguous = {key for key in requested if available.get(key, 0) > 1}
if missing or ambiguous:
# Fail before the update pass begins. This makes invalid
# selection handling atomic: no earlier coordinate is changed
# before the caller learns that a later selector is unusable.
invalid = missing | ambiguous
labels = ", ".join(f"{name}/{offset}" for name, offset in sorted(invalid))
raise click.UsageError(
f"loops --mobility-check coordinate is not uniquely available: {labels}"
)
# graphCoordData exposes one entry per packed U component, so a
# multi-component subhinge appears repeatedly. Visit each physical
# provider once, then explicitly iterate its Q rows in local order.
visited: set[int] = set()
for column in range(graph_data.nU()):
coordinate, _ = graph_data.coordAt(column)
try:
if coordinate.id() in visited or not isinstance(coordinate, kd.PhysicalSubhinge):
continue
visited.add(coordinate.id())
for offset in range(coordinate.nQ()):
key = (coordinate.name(), offset)
if requested is not None and key not in requested:
continue
updateSubhingeRangeLimits(subgraph, coordinate, offset)
finally:
del coordinate
finally:
del graph_data
def _loopSubhingeCoordinates(
subgraph: kd.SubGraph,
activity_tolerance: float = 1.0e-10,
) -> tuple[tuple[int, int], ...]:
"""Return every DOF of each subhinge active in loop equations.
Parameters
----------
subgraph: kd.SubGraph
Graph whose velocity-constraint matrix is inspected.
activity_tolerance: float
Minimum column norm considered constraint-active.
Defaults to 1.0e-10.
Returns
-------
tuple[tuple[int, int], ...]
Coordinate-ID and local-DOF pairs in graph traversal order.
"""
graph_data = subgraph.graphCoordData()
matrix = np.asarray(kd.Algorithms.evalVelocityConstraintMatrix(subgraph, True), dtype=float)
active_subhinge_ids: list[int] = []
try:
active = np.linalg.norm(matrix, axis=0) > activity_tolerance
for column in np.flatnonzero(active):
coordinate, _ = graph_data.coordAt(int(column))
try:
coordinate_id = coordinate.id()
if coordinate_id not in active_subhinge_ids:
active_subhinge_ids.append(coordinate_id)
finally:
del coordinate
result = []
for coordinate_id in active_subhinge_ids:
for column in range(graph_data.nU()):
coordinate, _ = graph_data.coordAt(column)
try:
if coordinate.id() == coordinate_id:
result.extend(
(coordinate_id, dof_index) for dof_index in range(coordinate.nU())
)
break
finally:
del coordinate
return tuple(result)
finally:
del graph_data
def _coordinateMobilityEvidence(
subgraph: kd.SubGraph,
group: LoopGroup,
coordinate_id: int,
dof_index: int,
states: tuple[np.ndarray, ...],
complete_connected_range: bool,
*,
rank_tolerance: float = 1.0e-9,
displacement_tolerance: float = 1.0e-9,
) -> CoordinateMobilityEvidence:
"""Classify a nominally immobile coordinate over retained closed states.
At every retained pose, the coordinate's row in an orthonormal null-space
basis measures whether any constraint-compatible velocity uses that DOF.
Zero participation at one pose is only configuration-dependent evidence.
``INERT`` requires zero participation and displacement over a complete
connected one-dimensional range; incomplete coverage yields a candidate.
Parameters
----------
subgraph: kd.SubGraph
Graph used to evaluate constraint matrices at sampled states.
group: LoopGroup
Connected loop group containing the target coordinate.
coordinate_id: int
Stable native ID of the target coordinate provider.
dof_index: int
Local target offset within its provider.
states: tuple[np.ndarray, ...]
Closed Q states retained during range continuation.
complete_connected_range: bool
Whether the samples cover both hard endpoints
of the connected one-mobility branch.
rank_tolerance: float
Relative threshold for null-space participation.
Defaults to 1.0e-9.
displacement_tolerance: float
Largest observed Q displacement considered zero.
Defaults to 1.0e-9.
Returns
-------
CoordinateMobilityEvidence
Classification and numerical evidence for the target coordinate.
"""
graph_data = subgraph.graphCoordData()
saved_q = np.asarray(subgraph.getQ(), dtype=float).copy()
coordinate_name = ""
group_column = None
q_index = None
try:
# Resolve both packed indices once so every sample uses the same DOF.
for local_group_column, full_column in enumerate(group.columns):
coordinate, local_offset = graph_data.coordAt(full_column)
try:
if coordinate.id() == coordinate_id and local_offset == dof_index:
coordinate_name = coordinate.name()
group_column = local_group_column
q_index = int(graph_data.coordOffsets(coordinate).Q + dof_index)
break
finally:
del coordinate
if group_column is None or q_index is None:
raise RuntimeError("locally immobile loop coordinate disappeared")
initial_value = float(states[0][q_index])
max_participation = 0.0
max_displacement = 0.0
nullities: list[int] = []
for state in states:
subgraph.setQ(state)
matrix = _groupMatrix(subgraph, group)
# An orthonormal basis makes the row norm invariant to basis choice.
nullspace = linalg.null_space(matrix, rcond=rank_tolerance)
nullities.append(int(nullspace.shape[1]))
if nullspace.shape[1]:
max_participation = max(
max_participation,
float(np.linalg.norm(nullspace[group_column, :])),
)
max_displacement = max(
max_displacement,
abs(float(state[q_index]) - initial_value),
)
remains_zero = (
max_participation <= rank_tolerance and max_displacement <= displacement_tolerance
)
coverage_is_complete = (
complete_connected_range
and bool(nullities)
and all(nullity == 1 for nullity in nullities)
)
if not remains_zero:
classification = "CONFIGURATION-DEPENDENT"
elif coverage_is_complete:
classification = "INERT"
else:
classification = "INERT CANDIDATE"
return CoordinateMobilityEvidence(
coordinate_name,
dof_index,
classification,
len(states),
max_participation,
max_displacement,
coverage_is_complete,
)
finally:
# Evidence evaluation must not change the caller-visible configuration.
subgraph.setQ(saved_q)
del graph_data
def _coordinateParticipationRank(
subgraph: kd.SubGraph,
group: LoopGroup,
coordinate_id: int,
*,
rank_tolerance: float = 1.0e-9,
) -> tuple[int, int, tuple[tuple[float, float, float], ...]]:
"""Return local participation rank and an aligned three-axis basis.
Parameters
----------
subgraph: kd.SubGraph
Graph supplying the current constraint matrix.
group: LoopGroup
Connected loop group containing the coordinate.
coordinate_id: int
Stable native ID of the coordinate provider.
rank_tolerance: float
Relative singular-value threshold used for rank.
Defaults to 1.0e-9.
Returns
-------
tuple[int, int, tuple[tuple[float, float, float], ...]]
Participation rank, coordinate DOF count, and aligned three-axis basis.
"""
graph_data = subgraph.graphCoordData()
coordinate_rows = []
coordinate_dofs = 0
try:
for local_group_column, full_column in enumerate(group.columns):
coordinate, _ = graph_data.coordAt(full_column)
try:
if coordinate.id() == coordinate_id:
coordinate_rows.append(local_group_column)
coordinate_dofs = coordinate.nU()
finally:
del coordinate
finally:
del graph_data
if not coordinate_rows or coordinate_dofs == 0:
raise RuntimeError("indirect-chart loop coordinate disappeared")
matrix = _groupMatrix(subgraph, group)
nullspace = linalg.null_space(matrix, rcond=rank_tolerance)
participation = nullspace[np.asarray(coordinate_rows), :]
rank = _constraintRank(participation, rank_tolerance)
# Left singular vectors align the first axes with the locally active BALL
# velocity subspace and the remaining axes with its orthogonal complement.
left_vectors, _, _ = np.linalg.svd(participation, full_matrices=True)
axes = tuple(tuple(float(value) for value in left_vectors[:, index]) for index in range(3))
return rank, coordinate_dofs, axes
def _setLoopRangeLimits(
subgraph: kd.SubGraph,
*,
report_progress: bool = False,
progress_with_offsets: bool = False,
selected_names: tuple[str, ...] | None = None,
selected_dofs: tuple[tuple[str, int], ...] | None = None,
evidence_output: list[CoordinateMobilityEvidence] | None = None,
ball_hint_output: list[BallReductionHint] | None = None,
status_output: list[LoopMobilityStatus] | None = None,
inert_tolerance: float = 2.0e-7,
apply_limits: bool = True,
) -> tuple[FeasibleRangeLimit, ...]:
"""Analyze loop-coordinate ranges and optionally apply supported Q limits.
This pipeline inventories active subhinge DOFs, continues scalar DOFs over
closure-feasible ranges, retains accepted states for global inertness
evidence, derives BALL reduction hints, and emits status records. Direct
multi-DOF charts store each measured Q row independently. Indirect charts
remain explicitly unresolved by the scalar scanner. Temporary states and
solver partitions are restored before the function returns.
Parameters
----------
subgraph: kd.SubGraph
Graph whose loop-coordinate ranges are analyzed.
report_progress: bool
Whether to emit live per-coordinate progress.
Defaults to ``False``.
progress_with_offsets: bool
Whether progress labels include local offsets.
Defaults to ``False``.
selected_names: tuple[str, ...] | None
Optional subhinge names used by range-update commands.
selected_dofs: tuple[tuple[str, int], ...] | None
Optional name/offset pairs used by mobility checks.
evidence_output: list[CoordinateMobilityEvidence] | None
Optional list populated with inertness evidence.
ball_hint_output: list[BallReductionHint] | None
Optional list populated with spherical-joint hints.
status_output: list[LoopMobilityStatus] | None
Optional list populated with report-ready statuses.
apply_limits: bool
Whether measured endpoint pairs are stored on subhinges.
Defaults to ``True``.
Returns
-------
tuple[FeasibleRangeLimit, ...]
Measurements produced for coordinates whose scalar scans completed.
"""
# Phase 1 inventories scope. Read-only analysis starts with loop-active
# coordinates; limit application also includes physical tree coordinates.
coordinates = _loopSubhingeCoordinates(subgraph)
if apply_limits:
# The update inventory includes free tree coordinates, e.g. a base pin.
data = subgraph.graphCoordData()
try:
physical_coordinates = []
for column in range(data.nU()):
coordinate, offset = data.coordAt(column)
try:
if isinstance(coordinate, kd.PhysicalSubhinge):
physical_coordinates.append((coordinate.id(), offset))
finally:
del coordinate
coordinates = tuple(physical_coordinates)
finally:
del data
# Phase 2 maps coordinates to connected closures whose retained states and
# scan completeness determine later inertness evidence.
loop_groups = discoverLoopGroups(subgraph)
group_by_coordinate = {
coordinate_id: group_index
for group_index, group in enumerate(loop_groups)
for coordinate_id in group.coordinate_ids
}
missing_group_ids = {
coordinate_id
for coordinate_id, _ in coordinates
if coordinate_id not in group_by_coordinate
}
if missing_group_ids:
# Some imported models express closure with non-cut bilateral rows.
# Keep those coordinates reportable even though edit-oriented cut-joint
# grouping intentionally excludes them.
matrix = np.asarray(kd.Algorithms.evalVelocityConstraintMatrix(subgraph, True), dtype=float)
fallback_index = len(loop_groups)
fallback_group = LoopGroup(
(),
tuple(range(matrix.shape[0])),
tuple(range(matrix.shape[1])),
tuple(sorted(missing_group_ids)),
(),
)
loop_groups = (*loop_groups, fallback_group)
group_by_coordinate.update(
{coordinate_id: fallback_index for coordinate_id in missing_group_ids}
)
# Every group starts with the same nominal witness. Continuation appends
# distinct closed states and the finalizer restores this configuration.
nominal_q = np.asarray(subgraph.getQ(), dtype=float).copy()
sampled_states = {group_index: [nominal_q.copy()] for group_index in range(len(loop_groups))}
complete_group_ranges: set[int] = set()
zero_local_coordinates: list[tuple[int, int, int]] = []
reduction_hint_coordinates: dict[int, tuple[str, int]] = {}
summaries: list[FeasibleRangeLimit] = []
range_results: dict[tuple[int, int], tuple[float, float]] = {}
graph_data = subgraph.graphCoordData()
try:
# Determine one name-column width before emitting live progress lines.
coordinate_names: dict[int, str] = {}
coordinate_labels: dict[tuple[int, int], str] = {}
participating_ids = {coordinate_id for coordinate_id, _ in coordinates}
for column in range(graph_data.nU()):
coordinate, local_offset = graph_data.coordAt(column)
try:
if coordinate.id() in participating_ids:
coordinate_names[coordinate.id()] = coordinate.name()
coordinate_labels[(coordinate.id(), local_offset)] = _subhingeLabel(
coordinate, local_offset
)
finally:
del coordinate
if selected_names is not None and selected_dofs is not None:
raise ValueError("select ranges by names or name/offset pairs, not both")
if selected_names is not None:
# Validate the complete selection before starting a potentially
# lengthy scan, and retain the user's requested ordering.
if "all" in selected_names:
if len(selected_names) != 1:
raise click.UsageError(
"loops --ranges-update all cannot be combined with names"
)
else:
if len(set(selected_names)) != len(selected_names):
raise click.UsageError("loops --ranges-update subhinge names must be unique")
ids_by_name: dict[str, list[int]] = {}
for coordinate_id, name in coordinate_names.items():
ids_by_name.setdefault(name, []).append(coordinate_id)
missing = [name for name in selected_names if name not in ids_by_name]
if missing:
raise click.UsageError(
"loops --ranges-update subhinge is not active in a "
"loop: "
f"{', '.join(missing)}"
)
ambiguous = [name for name in selected_names if len(ids_by_name[name]) != 1]
if ambiguous:
raise click.UsageError(
f"loops --ranges-update subhinge name is ambiguous: {', '.join(ambiguous)}"
)
selected_ids = tuple(ids_by_name[name][0] for name in selected_names)
coordinates = tuple(
item
for coordinate_id in selected_ids
for item in coordinates
if item[0] == coordinate_id
)
elif selected_dofs is not None:
# Range-check selectors identify one specific component; a missing
# CLI suffix has already been normalized to offset zero.
ids_by_name: dict[str, list[int]] = {}
for coordinate_id, name in coordinate_names.items():
ids_by_name.setdefault(name, []).append(coordinate_id)
missing = [name for name, _ in selected_dofs if name not in ids_by_name]
if missing:
raise click.UsageError(
f"loops --mobility-check subhinge is not active in a loop: {', '.join(missing)}"
)
ambiguous = [name for name, _ in selected_dofs if len(ids_by_name[name]) != 1]
if ambiguous:
raise click.UsageError(
f"loops --mobility-check subhinge name is ambiguous: {', '.join(ambiguous)}"
)
requested = tuple((ids_by_name[name][0], offset) for name, offset in selected_dofs)
unavailable = [
f"{name}/{offset}"
for name, offset in selected_dofs
if (ids_by_name[name][0], offset) not in coordinates
]
if unavailable:
raise click.UsageError(
"loops --mobility-check offset is outside the active subhinge: "
f"{', '.join(unavailable)}"
)
coordinates = requested
# Freeze labels before scanning because coordinate wrappers are released
# between iterations and report order must remain deterministic.
selected_coordinate_labels = (
f"{coordinate_names[coordinate_id]}/{dof_index}"
if progress_with_offsets
else coordinate_labels[(coordinate_id, dof_index)]
for coordinate_id, dof_index in coordinates
)
name_width = max(map(len, selected_coordinate_labels), default=0)
# Measure all ranges first so newly assigned limits cannot affect scans.
for coordinate_id, dof_index in coordinates:
target = None
for column in range(graph_data.nU()):
coordinate, _ = graph_data.coordAt(column)
if coordinate.id() == coordinate_id:
target = coordinate
break
del coordinate
if target is None:
raise RuntimeError(f"loop coordinate ID {coordinate_id} disappeared")
try:
if isinstance(target, kd.SphericalSubhinge):
# Evaluate BALL reducibility as one coordinate block rather
# than interpreting basis-dependent scalar components.
reduction_hint_coordinates[coordinate_id] = (
target.name(),
group_by_coordinate[coordinate_id],
)
if report_progress:
# Keep the line open while the two continuation searches run.
progress_label = (
f"{target.name()}/{dof_index}"
if progress_with_offsets
else _subhingeLabel(target, dof_index)
)
click.echo(
f"Checking {progress_label:<{name_width}}",
nl=False,
)
old_limits = None
supports_limits = isinstance(target, kd.PhysicalSubhinge)
if supports_limits:
# Range metadata has one min/max row for every Q coordinate.
stored_limits = target.getRangeLimits()
old_values = np.asarray(
getattr(stored_limits, "magnitude", stored_limits),
dtype=float,
)[dof_index]
if not np.isnan(old_values).any():
old_limits = (float(old_values[0]), float(old_values[1]))
try:
group_index = group_by_coordinate[coordinate_id]
def retainFeasibleState(
state: np.ndarray,
*,
destination: list[np.ndarray] = sampled_states[group_index],
) -> None:
"""Retain a closed state for later nullspace evidence.
Parameters
----------
state: np.ndarray
Newly accepted full Q state.
destination: list[np.ndarray]
Group-specific state list receiving it.
Defaults to ``sampled_states[group_index]```.
"""
# Avoid adjacent duplicates from boundary refinement.
if not np.array_equal(destination[-1], state):
destination.append(np.asarray(state, dtype=float).copy())
measured = feasibleRangeLimit(
subgraph,
target,
dof_index=dof_index,
sample_callback=retainFeasibleState,
)
except FrozenLoopCoordinateError:
if report_progress:
# Respect persistent user freezes and keep scanning.
click.echo(": FROZEN")
if status_output is not None:
status_output.append(
LoopMobilityStatus(
target.name(),
type(target).__name__,
dof_index,
"FROZEN",
None,
"Coordinate was already frozen by the user.",
)
)
continue
except ZeroLocalMobilityError:
if report_progress:
# This coordinate is zero in the nominal nullspace.
click.echo(": ZERO LOCAL MOBILITY")
zero_local_coordinates.append(
(coordinate_id, dof_index, group_by_coordinate[coordinate_id])
)
continue
except IndirectCoordinateChartError:
if report_progress:
# Quaternion and other indirect charts cannot be driven
# one scalar component at a time by this range scanner.
click.echo(": UNRESOLVED (indirect coordinate chart)")
if status_output is not None:
status_output.append(
LoopMobilityStatus(
target.name(),
type(target).__name__,
dof_index,
"UNRESOLVED",
None,
"Scalar continuation cannot represent this indirect coordinate chart.",
)
)
continue
except LockedLoopError:
if report_progress:
# A missing direct chart prevents a mobility conclusion.
click.echo(": UNRESOLVED")
if status_output is not None:
status_output.append(
LoopMobilityStatus(
target.name(),
type(target).__name__,
dof_index,
"UNRESOLVED",
None,
"No direct independent-coordinate chart was found.",
)
)
continue
except RangeLimitComputationError as error:
if report_progress:
click.echo(": UNRESOLVED (range computation failed)")
if supports_limits and apply_limits:
range_results[(coordinate_id, dof_index)] = (np.nan, np.nan)
if status_output is not None:
status_output.append(
LoopMobilityStatus(
target.name(),
type(target).__name__,
dof_index,
"UNRESOLVED",
None,
str(error),
)
)
continue
measured = replace(
measured,
lower=measured.lower if measured.lower_limited else -np.inf,
upper=measured.upper if measured.upper_limited else np.inf,
old_limits=old_limits,
range_limits_supported=supports_limits,
)
if np.isnan((measured.lower, measured.upper)).any():
# Each endpoint is independent: retain a valid opposite
# endpoint while marking only failed directions as NaN.
summaries.append(measured)
if supports_limits and apply_limits:
range_results[(coordinate_id, dof_index)] = (
measured.lower,
measured.upper,
)
if status_output is not None:
status_output.append(
LoopMobilityStatus(
target.name(),
type(target).__name__,
dof_index,
"UNRESOLVED",
None,
"One or both range endpoints could not be computed.",
)
)
if report_progress:
click.echo(": UNRESOLVED (range endpoint computation failed)")
continue
if np.isfinite(
(measured.lower, measured.upper)
).all() and target.subhingeType() in (
kd.SubhingeType.PIN,
kd.SubhingeType.SPHERICAL,
kd.SubhingeType.SCREW,
):
# A rotational coordinate has at most one revolution of
# distinct physical values. Normalize before recording,
# reporting, or passing the interval to setRangeLimits().
bounded_lower, bounded_upper = _boundedAngularInterval(
measured.lower,
measured.upper,
measured.current,
)
measured = replace(
measured,
lower=bounded_lower,
upper=bounded_upper,
)
summaries.append(measured)
span = measured.upper - measured.lower
projection_inert = (
measured.lower_limited and measured.upper_limited and span <= inert_tolerance
)
if projection_inert and evidence_output is not None:
# An isolated projected value is stronger evidence than a
# zero Jacobian component at only the nominal pose.
evidence_output.append(
CoordinateMobilityEvidence(
target.name(),
dof_index,
"INERT",
len(sampled_states[group_index]),
0.0,
span,
True,
)
)
if status_output is not None:
is_angular = isinstance(target, (kd.PinSubhinge, kd.SphericalSubhinge))
mobility = (
"INERT"
if projection_inert
else "LOW"
if is_angular and 0.0 < span < np.deg2rad(10.0)
else "MOBILE"
)
lower_kind = "hard" if measured.lower_limited else "open"
upper_kind = "hard" if measured.upper_limited else "open"
status_output.append(
LoopMobilityStatus(
target.name(),
type(target).__name__,
dof_index,
mobility,
(measured.lower, measured.upper),
f"{lower_kind}; {upper_kind}",
)
)
if measured.lower_limited and measured.upper_limited and not projection_inert:
# A bounded scalar scan covers a one-mobility branch only
# after the sampled nullity checks performed below.
complete_group_ranges.add(group_index)
if supports_limits:
# Store the measured range, including open scan endpoints and
# degenerate intervals; these are metadata, not joint stops.
range_results[(coordinate_id, dof_index)] = (measured.lower, measured.upper)
if report_progress:
# Finish the pending progress line with the measured values.
_finishRangeLimitProgress(measured)
finally:
del target
# Phase 4 revisits nominally immobile coordinates using every closed
# state retained for their connected loop group.
for coordinate_id, dof_index, group_index in zero_local_coordinates:
evidence = _coordinateMobilityEvidence(
subgraph,
loop_groups[group_index],
coordinate_id,
dof_index,
tuple(sampled_states[group_index]),
group_index in complete_group_ranges,
)
if evidence_output is not None:
evidence_output.append(evidence)
if status_output is not None:
coordinate_type = (
coordinate_labels[(coordinate_id, dof_index)].rsplit("/", 1)[1].split("[", 1)[0]
)
coverage = (
"complete connected range"
if evidence.complete_connected_range
else "sampled range only"
)
status_output.append(
LoopMobilityStatus(
evidence.coordinate,
coordinate_type,
evidence.dof_index,
evidence.classification,
None,
f"{evidence.sampled_states} states; max participation "
f"{evidence.max_participation:.3g}; max displacement "
f"{evidence.max_displacement:.3g}; {coverage}",
)
)
if report_progress:
coverage = "complete" if evidence.complete_connected_range else "sampled"
click.echo(
f"Mobility evidence {evidence.coordinate}[{evidence.dof_index}]: "
f"{evidence.classification}; states={evidence.sampled_states}; "
f"max participation={evidence.max_participation:.3g}; "
f"max displacement={evidence.max_displacement:.3g}; "
f"coverage={coverage}"
)
# A rank-deficient spherical participation block is a basis-invariant
# hint to try an explicit fixed-axis gimbal; it is never deletion proof.
for coordinate_id, (coordinate_name, group_index) in reduction_hint_coordinates.items():
participation_rank, coordinate_dofs, axes = _coordinateParticipationRank(
subgraph,
loop_groups[group_index],
coordinate_id,
)
if participation_rank < coordinate_dofs and ball_hint_output is not None:
ball_hint_output.append(
BallReductionHint(
coordinate_name,
participation_rank,
coordinate_dofs,
axes,
)
)
if report_progress and participation_rank < coordinate_dofs:
click.echo(
f"BALL reduction candidate {coordinate_name}: local "
f"participation rank={participation_rank}/{coordinate_dofs}; "
"validate with an explicit-axis GIMBAL over the full range"
)
if not apply_limits:
return tuple(summaries)
for column in range(graph_data.nU()):
coordinate, local_offset = graph_data.coordAt(column)
try:
limits = range_results.get((coordinate.id(), local_offset))
if limits is not None:
# The setter takes one lower/upper pair and its Q-row offset.
coordinate.setRangeLimits(np.asarray(limits, dtype=float), local_offset)
finally:
del coordinate
return tuple(summaries)
finally:
del graph_data
[docs]
def updateLoopRangeLimits(
subgraph: kd.SubGraph,
selected_names: tuple[str, ...] = ("all",),
) -> None:
"""Find, apply, and report Q ranges for loop-active physical subhinges.
Parameters
----------
subgraph: kd.SubGraph
Graph whose physical range metadata is updated.
selected_names: tuple[str, ...]
Requested subhinge names, or ``("all",)``.
Defaults to ``("all",)``.
"""
# The numerical utility restores the nominal graph state after every scan.
coordinate_ranges = _setLoopRangeLimits(
subgraph,
report_progress=True,
selected_names=selected_names,
)
if not coordinate_ranges:
click.echo("No loop-coordinate ranges were determined.")
return
def _markdownCell(text: str) -> str:
"""Escape text for use in one Markdown table cell.
Parameters
----------
text: str
Unescaped cell content.
Returns
-------
str
Cell-safe text with pipes and line breaks escaped or replaced.
"""
return text.replace("|", "\\|").replace("\n", " ")
def _markdownTable(
headers: tuple[str, ...],
rows: list[tuple[str, ...]],
right_aligned: frozenset[int] = frozenset(),
) -> list[str]:
"""Return a Markdown table whose source columns are padded and aligned.
Parameters
----------
headers: tuple[str, ...]
Column headings in display order.
rows: list[tuple[str, ...]]
Cell values for each body row.
right_aligned: frozenset[int]
Zero-based columns rendered with right alignment.
Defaults to ``frozenset()``.
Returns
-------
list[str]
Markdown source lines for the complete table.
"""
# Include header widths so an empty or very narrow result remains readable.
widths = [len(header) for header in headers]
for row in rows:
if len(row) != len(headers):
raise ValueError("Markdown table row has the wrong number of columns")
widths = [max(width, len(cell)) for width, cell in zip(widths, row, strict=True)]
def formatRow(row: tuple[str, ...]) -> str:
"""Pad cells while preserving requested numeric right alignment.
Parameters
----------
row: tuple[str, ...]
Cell values matching the table's column count.
Returns
-------
str
One pipe-delimited Markdown table line.
"""
cells = []
for index, (cell, width) in enumerate(zip(row, widths, strict=True)):
cells.append(cell.rjust(width) if index in right_aligned else cell.ljust(width))
return "| " + " | ".join(cells) + " |"
separators = tuple(
("-" * max(3, width - 1) + ":")
if index in right_aligned
else (":" + "-" * max(3, width - 1))
for index, width in enumerate(widths)
)
return [formatRow(headers), formatRow(separators), *(formatRow(row) for row in rows)]
def _shortSubhingeType(coordinate_type: str, dof_index: int) -> str:
"""Convert a concrete pybind class name to a concise type/DOF label.
Parameters
----------
coordinate_type: str
Concrete Python subhinge class name.
dof_index: int
Local component offset represented by the row.
Returns
-------
str
Concise uppercase type label, including an offset when useful.
"""
short_type = coordinate_type.removesuffix("Subhinge").upper()
if dof_index or coordinate_type == "SphericalSubhinge":
short_type += f"[{dof_index}]"
return short_type
def _normalizedAngularInterval(lower: float, upper: float) -> tuple[float, float]:
"""Return an ordered equivalent interval spanning no more than one turn.
Parameters
----------
lower: float
Unnormalized lower angular endpoint in radians.
upper: float
Unnormalized upper angular endpoint in radians.
Returns
-------
tuple[float, float]
Ordered equivalent endpoints anchored in the principal chart.
"""
# A periodic coordinate cannot expose more than one revolution of distinct
# values. Anchor the ordered interval at an equivalent lower endpoint in
# the principal chart rather than wrapping its endpoints independently.
span = min(max(upper - lower, 0.0), 2.0 * math.pi)
if math.isclose(span, 2.0 * math.pi, abs_tol=1.0e-8):
return -math.pi, math.pi
normalized_lower = (lower + math.pi) % (2.0 * math.pi) - math.pi
return normalized_lower, normalized_lower + span
def _boundedAngularInterval(
lower: float,
upper: float,
current: float,
) -> tuple[float, float]:
"""Bound a rotational interval to one revolution around its current value.
Parameters
----------
lower: float
Measured lower angular endpoint in radians.
upper: float
Measured upper angular endpoint in radians.
current: float
Nominal angular coordinate value in radians.
Returns
-------
tuple[float, float]
Original endpoints when already bounded, otherwise one revolution
centered on ``current``.
"""
span = max(upper - lower, 0.0)
if span <= 2.0 * math.pi:
# Preserve bounded intervals and their relationship to the current Q.
return lower, upper
# Coverage beyond one revolution contains no additional physical angles.
return current - math.pi, current + math.pi
def _formatRangeSpan(magnitude: float, is_angular: bool) -> str:
"""Format angular spans in degrees and linear spans in meters.
Parameters
----------
magnitude: float
Interval width in native radians or meters.
is_angular: bool
Whether to convert the value from radians to degrees.
Returns
-------
str
Four-decimal span with a degree or meter suffix.
"""
# Explicit unit suffixes keep mixed rotational and translational report
# rows directly interpretable without consulting the model conventions.
value = np.rad2deg(magnitude) if is_angular else magnitude
suffix = "°" if is_angular else "m"
return f"{value:.4f}{suffix}"
def _formatRangeEndpoint(value: float) -> str:
"""Format one displayed range endpoint with four decimal places.
Parameters
----------
value: float
Endpoint value in its native coordinate unit.
Returns
-------
str
Fixed-precision endpoint text.
"""
return f"{value:.4f}"
@dataclass(frozen=True)
class _MobilityAnalysisResult:
"""Retain one non-mutating scan for the public mobility operations."""
ranges: tuple[FeasibleRangeLimit, ...]
evidence: tuple[CoordinateMobilityEvidence, ...]
ball_hints: tuple[BallReductionHint, ...]
statuses: tuple[LoopMobilityStatus, ...]
def _mobilityAnalysis(
multibody: kd.SubGraph,
selected_dofs: tuple[tuple[str, int], ...] | None = None,
*,
report_progress: bool = False,
apply_limits: bool = False,
) -> _MobilityAnalysisResult:
"""Run the shared non-mutating scan for public mobility operations.
Range continuation supplies the closed-state witnesses needed by inert-DOF
and BALL-reduction analysis, so the three public operations share this one
private result when called through :func:`mobilityCheck`.
Parameters
----------
multibody: kd.SubGraph
SubGraph-compatible multibody to analyze.
selected_dofs: tuple[tuple[str, int], ...] | None
Optional coordinate-name and local-offset selections.
report_progress: bool
Whether to emit continuation progress.
Defaults to ``False``.
apply_limits: bool
Whether to store measured ranges after preserving the
pre-scan limits for comparison and reporting.
Defaults to ``False``.
Returns
-------
_MobilityAnalysisResult
Immutable range, inertness, spherical-hint, and status collections.
"""
evidence: list[CoordinateMobilityEvidence] = []
hints: list[BallReductionHint] = []
statuses: list[LoopMobilityStatus] = []
ranges = _setLoopRangeLimits(
multibody,
report_progress=report_progress,
progress_with_offsets=report_progress,
selected_dofs=selected_dofs,
evidence_output=evidence,
ball_hint_output=hints,
status_output=statuses,
apply_limits=apply_limits,
)
# Stable coordinate/offset ordering keeps API consumers and reports
# diff-friendly across equivalent runs.
statuses.sort(key=lambda item: (item.coordinate, item.dof_index))
return _MobilityAnalysisResult(
ranges,
tuple(evidence),
tuple(hints),
tuple(statuses),
)
[docs]
def checkRangeLimits(
multibody: kd.SubGraph,
selected_dofs: tuple[tuple[str, int], ...] | None = None,
*,
report_progress: bool = False,
_analysis: _MobilityAnalysisResult | None = None,
) -> tuple[tuple[FeasibleRangeLimit, ...], tuple[LoopMobilityStatus, ...]]:
"""Check feasible ranges without applying them to the multibody.
``multibody`` is first so this operation can move to :class:`SubGraph`
without changing its user-facing arguments. The returned statuses include
unresolved or immobile coordinates for which no interval was measured.
Parameters
----------
multibody: kd.SubGraph
SubGraph-compatible multibody to analyze.
selected_dofs: tuple[tuple[str, int], ...] | None
Optional coordinate-name and local-offset selections.
report_progress: bool
Whether to emit continuation progress.
Defaults to ``False``.
_analysis: _MobilityAnalysisResult | None
Optional shared internal result supplied by ``mobilityCheck``.
Returns
-------
tuple[tuple[FeasibleRangeLimit, ...], tuple[LoopMobilityStatus, ...]]
The completed range measurements and report-ready coordinate statuses.
"""
analysis = _analysis or _mobilityAnalysis(
multibody,
selected_dofs,
report_progress=report_progress,
)
return analysis.ranges, analysis.statuses
[docs]
def detectInertDofs(
multibody: kd.SubGraph,
selected_dofs: tuple[tuple[str, int], ...] | None = None,
*,
tolerance: float = 2.0e-7,
update_ranges: bool = True,
) -> tuple[CoordinateMobilityEvidence, ...]:
"""Update ranges and return physical coordinates with finite zero spans.
Only two finite endpoints can establish inertness. Open intervals and
failed measurements therefore remain non-inert without auxiliary flags.
Parameters
----------
multibody: kd.SubGraph
SubGraph-compatible multibody whose ranges are inspected.
selected_dofs: tuple[tuple[str, int], ...] | None
Optional coordinate-name and local-offset selections.
tolerance: float
Largest finite interval width classified as inert.
Defaults to 2.0e-7.
update_ranges: bool
Whether to recompute stored ranges before classification.
Defaults to ``True``.
Returns
-------
tuple[CoordinateMobilityEvidence, ...]
Deterministically ordered evidence for coordinates classified inert.
"""
# A negative tolerance would turn the interval-width test into an
# impossible condition and usually signals a caller error, so reject it
# instead of quietly returning no inert coordinates.
if tolerance < 0.0:
raise ValueError("inert-DOF tolerance must be nonnegative")
if update_ranges:
# Refresh metadata first so classification reflects the mechanism's
# current configuration and constraints rather than stale model input.
updateRangeLimits(multibody, selected_dofs)
# The optional selection is reapplied while reading because stored limits
# also contain coordinates outside the user's requested mobility check.
requested = set(selected_dofs) if selected_dofs is not None else None
evidence: list[CoordinateMobilityEvidence] = []
graph_data = multibody.graphCoordData()
visited: set[int] = set()
try:
# Traverse providers rather than packed columns. This keeps a spherical
# subhinge's three Q rows together and prevents duplicate evidence.
for column in range(graph_data.nU()):
coordinate, _ = graph_data.coordAt(column)
try:
if coordinate.id() in visited or not isinstance(coordinate, kd.PhysicalSubhinge):
continue
visited.add(coordinate.id())
stored_limits = coordinate.getRangeLimits()
limits = np.asarray(
getattr(stored_limits, "magnitude", stored_limits),
dtype=float,
)
for offset, (lower, upper) in enumerate(limits):
if requested is not None and (coordinate.name(), offset) not in requested:
continue
span = upper - lower
# Finiteness is the evidence-quality gate: infinities mean
# at least one side remained open, while NaN means the
# calculation failed. Only two demonstrated boundaries may
# support the small-interval inertness conclusion.
if np.isfinite((lower, upper)).all() and 0.0 <= span <= tolerance:
evidence.append(
CoordinateMobilityEvidence(
coordinate.name(),
offset,
"INERT",
0,
0.0,
float(span),
True,
)
)
finally:
del coordinate
finally:
del graph_data
# Stable name/offset ordering makes direct API results and generated reports
# deterministic even when native graph traversal order changes.
evidence.sort(key=lambda item: (item.coordinate, item.dof_index))
return tuple(evidence)
[docs]
def findReducibleBallJoints(
multibody: kd.SubGraph,
selected_dofs: tuple[tuple[str, int], ...] | None = None,
*,
report_progress: bool = False,
_analysis: _MobilityAnalysisResult | None = None,
) -> tuple[BallReductionHint, ...]:
"""Return BALL joints whose local rotational subspace merits reduction.
Results are candidates rather than topology edits; range-wide fixed-axis
validation remains required before a BALL is replaced. This separation
keeps a future ``SubGraph`` method non-mutating and independently usable.
Parameters
----------
multibody: kd.SubGraph
SubGraph-compatible multibody to analyze.
selected_dofs: tuple[tuple[str, int], ...] | None
Optional coordinate-name and local-offset selections.
report_progress: bool
Whether to emit continuation progress.
Defaults to ``False``.
_analysis: _MobilityAnalysisResult | None
Optional shared internal result supplied by ``mobilityCheck``.
Returns
-------
tuple[BallReductionHint, ...]
Advisory spherical-joint reduction candidates.
"""
analysis = _analysis or _mobilityAnalysis(
multibody,
selected_dofs,
report_progress=report_progress,
)
return analysis.ball_hints
[docs]
def mobilityCheck(
multibody: kd.SubGraph,
selected_dofs: tuple[tuple[str, int], ...] | None = None,
*,
report_progress: bool = False,
) -> tuple[
tuple[CoordinateMobilityEvidence, ...],
tuple[BallReductionHint, ...],
tuple[LoopMobilityStatus, ...],
]:
"""Update ranges and combine inertness, BALL hints, and status records.
Parameters
----------
multibody: kd.SubGraph
SubGraph-compatible multibody to analyze.
selected_dofs: tuple[tuple[str, int], ...] | None
Optional coordinate-name and local-offset selections.
report_progress: bool
Whether to emit continuation progress.
Defaults to False.
Returns
-------
tuple[
tuple[CoordinateMobilityEvidence, ...],
tuple[BallReductionHint, ...],
tuple[LoopMobilityStatus, ...],
]
Inert-coordinate evidence, spherical reduction hints, and report-ready
coordinate statuses.
"""
# Analyze and persist ranges in one pass. The scanner captures each old
# limit before writing its measured result, so an unset input remains
# distinguishable from a range computed earlier in this command.
analysis = _mobilityAnalysis(
multibody,
selected_dofs,
report_progress=report_progress,
apply_limits=True,
)
_ranges, statuses = checkRangeLimits(
multibody,
selected_dofs,
report_progress=report_progress,
_analysis=analysis,
)
hints = findReducibleBallJoints(
multibody,
selected_dofs,
report_progress=report_progress,
_analysis=analysis,
)
# Keep the historical tuple shape consumed by the CLI report layer while
# allowing each lower-level operation to remain independently callable.
return analysis.evidence, hints, statuses
[docs]
def loopMobilityInfo(
subgraph: kd.SubGraph,
source: Path,
selected_dofs: tuple[tuple[str, int], ...] | None = None,
*,
report_progress: bool = False,
) -> str:
"""Update ranges and return a Markdown loop-coordinate mobility report.
The report combines continuation status, feasible spans, and advisory BALL
hints without editing topology. Optional selections restrict projection
work while retaining the same report schema.
Parameters
----------
subgraph: kd.SubGraph
Graph to analyze and update.
source: Path
Source-model path displayed in the report.
selected_dofs: tuple[tuple[str, int], ...] | None
Optional coordinate-name and local-offset selections.
report_progress: bool
Whether to emit continuation progress.
Defaults to False.
Returns
-------
str
Markdown mobility report text.
"""
# Keep Markdown policy outside the reusable numerical method so callers can
# consume typed results without fabricating a source path or parsing text.
_evidence, hints, statuses = mobilityCheck(
subgraph,
selected_dofs,
report_progress=report_progress,
)
counts: dict[str, int] = {}
for status in statuses:
counts[status.mobility] = counts.get(status.mobility, 0) + 1
summary = (
", ".join(f"{count} {mobility}" for mobility, count in sorted(counts.items()))
or "no loop-coordinate entries"
)
# Build sections as lines so stdout and file destinations receive identical
# Markdown, including final-newline behavior.
lines = [
"# Loop-mobility information",
"",
f"- Source model: `{source}`" if not os.getenv("DTEST_RUNNING", False) else "",
f"- Summary: {summary}",
"- Analysis mode: measured physical-subhinge ranges were updated",
"",
"## Loop-coordinate status",
"",
"Angular coordinates are `LOW` when their nonzero measured span is less than 10 degrees.",
"",
]
status_rows: list[tuple[str, ...]] = []
for status in statuses:
coordinate_type = _shortSubhingeType(
status.coordinate_type,
status.dof_index,
)
feasible_range = "—"
span = "—"
details = status.details
if status.feasible_range is not None:
lower, upper = status.feasible_range
magnitude = upper - lower
is_angular = status.coordinate_type in (
"PinSubhinge",
"SphericalSubhinge",
)
if is_angular:
lower, upper = _normalizedAngularInterval(lower, upper)
magnitude = upper - lower
# Four fixed decimals keep ranges compact and directly comparable.
feasible_range = f"[{lower:.4f}, {upper:.4f}]"
span = _formatRangeSpan(magnitude, is_angular)
status_rows.append(
(
_markdownCell(status.coordinate),
coordinate_type,
status.mobility,
feasible_range,
span,
_markdownCell(details),
)
)
lines.extend(
_markdownTable(
(
"Loop subhinge",
"Type/DOF",
"Mobility",
"Feasible range",
"Full span",
"Details",
),
status_rows,
frozenset((3, 4)),
)
)
if hints:
# Local BALL rank is advisory; actual reduction requires the separate
# range-wide fixed-axis validator.
lines.extend(
[
"",
"## BALL reduction candidates",
"",
"A local rank deficiency is a hint to run `loops --reduce-ball`; it is not proof that a fixed axis is globally inert.",
"",
]
)
hint_rows = []
for hint in sorted(hints, key=lambda item: item.coordinate):
hint_rows.append(
(
_markdownCell(hint.coordinate),
f"{hint.participation_rank}/{hint.coordinate_dofs}",
"Validate with `loops --reduce-ball`",
)
)
lines.extend(
_markdownTable(
(
"BALL spherical subhinge",
"Local participation rank",
"Suggested next step",
),
hint_rows,
frozenset((1,)),
)
)
lines.extend(
[
"",
"## Suggested next steps",
"",
"- Review `LOW` and `CONFIGURATION-DEPENDENT` entries before changing mobility.",
"- Remove only range-proven scalar `INERT` entries with `loops --remove-inert`.",
"- Test BALL candidates with `loops --reduce-ball`; incomplete proof leaves the BALL unchanged.",
"- Investigate `UNRESOLVED` entries manually before modifying their hinges.",
"",
]
)
return "\n".join(lines)
[docs]
def writeLoopMobilityInfo(
subgraph: kd.SubGraph,
source: Path,
destination: str,
selected_dofs: tuple[tuple[str, int], ...] | None = None,
) -> None:
"""Write a loop-mobility Markdown report to a file or standard output.
Parameters
----------
subgraph: kd.SubGraph
Graph to analyze and update.
source: Path
Source-model path displayed in the report.
destination: str
Output filename, or ``-`` for standard output.
selected_dofs: tuple[tuple[str, int], ...] | None
Optional coordinate-name and local-offset selections.
"""
# Range checks can take long enough that users need coordinate-level
# feedback before the complete Markdown report becomes available.
report = loopMobilityInfo(
subgraph,
source,
selected_dofs,
report_progress=True,
)
if destination == "-":
click.echo(report, nl=False)
return
# Path.write_text deliberately replaces the explicitly selected report file.
Path(destination).write_text(report, encoding="utf-8")
def _storedRangeSpan(lower: float, upper: float, subhinge_type: kd.SubhingeType) -> str:
"""Format a stored Q span in its physical units without changing its limits.
Parameters
----------
lower: float
Stored lower endpoint.
upper: float
Stored upper endpoint.
subhinge_type: kd.SubhingeType
Coordinate type determining the displayed unit.
Returns
-------
str
Formatted span, or an em dash when either endpoint is unavailable.
"""
if np.isnan([lower, upper]).any():
return "—"
# Report the stored width exactly so legacy invalid intervals remain visible.
span = upper - lower
if subhinge_type in (kd.SubhingeType.PIN, kd.SubhingeType.SPHERICAL, kd.SubhingeType.SCREW):
return _formatRangeSpan(span, True)
if subhinge_type in (kd.SubhingeType.LINEAR, kd.SubhingeType.LINEAR3):
return _formatRangeSpan(span, False)
# Quaternion Q components are dimensionless, not angles or translations.
return f"{span:.4f}"
def _subhingeTypeName(subhinge_type: kd.SubhingeType) -> str:
"""Return the kdFlex subhinge enum label used in report type columns.
Parameters
----------
subhinge_type: kd.SubhingeType
Native subhinge enumeration value.
Returns
-------
str
Enumeration member name used in reports.
"""
return subhinge_type.name
def _getVec(dark: QuantityOrValue[NDArray[np.float64]]) -> NDArray[np.float64]:
# strip out units if needed
if isinstance(dark, np.ndarray):
return dark
else:
return dark.m
[docs]
def loopStoredRangesInfo(subgraph: kd.SubGraph, source: Path) -> str:
"""Return stored range limits for every physical subhinge Q coordinate.
This inventory performs no closure solve and does not infer feasibility. It
reports exactly what the currently loaded model stores, making it suitable
for quickly checking the result of ``loops --ranges-update``.
Parameters
----------
subgraph: kd.SubGraph
Graph whose stored physical-subhinge limits are inventoried.
source: Path
Source-model path displayed in the report.
Returns
-------
str
Markdown inventory of stored range-limit rows.
"""
rows: list[tuple[str, ...]] = []
graph_data = subgraph.graphCoordData()
visited: set[int] = set()
try:
for column in range(graph_data.nU()):
coordinate, _ = graph_data.coordAt(column)
try:
if coordinate.id() in visited or not isinstance(coordinate, kd.PhysicalSubhinge):
continue
visited.add(coordinate.id())
# Keep stored endpoints unchanged; format the span in physical units.
# limits = np.asarray(coordinate.getRangeLimits(), dtype=float)
limits = _getVec(coordinate.getRangeLimits())
for offset, (lower, upper) in enumerate(limits):
rows.append(
(
_markdownCell(f"{coordinate.name()}/{offset}"),
_subhingeTypeName(coordinate.subhingeType()),
"—" if np.isnan(lower) else _formatRangeEndpoint(lower),
"—" if np.isnan(upper) else _formatRangeEndpoint(upper),
_storedRangeSpan(lower, upper, coordinate.subhingeType()),
)
)
finally:
del coordinate
finally:
del graph_data
# rows.sort(key=lambda row: row[0])
lines = [
"# Stored physical-subhinge Q ranges",
"",
f"- Source model: `{source}`" if not os.getenv("DTEST_RUNNING", False) else "",
"- Analysis mode: stored values only; no feasibility solve was run",
"- Bounds: radians for angles, meters for translations; quaternion components are dimensionless",
"- Spans: degrees for angles, meters for translations",
f"- Q-coordinate rows: {len(rows)}",
"",
]
lines.extend(
_markdownTable(
("Physical subhinge/Q", "Type", "Lower limit", "Upper limit", "Span"),
rows,
frozenset((2, 3, 4)),
)
)
lines.append("")
return "\n".join(lines)
[docs]
def writeLoopStoredRangesInfo(
subgraph: kd.SubGraph,
source: Path,
destination: str,
) -> None:
"""Write the stored Q-coordinate range inventory to a destination.
Parameters
----------
subgraph: kd.SubGraph
Graph whose stored ranges are inventoried.
source: Path
Source-model path displayed in the report.
destination: str
Output filename, or ``-`` for standard output.
"""
report = loopStoredRangesInfo(subgraph, source)
if destination == "-":
click.echo(report, nl=False)
return
Path(destination).write_text(report, encoding="utf-8")
[docs]
class LoopAnalyzer:
"""Provide reusable analysis services for one live closed-loop graph.
The class owns no native object and caches no configuration. Each operation
delegates to the functional analysis pipeline, which restores the
caller-visible graph state after its temporary continuation solves.
"""
def __init__(self, subgraph: kd.SubGraph) -> None:
"""Bind analysis operations to a graph without taking ownership.
Parameters
----------
subgraph: kd.SubGraph
Live graph retained by reference for later operations.
"""
# The caller retains the graph and controls its native-object lifetime.
self.subgraph = subgraph
[docs]
def scan(
self,
selected_names: tuple[str, ...] = ("all",),
) -> tuple[
tuple[FeasibleRangeLimit, ...],
tuple[CoordinateMobilityEvidence, ...],
tuple[BallReductionHint, ...],
tuple[LoopMobilityStatus, ...],
]:
"""Return ranges, inertness evidence, BALL hints, and status records.
``scan`` selects the non-mutating pipeline used by advisory tooling. The
four result collections are kept separate so callers can choose their
own presentation without parsing Markdown output.
Parameters
----------
selected_names: tuple[str, ...]
Requested subhinge names, or ``("all",)``.
Defaults to ``("all",)``.
Returns
-------
tuple[
tuple[FeasibleRangeLimit, ...],
tuple[CoordinateMobilityEvidence, ...],
tuple[BallReductionHint, ...],
tuple[LoopMobilityStatus, ...],
]
Range measurements, inertness evidence, spherical reduction hints,
and report-ready statuses.
"""
evidence: list[CoordinateMobilityEvidence] = []
hints: list[BallReductionHint] = []
statuses: list[LoopMobilityStatus] = []
ranges = _setLoopRangeLimits(
self.subgraph,
selected_names=selected_names,
evidence_output=evidence,
ball_hint_output=hints,
status_output=statuses,
apply_limits=False,
)
return ranges, tuple(evidence), tuple(hints), tuple(statuses)
[docs]
def updateRanges(self, selected_names: tuple[str, ...] = ("all",)) -> None:
"""Measure, report, and store Q range limits on the bound graph.
Parameters
----------
selected_names: tuple[str, ...]
Requested subhinge names, or ``("all",)``.
Defaults to ``("all",)``.
"""
# Retain the established live-progress output for long continuation runs.
updateLoopRangeLimits(self.subgraph, selected_names)
[docs]
def writeMobilityCheck(
self,
source: Path,
destination: str,
selected_dofs: tuple[tuple[str, int], ...] | None,
) -> None:
"""Update ranges and write the resulting mobility-analysis report.
Parameters
----------
source: Path
Source-model path displayed in the report.
destination: str
Output filename, or ``-`` for standard output.
selected_dofs: tuple[tuple[str, int], ...] | None
Optional coordinate-name and offset selections.
"""
writeLoopMobilityInfo(self.subgraph, source, destination, selected_dofs)
[docs]
def writeRangesInfo(self, source: Path, destination: str) -> None:
"""Write stored Q-coordinate ranges without running analysis.
Parameters
----------
source: Path
Source-model path displayed in the report.
destination: str
Output filename, or ``-`` for standard output.
"""
writeLoopStoredRangesInfo(self.subgraph, source, destination)
[docs]
class LoopModelEditor:
"""Apply validated topology and geometry changes to one serialized model.
Keeping mutation behind this class makes launcher control flow explicit and
prevents report-only operations from accidentally selecting an editing
path. Calls reload ``model_file`` and return a live model plus an audit
summary of accepted changes.
"""
def __init__(self, model_file: Path) -> None:
"""Retain the source path without loading or modifying the model.
Parameters
----------
model_file: Path
Serialized model used by later editing operations.
"""
self.model_file = model_file
[docs]
def simplify(
self,
remove_inert: tuple[str, ...],
reduce_ball: tuple[str, ...],
reduce_spherical: tuple[str, ...],
) -> tuple[LoadedModel, tuple[str, ...], tuple[str, ...]]:
"""Remove proven inert coordinates and reduce validated spherical hinges.
Parameters
----------
remove_inert: tuple[str, ...]
Requested scalar subhinge names to remove.
reduce_ball: tuple[str, ...]
Requested spherical subhinge names to reduce.
reduce_spherical: tuple[str, ...]
Requested spherical subhinge names to reduce in any parent hinge.
Returns
-------
tuple[LoadedModel, tuple[str, ...], tuple[str, ...]]
Reloaded model, applied geometry changes, and mobility changes.
"""
return loadSimplifiedModel(self.model_file, remove_inert, reduce_ball, reduce_spherical)
