98051ba0c9
- Move existing OndselSolver, GNN ML layer, and tooling into GNN/ directory for integration in later phases - Add Create addon scaffold: package.xml, Init.py - Add expression DAG with eval, symbolic diff, simplification - Add parameter table with fixed/free variable tracking - Add quaternion rotation as polynomial Expr trees - Add RigidBody entity (7 DOF: position + unit quaternion) - Add constraint classes: Coincident, DistancePointPoint, Fixed - Add Newton-Raphson solver with symbolic Jacobian + numpy lstsq - Add pre-solve passes: substitution + single-equation - Add DOF counting via Jacobian SVD rank - Add KindredSolver IKCSolver bridge for kcsolve integration - Add 82 unit tests covering all modules Registers as 'kindred' solver via kcsolve.register_solver() when loaded by Create's addon_loader.
46 lines
972 B
Python
46 lines
972 B
Python
"""Degrees-of-freedom counting via Jacobian rank."""
|
|
|
|
from __future__ import annotations
|
|
|
|
from typing import List
|
|
|
|
import numpy as np
|
|
|
|
from .expr import Expr
|
|
from .params import ParamTable
|
|
|
|
|
|
def count_dof(
|
|
residuals: List[Expr],
|
|
params: ParamTable,
|
|
rank_tol: float = 1e-8,
|
|
) -> int:
|
|
"""Compute DOF = n_free_params - rank(Jacobian).
|
|
|
|
Evaluates the Jacobian numerically at the current parameter values
|
|
and computes its rank via SVD.
|
|
"""
|
|
free = params.free_names()
|
|
n_free = len(free)
|
|
n_res = len(residuals)
|
|
|
|
if n_free == 0:
|
|
return 0
|
|
if n_res == 0:
|
|
return n_free
|
|
|
|
env = params.get_env()
|
|
|
|
J = np.empty((n_res, n_free))
|
|
for i, r in enumerate(residuals):
|
|
for j, name in enumerate(free):
|
|
J[i, j] = r.diff(name).simplify().eval(env)
|
|
|
|
if J.size == 0:
|
|
return n_free
|
|
|
|
sv = np.linalg.svd(J, compute_uv=False)
|
|
rank = int(np.sum(sv > rank_tol))
|
|
|
|
return n_free - rank
|