533ca91774
Add 18 new constraint classes covering all BaseJointKind types from Types.h: - Point: PointOnLine (2r), PointInPlane (1r) - Orientation: Parallel (2r), Perpendicular (1r), Angle (1r) - Surface: Concentric (4r), Tangent (1r), Planar (3r), LineInPlane (2r) - Kinematic: Ball (3r), Revolute (5r), Cylindrical (4r), Slider (5r), Screw (5r), Universal (4r) - Mechanical: Gear (1r), RackPinion (1r) - Stubs: Cam, Slot, DistanceCylSph New modules: - geometry.py: marker axis extraction, vector ops (dot3, cross3, sub3), geometric primitives (point_plane_distance, point_line_perp_components) - bfgs.py: L-BFGS-B fallback solver via scipy for when Newton fails solver.py changes: - Wire all 20 supported types in _build_constraint() - BFGS fallback after Newton-Raphson in solve() 183 tests passing (up from 82), including: - DOF counting for every joint type - Solve convergence from displaced initial conditions - Multi-body mechanisms (four-bar linkage, slider-crank, revolute chain)
71 lines
2.4 KiB
Python
71 lines
2.4 KiB
Python
"""Tests for the BFGS fallback solver."""
|
|
|
|
import math
|
|
|
|
import pytest
|
|
from kindred_solver.bfgs import bfgs_solve
|
|
from kindred_solver.expr import Const, Var
|
|
from kindred_solver.params import ParamTable
|
|
|
|
|
|
class TestBFGSBasic:
|
|
def test_single_linear(self):
|
|
"""Solve x - 3 = 0."""
|
|
pt = ParamTable()
|
|
x = pt.add("x", 0.0)
|
|
assert bfgs_solve([x - Const(3.0)], pt) is True
|
|
assert abs(pt.get_value("x") - 3.0) < 1e-8
|
|
|
|
def test_single_quadratic(self):
|
|
"""Solve x^2 - 4 = 0 from x=1 → x=2."""
|
|
pt = ParamTable()
|
|
x = pt.add("x", 1.0)
|
|
assert bfgs_solve([x * x - Const(4.0)], pt) is True
|
|
assert abs(pt.get_value("x") - 2.0) < 1e-8
|
|
|
|
def test_two_variables(self):
|
|
"""Solve x + y = 5, x - y = 1."""
|
|
pt = ParamTable()
|
|
x = pt.add("x", 0.0)
|
|
y = pt.add("y", 0.0)
|
|
assert bfgs_solve([x + y - Const(5.0), x - y - Const(1.0)], pt) is True
|
|
assert abs(pt.get_value("x") - 3.0) < 1e-8
|
|
assert abs(pt.get_value("y") - 2.0) < 1e-8
|
|
|
|
def test_empty_system(self):
|
|
pt = ParamTable()
|
|
assert bfgs_solve([], pt) is True
|
|
|
|
def test_with_quat_renorm(self):
|
|
"""Quaternion re-normalization during BFGS."""
|
|
pt = ParamTable()
|
|
qw = pt.add("qw", 0.9)
|
|
qx = pt.add("qx", 0.1)
|
|
qy = pt.add("qy", 0.1)
|
|
qz = pt.add("qz", 0.1)
|
|
r = qw * qw + qx * qx + qy * qy + qz * qz - Const(1.0)
|
|
groups = [("qw", "qx", "qy", "qz")]
|
|
assert bfgs_solve([r], pt, quat_groups=groups) is True
|
|
w, x, y, z = (pt.get_value(n) for n in ["qw", "qx", "qy", "qz"])
|
|
norm = math.sqrt(w**2 + x**2 + y**2 + z**2)
|
|
assert abs(norm - 1.0) < 1e-8
|
|
|
|
|
|
class TestBFGSGeometric:
|
|
def test_distance_constraint(self):
|
|
"""x^2 - 25 = 0 from x=3 → x=5."""
|
|
pt = ParamTable()
|
|
x = pt.add("x", 3.0)
|
|
assert bfgs_solve([x * x - Const(25.0)], pt) is True
|
|
assert abs(pt.get_value("x") - 5.0) < 1e-8
|
|
|
|
def test_difficult_initial_guess(self):
|
|
"""BFGS should handle worse initial guesses than Newton."""
|
|
pt = ParamTable()
|
|
x = pt.add("x", 100.0)
|
|
y = pt.add("y", -50.0)
|
|
residuals = [x + y - Const(5.0), x - y - Const(1.0)]
|
|
assert bfgs_solve(residuals, pt) is True
|
|
assert abs(pt.get_value("x") - 3.0) < 1e-6
|
|
assert abs(pt.get_value("y") - 2.0) < 1e-6
|