Merge pull request 'feat(solver): Phase 4+5 — diagnostics, preferences, assembly integration' (#34) from feat/phase5-assembly-integration into main

Reviewed-on: #34

kindred_solver/bfgs.py

@@ -28,11 +28,16 @@ def bfgs_solve(
     quat_groups: List[tuple[str, str, str, str]] | None = None,
     max_iter: int = 200,
     tol: float = 1e-10,
+    weight_vector: "np.ndarray | None" = None,
 ) -> bool:
     """Solve ``residuals == 0`` by minimizing sum of squared residuals.
 
     Falls back gracefully to False if scipy is not available.
 
+    When *weight_vector* is provided, residuals are scaled by
+    ``sqrt(w)`` so that the objective becomes
+    ``0.5 * sum(w_i * r_i^2)`` — equivalent to weighted least-squares.
+
     Returns True if converged (||r|| < tol).
     """
     if not _HAS_SCIPY:
@@ -53,7 +58,21 @@ def bfgs_solve(
             row.append(r.diff(name).simplify())
         jac_exprs.append(row)
 
-    def objective_and_grad(x_vec):
+    # Pre-compute scaling for weighted minimum-norm
+    if weight_vector is not None:
+        w_sqrt = np.sqrt(weight_vector)
+        w_inv_sqrt = 1.0 / w_sqrt
+    else:
+        w_sqrt = None
+        w_inv_sqrt = None
+
+    def objective_and_grad(y_vec):
+        # Transform back from scaled space if weighted
+        if w_inv_sqrt is not None:
+            x_vec = y_vec * w_inv_sqrt
+        else:
+            x_vec = y_vec
+
         # Update params
         params.set_free_vector(x_vec)
         if quat_groups:
@@ -71,23 +90,38 @@ def bfgs_solve(
             for j in range(n_free):
                 J[i, j] = jac_exprs[i][j].eval(env)
 
-        # Gradient of f = sum(r_i * dr_i/dx_j) = J^T @ r
+        # Gradient of f w.r.t. x = J^T @ r
-        grad = J.T @ r_vals
+        grad_x = J.T @ r_vals
+
+        # Chain rule: df/dy = df/dx * dx/dy = grad_x * w_inv_sqrt
+        if w_inv_sqrt is not None:
+            grad = grad_x * w_inv_sqrt
+        else:
+            grad = grad_x
 
         return f, grad
 
     x0 = params.get_free_vector().copy()
 
+    # Transform initial guess to scaled space
+    if w_sqrt is not None:
+        y0 = x0 * w_sqrt
+    else:
+        y0 = x0
+
     result = _scipy_minimize(
         objective_and_grad,
-        x0,
+        y0,
         method="L-BFGS-B",
         jac=True,
         options={"maxiter": max_iter, "ftol": tol * tol, "gtol": tol},
     )
 
-    # Apply final result
+    # Apply final result (transform back from scaled space)
-    params.set_free_vector(result.x)
+    if w_inv_sqrt is not None:
+        params.set_free_vector(result.x * w_inv_sqrt)
+    else:
+        params.set_free_vector(result.x)
     if quat_groups:
         _renormalize_quats(params, quat_groups)
 

kindred_solver/constraints.py

@@ -77,9 +77,15 @@ class DistancePointPointConstraint(ConstraintBase):
         self.marker_j_pos = marker_j_pos
         self.distance = distance
 
+    def world_points(self) -> tuple[tuple[Expr, Expr, Expr], tuple[Expr, Expr, Expr]]:
+        """Return (world_point_i, world_point_j) expression tuples."""
+        return (
+            self.body_i.world_point(*self.marker_i_pos),
+            self.body_j.world_point(*self.marker_j_pos),
+        )
+
     def residuals(self) -> List[Expr]:
-        wx_i, wy_i, wz_i = self.body_i.world_point(*self.marker_i_pos)
+        (wx_i, wy_i, wz_i), (wx_j, wy_j, wz_j) = self.world_points()
-        wx_j, wy_j, wz_j = self.body_j.world_point(*self.marker_j_pos)
         dx = wx_i - wx_j
         dy = wy_i - wy_j
         dz = wz_i - wz_j

kindred_solver/diagnostics.py

@@ -0,0 +1,299 @@
+"""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

kindred_solver/newton.py

@@ -17,6 +17,8 @@ def newton_solve(
     quat_groups: List[tuple[str, str, str, str]] | None = None,
     max_iter: int = 50,
     tol: float = 1e-10,
+    post_step: "Callable[[ParamTable], None] | None" = None,
+    weight_vector: "np.ndarray | None" = None,
 ) -> bool:
     """Solve ``residuals == 0`` by Newton-Raphson.
 
@@ -33,6 +35,14 @@ def newton_solve(
         Maximum Newton iterations.
     tol:
         Convergence threshold on ``||r||``.
+    post_step:
+        Optional callback invoked after each parameter update, before
+        quaternion renormalization.  Used for half-space correction.
+    weight_vector:
+        Optional 1-D array of length ``n_free``.  When provided, the
+        lstsq step is column-scaled to produce the weighted
+        minimum-norm solution (prefer small movements in
+        high-weight parameters).
 
     Returns True if converged within *max_iter* iterations.
     """
@@ -67,13 +77,24 @@ def newton_solve(
                 J[i, j] = jac_exprs[i][j].eval(env)
 
         # Solve J @ dx = -r  (least-squares handles rank-deficient)
-        dx, _, _, _ = np.linalg.lstsq(J, -r_vec, rcond=None)
+        if weight_vector is not None:
+            # Column-scale J by W^{-1/2} for weighted minimum-norm
+            w_inv_sqrt = 1.0 / np.sqrt(weight_vector)
+            J_scaled = J * w_inv_sqrt[np.newaxis, :]
+            dx_scaled, _, _, _ = np.linalg.lstsq(J_scaled, -r_vec, rcond=None)
+            dx = dx_scaled * w_inv_sqrt
+        else:
+            dx, _, _, _ = np.linalg.lstsq(J, -r_vec, rcond=None)
 
         # Update parameters
         x = params.get_free_vector()
         x += dx
         params.set_free_vector(x)
 
+        # Half-space correction (before quat renormalization)
+        if post_step:
+            post_step(params)
+
         # Re-normalize quaternions
         if quat_groups:
             _renormalize_quats(params, quat_groups)

kindred_solver/params.py

@@ -81,3 +81,26 @@ class ParamTable:
         """Bulk-update free parameters from a 1-D array."""
         for i, name in enumerate(self._free_order):
             self._values[name] = float(vec[i])
+
+    def snapshot(self) -> Dict[str, float]:
+        """Capture current values as a checkpoint."""
+        return dict(self._values)
+
+    def restore(self, snap: Dict[str, float]):
+        """Restore parameter values from a checkpoint."""
+        for name, val in snap.items():
+            if name in self._values:
+                self._values[name] = val
+
+    def movement_cost(
+        self,
+        start: Dict[str, float],
+        weights: Dict[str, float] | None = None,
+    ) -> float:
+        """Weighted sum of squared displacements from start."""
+        cost = 0.0
+        for name in self._free_order:
+            w = weights.get(name, 1.0) if weights else 1.0
+            delta = self._values[name] - start.get(name, self._values[name])
+            cost += delta * delta * w
+        return cost

kindred_solver/preference.py

@@ -0,0 +1,325 @@
+"""Solution preference: half-space tracking and minimum-movement weighting.
+
+Half-space tracking preserves the initial configuration branch across
+Newton iterations.  For constraints with multiple valid solutions
+(e.g. distance can be satisfied on either side), we record which
+"half-space" the initial state lives in and correct the solver step
+if it crosses to the wrong branch.
+
+Minimum-movement weighting scales the Newton/BFGS step so that
+quaternion parameters (rotation) are penalised more than translation
+parameters, yielding the physically-nearest solution.
+"""
+
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass, field
+from typing import Callable, List
+
+import numpy as np
+
+from .constraints import (
+    AngleConstraint,
+    ConstraintBase,
+    DistancePointPointConstraint,
+    ParallelConstraint,
+    PerpendicularConstraint,
+)
+from .geometry import cross3, dot3, marker_z_axis
+from .params import ParamTable
+
+
+@dataclass
+class HalfSpace:
+    """Tracks which branch of a branching constraint the solution should stay in."""
+
+    constraint_index: int  # index in ctx.constraints
+    reference_sign: float  # +1.0 or -1.0, captured at setup
+    indicator_fn: Callable[[dict[str, float]], float]  # returns signed value
+    param_names: list[str] = field(default_factory=list)  # params to flip
+    correction_fn: Callable[[ParamTable, float], None] | None = None
+
+
+def compute_half_spaces(
+    constraint_objs: list[ConstraintBase],
+    constraint_indices: list[int],
+    params: ParamTable,
+) -> list[HalfSpace]:
+    """Build half-space trackers for all branching constraints.
+
+    Evaluates each constraint's indicator function at the current
+    parameter values to capture the reference sign.
+    """
+    env = params.get_env()
+    half_spaces: list[HalfSpace] = []
+
+    for i, obj in enumerate(constraint_objs):
+        hs = _build_half_space(obj, constraint_indices[i], env, params)
+        if hs is not None:
+            half_spaces.append(hs)
+
+    return half_spaces
+
+
+def apply_half_space_correction(
+    params: ParamTable,
+    half_spaces: list[HalfSpace],
+) -> None:
+    """Check each half-space and correct if the solver crossed a branch.
+
+    Called as a post_step callback from newton_solve.
+    """
+    if not half_spaces:
+        return
+
+    env = params.get_env()
+    for hs in half_spaces:
+        current_val = hs.indicator_fn(env)
+        current_sign = (
+            math.copysign(1.0, current_val)
+            if abs(current_val) > 1e-14
+            else hs.reference_sign
+        )
+        if current_sign != hs.reference_sign and hs.correction_fn is not None:
+            hs.correction_fn(params, current_val)
+            # Re-read env after correction for subsequent half-spaces
+            env = params.get_env()
+
+
+def _build_half_space(
+    obj: ConstraintBase,
+    constraint_idx: int,
+    env: dict[str, float],
+    params: ParamTable,
+) -> HalfSpace | None:
+    """Build a HalfSpace for a branching constraint, or None if not branching."""
+
+    if isinstance(obj, DistancePointPointConstraint) and obj.distance > 0:
+        return _distance_half_space(obj, constraint_idx, env, params)
+
+    if isinstance(obj, ParallelConstraint):
+        return _parallel_half_space(obj, constraint_idx, env, params)
+
+    if isinstance(obj, AngleConstraint):
+        return _angle_half_space(obj, constraint_idx, env, params)
+
+    if isinstance(obj, PerpendicularConstraint):
+        return _perpendicular_half_space(obj, constraint_idx, env, params)
+
+    return None
+
+
+def _distance_half_space(
+    obj: DistancePointPointConstraint,
+    constraint_idx: int,
+    env: dict[str, float],
+    params: ParamTable,
+) -> HalfSpace | None:
+    """Half-space for DistancePointPoint: track displacement direction.
+
+    The indicator is the dot product of the current displacement with
+    the reference displacement direction.  If the solver flips to the
+    opposite side, we reflect the moving body's position.
+    """
+    p_i, p_j = obj.world_points()
+
+    # Evaluate reference displacement direction
+    dx = p_i[0].eval(env) - p_j[0].eval(env)
+    dy = p_i[1].eval(env) - p_j[1].eval(env)
+    dz = p_i[2].eval(env) - p_j[2].eval(env)
+    dist = math.sqrt(dx * dx + dy * dy + dz * dz)
+
+    if dist < 1e-14:
+        return None  # points coincident, no branch to track
+
+    # Reference unit direction
+    nx, ny, nz = dx / dist, dy / dist, dz / dist
+
+    # Build indicator: dot(displacement, reference_direction)
+    # Use Expr evaluation for speed
+    disp_x, disp_y, disp_z = p_i[0] - p_j[0], p_i[1] - p_j[1], p_i[2] - p_j[2]
+
+    def indicator(e: dict[str, float]) -> float:
+        return disp_x.eval(e) * nx + disp_y.eval(e) * ny + disp_z.eval(e) * nz
+
+    ref_sign = math.copysign(1.0, indicator(env))
+
+    # Correction: reflect body_j position along reference direction
+    # (or body_i if body_j is grounded)
+    moving_body = obj.body_j if not obj.body_j.grounded else obj.body_i
+    if moving_body.grounded:
+        return None  # both grounded, nothing to correct
+
+    px_name = f"{moving_body.part_id}/tx"
+    py_name = f"{moving_body.part_id}/ty"
+    pz_name = f"{moving_body.part_id}/tz"
+
+    sign_flip = -1.0 if moving_body is obj.body_j else 1.0
+
+    def correction(p: ParamTable, _val: float) -> None:
+        # Reflect displacement: negate the component along reference direction
+        e = p.get_env()
+        cur_dx = disp_x.eval(e)
+        cur_dy = disp_y.eval(e)
+        cur_dz = disp_z.eval(e)
+        # Project displacement onto reference direction
+        proj = cur_dx * nx + cur_dy * ny + cur_dz * nz
+        # Reflect: subtract 2*proj*n from the moving body's position
+        if not p.is_fixed(px_name):
+            p.set_value(px_name, p.get_value(px_name) + sign_flip * 2.0 * proj * nx)
+        if not p.is_fixed(py_name):
+            p.set_value(py_name, p.get_value(py_name) + sign_flip * 2.0 * proj * ny)
+        if not p.is_fixed(pz_name):
+            p.set_value(pz_name, p.get_value(pz_name) + sign_flip * 2.0 * proj * nz)
+
+    return HalfSpace(
+        constraint_index=constraint_idx,
+        reference_sign=ref_sign,
+        indicator_fn=indicator,
+        param_names=[px_name, py_name, pz_name],
+        correction_fn=correction,
+    )
+
+
+def _parallel_half_space(
+    obj: ParallelConstraint,
+    constraint_idx: int,
+    env: dict[str, float],
+    params: ParamTable,
+) -> HalfSpace:
+    """Half-space for Parallel: track same-direction vs opposite-direction.
+
+    Indicator: dot(z_i, z_j).  Positive = same direction, negative = opposite.
+    """
+    z_i = marker_z_axis(obj.body_i, obj.marker_i_quat)
+    z_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
+    dot_expr = dot3(z_i, z_j)
+
+    def indicator(e: dict[str, float]) -> float:
+        return dot_expr.eval(e)
+
+    ref_val = indicator(env)
+    ref_sign = math.copysign(1.0, ref_val) if abs(ref_val) > 1e-14 else 1.0
+
+    # No geometric correction — just let the indicator track.
+    # The Newton solver naturally handles this via the cross-product residual.
+    # We only need to detect and report branch flips.
+    return HalfSpace(
+        constraint_index=constraint_idx,
+        reference_sign=ref_sign,
+        indicator_fn=indicator,
+    )
+
+
+# ============================================================================
+# Minimum-movement weighting
+# ============================================================================
+
+# Scale factor so that a 1-radian rotation is penalised as much as a
+# (180/pi)-unit translation.  This makes the weighted minimum-norm
+# step prefer translating over rotating for the same residual reduction.
+QUAT_WEIGHT = (180.0 / math.pi) ** 2  # ~3283
+
+
+def build_weight_vector(params: ParamTable) -> np.ndarray:
+    """Build diagonal weight vector: 1.0 for translation, QUAT_WEIGHT for quaternion.
+
+    Returns a 1-D array of length ``params.n_free()``.
+    """
+    free = params.free_names()
+    w = np.ones(len(free))
+    quat_suffixes = ("/qw", "/qx", "/qy", "/qz")
+    for i, name in enumerate(free):
+        if any(name.endswith(s) for s in quat_suffixes):
+            w[i] = QUAT_WEIGHT
+    return w
+
+
+def _angle_half_space(
+    obj: AngleConstraint,
+    constraint_idx: int,
+    env: dict[str, float],
+    params: ParamTable,
+) -> HalfSpace | None:
+    """Half-space for Angle: track sign of sin(angle) via cross product.
+
+    For angle constraints, the dot product is fixed (= cos(angle)),
+    but sin can be +/-. We track the cross product magnitude sign.
+    """
+    if abs(obj.angle) < 1e-14 or abs(obj.angle - math.pi) < 1e-14:
+        return None  # 0 or 180 degrees — no branch ambiguity
+
+    z_i = marker_z_axis(obj.body_i, obj.marker_i_quat)
+    z_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
+    cx, cy, cz = cross3(z_i, z_j)
+
+    # Use the magnitude of the cross product's z-component as indicator
+    # (or whichever component is largest at setup time)
+    cx_val = cx.eval(env)
+    cy_val = cy.eval(env)
+    cz_val = cz.eval(env)
+
+    # Pick the dominant cross product component
+    components = [
+        (abs(cx_val), cx, cx_val),
+        (abs(cy_val), cy, cy_val),
+        (abs(cz_val), cz, cz_val),
+    ]
+    _, best_expr, best_val = max(components, key=lambda t: t[0])
+
+    if abs(best_val) < 1e-14:
+        return None
+
+    def indicator(e: dict[str, float]) -> float:
+        return best_expr.eval(e)
+
+    ref_sign = math.copysign(1.0, best_val)
+
+    return HalfSpace(
+        constraint_index=constraint_idx,
+        reference_sign=ref_sign,
+        indicator_fn=indicator,
+    )
+
+
+def _perpendicular_half_space(
+    obj: PerpendicularConstraint,
+    constraint_idx: int,
+    env: dict[str, float],
+    params: ParamTable,
+) -> HalfSpace | None:
+    """Half-space for Perpendicular: track which quadrant.
+
+    The dot product is constrained to 0, but the cross product sign
+    distinguishes which "side" of perpendicular.
+    """
+    z_i = marker_z_axis(obj.body_i, obj.marker_i_quat)
+    z_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
+    cx, cy, cz = cross3(z_i, z_j)
+
+    # Pick the dominant cross product component
+    cx_val = cx.eval(env)
+    cy_val = cy.eval(env)
+    cz_val = cz.eval(env)
+
+    components = [
+        (abs(cx_val), cx, cx_val),
+        (abs(cy_val), cy, cy_val),
+        (abs(cz_val), cz, cz_val),
+    ]
+    _, best_expr, best_val = max(components, key=lambda t: t[0])
+
+    if abs(best_val) < 1e-14:
+        return None
+
+    def indicator(e: dict[str, float]) -> float:
+        return best_expr.eval(e)
+
+    ref_sign = math.copysign(1.0, best_val)
+
+    return HalfSpace(
+        constraint_index=constraint_idx,
+        reference_sign=ref_sign,
+        indicator_fn=indicator,
+    )

kindred_solver/solver.py

@@ -33,10 +33,16 @@ from .constraints import (
     UniversalConstraint,
 )
 from .decompose import decompose, solve_decomposed
+from .diagnostics import find_overconstrained
 from .dof import count_dof
 from .entities import RigidBody
 from .newton import newton_solve
 from .params import ParamTable
+from .preference import (
+    apply_half_space_correction,
+    build_weight_vector,
+    compute_half_spaces,
+)
 from .prepass import single_equation_pass, substitution_pass
 
 # Assemblies with fewer free bodies than this use the monolithic path.
@@ -76,124 +82,282 @@ _SUPPORTED = {
 class KindredSolver(kcsolve.IKCSolver):
     """Expression-based Newton-Raphson constraint solver."""
 
+    def __init__(self):
+        super().__init__()
+        self._drag_ctx = None
+        self._drag_parts = None
+        self._limits_warned = False
+
     def name(self):
         return "Kindred (Newton-Raphson)"
 
     def supported_joints(self):
         return list(_SUPPORTED)
 
+    # ── Static solve ────────────────────────────────────────────────
+
     def solve(self, ctx):
-        params = ParamTable()
+        system = _build_system(ctx)
-        bodies = {}  # part_id -> RigidBody
 
-        # 1. Build entities from parts
+        # Warn once per solver instance if any constraints have limits
-        for part in ctx.parts:
+        if not self._limits_warned:
-            pos = tuple(part.placement.position)
+            for c in ctx.constraints:
-            quat = tuple(part.placement.quaternion)  # (w, x, y, z)
+                if c.limits:
-            body = RigidBody(
+                    import logging
-                part.id,
-                params,
-                position=pos,
-                quaternion=quat,
-                grounded=part.grounded,
-            )
-            bodies[part.id] = body
 
-        # 2. Build constraint residuals (track index mapping for decomposition)
+                    logging.getLogger(__name__).warning(
-        all_residuals = []
+                        "Joint limits on '%s' ignored "
-        constraint_objs = []
+                        "(not yet supported by Kindred solver)",
-        constraint_indices = []  # parallel to constraint_objs: index in ctx.constraints
+                        c.id,
+                    )
+                    self._limits_warned = True
+                    break
 
-        for idx, c in enumerate(ctx.constraints):
+        # Solution preference: half-spaces, weight vector
-            if not c.activated:
+        half_spaces = compute_half_spaces(
-                continue
+            system.constraint_objs,
-            body_i = bodies.get(c.part_i)
+            system.constraint_indices,
-            body_j = bodies.get(c.part_j)
+            system.params,
-            if body_i is None or body_j is None:
+        )
-                continue
+        weight_vec = build_weight_vector(system.params)
 
-            marker_i_pos = tuple(c.marker_i.position)
+        if half_spaces:
-            marker_j_pos = tuple(c.marker_j.position)
+            post_step_fn = lambda p: apply_half_space_correction(p, half_spaces)
+        else:
+            post_step_fn = None
 
-            obj = _build_constraint(
+        # Pre-passes on full system
-                c.type,
+        residuals = substitution_pass(system.all_residuals, system.params)
-                body_i,
+        residuals = single_equation_pass(residuals, system.params)
-                marker_i_pos,
-                body_j,
-                marker_j_pos,
-                c.marker_i,
-                c.marker_j,
-                c.params,
-            )
-            if obj is None:
-                continue
-            constraint_objs.append(obj)
-            constraint_indices.append(idx)
-            all_residuals.extend(obj.residuals())
 
-        # 3. Add quaternion normalization residuals for non-grounded bodies
+        # Solve (decomposed for large assemblies, monolithic for small)
-        quat_groups = []
+        n_free_bodies = sum(1 for b in system.bodies.values() if not b.grounded)
-        for body in bodies.values():
-            if not body.grounded:
-                all_residuals.append(body.quat_norm_residual())
-                quat_groups.append(body.quat_param_names())
-
-        # 4. Pre-passes on full system
-        all_residuals = substitution_pass(all_residuals, params)
-        all_residuals = single_equation_pass(all_residuals, params)
-
-        # 5. Solve (decomposed for large assemblies, monolithic for small)
-        n_free_bodies = sum(1 for b in bodies.values() if not b.grounded)
         if n_free_bodies >= _DECOMPOSE_THRESHOLD:
-            grounded_ids = {pid for pid, b in bodies.items() if b.grounded}
+            grounded_ids = {pid for pid, b in system.bodies.items() if b.grounded}
             clusters = decompose(ctx.constraints, grounded_ids)
             if len(clusters) > 1:
                 converged = solve_decomposed(
                     clusters,
-                    bodies,
+                    system.bodies,
-                    constraint_objs,
+                    system.constraint_objs,
-                    constraint_indices,
+                    system.constraint_indices,
-                    params,
+                    system.params,
                 )
             else:
                 converged = _monolithic_solve(
-                    all_residuals,
+                    residuals,
-                    params,
+                    system.params,
-                    quat_groups,
+                    system.quat_groups,
+                    post_step=post_step_fn,
+                    weight_vector=weight_vec,
                 )
         else:
-            converged = _monolithic_solve(all_residuals, params, quat_groups)
+            converged = _monolithic_solve(
+                residuals,
+                system.params,
+                system.quat_groups,
+                post_step=post_step_fn,
+                weight_vector=weight_vec,
+            )
 
-        # 6. DOF
+        # DOF
-        dof = count_dof(all_residuals, params)
+        dof = count_dof(residuals, system.params)
 
-        # 7. Build result
+        # Build result
         result = kcsolve.SolveResult()
         result.status = (
             kcsolve.SolveStatus.Success if converged else kcsolve.SolveStatus.Failed
         )
         result.dof = dof
 
-        env = params.get_env()
+        # Diagnostics on failure
-        placements = []
+        if not converged:
-        for body in bodies.values():
+            result.diagnostics = _run_diagnostics(
-            if body.grounded:
+                residuals,
-                continue
+                system.params,
-            pr = kcsolve.SolveResult.PartResult()
+                system.residual_ranges,
-            pr.id = body.part_id
+                ctx,
-            pr.placement = kcsolve.Transform()
+            )
-            pr.placement.position = list(body.extract_position(env))
-            pr.placement.quaternion = list(body.extract_quaternion(env))
-            placements.append(pr)
 
-        result.placements = placements
+        result.placements = _extract_placements(system.params, system.bodies)
         return result
 
+    # ── Incremental update ──────────────────────────────────────────
+    # The base class default (delegates to solve()) is correct here:
+    # solve() uses current placements as initial guess, so small
+    # parameter changes converge quickly without special handling.
+
+    # ── Interactive drag ────────────────────────────────────────────
+
+    def pre_drag(self, ctx, drag_parts):
+        self._drag_ctx = ctx
+        self._drag_parts = set(drag_parts)
+        return self.solve(ctx)
+
+    def drag_step(self, drag_placements):
+        ctx = self._drag_ctx
+        if ctx is None:
+            return kcsolve.SolveResult()
+        for pr in drag_placements:
+            for part in ctx.parts:
+                if part.id == pr.id:
+                    part.placement = pr.placement
+                    break
+        return self.solve(ctx)
+
+    def post_drag(self):
+        self._drag_ctx = None
+        self._drag_parts = None
+
+    # ── Diagnostics ─────────────────────────────────────────────────
+
+    def diagnose(self, ctx):
+        system = _build_system(ctx)
+        residuals = substitution_pass(system.all_residuals, system.params)
+        return _run_diagnostics(
+            residuals,
+            system.params,
+            system.residual_ranges,
+            ctx,
+        )
+
     def is_deterministic(self):
         return True
 
 
-def _monolithic_solve(all_residuals, params, quat_groups):
+class _System:
+    """Intermediate representation of a built constraint system."""
+
+    __slots__ = (
+        "params",
+        "bodies",
+        "constraint_objs",
+        "constraint_indices",
+        "all_residuals",
+        "residual_ranges",
+        "quat_groups",
+    )
+
+
+def _build_system(ctx):
+    """Build the solver's internal representation from a SolveContext.
+
+    Returns a _System with params, bodies, constraint objects,
+    residuals, residual-to-constraint mapping, and quaternion groups.
+    """
+    system = _System()
+    params = ParamTable()
+    bodies = {}  # part_id -> RigidBody
+
+    # 1. Build entities from parts
+    for part in ctx.parts:
+        pos = tuple(part.placement.position)
+        quat = tuple(part.placement.quaternion)  # (w, x, y, z)
+        body = RigidBody(
+            part.id,
+            params,
+            position=pos,
+            quaternion=quat,
+            grounded=part.grounded,
+        )
+        bodies[part.id] = body
+
+    # 2. Build constraint residuals (track index mapping for decomposition)
+    all_residuals = []
+    constraint_objs = []
+    constraint_indices = []  # parallel to constraint_objs: index in ctx.constraints
+
+    for idx, c in enumerate(ctx.constraints):
+        if not c.activated:
+            continue
+        body_i = bodies.get(c.part_i)
+        body_j = bodies.get(c.part_j)
+        if body_i is None or body_j is None:
+            continue
+
+        marker_i_pos = tuple(c.marker_i.position)
+        marker_j_pos = tuple(c.marker_j.position)
+
+        obj = _build_constraint(
+            c.type,
+            body_i,
+            marker_i_pos,
+            body_j,
+            marker_j_pos,
+            c.marker_i,
+            c.marker_j,
+            c.params,
+        )
+        if obj is None:
+            continue
+        constraint_objs.append(obj)
+        constraint_indices.append(idx)
+        all_residuals.extend(obj.residuals())
+
+    # 3. Build residual-to-constraint mapping
+    residual_ranges = []  # (start_row, end_row, constraint_idx)
+    row = 0
+    for i, obj in enumerate(constraint_objs):
+        n = len(obj.residuals())
+        residual_ranges.append((row, row + n, constraint_indices[i]))
+        row += n
+
+    # 4. Add quaternion normalization residuals for non-grounded bodies
+    quat_groups = []
+    for body in bodies.values():
+        if not body.grounded:
+            all_residuals.append(body.quat_norm_residual())
+            quat_groups.append(body.quat_param_names())
+
+    system.params = params
+    system.bodies = bodies
+    system.constraint_objs = constraint_objs
+    system.constraint_indices = constraint_indices
+    system.all_residuals = all_residuals
+    system.residual_ranges = residual_ranges
+    system.quat_groups = quat_groups
+    return system
+
+
+def _run_diagnostics(residuals, params, residual_ranges, ctx):
+    """Run overconstrained detection and return kcsolve diagnostics."""
+    diagnostics = []
+    if not hasattr(kcsolve, "ConstraintDiagnostic"):
+        return diagnostics
+
+    diags = find_overconstrained(residuals, params, residual_ranges)
+    for d in diags:
+        cd = kcsolve.ConstraintDiagnostic()
+        cd.constraint_id = ctx.constraints[d.constraint_index].id
+        cd.kind = (
+            kcsolve.DiagnosticKind.Redundant
+            if d.kind == "redundant"
+            else kcsolve.DiagnosticKind.Conflicting
+        )
+        cd.detail = d.detail
+        diagnostics.append(cd)
+    return diagnostics
+
+
+def _extract_placements(params, bodies):
+    """Extract solved placements from the parameter table."""
+    env = params.get_env()
+    placements = []
+    for body in bodies.values():
+        if body.grounded:
+            continue
+        pr = kcsolve.SolveResult.PartResult()
+        pr.id = body.part_id
+        pr.placement = kcsolve.Transform()
+        pr.placement.position = list(body.extract_position(env))
+        pr.placement.quaternion = list(body.extract_quaternion(env))
+        placements.append(pr)
+    return placements
+
+
+def _monolithic_solve(
+    all_residuals, params, quat_groups, post_step=None, weight_vector=None
+):
     """Newton-Raphson solve with BFGS fallback on the full system."""
     converged = newton_solve(
         all_residuals,
@@ -201,6 +365,8 @@ def _monolithic_solve(all_residuals, params, quat_groups):
         quat_groups=quat_groups,
         max_iter=100,
         tol=1e-10,
+        post_step=post_step,
+        weight_vector=weight_vector,
     )
     if not converged:
         converged = bfgs_solve(
@@ -209,6 +375,7 @@ def _monolithic_solve(all_residuals, params, quat_groups):
             quat_groups=quat_groups,
             max_iter=200,
             tol=1e-10,
+            weight_vector=weight_vector,
         )
     return converged
 

tests/console_test_phase5.py

@@ -0,0 +1,355 @@
+"""
+Phase 5 in-client console tests.
+
+Paste into the FreeCAD Python console (or run via: exec(open(...).read())).
+Tests the full Assembly -> KindredSolver pipeline without the unittest harness.
+
+Expected output: all lines print PASS. Any FAIL indicates a regression.
+"""
+
+import FreeCAD as App
+import JointObject
+import kcsolve
+
+_pref = App.ParamGet("User parameter:BaseApp/Preferences/Mod/Assembly")
+_orig_solver = _pref.GetString("Solver", "")
+
+_results = []
+
+
+def _report(name, passed, detail=""):
+    status = "PASS" if passed else "FAIL"
+    msg = f"  [{status}] {name}"
+    if detail:
+        msg += f" — {detail}"
+    print(msg)
+    _results.append((name, passed))
+
+
+def _new_doc(name="Phase5Test"):
+    if App.ActiveDocument and App.ActiveDocument.Name == name:
+        App.closeDocument(name)
+    App.newDocument(name)
+    App.setActiveDocument(name)
+    return App.ActiveDocument
+
+
+def _cleanup(doc):
+    App.closeDocument(doc.Name)
+
+
+def _make_assembly(doc):
+    asm = doc.addObject("Assembly::AssemblyObject", "Assembly")
+    asm.resetSolver()
+    jg = asm.newObject("Assembly::JointGroup", "Joints")
+    return asm, jg
+
+
+def _make_box(asm, x=0, y=0, z=0, size=10):
+    box = asm.newObject("Part::Box", "Box")
+    box.Length = size
+    box.Width = size
+    box.Height = size
+    box.Placement = App.Placement(App.Vector(x, y, z), App.Rotation())
+    return box
+
+
+def _ground(jg, obj):
+    gnd = jg.newObject("App::FeaturePython", "GroundedJoint")
+    JointObject.GroundedJoint(gnd, obj)
+    return gnd
+
+
+def _make_joint(jg, joint_type, ref1, ref2):
+    joint = jg.newObject("App::FeaturePython", "Joint")
+    JointObject.Joint(joint, joint_type)
+    refs = [[ref1[0], ref1[1]], [ref2[0], ref2[1]]]
+    joint.Proxy.setJointConnectors(joint, refs)
+    return joint
+
+
+# ── Test 1: Registry ────────────────────────────────────────────────
+
+
+def test_solver_registry():
+    """Verify kindred solver is registered and available."""
+    names = kcsolve.available()
+    _report(
+        "registry: kindred in available()", "kindred" in names, f"available={names}"
+    )
+
+    solver = kcsolve.load("kindred")
+    _report("registry: load('kindred') succeeds", solver is not None)
+    _report(
+        "registry: solver name",
+        solver.name() == "Kindred (Newton-Raphson)",
+        f"got '{solver.name()}'",
+    )
+
+    joints = solver.supported_joints()
+    _report(
+        "registry: supported_joints non-empty",
+        len(joints) > 0,
+        f"{len(joints)} joint types",
+    )
+
+
+# ── Test 2: Preference switching ────────────────────────────────────
+
+
+def test_preference_switching():
+    """Verify solver preference controls which backend is used."""
+    doc = _new_doc("PrefTest")
+    try:
+        # Set to kindred
+        _pref.SetString("Solver", "kindred")
+        asm, jg = _make_assembly(doc)
+
+        box1 = _make_box(asm, 0, 0, 0)
+        box2 = _make_box(asm, 50, 0, 0)
+        _ground(box1)
+        _make_joint(jg, 0, [box1, ["Face6", "Vertex7"]], [box2, ["Face6", "Vertex7"]])
+
+        result = asm.solve()
+        _report(
+            "pref: kindred solve succeeds", result == 0, f"solve() returned {result}"
+        )
+
+        # Switch back to ondsel
+        _pref.SetString("Solver", "ondsel")
+        asm.resetSolver()
+        result2 = asm.solve()
+        _report(
+            "pref: ondsel solve succeeds after switch",
+            result2 == 0,
+            f"solve() returned {result2}",
+        )
+    finally:
+        _cleanup(doc)
+
+
+# ── Test 3: Fixed joint ─────────────────────────────────────────────
+
+
+def test_fixed_joint():
+    """Two boxes + ground + fixed joint -> placements match."""
+    _pref.SetString("Solver", "kindred")
+    doc = _new_doc("FixedTest")
+    try:
+        asm, jg = _make_assembly(doc)
+        box1 = _make_box(asm, 10, 20, 30)
+        box2 = _make_box(asm, 40, 50, 60)
+        _ground(box2)
+        _make_joint(jg, 0, [box2, ["Face6", "Vertex7"]], [box1, ["Face6", "Vertex7"]])
+
+        same = box1.Placement.isSame(box2.Placement, 1e-6)
+        _report(
+            "fixed: box1 matches box2 placement",
+            same,
+            f"box1={box1.Placement.Base}, box2={box2.Placement.Base}",
+        )
+    finally:
+        _cleanup(doc)
+
+
+# ── Test 4: Revolute joint + DOF ─────────────────────────────────────
+
+
+def test_revolute_dof():
+    """Revolute joint -> solve succeeds, DOF = 1."""
+    _pref.SetString("Solver", "kindred")
+    doc = _new_doc("RevoluteTest")
+    try:
+        asm, jg = _make_assembly(doc)
+        box1 = _make_box(asm, 0, 0, 0)
+        box2 = _make_box(asm, 100, 0, 0)
+        _ground(box1)
+        _make_joint(jg, 1, [box1, ["Face6", "Vertex7"]], [box2, ["Face6", "Vertex7"]])
+
+        result = asm.solve()
+        _report("revolute: solve succeeds", result == 0, f"solve() returned {result}")
+
+        dof = asm.getLastDoF()
+        _report("revolute: DOF = 1", dof == 1, f"DOF = {dof}")
+    finally:
+        _cleanup(doc)
+
+
+# ── Test 5: No ground ───────────────────────────────────────────────
+
+
+def test_no_ground():
+    """No grounded parts -> returns -6."""
+    _pref.SetString("Solver", "kindred")
+    doc = _new_doc("NoGroundTest")
+    try:
+        asm, jg = _make_assembly(doc)
+        box1 = _make_box(asm, 0, 0, 0)
+        box2 = _make_box(asm, 50, 0, 0)
+
+        joint = jg.newObject("App::FeaturePython", "Joint")
+        JointObject.Joint(joint, 0)
+        refs = [[box1, ["Face6", "Vertex7"]], [box2, ["Face6", "Vertex7"]]]
+        joint.Proxy.setJointConnectors(joint, refs)
+
+        result = asm.solve()
+        _report("no-ground: returns -6", result == -6, f"solve() returned {result}")
+    finally:
+        _cleanup(doc)
+
+
+# ── Test 6: Solve stability ─────────────────────────────────────────
+
+
+def test_stability():
+    """Solving twice gives identical placements."""
+    _pref.SetString("Solver", "kindred")
+    doc = _new_doc("StabilityTest")
+    try:
+        asm, jg = _make_assembly(doc)
+        box1 = _make_box(asm, 10, 20, 30)
+        box2 = _make_box(asm, 40, 50, 60)
+        _ground(box2)
+        _make_joint(jg, 0, [box2, ["Face6", "Vertex7"]], [box1, ["Face6", "Vertex7"]])
+
+        asm.solve()
+        plc1 = App.Placement(box1.Placement)
+        asm.solve()
+        plc2 = box1.Placement
+
+        same = plc1.isSame(plc2, 1e-6)
+        _report("stability: two solves identical", same)
+    finally:
+        _cleanup(doc)
+
+
+# ── Test 7: Standalone solver API ────────────────────────────────────
+
+
+def test_standalone_api():
+    """Use kcsolve types directly without FreeCAD Assembly objects."""
+    solver = kcsolve.load("kindred")
+
+    # Two parts: one grounded, one free
+    p1 = kcsolve.Part()
+    p1.id = "base"
+    p1.placement = kcsolve.Transform.identity()
+    p1.grounded = True
+
+    p2 = kcsolve.Part()
+    p2.id = "arm"
+    p2.placement = kcsolve.Transform()
+    p2.placement.position = [100.0, 0.0, 0.0]
+    p2.placement.quaternion = [1.0, 0.0, 0.0, 0.0]
+    p2.grounded = False
+
+    # Fixed joint
+    c = kcsolve.Constraint()
+    c.id = "fix1"
+    c.part_i = "base"
+    c.marker_i = kcsolve.Transform.identity()
+    c.part_j = "arm"
+    c.marker_j = kcsolve.Transform.identity()
+    c.type = kcsolve.BaseJointKind.Fixed
+
+    ctx = kcsolve.SolveContext()
+    ctx.parts = [p1, p2]
+    ctx.constraints = [c]
+
+    result = solver.solve(ctx)
+    _report(
+        "standalone: solve status",
+        result.status == kcsolve.SolveStatus.Success,
+        f"status={result.status}",
+    )
+    _report("standalone: DOF = 0", result.dof == 0, f"dof={result.dof}")
+
+    # Check that arm moved to origin
+    for pr in result.placements:
+        if pr.id == "arm":
+            dist = sum(x**2 for x in pr.placement.position) ** 0.5
+            _report("standalone: arm at origin", dist < 1e-6, f"distance={dist:.2e}")
+            break
+
+
+# ── Test 8: Diagnose API ────────────────────────────────────────────
+
+
+def test_diagnose():
+    """Diagnose overconstrained system via standalone API."""
+    solver = kcsolve.load("kindred")
+
+    p1 = kcsolve.Part()
+    p1.id = "base"
+    p1.placement = kcsolve.Transform.identity()
+    p1.grounded = True
+
+    p2 = kcsolve.Part()
+    p2.id = "arm"
+    p2.placement = kcsolve.Transform()
+    p2.placement.position = [50.0, 0.0, 0.0]
+    p2.placement.quaternion = [1.0, 0.0, 0.0, 0.0]
+    p2.grounded = False
+
+    # Two fixed joints = overconstrained
+    c1 = kcsolve.Constraint()
+    c1.id = "fix1"
+    c1.part_i = "base"
+    c1.marker_i = kcsolve.Transform.identity()
+    c1.part_j = "arm"
+    c1.marker_j = kcsolve.Transform.identity()
+    c1.type = kcsolve.BaseJointKind.Fixed
+
+    c2 = kcsolve.Constraint()
+    c2.id = "fix2"
+    c2.part_i = "base"
+    c2.marker_i = kcsolve.Transform.identity()
+    c2.part_j = "arm"
+    c2.marker_j = kcsolve.Transform.identity()
+    c2.type = kcsolve.BaseJointKind.Fixed
+
+    ctx = kcsolve.SolveContext()
+    ctx.parts = [p1, p2]
+    ctx.constraints = [c1, c2]
+
+    diags = solver.diagnose(ctx)
+    _report(
+        "diagnose: returns diagnostics", len(diags) > 0, f"{len(diags)} diagnostic(s)"
+    )
+    if diags:
+        kinds = [d.kind for d in diags]
+        _report(
+            "diagnose: found redundant",
+            kcsolve.DiagnosticKind.Redundant in kinds,
+            f"kinds={[str(k) for k in kinds]}",
+        )
+
+
+# ── Run all ──────────────────────────────────────────────────────────
+
+
+def run_all():
+    print("\n=== Phase 5 Console Tests ===\n")
+
+    test_solver_registry()
+    test_preference_switching()
+    test_fixed_joint()
+    test_revolute_dof()
+    test_no_ground()
+    test_stability()
+    test_standalone_api()
+    test_diagnose()
+
+    # Restore original preference
+    _pref.SetString("Solver", _orig_solver)
+
+    # Summary
+    passed = sum(1 for _, p in _results if p)
+    total = len(_results)
+    print(f"\n=== {passed}/{total} passed ===\n")
+    if passed < total:
+        failed = [name for name, p in _results if not p]
+        print(f"FAILED: {', '.join(failed)}")
+
+
+run_all()

tests/test_diagnostics.py

@@ -0,0 +1,296 @@
+"""Tests for per-entity DOF diagnostics and overconstrained detection."""
+
+import math
+
+import numpy as np
+import pytest
+from kindred_solver.constraints import (
+    CoincidentConstraint,
+    CylindricalConstraint,
+    DistancePointPointConstraint,
+    FixedConstraint,
+    ParallelConstraint,
+    RevoluteConstraint,
+)
+from kindred_solver.diagnostics import (
+    ConstraintDiag,
+    EntityDOF,
+    find_overconstrained,
+    per_entity_dof,
+)
+from kindred_solver.entities import RigidBody
+from kindred_solver.params import ParamTable
+
+
+def _make_two_bodies(
+    params,
+    pos_a=(0, 0, 0),
+    pos_b=(5, 0, 0),
+    quat_a=(1, 0, 0, 0),
+    quat_b=(1, 0, 0, 0),
+    ground_a=True,
+    ground_b=False,
+):
+    body_a = RigidBody(
+        "a", params, position=pos_a, quaternion=quat_a, grounded=ground_a
+    )
+    body_b = RigidBody(
+        "b", params, position=pos_b, quaternion=quat_b, grounded=ground_b
+    )
+    return body_a, body_b
+
+
+def _build_residuals_and_ranges(constraint_objs, bodies, params):
+    """Build residuals list, quat norms, and residual_ranges."""
+    all_residuals = []
+    residual_ranges = []
+    row = 0
+    for i, obj in enumerate(constraint_objs):
+        r = obj.residuals()
+        n = len(r)
+        residual_ranges.append((row, row + n, i))
+        all_residuals.extend(r)
+        row += n
+
+    for body in bodies.values():
+        if not body.grounded:
+            all_residuals.append(body.quat_norm_residual())
+
+    return all_residuals, residual_ranges
+
+
+# ============================================================================
+# Per-entity DOF tests
+# ============================================================================
+
+
+class TestPerEntityDOF:
+    """Per-entity DOF computation."""
+
+    def test_unconstrained_body_6dof(self):
+        """Unconstrained non-grounded body has 6 DOF."""
+        params = ParamTable()
+        body = RigidBody(
+            "b", params, position=(0, 0, 0), quaternion=(1, 0, 0, 0), grounded=False
+        )
+        bodies = {"b": body}
+
+        # Only quat norm constraint
+        residuals = [body.quat_norm_residual()]
+
+        result = per_entity_dof(residuals, params, bodies)
+        assert len(result) == 1
+        assert result[0].entity_id == "b"
+        assert result[0].remaining_dof == 6
+        assert len(result[0].free_motions) == 6
+
+    def test_fixed_body_0dof(self):
+        """Body welded to ground has 0 DOF."""
+        params = ParamTable()
+        body_a, body_b = _make_two_bodies(params)
+        bodies = {"a": body_a, "b": body_b}
+
+        c = FixedConstraint(
+            body_a,
+            (0, 0, 0),
+            (1, 0, 0, 0),
+            body_b,
+            (0, 0, 0),
+            (1, 0, 0, 0),
+        )
+        residuals, _ = _build_residuals_and_ranges([c], bodies, params)
+
+        result = per_entity_dof(residuals, params, bodies)
+        # Only non-grounded body (b) reported
+        assert len(result) == 1
+        assert result[0].entity_id == "b"
+        assert result[0].remaining_dof == 0
+        assert len(result[0].free_motions) == 0
+
+    def test_revolute_1dof(self):
+        """Revolute joint leaves 1 DOF (rotation about Z)."""
+        params = ParamTable()
+        body_a, body_b = _make_two_bodies(params, pos_b=(0, 0, 0))
+        bodies = {"a": body_a, "b": body_b}
+
+        c = RevoluteConstraint(
+            body_a,
+            (0, 0, 0),
+            (1, 0, 0, 0),
+            body_b,
+            (0, 0, 0),
+            (1, 0, 0, 0),
+        )
+        residuals, _ = _build_residuals_and_ranges([c], bodies, params)
+
+        result = per_entity_dof(residuals, params, bodies)
+        assert len(result) == 1
+        assert result[0].remaining_dof == 1
+        # Should have one free motion that mentions rotation
+        assert len(result[0].free_motions) == 1
+        assert "rotation" in result[0].free_motions[0].lower()
+
+    def test_cylindrical_2dof(self):
+        """Cylindrical joint leaves 2 DOF (rotation about Z + translation along Z)."""
+        params = ParamTable()
+        body_a, body_b = _make_two_bodies(params, pos_b=(0, 0, 0))
+        bodies = {"a": body_a, "b": body_b}
+
+        c = CylindricalConstraint(
+            body_a,
+            (0, 0, 0),
+            (1, 0, 0, 0),
+            body_b,
+            (0, 0, 0),
+            (1, 0, 0, 0),
+        )
+        residuals, _ = _build_residuals_and_ranges([c], bodies, params)
+
+        result = per_entity_dof(residuals, params, bodies)
+        assert len(result) == 1
+        assert result[0].remaining_dof == 2
+        assert len(result[0].free_motions) == 2
+
+    def test_coincident_3dof(self):
+        """Coincident (ball) joint leaves 3 DOF (3 rotations)."""
+        params = ParamTable()
+        body_a, body_b = _make_two_bodies(params, pos_b=(0, 0, 0))
+        bodies = {"a": body_a, "b": body_b}
+
+        c = CoincidentConstraint(body_a, (0, 0, 0), body_b, (0, 0, 0))
+        residuals, _ = _build_residuals_and_ranges([c], bodies, params)
+
+        result = per_entity_dof(residuals, params, bodies)
+        assert len(result) == 1
+        assert result[0].remaining_dof == 3
+        # All 3 should be rotations
+        for motion in result[0].free_motions:
+            assert "rotation" in motion.lower()
+
+    def test_no_constraints_6dof(self):
+        """No residuals at all gives 6 DOF."""
+        params = ParamTable()
+        body = RigidBody(
+            "b", params, position=(0, 0, 0), quaternion=(1, 0, 0, 0), grounded=False
+        )
+        bodies = {"b": body}
+
+        result = per_entity_dof([], params, bodies)
+        assert len(result) == 1
+        assert result[0].remaining_dof == 6
+
+    def test_grounded_body_excluded(self):
+        """Grounded bodies are not reported."""
+        params = ParamTable()
+        body_a, body_b = _make_two_bodies(params)
+        bodies = {"a": body_a, "b": body_b}
+
+        residuals = [body_b.quat_norm_residual()]
+        result = per_entity_dof(residuals, params, bodies)
+
+        entity_ids = [r.entity_id for r in result]
+        assert "a" not in entity_ids  # grounded
+        assert "b" in entity_ids
+
+    def test_multiple_bodies(self):
+        """Two free bodies: each gets its own DOF report."""
+        params = ParamTable()
+        body_g = RigidBody(
+            "g", params, position=(0, 0, 0), quaternion=(1, 0, 0, 0), grounded=True
+        )
+        body_b = RigidBody(
+            "b", params, position=(5, 0, 0), quaternion=(1, 0, 0, 0), grounded=False
+        )
+        body_c = RigidBody(
+            "c", params, position=(10, 0, 0), quaternion=(1, 0, 0, 0), grounded=False
+        )
+        bodies = {"g": body_g, "b": body_b, "c": body_c}
+
+        # Fix b to ground, leave c unconstrained
+        c_fix = FixedConstraint(
+            body_g,
+            (0, 0, 0),
+            (1, 0, 0, 0),
+            body_b,
+            (0, 0, 0),
+            (1, 0, 0, 0),
+        )
+        residuals, _ = _build_residuals_and_ranges([c_fix], bodies, params)
+
+        result = per_entity_dof(residuals, params, bodies)
+        result_map = {r.entity_id: r for r in result}
+
+        assert result_map["b"].remaining_dof == 0
+        assert result_map["c"].remaining_dof == 6
+
+
+# ============================================================================
+# Overconstrained detection tests
+# ============================================================================
+
+
+class TestFindOverconstrained:
+    """Redundant and conflicting constraint detection."""
+
+    def test_well_constrained_no_diagnostics(self):
+        """Well-constrained system produces no diagnostics."""
+        params = ParamTable()
+        body_a, body_b = _make_two_bodies(params, pos_b=(0, 0, 0))
+        bodies = {"a": body_a, "b": body_b}
+
+        c = FixedConstraint(
+            body_a,
+            (0, 0, 0),
+            (1, 0, 0, 0),
+            body_b,
+            (0, 0, 0),
+            (1, 0, 0, 0),
+        )
+        residuals, ranges = _build_residuals_and_ranges([c], bodies, params)
+
+        diags = find_overconstrained(residuals, params, ranges)
+        assert len(diags) == 0
+
+    def test_duplicate_coincident_redundant(self):
+        """Duplicate coincident constraint is flagged as redundant."""
+        params = ParamTable()
+        body_a, body_b = _make_two_bodies(params, pos_b=(0, 0, 0))
+        bodies = {"a": body_a, "b": body_b}
+
+        c1 = CoincidentConstraint(body_a, (0, 0, 0), body_b, (0, 0, 0))
+        c2 = CoincidentConstraint(body_a, (0, 0, 0), body_b, (0, 0, 0))
+        residuals, ranges = _build_residuals_and_ranges([c1, c2], bodies, params)
+
+        diags = find_overconstrained(residuals, params, ranges)
+        assert len(diags) > 0
+        # At least one should be redundant
+        kinds = {d.kind for d in diags}
+        assert "redundant" in kinds
+
+    def test_conflicting_distance(self):
+        """Distance constraint that can't be satisfied is flagged as conflicting."""
+        params = ParamTable()
+        body_a, body_b = _make_two_bodies(params, pos_b=(0, 0, 0))
+        bodies = {"a": body_a, "b": body_b}
+
+        # Coincident forces distance=0, but distance constraint says 50
+        c1 = CoincidentConstraint(body_a, (0, 0, 0), body_b, (0, 0, 0))
+        c2 = DistancePointPointConstraint(
+            body_a,
+            (0, 0, 0),
+            body_b,
+            (0, 0, 0),
+            distance=50.0,
+        )
+        residuals, ranges = _build_residuals_and_ranges([c1, c2], bodies, params)
+
+        diags = find_overconstrained(residuals, params, ranges)
+        assert len(diags) > 0
+        kinds = {d.kind for d in diags}
+        assert "conflicting" in kinds
+
+    def test_empty_system_no_diagnostics(self):
+        """Empty system has no diagnostics."""
+        params = ParamTable()
+        diags = find_overconstrained([], params, [])
+        assert len(diags) == 0

tests/test_params.py

@@ -99,3 +99,50 @@ class TestParamTable:
         pt.unfix("a")
         assert pt.free_names() == ["a"]
         assert pt.n_free() == 1
+
+    def test_snapshot_restore_roundtrip(self):
+        """Snapshot captures values; restore brings them back."""
+        pt = ParamTable()
+        pt.add("x", 1.0)
+        pt.add("y", 2.0)
+        pt.add("z", 3.0, fixed=True)
+        snap = pt.snapshot()
+        pt.set_value("x", 99.0)
+        pt.set_value("y", 88.0)
+        pt.set_value("z", 77.0)
+        pt.restore(snap)
+        assert pt.get_value("x") == 1.0
+        assert pt.get_value("y") == 2.0
+        assert pt.get_value("z") == 3.0
+
+    def test_snapshot_is_independent_copy(self):
+        """Mutating snapshot dict does not affect the table."""
+        pt = ParamTable()
+        pt.add("a", 5.0)
+        snap = pt.snapshot()
+        snap["a"] = 999.0
+        assert pt.get_value("a") == 5.0
+
+    def test_movement_cost_no_weights(self):
+        """Movement cost is sum of squared displacements for free params."""
+        pt = ParamTable()
+        pt.add("x", 0.0)
+        pt.add("y", 0.0)
+        pt.add("z", 0.0, fixed=True)
+        snap = pt.snapshot()
+        pt.set_value("x", 3.0)
+        pt.set_value("y", 4.0)
+        pt.set_value("z", 100.0)  # fixed — ignored
+        assert pt.movement_cost(snap) == pytest.approx(25.0)
+
+    def test_movement_cost_with_weights(self):
+        """Weighted movement cost scales each displacement."""
+        pt = ParamTable()
+        pt.add("a", 0.0)
+        pt.add("b", 0.0)
+        snap = pt.snapshot()
+        pt.set_value("a", 1.0)
+        pt.set_value("b", 1.0)
+        weights = {"a": 4.0, "b": 9.0}
+        # cost = 1^2*4 + 1^2*9 = 13
+        assert pt.movement_cost(snap, weights) == pytest.approx(13.0)

tests/test_preference.py

@@ -0,0 +1,384 @@
+"""Tests for solution preference: half-space tracking and corrections."""
+
+import math
+
+import numpy as np
+import pytest
+from kindred_solver.constraints import (
+    AngleConstraint,
+    DistancePointPointConstraint,
+    ParallelConstraint,
+    PerpendicularConstraint,
+)
+from kindred_solver.entities import RigidBody
+from kindred_solver.newton import newton_solve
+from kindred_solver.params import ParamTable
+from kindred_solver.preference import (
+    apply_half_space_correction,
+    compute_half_spaces,
+)
+
+
+def _make_two_bodies(
+    params,
+    pos_a=(0, 0, 0),
+    pos_b=(5, 0, 0),
+    quat_a=(1, 0, 0, 0),
+    quat_b=(1, 0, 0, 0),
+    ground_a=True,
+    ground_b=False,
+):
+    """Create two bodies with given positions/orientations."""
+    body_a = RigidBody(
+        "a", params, position=pos_a, quaternion=quat_a, grounded=ground_a
+    )
+    body_b = RigidBody(
+        "b", params, position=pos_b, quaternion=quat_b, grounded=ground_b
+    )
+    return body_a, body_b
+
+
+class TestDistanceHalfSpace:
+    """Half-space tracking for DistancePointPoint constraint."""
+
+    def test_positive_x_stays_positive(self):
+        """Body starting at +X should stay at +X after solve."""
+        params = ParamTable()
+        body_a, body_b = _make_two_bodies(params, pos_b=(3, 0, 0))
+        c = DistancePointPointConstraint(
+            body_a,
+            (0, 0, 0),
+            body_b,
+            (0, 0, 0),
+            distance=5.0,
+        )
+        hs = compute_half_spaces([c], [0], params)
+        assert len(hs) == 1
+
+        # Solve with half-space correction
+        residuals = c.residuals()
+        residuals.append(body_b.quat_norm_residual())
+        quat_groups = [body_b.quat_param_names()]
+
+        def post_step(p):
+            apply_half_space_correction(p, hs)
+
+        converged = newton_solve(
+            residuals,
+            params,
+            quat_groups=quat_groups,
+            post_step=post_step,
+        )
+        assert converged
+        env = params.get_env()
+        # Body b should be at +X (x > 0), not -X
+        bx = env["b/tx"]
+        assert bx > 0, f"Expected positive X, got {bx}"
+        # Distance should be 5
+        dist = math.sqrt(bx**2 + env["b/ty"] ** 2 + env["b/tz"] ** 2)
+        assert dist == pytest.approx(5.0, abs=1e-8)
+
+    def test_negative_x_stays_negative(self):
+        """Body starting at -X should stay at -X after solve."""
+        params = ParamTable()
+        body_a, body_b = _make_two_bodies(params, pos_b=(-3, 0, 0))
+        c = DistancePointPointConstraint(
+            body_a,
+            (0, 0, 0),
+            body_b,
+            (0, 0, 0),
+            distance=5.0,
+        )
+        hs = compute_half_spaces([c], [0], params)
+        assert len(hs) == 1
+
+        residuals = c.residuals()
+        residuals.append(body_b.quat_norm_residual())
+        quat_groups = [body_b.quat_param_names()]
+
+        def post_step(p):
+            apply_half_space_correction(p, hs)
+
+        converged = newton_solve(
+            residuals,
+            params,
+            quat_groups=quat_groups,
+            post_step=post_step,
+        )
+        assert converged
+        env = params.get_env()
+        bx = env["b/tx"]
+        assert bx < 0, f"Expected negative X, got {bx}"
+
+    def test_zero_distance_no_halfspace(self):
+        """Zero distance constraint has no branch ambiguity."""
+        params = ParamTable()
+        body_a, body_b = _make_two_bodies(params, pos_b=(3, 0, 0))
+        c = DistancePointPointConstraint(
+            body_a,
+            (0, 0, 0),
+            body_b,
+            (0, 0, 0),
+            distance=0.0,
+        )
+        hs = compute_half_spaces([c], [0], params)
+        assert len(hs) == 0
+
+
+class TestParallelHalfSpace:
+    """Half-space tracking for Parallel constraint."""
+
+    def test_same_direction_tracked(self):
+        """Same-direction parallel: positive reference sign."""
+        params = ParamTable()
+        body_a, body_b = _make_two_bodies(params)
+        c = ParallelConstraint(body_a, (1, 0, 0, 0), body_b, (1, 0, 0, 0))
+        hs = compute_half_spaces([c], [0], params)
+        assert len(hs) == 1
+        assert hs[0].reference_sign == 1.0
+
+    def test_opposite_direction_tracked(self):
+        """Opposite-direction parallel: negative reference sign."""
+        params = ParamTable()
+        # Rotate body_b by 180 degrees about X: Z-axis flips
+        q_flip = (0, 1, 0, 0)  # 180 deg about X
+        body_a, body_b = _make_two_bodies(params, quat_b=q_flip)
+        c = ParallelConstraint(body_a, (1, 0, 0, 0), body_b, (1, 0, 0, 0))
+        hs = compute_half_spaces([c], [0], params)
+        assert len(hs) == 1
+        assert hs[0].reference_sign == -1.0
+
+
+class TestAngleHalfSpace:
+    """Half-space tracking for Angle constraint."""
+
+    def test_90_degree_angle(self):
+        """90-degree angle constraint creates a half-space."""
+        params = ParamTable()
+        # Rotate body_b by 90 degrees about X
+        q_90x = (math.cos(math.pi / 4), math.sin(math.pi / 4), 0, 0)
+        body_a, body_b = _make_two_bodies(params, quat_b=q_90x)
+        c = AngleConstraint(
+            body_a,
+            (1, 0, 0, 0),
+            body_b,
+            (1, 0, 0, 0),
+            angle=math.pi / 2,
+        )
+        hs = compute_half_spaces([c], [0], params)
+        assert len(hs) == 1
+
+    def test_zero_angle_no_halfspace(self):
+        """0-degree angle has no branch ambiguity."""
+        params = ParamTable()
+        body_a, body_b = _make_two_bodies(params)
+        c = AngleConstraint(
+            body_a,
+            (1, 0, 0, 0),
+            body_b,
+            (1, 0, 0, 0),
+            angle=0.0,
+        )
+        hs = compute_half_spaces([c], [0], params)
+        assert len(hs) == 0
+
+    def test_180_angle_no_halfspace(self):
+        """180-degree angle has no branch ambiguity."""
+        params = ParamTable()
+        body_a, body_b = _make_two_bodies(params)
+        c = AngleConstraint(
+            body_a,
+            (1, 0, 0, 0),
+            body_b,
+            (1, 0, 0, 0),
+            angle=math.pi,
+        )
+        hs = compute_half_spaces([c], [0], params)
+        assert len(hs) == 0
+
+
+class TestPerpendicularHalfSpace:
+    """Half-space tracking for Perpendicular constraint."""
+
+    def test_perpendicular_tracked(self):
+        """Perpendicular constraint creates a half-space."""
+        params = ParamTable()
+        # Rotate body_b by 90 degrees about X
+        q_90x = (math.cos(math.pi / 4), math.sin(math.pi / 4), 0, 0)
+        body_a, body_b = _make_two_bodies(params, quat_b=q_90x)
+        c = PerpendicularConstraint(
+            body_a,
+            (1, 0, 0, 0),
+            body_b,
+            (1, 0, 0, 0),
+        )
+        hs = compute_half_spaces([c], [0], params)
+        assert len(hs) == 1
+
+
+class TestNewtonPostStep:
+    """Verify Newton post_step callback works correctly."""
+
+    def test_callback_fires(self):
+        """post_step callback is invoked during Newton iterations."""
+        params = ParamTable()
+        x = params.add("x", 2.0)
+        from kindred_solver.expr import Const
+
+        residuals = [x - Const(5.0)]
+
+        call_count = [0]
+
+        def counter(p):
+            call_count[0] += 1
+
+        converged = newton_solve(residuals, params, post_step=counter)
+        assert converged
+        assert call_count[0] >= 1
+
+    def test_callback_does_not_break_convergence(self):
+        """A no-op callback doesn't prevent convergence."""
+        params = ParamTable()
+        x = params.add("x", 1.0)
+        y = params.add("y", 1.0)
+        from kindred_solver.expr import Const
+
+        residuals = [x - Const(3.0), y - Const(7.0)]
+
+        converged = newton_solve(residuals, params, post_step=lambda p: None)
+        assert converged
+        assert params.get_value("x") == pytest.approx(3.0)
+        assert params.get_value("y") == pytest.approx(7.0)
+
+
+class TestMixedHalfSpaces:
+    """Multiple branching constraints in one system."""
+
+    def test_multiple_constraints(self):
+        """compute_half_spaces handles mixed constraint types."""
+        params = ParamTable()
+        body_a, body_b = _make_two_bodies(params, pos_b=(5, 0, 0))
+
+        dist_c = DistancePointPointConstraint(
+            body_a,
+            (0, 0, 0),
+            body_b,
+            (0, 0, 0),
+            distance=5.0,
+        )
+        par_c = ParallelConstraint(body_a, (1, 0, 0, 0), body_b, (1, 0, 0, 0))
+
+        hs = compute_half_spaces([dist_c, par_c], [0, 1], params)
+        assert len(hs) == 2
+
+
+class TestBuildWeightVector:
+    """Weight vector construction."""
+
+    def test_translation_weight_one(self):
+        """Translation params get weight 1.0."""
+        from kindred_solver.preference import build_weight_vector
+
+        params = ParamTable()
+        params.add("body/tx", 0.0)
+        params.add("body/ty", 0.0)
+        params.add("body/tz", 0.0)
+        w = build_weight_vector(params)
+        np.testing.assert_array_equal(w, [1.0, 1.0, 1.0])
+
+    def test_quaternion_weight_high(self):
+        """Quaternion params get QUAT_WEIGHT."""
+        from kindred_solver.preference import QUAT_WEIGHT, build_weight_vector
+
+        params = ParamTable()
+        params.add("body/qw", 1.0)
+        params.add("body/qx", 0.0)
+        params.add("body/qy", 0.0)
+        params.add("body/qz", 0.0)
+        w = build_weight_vector(params)
+        np.testing.assert_array_equal(w, [QUAT_WEIGHT] * 4)
+
+    def test_mixed_params(self):
+        """Mixed translation and quaternion params get correct weights."""
+        from kindred_solver.preference import QUAT_WEIGHT, build_weight_vector
+
+        params = ParamTable()
+        params.add("b/tx", 0.0)
+        params.add("b/qw", 1.0)
+        params.add("b/ty", 0.0)
+        params.add("b/qx", 0.0)
+        w = build_weight_vector(params)
+        assert w[0] == pytest.approx(1.0)
+        assert w[1] == pytest.approx(QUAT_WEIGHT)
+        assert w[2] == pytest.approx(1.0)
+        assert w[3] == pytest.approx(QUAT_WEIGHT)
+
+    def test_fixed_params_excluded(self):
+        """Fixed params are not in free list, so not in weight vector."""
+        from kindred_solver.preference import build_weight_vector
+
+        params = ParamTable()
+        params.add("b/tx", 0.0, fixed=True)
+        params.add("b/ty", 0.0)
+        w = build_weight_vector(params)
+        assert len(w) == 1
+        assert w[0] == pytest.approx(1.0)
+
+
+class TestWeightedNewton:
+    """Weighted minimum-norm Newton solve."""
+
+    def test_well_constrained_same_result(self):
+        """Weighted and unweighted produce identical results for unique solution."""
+        from kindred_solver.expr import Const
+
+        # Fully determined system: x = 3, y = 7
+        params1 = ParamTable()
+        x1 = params1.add("x", 1.0)
+        y1 = params1.add("y", 1.0)
+        r1 = [x1 - Const(3.0), y1 - Const(7.0)]
+
+        params2 = ParamTable()
+        x2 = params2.add("x", 1.0)
+        y2 = params2.add("y", 1.0)
+        r2 = [x2 - Const(3.0), y2 - Const(7.0)]
+
+        newton_solve(r1, params1)
+        newton_solve(r2, params2, weight_vector=np.array([1.0, 100.0]))
+
+        assert params1.get_value("x") == pytest.approx(
+            params2.get_value("x"), abs=1e-10
+        )
+        assert params1.get_value("y") == pytest.approx(
+            params2.get_value("y"), abs=1e-10
+        )
+
+    def test_underconstrained_prefers_low_weight(self):
+        """Under-constrained: weighted solve moves high-weight params less."""
+        from kindred_solver.expr import Const
+
+        # 1 equation, 2 unknowns: x + y = 10 (from x=0, y=0)
+        params_unw = ParamTable()
+        xu = params_unw.add("x", 0.0)
+        yu = params_unw.add("y", 0.0)
+        ru = [xu + yu - Const(10.0)]
+
+        params_w = ParamTable()
+        xw = params_w.add("x", 0.0)
+        yw = params_w.add("y", 0.0)
+        rw = [xw + yw - Const(10.0)]
+
+        # Unweighted: lstsq gives equal movement
+        newton_solve(ru, params_unw)
+
+        # Weighted: y is 100x more expensive to move
+        newton_solve(rw, params_w, weight_vector=np.array([1.0, 100.0]))
+
+        # Both should satisfy x + y = 10
+        assert params_unw.get_value("x") + params_unw.get_value("y") == pytest.approx(
+            10.0
+        )
+        assert params_w.get_value("x") + params_w.get_value("y") == pytest.approx(10.0)
+
+        # Weighted solve should move y less than x
+        assert abs(params_w.get_value("y")) < abs(params_w.get_value("x"))