exoskeleton/code/core/sew_mapper2.py

# -*- coding: utf-8 -*-
"""
SEW retargeting for the real 7-DoF slave arm.

The geometric SEW construction provides elbow/wrist targets.  A bounded
least-squares recovery then enforces the joint limits from the slave URDF.
``compute_differential`` evaluates the local retargeting Jacobian with a
central finite difference on one fixed local branch and reports every event
that makes that differential invalid (failed recovery, branch jump, reach
clipping, or an active joint limit).

Assumptions matching ``real_slave_7dof.urdf``:
- upper-arm / forearm links extend along local -Y;
- elbow hinge axis is +X in the elbow joint local frame;
- shoulder axes are y-x-y with signs (-1,+1,-1);
- wrist axes are y-z-x with signs (-1,+1,+1).
"""
from __future__ import annotations

from dataclasses import dataclass
from typing import Dict, List, Optional, Sequence, Tuple

import numpy as np
import pinocchio as pin
from scipy.optimize import least_squares


# -------------------------- small SO(3) helpers -------------------------- #

def _hat(v: np.ndarray) -> np.ndarray:
    x, y, z = v
    return np.array([[0.0, -z,  y],
                     [z,  0.0, -x],
                     [-y, x,  0.0]], dtype=float)

def _normalize(v: np.ndarray, eps: float = 1e-12) -> np.ndarray:
    n = float(np.linalg.norm(v))
    return v * 0.0 if n < eps else (v / n)


def _wrap_angle_delta(delta: np.ndarray) -> np.ndarray:
    """Return revolute-joint differences on the principal interval [-pi, pi)."""
    delta = np.asarray(delta, dtype=float)
    return (delta + np.pi) % (2.0 * np.pi) - np.pi

def _rodrigues(axis: np.ndarray, angle: float) -> np.ndarray:
    a = _normalize(axis)
    K = _hat(a)
    return np.eye(3) + np.sin(angle) * K + (1.0 - np.cos(angle)) * (K @ K)

def _ball_rot(axis_order: str, q3: np.ndarray) -> np.ndarray:
    """Intrinsic chain: R = R(a1,q1) R(a2,q2) R(a3,q3) with a in {x,y,z} unit axes."""
    axes = {
        "x": np.array([1.0, 0.0, 0.0]),
        "y": np.array([0.0, 1.0, 0.0]),
        "z": np.array([0.0, 0.0, 1.0]),
    }
    ao = axis_order.lower()
    a1, a2, a3 = axes[ao[0]], axes[ao[1]], axes[ao[2]]
    return _rodrigues(a1, q3[0]) @ _rodrigues(a2, q3[1]) @ _rodrigues(a3, q3[2])

def _solve_ball_gn(
    R_des: np.ndarray,
    axis_order: str,
    q_seed: np.ndarray,
    iters: int = 15,
    damp: float = 1e-4,
    fd_eps: float = 1e-6,
) -> np.ndarray:
    """Gauss-Newton solve q s.t. R(q) ~= R_des. Works for repeated axes (e.g. yxy)."""
    q = q_seed.astype(float).copy()
    ao = axis_order.lower()

    for _ in range(iters):
        R_cur = _ball_rot(ao, q)
        err = pin.log3(R_cur.T @ R_des)
        if float(np.linalg.norm(err)) < 1e-10:
            break

        J = np.zeros((3, 3), dtype=float)
        for i in range(3):
            dq = np.zeros(3, dtype=float)
            dq[i] = fd_eps
            R_p = _ball_rot(ao, q + dq)
            err_p = pin.log3(R_p.T @ R_des)
            J[:, i] = (err_p - err) / fd_eps

        A = J @ J.T + damp * np.eye(3)
        dq = - J.T @ np.linalg.solve(A, err)
        q += dq

    q = (q + np.pi) % (2 * np.pi) - np.pi
    return q


# -------------------------- joint config -------------------------- #

@dataclass(frozen=True)
class BallJointConfig:
    axis_order: str
    joint_names: Tuple[str, str, str]
    signs: Tuple[float, float, float] = (1.0, 1.0, 1.0)

    def resolve_qidx(self, model: pin.Model) -> Tuple[int, int, int]:
        idxs = []
        for name in self.joint_names:
            jid = int(model.getJointId(name))
            if jid <= 0 or jid >= model.njoints:
                raise ValueError(
                    f"Joint not found: {name!r}; "
                    f"available={list(model.names)[1:]}"
                )
            if model.joints[jid].nq != 1 or model.joints[jid].nv != 1:
                raise ValueError(f"Expected scalar revolute joint: {name!r}")
            idxs.append(model.joints[jid].idx_q)
        return (idxs[0], idxs[1], idxs[2])

    def eff_from_q(self, q: np.ndarray, qidx: Tuple[int, int, int]) -> np.ndarray:
        s0, s1, s2 = self.signs
        return np.array([s0 * q[qidx[0]], s1 * q[qidx[1]], s2 * q[qidx[2]]], dtype=float)

    def write_from_eff(self, q: np.ndarray, qidx: Tuple[int, int, int], q_eff: np.ndarray) -> None:
        s0, s1, s2 = self.signs
        q[qidx[0]] = s0 * float(q_eff[0])
        q[qidx[1]] = s1 * float(q_eff[1])
        q[qidx[2]] = s2 * float(q_eff[2])


# -------------------------- mapper -------------------------- #

class SEWMapper:
    """
    Minimal SEW mapper.
    You must provide correct frame names and 7 joint names order:
      [S1,S2,S3, EL, W1,W2,W3]
    """
    def __init__(
        self,
        master_model: pin.Model,
        slave_model: pin.Model,
        # frames
        m_shoulder: str, m_elbow: str, m_wrist: str, m_ee: str,
        s_shoulder: str, s_elbow: str, s_wrist: str, s_ee: str,
        # joints order (7)
        master_joint_names: Tuple[str, ...],
        slave_joint_names: Tuple[str, ...],
        # slave configs
        slave_shoulder_cfg: BallJointConfig,
        slave_wrist_cfg: BallJointConfig,
        # elbow axis in elbow joint local frame
        slave_elbow_axis_local: np.ndarray = np.array([1.0, 0.0, 0.0]),
        up_dir: np.ndarray = np.array([0.0, 0.0, 1.0]),
        eps_clip: float = 1e-3,
        position_tolerance: float = 2e-4,
        orientation_tolerance: float = 2e-3,
        joint_limit_margin: float = 1e-4,
        debug: bool = False,
    ):
        self.m_model = master_model
        self.m_data = master_model.createData()
        self.s_model = slave_model
        self.s_data = slave_model.createData()

        # frames
        def checked_frame_id(model: pin.Model, name: str) -> int:
            fid = int(model.getFrameId(name))
            if fid < 0 or fid >= model.nframes:
                raise ValueError(
                    f"Frame not found: {name!r}; "
                    f"available={[frame.name for frame in model.frames]}"
                )
            return fid

        self.fid_mS = checked_frame_id(master_model, m_shoulder)
        self.fid_mE = checked_frame_id(master_model, m_elbow)
        self.fid_mW = checked_frame_id(master_model, m_wrist)
        self.fid_mEE = checked_frame_id(master_model, m_ee)

        self.fid_sS = checked_frame_id(slave_model, s_shoulder)
        self.fid_sE = checked_frame_id(slave_model, s_elbow)
        self.fid_sW = checked_frame_id(slave_model, s_wrist)
        self.fid_sEE = checked_frame_id(slave_model, s_ee)

        # joints mapping (7)
        if len(master_joint_names) != 7 or len(slave_joint_names) != 7:
            raise ValueError("master_joint_names and slave_joint_names must be length 7")
        def checked_joint_id(model: pin.Model, name: str) -> int:
            jid = int(model.getJointId(name))
            if jid <= 0 or jid >= model.njoints:
                raise ValueError(
                    f"Joint not found: {name!r}; "
                    f"available={list(model.names)[1:]}"
                )
            joint = model.joints[jid]
            if joint.nq != 1 or joint.nv != 1:
                raise ValueError(f"Expected scalar revolute joint: {name!r}")
            return jid

        self.m_joint_ids = [
            checked_joint_id(master_model, name) for name in master_joint_names
        ]
        self.s_joint_ids = [
            checked_joint_id(slave_model, name) for name in slave_joint_names
        ]
        self.m_qidx7 = [master_model.joints[j].idx_q for j in self.m_joint_ids]
        self.s_qidx7 = [slave_model.joints[j].idx_q for j in self.s_joint_ids]
        self.jid_s_elbow = self.s_joint_ids[3]  # joint id, not qidx

        # configs
        self.sh_cfg = slave_shoulder_cfg
        self.wr_cfg = slave_wrist_cfg
        self.sh_qidx = self.sh_cfg.resolve_qidx(slave_model)
        self.wr_qidx = self.wr_cfg.resolve_qidx(slave_model)

        self.elbow_axis_local = _normalize(slave_elbow_axis_local)
        self.up = _normalize(up_dir)
        self.eps_clip = float(eps_clip)
        self.position_tolerance = float(position_tolerance)
        self.orientation_tolerance = float(orientation_tolerance)
        self.joint_limit_margin = float(joint_limit_margin)
        self.debug = bool(debug)

        self.s_lower7 = np.array(
            [slave_model.lowerPositionLimit[idx] for idx in self.s_qidx7], dtype=float
        )
        self.s_upper7 = np.array(
            [slave_model.upperPositionLimit[idx] for idx in self.s_qidx7], dtype=float
        )
        if (
            not np.all(np.isfinite(self.s_lower7))
            or not np.all(np.isfinite(self.s_upper7))
            or np.any(self.s_lower7 >= self.s_upper7)
        ):
            raise ValueError("All seven slave joints must have finite, ordered URDF limits")

        # segment lengths from slave neutral
        qs0 = pin.neutral(slave_model)
        self._fk_slave(qs0)
        pS = self._pos(self.s_data, self.fid_sS)
        pE = self._pos(self.s_data, self.fid_sE)
        pW = self._pos(self.s_data, self.fid_sW)
        self.L1 = float(np.linalg.norm(pE - pS))
        self.L2 = float(np.linalg.norm(pW - pE))
        self.pS_s_fixed = pS.copy()

    # ---- FK helpers ----
    def _fk_master(self, q_m: np.ndarray) -> None:
        pin.forwardKinematics(self.m_model, self.m_data, q_m)
        pin.updateFramePlacements(self.m_model, self.m_data)

    def _fk_slave(self, q_s: np.ndarray) -> None:
        pin.forwardKinematics(self.s_model, self.s_data, q_s)
        pin.updateFramePlacements(self.s_model, self.s_data)

    @staticmethod
    def _pos(data: pin.Data, fid: int) -> np.ndarray:
        return data.oMf[fid].translation.copy()

    @staticmethod
    def _rot(data: pin.Data, fid: int) -> np.ndarray:
        return data.oMf[fid].rotation.copy()

    # ---- vector helpers ----
    def _build_q(self, model: pin.Model, qidx7: Sequence[int], q7: np.ndarray) -> np.ndarray:
        q = pin.neutral(model)
        for idx, value in zip(qidx7, q7):
            q[idx] = float(value)
        return q

    def _slave_q7(self, q_s: np.ndarray) -> np.ndarray:
        return np.array([q_s[idx] for idx in self.s_qidx7], dtype=float)

    def _write_slave_q7(self, q_s: np.ndarray, q_s7: np.ndarray) -> None:
        for idx, value in zip(self.s_qidx7, q_s7):
            q_s[idx] = float(value)

    def _interior_clip(self, q_s7: np.ndarray) -> np.ndarray:
        # scipy requires x0 to be feasible.  Keeping it strictly inside also
        # avoids declaring the seed itself to be the active-set solution.
        pad = np.minimum(1e-9, 0.25 * (self.s_upper7 - self.s_lower7))
        return np.minimum(np.maximum(q_s7, self.s_lower7 + pad), self.s_upper7 - pad)

    # ---- target construction and bounded recovery ----
    def _target_from_master(self, q_m7: np.ndarray) -> Dict:
        q_m7 = np.asarray(q_m7, dtype=float)
        if q_m7.shape != (7,) or not np.all(np.isfinite(q_m7)):
            raise ValueError("q_m7 must be a finite vector with shape (7,)")

        q_m = self._build_q(self.m_model, self.m_qidx7, q_m7)
        self._fk_master(q_m)
        pS_m = self._pos(self.m_data, self.fid_mS)
        pE_m = self._pos(self.m_data, self.fid_mE)
        pW_m = self._pos(self.m_data, self.fid_mW)
        RmEE = self._rot(self.m_data, self.fid_mEE)

        events: List[str] = []
        r_m = pW_m - pS_m
        d_m = float(np.linalg.norm(r_m))
        hard_geometry_valid = d_m > 1e-9
        if not hard_geometry_valid:
            events.append("master_shoulder_wrist_degenerate")
            xhat = np.array([1.0, 0.0, 0.0])
        else:
            xhat = r_m / d_m

        arm_normal_raw = np.cross(pE_m - pS_m, pW_m - pS_m)
        arm_normal_norm = float(np.linalg.norm(arm_normal_raw))
        if arm_normal_norm < 1e-8:
            events.append("master_arm_plane_degenerate")
            hard_geometry_valid = False
            nm = self.up - float(np.dot(self.up, xhat)) * xhat
            if float(np.linalg.norm(nm)) < 1e-8:
                nm = np.array([0.0, 1.0, 0.0])
            nm = _normalize(nm)
        else:
            nm = arm_normal_raw / arm_normal_norm

        nref_tilde = self.up - float(np.dot(self.up, xhat)) * xhat
        reference_axis_norm = float(np.linalg.norm(nref_tilde))
        reference_fallback = reference_axis_norm < 1e-6
        if reference_fallback:
            events.append("reference_axis_fallback")
            candidates = (
                np.array([0.0, 1.0, 0.0]),
                np.array([1.0, 0.0, 0.0]),
            )
            nref_tilde = max(
                (axis - float(np.dot(axis, xhat)) * xhat for axis in candidates),
                key=np.linalg.norm,
            )
        nref = _normalize(nref_tilde)
        phi = float(
            np.arctan2(
                np.dot(xhat, np.cross(nref, nm)),
                np.dot(nref, nm),
            )
        )

        d_min = abs(self.L1 - self.L2) + self.eps_clip
        d_max = (self.L1 + self.L2) - self.eps_clip
        d_s = float(np.clip(d_m, d_min, d_max))
        if d_m < d_min:
            clip_region = "lower"
            events.append("reach_clipped_lower")
        elif d_m > d_max:
            clip_region = "upper"
            events.append("reach_clipped_upper")
        else:
            clip_region = "none"

        pS_s = self.pS_s_fixed
        pW_s_ref = pS_s + d_s * xhat
        e3 = _normalize(_rodrigues(xhat, phi) @ nref)
        e2 = _normalize(np.cross(xhat, e3))
        cos_th_raw = (self.L1**2 + d_s**2 - self.L2**2) / (2.0 * self.L1 * d_s)
        cos_th = float(np.clip(cos_th_raw, -1.0, 1.0))
        sin_th = float(np.sqrt(max(0.0, 1.0 - cos_th * cos_th)))
        pE_s_ref = pS_s + self.L1 * (cos_th * xhat + sin_th * e2)

        # Desired shoulder-link orientation used only to create a strong seed.
        u = _normalize(pE_s_ref - pS_s)
        x_axis = e3
        y_axis = -u
        z_axis = _normalize(np.cross(x_axis, y_axis))
        y_axis = _normalize(np.cross(z_axis, x_axis))
        RS_des = np.column_stack([x_axis, y_axis, z_axis])

        return {
            "q_m": q_m,
            "pE_s_ref": pE_s_ref,
            "pW_s_ref": pW_s_ref,
            "RmEE": RmEE,
            "RS_des": RS_des,
            "events": events,
            "hard_geometry_valid": hard_geometry_valid,
            "reference_fallback": reference_fallback,
            "reference_axis_norm": reference_axis_norm,
            "phi_rad": phi,
            "reach_clipped": clip_region != "none",
            "clip_region": clip_region,
            "master_reach": d_m,
            "slave_reach": d_s,
            "reach_lower_margin_m": d_m - d_min,
            "reach_upper_margin_m": d_max - d_m,
            "master_arm_normal_norm": arm_normal_norm,
        }

    def _staged_seed(self, target: Dict, q_seed: np.ndarray) -> np.ndarray:
        """Analytic/sequential solve used as a seed; it is never returned unchecked."""
        q_s = q_seed.copy()

        q_sh_seed = self.sh_cfg.eff_from_q(q_s, self.sh_qidx)
        q_sh_eff = _solve_ball_gn(
            target["RS_des"], self.sh_cfg.axis_order, q_sh_seed
        )
        self.sh_cfg.write_from_eff(q_s, self.sh_qidx, q_sh_eff)

        # The one-dimensional search respects the actual elbow limits.
        theta_lo = float(self.s_lower7[3])
        theta_hi = float(self.s_upper7[3])

        def wrist_err(theta: float) -> float:
            q_tmp = q_s.copy()
            q_tmp[self.s_qidx7[3]] = theta
            self._fk_slave(q_tmp)
            return float(
                np.linalg.norm(
                    self._pos(self.s_data, self.fid_sW) - target["pW_s_ref"]
                )
            )

        thetas = np.linspace(theta_lo, theta_hi, 181)
        errs = np.array([wrist_err(theta) for theta in thetas])
        k = int(np.argmin(errs))
        lo = float(thetas[max(0, k - 1)])
        hi = float(thetas[min(len(thetas) - 1, k + 1)])
        gr = (np.sqrt(5.0) - 1.0) / 2.0
        x1 = hi - gr * (hi - lo)
        x2 = lo + gr * (hi - lo)
        f1, f2 = wrist_err(x1), wrist_err(x2)
        for _ in range(20):
            if f1 > f2:
                lo, x1, f1 = x1, x2, f2
                x2 = lo + gr * (hi - lo)
                f2 = wrist_err(x2)
            else:
                hi, x2, f2 = x2, x1, f1
                x1 = hi - gr * (hi - lo)
                f1 = wrist_err(x1)
        q_s[self.s_qidx7[3]] = float(x1 if f1 < f2 else x2)
        self._fk_slave(q_s)

        RW = self._rot(self.s_data, self.fid_sW)
        q_wr_seed = self.wr_cfg.eff_from_q(q_s, self.wr_qidx)
        q_wr_eff = _solve_ball_gn(
            RW.T @ target["RmEE"], self.wr_cfg.axis_order, q_wr_seed
        )
        self.wr_cfg.write_from_eff(q_s, self.wr_qidx, q_wr_eff)
        return q_s

    def _bounded_recovery(
        self,
        target: Dict,
        q_template: np.ndarray,
        q_seed7: np.ndarray,
    ) -> Tuple[np.ndarray, object]:
        q_seed7 = self._interior_clip(np.asarray(q_seed7, dtype=float))
        q_regularization_ref = q_seed7.copy()

        def residual(q_s7: np.ndarray) -> np.ndarray:
            q_s = q_template.copy()
            self._write_slave_q7(q_s, q_s7)
            self._fk_slave(q_s)
            pE = self._pos(self.s_data, self.fid_sE)
            pW = self._pos(self.s_data, self.fid_sW)
            RsEE = self._rot(self.s_data, self.fid_sEE)
            return np.concatenate(
                (
                    5.0 * (pE - target["pE_s_ref"]),
                    5.0 * (pW - target["pW_s_ref"]),
                    pin.log3(RsEE.T @ target["RmEE"]),
                    1e-5 * _wrap_angle_delta(q_s7 - q_regularization_ref),
                )
            )

        result = least_squares(
            residual,
            q_seed7,
            bounds=(self.s_lower7, self.s_upper7),
            method="trf",
            ftol=1e-12,
            xtol=1e-12,
            gtol=1e-12,
            max_nfev=400,
        )
        q_s = q_template.copy()
        self._write_slave_q7(q_s, result.x)
        return q_s, result

    def _solution_metrics(self, q_s: np.ndarray, target: Dict) -> Dict:
        self._fk_slave(q_s)
        q_s7 = self._slave_q7(q_s)
        elbow_error = float(
            np.linalg.norm(self._pos(self.s_data, self.fid_sE) - target["pE_s_ref"])
        )
        wrist_error = float(
            np.linalg.norm(self._pos(self.s_data, self.fid_sW) - target["pW_s_ref"])
        )
        orientation_error = float(
            np.linalg.norm(
                pin.log3(
                    self._rot(self.s_data, self.fid_sEE).T @ target["RmEE"]
                )
            )
        )
        lower_clearance = q_s7 - self.s_lower7
        upper_clearance = self.s_upper7 - q_s7
        violation = np.flatnonzero(
            (lower_clearance < -1e-9) | (upper_clearance < -1e-9)
        )
        active = np.flatnonzero(
            np.minimum(lower_clearance, upper_clearance) <= self.joint_limit_margin
        )
        return {
            "q_s7": q_s7,
            "elbow_position_error": elbow_error,
            "wrist_position_error": wrist_error,
            "orientation_error": orientation_error,
            "joint_limit_violation_indices": violation.tolist(),
            "joint_limit_active_indices": active.tolist(),
            "minimum_joint_limit_clearance": float(
                np.min(np.minimum(lower_clearance, upper_clearance))
            ),
        }

    # ---- public API ----
    def retarget(
        self,
        q_m7: np.ndarray,
        q_s_init: Optional[np.ndarray] = None,
    ) -> Tuple[np.ndarray, Dict]:
        """
        Retarget one master pose with bounded recovery.

        ``success`` means the returned slave pose respects its URDF limits and
        meets the configured elbow/wrist/orientation tolerances.  ``smooth`` is
        stricter: it is false at reach clipping, fallback geometry, or an active
        joint limit.  A differential may only be used when both are true.
        """
        q_m7 = np.asarray(q_m7, dtype=float)
        target = self._target_from_master(q_m7)

        if q_s_init is not None:
            q_template = np.asarray(q_s_init, dtype=float).copy()
            if q_template.shape != (self.s_model.nq,) or not np.all(np.isfinite(q_template)):
                raise ValueError(
                    f"q_s_init must be finite with shape ({self.s_model.nq},)"
                )
            # Supplying q_s_init explicitly requests this local branch.
            seeds = [self._slave_q7(q_template)]
        else:
            q_template = pin.neutral(self.s_model)
            staged = self._staged_seed(target, q_template)
            midpoint = 0.5 * (self.s_lower7 + self.s_upper7)
            seeds = [self._slave_q7(staged), midpoint]

        candidates = [
            self._bounded_recovery(target, q_template, seed) for seed in seeds
        ]
        candidate_metrics = [
            self._solution_metrics(q_s, target) for q_s, _ in candidates
        ]

        def task_score(metrics: Dict) -> float:
            return (
                25.0 * metrics["elbow_position_error"] ** 2
                + 25.0 * metrics["wrist_position_error"] ** 2
                + metrics["orientation_error"] ** 2
            )

        selected_index = int(
            np.argmin([task_score(metrics) for metrics in candidate_metrics])
        )
        q_s, result = candidates[selected_index]
        metrics = candidate_metrics[selected_index]

        events = list(target["events"])
        if metrics["joint_limit_violation_indices"]:
            events.append("joint_limit_violation")
        if metrics["joint_limit_active_indices"]:
            events.append("joint_limit_active")

        finite = bool(
            np.all(np.isfinite(metrics["q_s7"]))
            and np.isfinite(metrics["elbow_position_error"])
            and np.isfinite(metrics["wrist_position_error"])
            and np.isfinite(metrics["orientation_error"])
        )
        within_tolerance = bool(
            metrics["elbow_position_error"] <= self.position_tolerance
            and metrics["wrist_position_error"] <= self.position_tolerance
            and metrics["orientation_error"] <= self.orientation_tolerance
        )
        success = bool(
            finite
            and result.success
            and target["hard_geometry_valid"]
            and not metrics["joint_limit_violation_indices"]
            and within_tolerance
        )
        if not result.success:
            events.append("bounded_solver_not_converged")
        if not within_tolerance:
            events.append("task_tolerance_exceeded")
        if not target["hard_geometry_valid"]:
            events.append("invalid_master_geometry")

        nonsmooth_events = {
            "reach_clipped_lower",
            "reach_clipped_upper",
            "reference_axis_fallback",
            "master_shoulder_wrist_degenerate",
            "master_arm_plane_degenerate",
            "joint_limit_active",
            "joint_limit_violation",
        }
        smooth = bool(success and not any(event in nonsmooth_events for event in events))
        position_error = max(
            metrics["elbow_position_error"], metrics["wrist_position_error"]
        )
        dbg = {
            **metrics,
            "success": success,
            "valid": success,
            "invalid": not success,
            "smooth": smooth,
            "events": tuple(dict.fromkeys(events)),
            "pE_s_ref": target["pE_s_ref"].copy(),
            "pW_s_ref": target["pW_s_ref"].copy(),
            "reach_clipped": target["reach_clipped"],
            "clipped": target["reach_clipped"],
            "clip_region": target["clip_region"],
            "near_limit": bool(metrics["joint_limit_active_indices"]),
            "position_error": position_error,
            "reference_fallback": target["reference_fallback"],
            "reference_axis_norm": target["reference_axis_norm"],
            "phi_rad": target["phi_rad"],
            "master_reach": target["master_reach"],
            "slave_reach": target["slave_reach"],
            "reach_lower_margin_m": target["reach_lower_margin_m"],
            "reach_upper_margin_m": target["reach_upper_margin_m"],
            "master_arm_normal_norm": target["master_arm_normal_norm"],
            "solver_success": bool(result.success),
            "solver_status": int(result.status),
            "solver_cost": float(result.cost),
            "solver_nfev": int(result.nfev),
            "selected_seed_index": selected_index,
        }
        self._fk_slave(q_s)
        return q_s, dbg

    def compute_differential(
        self,
        q_m7: np.ndarray,
        q_s_init: Optional[np.ndarray] = None,
        fd_step: float = 1e-4,
        branch_jump_threshold: float = 0.25,
        consistency_tolerance: float = 5e-2,
    ) -> Tuple[np.ndarray, Dict]:
        """
        Compute A = d(q_slave)/d(q_master) by a central finite difference.

        Every +/- solve starts from the same bounded base solution.  Slave
        angle differences are wrapped before division.  Invalid/nonsmooth
        columns are filled with NaN; callers must also check ``info["valid"]``.
        """
        q_m7 = np.asarray(q_m7, dtype=float)
        if q_m7.shape != (7,) or not np.all(np.isfinite(q_m7)):
            raise ValueError("q_m7 must be a finite vector with shape (7,)")
        if not np.isfinite(fd_step) or fd_step <= 0.0:
            raise ValueError("fd_step must be finite and positive")
        if branch_jump_threshold <= 0.0 or consistency_tolerance <= 0.0:
            raise ValueError("differential thresholds must be positive")

        base_q_s, base_dbg = self.retarget(q_m7, q_s_init=q_s_init)
        base_q7 = self._slave_q7(base_q_s)
        A = np.full((7, 7), np.nan, dtype=float)
        events: List[str] = []
        columns: List[Dict] = []

        if not base_dbg["success"]:
            events.append("base_invalid")
        if not base_dbg["smooth"]:
            events.append("base_nonsmooth")

        if base_dbg["success"] and base_dbg["smooth"]:
            for j in range(7):
                q_plus = q_m7.copy()
                q_minus = q_m7.copy()
                q_plus[j] += fd_step
                q_minus[j] -= fd_step

                plus_q_s, plus_dbg = self.retarget(q_plus, q_s_init=base_q_s)
                minus_q_s, minus_dbg = self.retarget(q_minus, q_s_init=base_q_s)
                plus_q7 = self._slave_q7(plus_q_s)
                minus_q7 = self._slave_q7(minus_q_s)

                plus_jump = float(
                    np.max(np.abs(_wrap_angle_delta(plus_q7 - base_q7)))
                )
                minus_jump = float(
                    np.max(np.abs(_wrap_angle_delta(minus_q7 - base_q7)))
                )
                fwd = _wrap_angle_delta(plus_q7 - base_q7) / fd_step
                bwd = _wrap_angle_delta(base_q7 - minus_q7) / fd_step
                consistency = float(
                    np.linalg.norm(fwd - bwd)
                    / (1.0 + max(np.linalg.norm(fwd), np.linalg.norm(bwd)))
                )

                column_events: List[str] = []
                if not plus_dbg["success"] or not minus_dbg["success"]:
                    column_events.append("perturbation_invalid")
                if not plus_dbg["smooth"] or not minus_dbg["smooth"]:
                    column_events.append("perturbation_nonsmooth")
                if (
                    plus_dbg["clip_region"] != base_dbg["clip_region"]
                    or minus_dbg["clip_region"] != base_dbg["clip_region"]
                ):
                    column_events.append("clip_region_changed")
                if (
                    plus_jump > branch_jump_threshold
                    or minus_jump > branch_jump_threshold
                ):
                    column_events.append("branch_jump")
                if consistency > consistency_tolerance:
                    column_events.append("one_sided_derivative_mismatch")

                column_valid = not column_events
                if column_valid:
                    A[:, j] = _wrap_angle_delta(plus_q7 - minus_q7) / (
                        2.0 * fd_step
                    )
                else:
                    events.extend(f"column_{j}:{event}" for event in column_events)

                columns.append(
                    {
                        "index": j,
                        "valid": column_valid,
                        "events": tuple(column_events),
                        "plus_success": plus_dbg["success"],
                        "minus_success": minus_dbg["success"],
                        "plus_jump": plus_jump,
                        "minus_jump": minus_jump,
                        "one_sided_consistency": consistency,
                    }
                )

        valid = bool(
            base_dbg["success"]
            and base_dbg["smooth"]
            and len(columns) == 7
            and all(column["valid"] for column in columns)
            and np.all(np.isfinite(A))
        )
        info = {
            "valid": valid,
            "invalid": not valid,
            "smooth": valid,
            "events": tuple(dict.fromkeys(events)),
            "fd_step": float(fd_step),
            "fixed_branch_seed": base_q7.copy(),
            "base": base_dbg,
            "columns": tuple(columns),
        }
        self._fk_slave(base_q_s)
        return A, info

    def retarget_with_differential(
        self,
        q_m7: np.ndarray,
        q_s_init: Optional[np.ndarray] = None,
        fd_step: float = 1e-4,
        branch_jump_threshold: float = 0.25,
        consistency_tolerance: float = 5e-2,
    ) -> Tuple[np.ndarray, np.ndarray, Dict]:
        """
        Stable simulation-facing interface returning ``(q_s, A, debug)``.

        ``debug["success"]`` describes the bounded pose recovery;
        ``debug["differential_valid"]`` must be checked independently before
        using A for velocity/force mapping.
        """
        q_s, pose_debug = self.retarget(q_m7, q_s_init=q_s_init)
        A, differential_debug = self.compute_differential(
            q_m7,
            q_s_init=q_s,
            fd_step=fd_step,
            branch_jump_threshold=branch_jump_threshold,
            consistency_tolerance=consistency_tolerance,
        )
        debug = {
            **pose_debug,
            "differential_valid": bool(differential_debug["valid"]),
            "differential": differential_debug,
        }
        self._fk_slave(q_s)
        return q_s, A, debug