b4b8724ff1
- Add per-entity DOF analysis via Jacobian SVD (diagnostics.py) - Add overconstrained detection: redundant vs conflicting constraints - Add half-space tracking to preserve configuration branch (preference.py) - Add minimum-movement weighting for least-squares solve - Extend BFGS fallback with weight vector and quaternion renormalization - Add snapshot/restore and env accessor to ParamTable - Fix DistancePointPointConstraint sign for half-space tracking
300 lines
9.6 KiB
Python
300 lines
9.6 KiB
Python
"""Per-entity DOF diagnostics and overconstrained detection.
|
|
|
|
Provides per-body remaining degrees of freedom, human-readable free
|
|
motion labels, and redundant/conflicting constraint identification.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
from dataclasses import dataclass, field
|
|
from typing import List
|
|
|
|
import numpy as np
|
|
|
|
from .entities import RigidBody
|
|
from .expr import Expr
|
|
from .params import ParamTable
|
|
|
|
# -- Per-entity DOF -----------------------------------------------------------
|
|
|
|
|
|
@dataclass
|
|
class EntityDOF:
|
|
"""DOF report for a single entity (rigid body)."""
|
|
|
|
entity_id: str
|
|
remaining_dof: int # 0 = well-constrained
|
|
free_motions: list[str] = field(default_factory=list)
|
|
|
|
|
|
def per_entity_dof(
|
|
residuals: list[Expr],
|
|
params: ParamTable,
|
|
bodies: dict[str, RigidBody],
|
|
rank_tol: float = 1e-8,
|
|
) -> list[EntityDOF]:
|
|
"""Compute remaining DOF for each non-grounded body.
|
|
|
|
For each body, extracts the Jacobian columns corresponding to its
|
|
7 parameters, performs SVD to find constrained directions, and
|
|
classifies null-space vectors as translations or rotations.
|
|
"""
|
|
free = params.free_names()
|
|
n_res = len(residuals)
|
|
env = params.get_env()
|
|
|
|
if n_res == 0:
|
|
# No constraints — every free body has 6 DOF
|
|
result = []
|
|
for pid, body in bodies.items():
|
|
if body.grounded:
|
|
continue
|
|
result.append(
|
|
EntityDOF(
|
|
entity_id=pid,
|
|
remaining_dof=6,
|
|
free_motions=[
|
|
"translation along X",
|
|
"translation along Y",
|
|
"translation along Z",
|
|
"rotation about X",
|
|
"rotation about Y",
|
|
"rotation about Z",
|
|
],
|
|
)
|
|
)
|
|
return result
|
|
|
|
# Build column index mapping: param_name -> column index in free list
|
|
free_index = {name: i for i, name in enumerate(free)}
|
|
|
|
# Build full Jacobian (for efficiency, compute once)
|
|
n_free = len(free)
|
|
J_full = np.empty((n_res, n_free))
|
|
for i, r in enumerate(residuals):
|
|
for j, name in enumerate(free):
|
|
J_full[i, j] = r.diff(name).simplify().eval(env)
|
|
|
|
result = []
|
|
for pid, body in bodies.items():
|
|
if body.grounded:
|
|
continue
|
|
|
|
# Find column indices for this body's params
|
|
pfx = pid + "/"
|
|
body_param_names = [
|
|
pfx + "tx",
|
|
pfx + "ty",
|
|
pfx + "tz",
|
|
pfx + "qw",
|
|
pfx + "qx",
|
|
pfx + "qy",
|
|
pfx + "qz",
|
|
]
|
|
col_indices = [free_index[n] for n in body_param_names if n in free_index]
|
|
|
|
if not col_indices:
|
|
# All params fixed (shouldn't happen for non-grounded, but be safe)
|
|
result.append(EntityDOF(entity_id=pid, remaining_dof=0))
|
|
continue
|
|
|
|
# Extract submatrix: all residual rows, only this body's columns
|
|
J_sub = J_full[:, col_indices]
|
|
|
|
# SVD
|
|
U, sv, Vt = np.linalg.svd(J_sub, full_matrices=True)
|
|
constrained = int(np.sum(sv > rank_tol))
|
|
|
|
# Subtract 1 for the quaternion unit-norm constraint (already in residuals)
|
|
# The quat norm residual constrains 1 direction in the 7-D param space,
|
|
# so effective body DOF = 7 - 1 - constrained_by_other_constraints.
|
|
# But the quat norm IS one of the residual rows, so it's already counted
|
|
# in `constrained`. So: remaining = len(col_indices) - constrained
|
|
# But the quat norm takes 1 from 7 → 6 geometric DOF, and constrained
|
|
# includes the quat norm row. So remaining = 7 - constrained, which gives
|
|
# geometric remaining DOF directly.
|
|
remaining = len(col_indices) - constrained
|
|
|
|
# Classify null-space vectors as free motions
|
|
free_motions = []
|
|
if remaining > 0 and Vt.shape[0] > constrained:
|
|
null_space = Vt[constrained:] # rows = null vectors in param space
|
|
|
|
# Map column indices back to param types
|
|
param_types = []
|
|
for n in body_param_names:
|
|
if n in free_index:
|
|
if n.endswith(("/tx", "/ty", "/tz")):
|
|
param_types.append("t")
|
|
else:
|
|
param_types.append("q")
|
|
|
|
for null_vec in null_space:
|
|
label = _classify_motion(
|
|
null_vec, param_types, body_param_names, free_index
|
|
)
|
|
if label:
|
|
free_motions.append(label)
|
|
|
|
result.append(
|
|
EntityDOF(
|
|
entity_id=pid,
|
|
remaining_dof=remaining,
|
|
free_motions=free_motions,
|
|
)
|
|
)
|
|
|
|
return result
|
|
|
|
|
|
def _classify_motion(
|
|
null_vec: np.ndarray,
|
|
param_types: list[str],
|
|
body_param_names: list[str],
|
|
free_index: dict[str, int],
|
|
) -> str:
|
|
"""Classify a null-space vector as translation, rotation, or helical."""
|
|
# Split components into translation and rotation parts
|
|
trans_indices = [i for i, t in enumerate(param_types) if t == "t"]
|
|
rot_indices = [i for i, t in enumerate(param_types) if t == "q"]
|
|
|
|
trans_norm = np.linalg.norm(null_vec[trans_indices]) if trans_indices else 0.0
|
|
rot_norm = np.linalg.norm(null_vec[rot_indices]) if rot_indices else 0.0
|
|
|
|
total = trans_norm + rot_norm
|
|
if total < 1e-14:
|
|
return ""
|
|
|
|
trans_frac = trans_norm / total
|
|
rot_frac = rot_norm / total
|
|
|
|
# Determine dominant axis
|
|
if trans_frac > 0.8:
|
|
# Pure translation
|
|
axis = _dominant_axis(null_vec, trans_indices)
|
|
return f"translation along {axis}"
|
|
elif rot_frac > 0.8:
|
|
# Pure rotation
|
|
axis = _dominant_axis(null_vec, rot_indices)
|
|
return f"rotation about {axis}"
|
|
else:
|
|
# Mixed — helical
|
|
axis = _dominant_axis(null_vec, trans_indices)
|
|
return f"helical motion along {axis}"
|
|
|
|
|
|
def _dominant_axis(vec: np.ndarray, indices: list[int]) -> str:
|
|
"""Find the dominant axis (X/Y/Z) among the given component indices."""
|
|
if not indices:
|
|
return "?"
|
|
components = np.abs(vec[indices])
|
|
# Map to axis names — first 3 in group are X/Y/Z
|
|
axis_names = ["X", "Y", "Z"]
|
|
if len(components) >= 3:
|
|
idx = int(np.argmax(components[:3]))
|
|
return axis_names[idx]
|
|
elif len(components) == 1:
|
|
return axis_names[0]
|
|
else:
|
|
idx = int(np.argmax(components))
|
|
return axis_names[min(idx, 2)]
|
|
|
|
|
|
# -- Overconstrained detection ------------------------------------------------
|
|
|
|
|
|
@dataclass
|
|
class ConstraintDiag:
|
|
"""Diagnostic for a single constraint."""
|
|
|
|
constraint_index: int
|
|
kind: str # "redundant" | "conflicting"
|
|
detail: str
|
|
|
|
|
|
def find_overconstrained(
|
|
residuals: list[Expr],
|
|
params: ParamTable,
|
|
residual_ranges: list[tuple[int, int, int]],
|
|
rank_tol: float = 1e-8,
|
|
) -> list[ConstraintDiag]:
|
|
"""Identify redundant and conflicting constraints.
|
|
|
|
Algorithm (following SolvSpace's FindWhichToRemoveToFixJacobian):
|
|
1. Build full Jacobian J, compute rank.
|
|
2. If rank == n_residuals, not overconstrained — return empty.
|
|
3. For each constraint: remove its rows, check if rank is preserved
|
|
→ if so, the constraint is **redundant**.
|
|
4. Compute left null space, project residual vector F → if a
|
|
constraint's residuals contribute to this projection, it is
|
|
**conflicting** (redundant + unsatisfied).
|
|
"""
|
|
free = params.free_names()
|
|
n_free = len(free)
|
|
n_res = len(residuals)
|
|
|
|
if n_free == 0 or n_res == 0:
|
|
return []
|
|
|
|
env = params.get_env()
|
|
|
|
# Build Jacobian and residual vector
|
|
J = np.empty((n_res, n_free))
|
|
r_vec = np.empty(n_res)
|
|
for i, r in enumerate(residuals):
|
|
r_vec[i] = r.eval(env)
|
|
for j, name in enumerate(free):
|
|
J[i, j] = r.diff(name).simplify().eval(env)
|
|
|
|
# Full rank
|
|
sv_full = np.linalg.svd(J, compute_uv=False)
|
|
full_rank = int(np.sum(sv_full > rank_tol))
|
|
|
|
if full_rank >= n_res:
|
|
return [] # not overconstrained
|
|
|
|
# Left null space: columns of U beyond rank
|
|
U, sv, Vt = np.linalg.svd(J, full_matrices=True)
|
|
left_null = U[:, full_rank:] # shape (n_res, n_res - rank)
|
|
|
|
# Project residual onto left null space
|
|
null_residual = left_null.T @ r_vec # shape (n_res - rank,)
|
|
residual_projection = left_null @ null_residual # back to residual space
|
|
|
|
diags = []
|
|
for start, end, c_idx in residual_ranges:
|
|
# Remove this constraint's rows and check rank
|
|
mask = np.ones(n_res, dtype=bool)
|
|
mask[start:end] = False
|
|
J_reduced = J[mask]
|
|
|
|
if J_reduced.shape[0] == 0:
|
|
continue
|
|
|
|
sv_reduced = np.linalg.svd(J_reduced, compute_uv=False)
|
|
reduced_rank = int(np.sum(sv_reduced > rank_tol))
|
|
|
|
if reduced_rank >= full_rank:
|
|
# Removing this constraint preserves rank → redundant
|
|
# Check if it's also conflicting (contributes to unsatisfied null projection)
|
|
constraint_proj = np.linalg.norm(residual_projection[start:end])
|
|
if constraint_proj > rank_tol:
|
|
kind = "conflicting"
|
|
detail = (
|
|
f"Constraint {c_idx} is conflicting (redundant and unsatisfied)"
|
|
)
|
|
else:
|
|
kind = "redundant"
|
|
detail = (
|
|
f"Constraint {c_idx} is redundant (can be removed without effect)"
|
|
)
|
|
diags.append(
|
|
ConstraintDiag(
|
|
constraint_index=c_idx,
|
|
kind=kind,
|
|
detail=detail,
|
|
)
|
|
)
|
|
|
|
return diags
|