exoskeleton/code/core/retargeting_baselines.py

"""Reproducible retargeting contracts and H1 baseline implementations.

The historical retargeting scripts returned method-specific tuples and debug
dictionaries.  Formal trajectory experiments need every method to expose the
same success/failure semantics and solver diagnostics.  This module provides
that contract without changing :mod:`core.sew_mapper2`.

The two Cartesian baselines intentionally share one :class:`PoseTargetBuilder`
so their comparison changes only the inverse-kinematics method.  The builder
maps *changes* in the master shoulder-relative end-effector pose about frozen
reference configurations.  This makes the reference pose exactly attainable
on the slave while keeping the mapping explicit and reproducible.
"""

from __future__ import annotations

from dataclasses import dataclass
from enum import Enum
from time import perf_counter
from typing import Optional, Protocol, Sequence, runtime_checkable

import numpy as np
import pinocchio as pin

from .model_contract import (
    MASTER_FRAMES,
    MASTER_JOINT_NAMES,
    SLAVE_FRAMES,
    SLAVE_JOINT_NAMES,
    TeleoperationModels,
    finite_joint_limits,
    joint_q_indices,
    load_models,
    require_frame,
    safe_configuration,
)
from .sew_mapper2 import BallJointConfig, SEWMapper


def _readonly_array(value: np.ndarray, shape: tuple[int, ...], name: str) -> np.ndarray:
    array = np.asarray(value, dtype=float).copy()
    if array.shape != shape:
        raise ValueError(f"{name} must have shape {shape}, got {array.shape}")
    if not np.all(np.isfinite(array)):
        raise ValueError(f"{name} must contain only finite values")
    array.setflags(write=False)
    return array


def _pose_error(
    current_position: np.ndarray,
    current_rotation: np.ndarray,
    target: "PoseTarget",
) -> tuple[np.ndarray, np.ndarray]:
    """Return position and world-axis orientation errors."""
    position_error = target.position - current_position
    orientation_error = pin.log3(target.rotation @ current_rotation.T)
    return position_error, orientation_error


class RetargetingFailure(str, Enum):
    """Stable trajectory-level failure categories used by H1."""

    NONE = "none"
    INVALID_INPUT = "invalid_input"
    MASTER_LIMIT_VIOLATION = "master_limit_violation"
    DEGENERATE_GEOMETRY = "degenerate_geometry"
    JOINT_LIMIT_VIOLATION = "joint_limit_violation"
    TASK_TOLERANCE_EXCEEDED = "task_tolerance_exceeded"
    SOLVER_NOT_CONVERGED = "solver_not_converged"
    NUMERICAL_FAILURE = "numerical_failure"


class RetargetingSolverStatus(str, Enum):
    """Method-independent solver termination status."""

    CLOSED_FORM = "closed_form"
    CONVERGED = "converged"
    MAX_ITERATIONS = "max_iterations"
    INVALID_INPUT = "invalid_input"
    FAILED = "failed"


@dataclass(frozen=True)
class PoseTarget:
    """Slave end-effector target, expressed in the slave model world frame."""

    position: np.ndarray
    rotation: np.ndarray
    valid: bool = True
    smooth: bool = True
    events: tuple[str, ...] = ()

    def __post_init__(self) -> None:
        object.__setattr__(
            self, "position", _readonly_array(self.position, (3,), "position")
        )
        object.__setattr__(
            self, "rotation", _readonly_array(self.rotation, (3, 3), "rotation")
        )
        if not np.allclose(self.rotation.T @ self.rotation, np.eye(3), atol=1e-8):
            raise ValueError("rotation must be orthonormal")
        if np.linalg.det(self.rotation) <= 0.0:
            raise ValueError("rotation must be a proper rotation")
        object.__setattr__(
            self, "events", tuple(str(event) for event in self.events)
        )
        if self.smooth and not self.valid:
            raise ValueError("an invalid target cannot be smooth")


@dataclass(frozen=True)
class RetargetingDiagnostics:
    """Diagnostics recorded once per retargeting sample."""

    status: RetargetingSolverStatus
    iterations: int
    runtime_s: float
    cost: float
    position_error_m: float
    orientation_error_rad: float
    active_limit_indices: tuple[int, ...] = ()
    clipped: bool = False
    message: str = ""

    def __post_init__(self) -> None:
        if self.iterations < 0:
            raise ValueError("iterations must be non-negative")
        numeric = (
            self.runtime_s,
            self.cost,
            self.position_error_m,
            self.orientation_error_rad,
        )
        if any(not np.isfinite(item) or item < 0.0 for item in numeric):
            raise ValueError("diagnostic scalars must be finite and non-negative")


@dataclass(frozen=True)
class RetargetingResult:
    """Uniform output returned by every retargeting method."""

    method: str
    q_slave: np.ndarray
    success: bool
    smooth: bool
    failure: RetargetingFailure
    diagnostics: RetargetingDiagnostics
    target: Optional[PoseTarget] = None
    events: tuple[str, ...] = ()

    def __post_init__(self) -> None:
        q_slave = np.asarray(self.q_slave, dtype=float).copy()
        if q_slave.ndim != 1:
            raise ValueError("q_slave must be a one-dimensional configuration")
        if not np.all(np.isfinite(q_slave)):
            raise ValueError("q_slave must contain only finite values")
        q_slave.setflags(write=False)
        object.__setattr__(self, "q_slave", q_slave)
        if not self.method:
            raise ValueError("method must be non-empty")
        if self.success != (self.failure is RetargetingFailure.NONE):
            raise ValueError("success must be equivalent to failure == NONE")
        if self.smooth and not self.success:
            raise ValueError("a failed result cannot be smooth")


@runtime_checkable
class Retargeter(Protocol):
    """Structural interface consumed by a future trajectory experiment runner."""

    name: str

    def retarget(
        self,
        q_master: np.ndarray,
        q_slave_seed: Optional[np.ndarray] = None,
    ) -> RetargetingResult:
        ...


class PoseTargetProvider(Protocol):
    """Target policy shared by Cartesian retargeting baselines."""

    q_slave_reference: np.ndarray

    def build(self, q_master: np.ndarray) -> PoseTarget:
        ...


class PoseTargetBuilder:
    """Build one frozen, cross-embodiment Cartesian target convention."""

    def __init__(
        self,
        master_model: pin.Model,
        slave_model: pin.Model,
        *,
        master_joint_names: Sequence[str],
        slave_joint_names: Sequence[str],
        master_shoulder_frame: str,
        master_ee_frame: str,
        slave_shoulder_frame: str,
        slave_ee_frame: str,
        q_master_reference: Optional[np.ndarray] = None,
        q_slave_reference: Optional[np.ndarray] = None,
        translation_scale: Optional[float] = None,
    ) -> None:
        self.master_model = master_model
        self.slave_model = slave_model
        self.master_data = master_model.createData()
        self.slave_data = slave_model.createData()
        self.master_q_indices = joint_q_indices(master_model, master_joint_names)
        self.slave_q_indices = joint_q_indices(slave_model, slave_joint_names)
        self.master_shoulder_id = require_frame(master_model, master_shoulder_frame)
        self.master_ee_id = require_frame(master_model, master_ee_frame)
        self.slave_shoulder_id = require_frame(slave_model, slave_shoulder_frame)
        self.slave_ee_id = require_frame(slave_model, slave_ee_frame)

        q_m_ref = (
            safe_configuration(master_model, master_joint_names)
            if q_master_reference is None
            else np.asarray(q_master_reference, dtype=float)
        )
        q_s_ref = (
            safe_configuration(slave_model, slave_joint_names)
            if q_slave_reference is None
            else np.asarray(q_slave_reference, dtype=float)
        )
        self.q_master_reference = _readonly_array(
            q_m_ref, (master_model.nq,), "q_master_reference"
        )
        self.q_slave_reference = _readonly_array(
            q_s_ref, (slave_model.nq,), "q_slave_reference"
        )

        master_reference = self._master_pose(self.q_master_reference)
        slave_reference = self._slave_pose(self.q_slave_reference)
        self._master_relative_reference = (
            master_reference[0] - master_reference[2]
        )
        self._slave_position_reference = slave_reference[0]
        self._orientation_alignment = (
            slave_reference[1] @ master_reference[1].T
        )
        self._position_axis_alignment = (
            slave_reference[3] @ master_reference[3].T
        )

        master_reach = float(np.linalg.norm(self._master_relative_reference))
        slave_reach = float(
            np.linalg.norm(slave_reference[0] - slave_reference[2])
        )
        if master_reach <= 1e-9 or slave_reach <= 1e-9:
            raise ValueError("reference shoulder-to-EE distances must be non-zero")
        scale = slave_reach / master_reach if translation_scale is None else float(
            translation_scale
        )
        if not np.isfinite(scale) or scale <= 0.0:
            raise ValueError("translation_scale must be finite and positive")
        self.translation_scale = scale

    def _master_pose(
        self, q: np.ndarray
    ) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
        pin.forwardKinematics(self.master_model, self.master_data, q)
        pin.updateFramePlacements(self.master_model, self.master_data)
        ee = self.master_data.oMf[self.master_ee_id]
        shoulder = self.master_data.oMf[self.master_shoulder_id]
        return (
            ee.translation.copy(),
            ee.rotation.copy(),
            shoulder.translation.copy(),
            shoulder.rotation.copy(),
        )

    def _slave_pose(
        self, q: np.ndarray
    ) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
        pin.forwardKinematics(self.slave_model, self.slave_data, q)
        pin.updateFramePlacements(self.slave_model, self.slave_data)
        ee = self.slave_data.oMf[self.slave_ee_id]
        shoulder = self.slave_data.oMf[self.slave_shoulder_id]
        return (
            ee.translation.copy(),
            ee.rotation.copy(),
            shoulder.translation.copy(),
            shoulder.rotation.copy(),
        )

    def build(self, q_master: np.ndarray) -> PoseTarget:
        q_master = _readonly_array(
            q_master, (self.master_model.nq,), "q_master"
        )
        position, rotation, shoulder_position, _ = self._master_pose(q_master)
        relative = position - shoulder_position
        relative_delta = relative - self._master_relative_reference
        target_position = self._slave_position_reference + (
            self.translation_scale
            * self._position_axis_alignment
            @ relative_delta
        )
        target_rotation = self._orientation_alignment @ rotation
        return PoseTarget(target_position, target_rotation)


class SEWFeasiblePoseTargetBuilder:
    """Expose the SEW mapper's feasible wrist target to comparator IK methods.

    This builder supports the attribution block in which bounded DLS and
    task-priority IK receive exactly the same wrist position/orientation target
    as the proposed SEW recovery.  It deliberately does not expose the SEW
    elbow target to either comparator.
    """

    def __init__(
        self,
        mapper: SEWMapper,
        q_slave_reference: Optional[np.ndarray] = None,
    ) -> None:
        self.mapper = mapper
        reference = (
            safe_configuration(
                mapper.s_model,
                tuple(
                    mapper.s_model.names[joint_id]
                    for joint_id in mapper.s_joint_ids
                ),
            )
            if q_slave_reference is None
            else np.asarray(q_slave_reference, dtype=float)
        )
        self.q_slave_reference = _readonly_array(
            reference, (mapper.s_model.nq,), "q_slave_reference"
        )

    def build(self, q_master: np.ndarray) -> PoseTarget:
        q_master = np.asarray(q_master, dtype=float)
        if q_master.shape == (self.mapper.m_model.nq,):
            q_master7 = q_master[self.mapper.m_qidx7]
        elif q_master.shape == (7,):
            q_master7 = q_master
        else:
            raise ValueError(
                "q_master must be a full master configuration or a 7-vector"
            )
        target = self.mapper._target_from_master(q_master7)
        nonsmooth_events = {
            "reach_clipped_lower",
            "reach_clipped_upper",
            "reference_axis_fallback",
            "master_shoulder_wrist_degenerate",
            "master_arm_plane_degenerate",
        }
        events = tuple(str(event) for event in target["events"])
        valid = bool(target["hard_geometry_valid"])
        smooth = bool(
            valid and not any(event in nonsmooth_events for event in events)
        )
        return PoseTarget(
            target["pW_s_ref"],
            target["RmEE"],
            valid=valid,
            smooth=smooth,
            events=events,
        )


class _RetargetingBase:
    """Shared validation and kinematic diagnostics."""

    name = "retargeting_base"

    def __init__(
        self,
        models: TeleoperationModels,
        target_builder: PoseTargetProvider,
        *,
        master_joint_names: Sequence[str] = MASTER_JOINT_NAMES,
        slave_joint_names: Sequence[str] = SLAVE_JOINT_NAMES,
        slave_ee_frame: str = SLAVE_FRAMES["ee"],
        joint_limit_margin: float = 1e-4,
    ) -> None:
        self.master_model = models.master
        self.slave_model = models.slave
        self.target_builder = target_builder
        self.master_q_indices = joint_q_indices(
            self.master_model, master_joint_names
        )
        self.slave_q_indices = joint_q_indices(self.slave_model, slave_joint_names)
        self.master_lower, self.master_upper = finite_joint_limits(
            self.master_model, master_joint_names
        )
        self.slave_lower, self.slave_upper = finite_joint_limits(
            self.slave_model, slave_joint_names
        )
        self.slave_ee_id = require_frame(self.slave_model, slave_ee_frame)
        self.slave_data = self.slave_model.createData()
        self.joint_limit_margin = float(joint_limit_margin)
        if not np.isfinite(self.joint_limit_margin) or self.joint_limit_margin < 0.0:
            raise ValueError("joint_limit_margin must be finite and non-negative")

    def _master_configuration(
        self, q_master: np.ndarray
    ) -> tuple[Optional[np.ndarray], Optional[RetargetingFailure]]:
        q_input = np.asarray(q_master, dtype=float)
        if q_input.shape == (self.master_model.nq,):
            q = q_input.copy()
        elif q_input.shape == (len(self.master_q_indices),):
            q = pin.neutral(self.master_model)
            q[self.master_q_indices] = q_input
        else:
            return None, RetargetingFailure.INVALID_INPUT
        if not np.all(np.isfinite(q)):
            return None, RetargetingFailure.INVALID_INPUT
        q7 = q[self.master_q_indices]
        if np.any(q7 < self.master_lower) or np.any(q7 > self.master_upper):
            return None, RetargetingFailure.MASTER_LIMIT_VIOLATION
        return q, None

    def _slave_seed(self, seed: Optional[np.ndarray]) -> Optional[np.ndarray]:
        if seed is None:
            return np.asarray(
                self.target_builder.q_slave_reference, dtype=float
            ).copy()
        seed_array = np.asarray(seed, dtype=float)
        if seed_array.shape == (self.slave_model.nq,):
            q = seed_array.copy()
        elif seed_array.shape == (len(self.slave_q_indices),):
            q = pin.neutral(self.slave_model)
            q[self.slave_q_indices] = seed_array
        else:
            return None
        if not np.all(np.isfinite(q)):
            return None
        q7 = q[self.slave_q_indices]
        if np.any(q7 < self.slave_lower) or np.any(q7 > self.slave_upper):
            return None
        return q

    def _kinematics(
        self, q_slave: np.ndarray, target: PoseTarget
    ) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
        pin.forwardKinematics(self.slave_model, self.slave_data, q_slave)
        pin.updateFramePlacements(self.slave_model, self.slave_data)
        placement = self.slave_data.oMf[self.slave_ee_id]
        position_error, orientation_error = _pose_error(
            placement.translation, placement.rotation, target
        )
        jacobian = pin.computeFrameJacobian(
            self.slave_model,
            self.slave_data,
            q_slave,
            self.slave_ee_id,
            pin.ReferenceFrame.LOCAL_WORLD_ALIGNED,
        )
        return position_error, orientation_error, jacobian[:, self.slave_q_indices]

    def _active_limits(self, q_slave: np.ndarray) -> tuple[int, ...]:
        q7 = q_slave[self.slave_q_indices]
        clearance = np.minimum(q7 - self.slave_lower, self.slave_upper - q7)
        return tuple(
            int(index)
            for index in np.flatnonzero(clearance <= self.joint_limit_margin)
        )

    def _invalid_result(
        self,
        failure: RetargetingFailure,
        started_at: float,
        message: str,
    ) -> RetargetingResult:
        q = np.asarray(self.target_builder.q_slave_reference, dtype=float)
        diagnostics = RetargetingDiagnostics(
            status=RetargetingSolverStatus.INVALID_INPUT,
            iterations=0,
            runtime_s=max(0.0, perf_counter() - started_at),
            cost=0.0,
            position_error_m=0.0,
            orientation_error_rad=0.0,
            message=message,
        )
        return RetargetingResult(
            method=self.name,
            q_slave=q,
            success=False,
            smooth=False,
            failure=failure,
            diagnostics=diagnostics,
            events=(failure.value,),
        )

    def _invalid_target_result(
        self,
        target: PoseTarget,
        started_at: float,
    ) -> RetargetingResult:
        diagnostics = RetargetingDiagnostics(
            status=RetargetingSolverStatus.FAILED,
            iterations=0,
            runtime_s=max(0.0, perf_counter() - started_at),
            cost=0.0,
            position_error_m=0.0,
            orientation_error_rad=0.0,
            message="invalid Cartesian target",
        )
        events = tuple(
            dict.fromkeys(
                (*target.events, RetargetingFailure.DEGENERATE_GEOMETRY.value)
            )
        )
        return RetargetingResult(
            method=self.name,
            q_slave=self.target_builder.q_slave_reference,
            success=False,
            smooth=False,
            failure=RetargetingFailure.DEGENERATE_GEOMETRY,
            diagnostics=diagnostics,
            target=target,
            events=events,
        )


class ScaledJointSpaceRetargeter(_RetargetingBase):
    """Anthropometrically scaled joint-range mapping baseline."""

    name = "scaled_joint_space"

    def retarget(
        self,
        q_master: np.ndarray,
        q_slave_seed: Optional[np.ndarray] = None,
    ) -> RetargetingResult:
        del q_slave_seed  # Closed-form baseline has no branch state.
        started_at = perf_counter()
        q_m, failure = self._master_configuration(q_master)
        if failure is not None:
            return self._invalid_result(failure, started_at, failure.value)

        target = self.target_builder.build(q_m)
        if not target.valid:
            return self._invalid_target_result(target, started_at)
        normalized = (
            q_m[self.master_q_indices] - self.master_lower
        ) / (self.master_upper - self.master_lower)
        q_slave = pin.neutral(self.slave_model)
        q_slave[self.slave_q_indices] = self.slave_lower + normalized * (
            self.slave_upper - self.slave_lower
        )
        position_error, orientation_error, _ = self._kinematics(q_slave, target)
        active = self._active_limits(q_slave)
        position_norm = float(np.linalg.norm(position_error))
        orientation_norm = float(np.linalg.norm(orientation_error))
        diagnostics = RetargetingDiagnostics(
            status=RetargetingSolverStatus.CLOSED_FORM,
            iterations=0,
            runtime_s=perf_counter() - started_at,
            cost=0.5 * (position_norm**2 + orientation_norm**2),
            position_error_m=position_norm,
            orientation_error_rad=orientation_norm,
            active_limit_indices=active,
        )
        return RetargetingResult(
            method=self.name,
            q_slave=q_slave,
            success=True,
            smooth=bool(target.smooth and not active),
            failure=RetargetingFailure.NONE,
            diagnostics=diagnostics,
            target=target,
            events=(
                *target.events,
                *(("joint_limit_active",) if active else ()),
            ),
        )


class BoundedDLSIKRetargeter(_RetargetingBase):
    """Bounded iterative damped-least-squares Cartesian IK baseline."""

    name = "bounded_dls_ik"

    def __init__(
        self,
        models: TeleoperationModels,
        target_builder: PoseTargetProvider,
        *,
        damping: float = 2e-3,
        orientation_weight_m: float = 0.20,
        max_iterations: int = 160,
        step_limit_rad: float = 0.20,
        position_tolerance_m: float = 2e-4,
        orientation_tolerance_rad: float = 2e-3,
        **kwargs,
    ) -> None:
        super().__init__(models, target_builder, **kwargs)
        self.damping = float(damping)
        self.orientation_weight_m = float(orientation_weight_m)
        self.max_iterations = int(max_iterations)
        self.step_limit_rad = float(step_limit_rad)
        self.position_tolerance_m = float(position_tolerance_m)
        self.orientation_tolerance_rad = float(orientation_tolerance_rad)
        positive = (
            self.damping,
            self.orientation_weight_m,
            self.step_limit_rad,
            self.position_tolerance_m,
            self.orientation_tolerance_rad,
        )
        if any(not np.isfinite(value) or value <= 0.0 for value in positive):
            raise ValueError("DLS tuning parameters must be finite and positive")
        if self.max_iterations <= 0:
            raise ValueError("max_iterations must be positive")

    def _weighted_cost(
        self, q_slave: np.ndarray, target: PoseTarget
    ) -> tuple[float, np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
        ep, eo, jacobian = self._kinematics(q_slave, target)
        error = np.concatenate((ep, self.orientation_weight_m * eo))
        weighted_jacobian = jacobian.copy()
        weighted_jacobian[3:, :] *= self.orientation_weight_m
        return 0.5 * float(error @ error), error, ep, eo, weighted_jacobian

    def retarget(
        self,
        q_master: np.ndarray,
        q_slave_seed: Optional[np.ndarray] = None,
    ) -> RetargetingResult:
        started_at = perf_counter()
        q_m, failure = self._master_configuration(q_master)
        if failure is not None:
            return self._invalid_result(failure, started_at, failure.value)
        q_slave = self._slave_seed(q_slave_seed)
        if q_slave is None:
            return self._invalid_result(
                RetargetingFailure.INVALID_INPUT,
                started_at,
                "invalid q_slave_seed",
            )
        target = self.target_builder.build(q_m)
        if not target.valid:
            return self._invalid_target_result(target, started_at)

        status = RetargetingSolverStatus.MAX_ITERATIONS
        iterations = 0
        numerical_failure = False
        for iterations in range(1, self.max_iterations + 1):
            cost, error, ep, eo, jacobian = self._weighted_cost(q_slave, target)
            if (
                np.linalg.norm(ep) <= self.position_tolerance_m
                and np.linalg.norm(eo) <= self.orientation_tolerance_rad
            ):
                status = RetargetingSolverStatus.CONVERGED
                break
            try:
                normal = jacobian @ jacobian.T + (
                    self.damping**2
                ) * np.eye(6)
                step = jacobian.T @ np.linalg.solve(normal, error)
            except np.linalg.LinAlgError:
                numerical_failure = True
                status = RetargetingSolverStatus.FAILED
                break
            step_norm = float(np.linalg.norm(step))
            if not np.isfinite(step_norm):
                numerical_failure = True
                status = RetargetingSolverStatus.FAILED
                break
            if step_norm > self.step_limit_rad:
                step *= self.step_limit_rad / step_norm

            q7 = q_slave[self.slave_q_indices]
            accepted = False
            for scale in (1.0, 0.5, 0.25, 0.125, 0.0625):
                candidate = q_slave.copy()
                candidate[self.slave_q_indices] = np.clip(
                    q7 + scale * step, self.slave_lower, self.slave_upper
                )
                candidate_cost = self._weighted_cost(candidate, target)[0]
                if candidate_cost < cost:
                    q_slave = candidate
                    accepted = True
                    break
            if not accepted:
                # A deterministic small step lets the active set change while
                # still bounding the solver near a stationary point.
                candidate = q_slave.copy()
                candidate[self.slave_q_indices] = np.clip(
                    q7 + 0.01 * step, self.slave_lower, self.slave_upper
                )
                if self._weighted_cost(candidate, target)[0] < cost:
                    q_slave = candidate
                else:
                    status = RetargetingSolverStatus.FAILED
                    break

        cost, _, ep, eo, _ = self._weighted_cost(q_slave, target)
        position_norm = float(np.linalg.norm(ep))
        orientation_norm = float(np.linalg.norm(eo))
        converged = bool(
            position_norm <= self.position_tolerance_m
            and orientation_norm <= self.orientation_tolerance_rad
        )
        if converged:
            status = RetargetingSolverStatus.CONVERGED
            failure = RetargetingFailure.NONE
        elif numerical_failure:
            failure = RetargetingFailure.NUMERICAL_FAILURE
        elif iterations >= self.max_iterations:
            failure = RetargetingFailure.SOLVER_NOT_CONVERGED
        else:
            failure = RetargetingFailure.TASK_TOLERANCE_EXCEEDED
        active = self._active_limits(q_slave)
        diagnostics = RetargetingDiagnostics(
            status=status,
            iterations=iterations,
            runtime_s=perf_counter() - started_at,
            cost=cost,
            position_error_m=position_norm,
            orientation_error_rad=orientation_norm,
            active_limit_indices=active,
            clipped=bool(active),
        )
        events: list[str] = list(target.events)
        if failure is not RetargetingFailure.NONE:
            events.append(failure.value)
        if active:
            events.append("joint_limit_active")
        return RetargetingResult(
            method=self.name,
            q_slave=q_slave,
            success=converged,
            smooth=bool(converged and target.smooth and not active),
            failure=failure,
            diagnostics=diagnostics,
            target=target,
            events=tuple(events),
        )


class TaskPriorityIKRetargeter(BoundedDLSIKRetargeter):
    """Position-first IK with orientation, centering, and manipulability tasks."""

    name = "task_priority_ik"

    def __init__(
        self,
        models: TeleoperationModels,
        target_builder: PoseTargetProvider,
        *,
        joint_centering_gain: float = 0.05,
        manipulability_gain: float = 0.005,
        manipulability_fd_step_rad: float = 1e-4,
        manipulability_regularization: float = 1e-8,
        **kwargs,
    ) -> None:
        super().__init__(models, target_builder, **kwargs)
        self.joint_centering_gain = float(joint_centering_gain)
        self.manipulability_gain = float(manipulability_gain)
        self.manipulability_fd_step_rad = float(manipulability_fd_step_rad)
        self.manipulability_regularization = float(
            manipulability_regularization
        )
        if (
            not np.isfinite(self.joint_centering_gain)
            or self.joint_centering_gain < 0.0
        ):
            raise ValueError(
                "joint_centering_gain must be finite and non-negative"
            )
        nonnegative = (
            self.manipulability_gain,
            self.manipulability_fd_step_rad,
            self.manipulability_regularization,
        )
        if any(not np.isfinite(value) or value < 0.0 for value in nonnegative):
            raise ValueError(
                "manipulability settings must be finite and non-negative"
            )
        if self.manipulability_gain > 0.0 and (
            self.manipulability_fd_step_rad <= 0.0
            or self.manipulability_regularization <= 0.0
        ):
            raise ValueError(
                "enabled manipulability optimization needs positive FD and "
                "regularization values"
            )

    def _damped_pseudoinverse(self, matrix: np.ndarray) -> np.ndarray:
        rows = matrix.shape[0]
        return matrix.T @ np.linalg.solve(
            matrix @ matrix.T + (self.damping**2) * np.eye(rows),
            np.eye(rows),
        )

    def _log_manipulability(self, q_slave: np.ndarray) -> float:
        """Regularized log-volume of the six-dimensional velocity ellipsoid."""
        pin.forwardKinematics(self.slave_model, self.slave_data, q_slave)
        pin.updateFramePlacements(self.slave_model, self.slave_data)
        jacobian = pin.computeFrameJacobian(
            self.slave_model,
            self.slave_data,
            q_slave,
            self.slave_ee_id,
            pin.ReferenceFrame.LOCAL_WORLD_ALIGNED,
        )[:, self.slave_q_indices]
        sign, log_determinant = np.linalg.slogdet(
            jacobian @ jacobian.T
            + self.manipulability_regularization * np.eye(6)
        )
        return 0.5 * float(log_determinant) if sign > 0.0 else -np.inf

    def _manipulability_gradient(self, q_slave: np.ndarray) -> np.ndarray:
        if self.manipulability_gain == 0.0:
            return np.zeros(7, dtype=float)
        gradient = np.zeros(7, dtype=float)
        q7 = q_slave[self.slave_q_indices]
        step = self.manipulability_fd_step_rad
        for joint in range(7):
            plus = q_slave.copy()
            minus = q_slave.copy()
            plus7 = q7.copy()
            minus7 = q7.copy()
            plus7[joint] = min(self.slave_upper[joint], plus7[joint] + step)
            minus7[joint] = max(self.slave_lower[joint], minus7[joint] - step)
            denominator = plus7[joint] - minus7[joint]
            if denominator <= 0.0:
                continue
            plus[self.slave_q_indices] = plus7
            minus[self.slave_q_indices] = minus7
            gradient[joint] = (
                self._log_manipulability(plus)
                - self._log_manipulability(minus)
            ) / denominator
        return gradient

    def retarget(
        self,
        q_master: np.ndarray,
        q_slave_seed: Optional[np.ndarray] = None,
    ) -> RetargetingResult:
        started_at = perf_counter()
        q_m, failure = self._master_configuration(q_master)
        if failure is not None:
            return self._invalid_result(failure, started_at, failure.value)
        q_slave = self._slave_seed(q_slave_seed)
        if q_slave is None:
            return self._invalid_result(
                RetargetingFailure.INVALID_INPUT,
                started_at,
                "invalid q_slave_seed",
            )
        target = self.target_builder.build(q_m)
        if not target.valid:
            return self._invalid_target_result(target, started_at)
        center = 0.5 * (self.slave_lower + self.slave_upper)

        status = RetargetingSolverStatus.MAX_ITERATIONS
        numerical_failure = False
        iterations = 0
        for iterations in range(1, self.max_iterations + 1):
            cost, _, ep, eo, jacobian = self._weighted_cost(q_slave, target)
            if (
                np.linalg.norm(ep) <= self.position_tolerance_m
                and np.linalg.norm(eo) <= self.orientation_tolerance_rad
            ):
                status = RetargetingSolverStatus.CONVERGED
                break
            try:
                jp = jacobian[:3, :]
                jo = jacobian[3:, :] / self.orientation_weight_m
                jp_inverse = self._damped_pseudoinverse(jp)
                dq_position = jp_inverse @ ep
                null_position = np.eye(7) - jp_inverse @ jp

                jo_null = jo @ null_position
                jo_null_inverse = self._damped_pseudoinverse(jo_null)
                dq_orientation = jo_null_inverse @ (eo - jo @ dq_position)

                full_jacobian = np.vstack((jp, jo))
                full_inverse = self._damped_pseudoinverse(full_jacobian)
                null_full = np.eye(7) - full_inverse @ full_jacobian
                q7 = q_slave[self.slave_q_indices]
                dq_center = (
                    self.joint_centering_gain * null_full @ (center - q7)
                )
                dq_manipulability = (
                    self.manipulability_gain
                    * null_full
                    @ self._manipulability_gradient(q_slave)
                )
                step = (
                    dq_position
                    + dq_orientation
                    + dq_center
                    + dq_manipulability
                )
            except np.linalg.LinAlgError:
                numerical_failure = True
                status = RetargetingSolverStatus.FAILED
                break
            step_norm = float(np.linalg.norm(step))
            if not np.isfinite(step_norm):
                numerical_failure = True
                status = RetargetingSolverStatus.FAILED
                break
            if step_norm > self.step_limit_rad:
                step *= self.step_limit_rad / step_norm

            q7 = q_slave[self.slave_q_indices]
            accepted = False
            for scale in (1.0, 0.5, 0.25, 0.125, 0.0625):
                candidate = q_slave.copy()
                candidate[self.slave_q_indices] = np.clip(
                    q7 + scale * step, self.slave_lower, self.slave_upper
                )
                if self._weighted_cost(candidate, target)[0] < cost:
                    q_slave = candidate
                    accepted = True
                    break
            if not accepted:
                status = RetargetingSolverStatus.FAILED
                break

        cost, _, ep, eo, _ = self._weighted_cost(q_slave, target)
        position_norm = float(np.linalg.norm(ep))
        orientation_norm = float(np.linalg.norm(eo))
        converged = bool(
            position_norm <= self.position_tolerance_m
            and orientation_norm <= self.orientation_tolerance_rad
        )
        if converged:
            status = RetargetingSolverStatus.CONVERGED
            failure = RetargetingFailure.NONE
        elif numerical_failure:
            failure = RetargetingFailure.NUMERICAL_FAILURE
        elif iterations >= self.max_iterations:
            failure = RetargetingFailure.SOLVER_NOT_CONVERGED
        else:
            failure = RetargetingFailure.TASK_TOLERANCE_EXCEEDED
        active = self._active_limits(q_slave)
        diagnostics = RetargetingDiagnostics(
            status=status,
            iterations=iterations,
            runtime_s=perf_counter() - started_at,
            cost=cost,
            position_error_m=position_norm,
            orientation_error_rad=orientation_norm,
            active_limit_indices=active,
            clipped=bool(active),
            message="secondary_objective=log_manipulability",
        )
        events: list[str] = list(target.events)
        if failure is not RetargetingFailure.NONE:
            events.append(failure.value)
        if active:
            events.append("joint_limit_active")
        return RetargetingResult(
            method=self.name,
            q_slave=q_slave,
            success=converged,
            smooth=bool(converged and target.smooth and not active),
            failure=failure,
            diagnostics=diagnostics,
            target=target,
            events=tuple(events),
        )


class SEWRetargeterAdapter:
    """Expose the existing bounded SEW mapper through :class:`Retargeter`."""

    name = "sew"

    def __init__(self, mapper) -> None:
        self.mapper = mapper

    def retarget(
        self,
        q_master: np.ndarray,
        q_slave_seed: Optional[np.ndarray] = None,
    ) -> RetargetingResult:
        started_at = perf_counter()
        try:
            q_slave, debug = self.mapper.retarget(
                q_master, q_s_init=q_slave_seed
            )
        except (ValueError, FloatingPointError) as error:
            diagnostics = RetargetingDiagnostics(
                status=RetargetingSolverStatus.INVALID_INPUT,
                iterations=0,
                runtime_s=perf_counter() - started_at,
                cost=0.0,
                position_error_m=0.0,
                orientation_error_rad=0.0,
                message=str(error),
            )
            return RetargetingResult(
                method=self.name,
                q_slave=pin.neutral(self.mapper.s_model),
                success=False,
                smooth=False,
                failure=RetargetingFailure.INVALID_INPUT,
                diagnostics=diagnostics,
                events=(RetargetingFailure.INVALID_INPUT.value,),
            )
        events = tuple(str(event) for event in debug.get("events", ()))
        if debug.get("success", False):
            failure = RetargetingFailure.NONE
        elif "invalid_master_geometry" in events:
            failure = RetargetingFailure.DEGENERATE_GEOMETRY
        elif "joint_limit_violation" in events:
            failure = RetargetingFailure.JOINT_LIMIT_VIOLATION
        elif "task_tolerance_exceeded" in events:
            failure = RetargetingFailure.TASK_TOLERANCE_EXCEEDED
        elif "bounded_solver_not_converged" in events:
            failure = RetargetingFailure.SOLVER_NOT_CONVERGED
        else:
            failure = RetargetingFailure.NUMERICAL_FAILURE
        diagnostics = RetargetingDiagnostics(
            status=(
                RetargetingSolverStatus.CONVERGED
                if failure is RetargetingFailure.NONE
                else RetargetingSolverStatus.FAILED
            ),
            iterations=int(debug.get("solver_nfev", 0)),
            runtime_s=perf_counter() - started_at,
            cost=max(0.0, float(debug.get("solver_cost", 0.0))),
            position_error_m=max(0.0, float(debug.get("position_error", 0.0))),
            orientation_error_rad=max(
                0.0, float(debug.get("orientation_error", 0.0))
            ),
            active_limit_indices=tuple(
                int(index)
                for index in debug.get("joint_limit_active_indices", ())
            ),
            clipped=bool(debug.get("reach_clipped", False)),
        )
        return RetargetingResult(
            method=self.name,
            q_slave=q_slave,
            success=failure is RetargetingFailure.NONE,
            smooth=bool(debug.get("smooth", False)),
            failure=failure,
            diagnostics=diagnostics,
            events=events,
        )


def build_canonical_baselines(
    models: Optional[TeleoperationModels] = None,
) -> dict[str, Retargeter]:
    """Build the three frozen H1 baselines for the canonical URDF pair."""
    canonical_models = load_models() if models is None else models
    target_builder = PoseTargetBuilder(
        canonical_models.master,
        canonical_models.slave,
        master_joint_names=MASTER_JOINT_NAMES,
        slave_joint_names=SLAVE_JOINT_NAMES,
        master_shoulder_frame=MASTER_FRAMES["shoulder"],
        master_ee_frame=MASTER_FRAMES["ee"],
        slave_shoulder_frame=SLAVE_FRAMES["shoulder"],
        slave_ee_frame=SLAVE_FRAMES["ee"],
    )
    baselines: tuple[Retargeter, ...] = (
        ScaledJointSpaceRetargeter(canonical_models, target_builder),
        BoundedDLSIKRetargeter(canonical_models, target_builder),
        TaskPriorityIKRetargeter(canonical_models, target_builder),
    )
    return {baseline.name: baseline for baseline in baselines}


def build_canonical_sew_target_baselines(
    models: Optional[TeleoperationModels] = None,
) -> tuple[dict[str, Retargeter], SEWRetargeterAdapter]:
    """Build the H1 common-target attribution baselines and proposed adapter."""
    canonical_models = load_models() if models is None else models
    mapper = SEWMapper(
        master_model=canonical_models.master,
        slave_model=canonical_models.slave,
        m_shoulder=MASTER_FRAMES["shoulder"],
        m_elbow=MASTER_FRAMES["elbow"],
        m_wrist=MASTER_FRAMES["wrist"],
        m_ee=MASTER_FRAMES["ee"],
        s_shoulder=SLAVE_FRAMES["shoulder"],
        s_elbow=SLAVE_FRAMES["elbow"],
        s_wrist=SLAVE_FRAMES["wrist"],
        s_ee=SLAVE_FRAMES["wrist"],
        master_joint_names=MASTER_JOINT_NAMES,
        slave_joint_names=SLAVE_JOINT_NAMES,
        slave_shoulder_cfg=BallJointConfig(
            axis_order="yxy",
            joint_names=SLAVE_JOINT_NAMES[:3],
            signs=(-1.0, 1.0, -1.0),
        ),
        slave_wrist_cfg=BallJointConfig(
            axis_order="yzx",
            joint_names=SLAVE_JOINT_NAMES[4:],
            signs=(-1.0, 1.0, 1.0),
        ),
    )
    target_builder = SEWFeasiblePoseTargetBuilder(mapper)
    baselines: tuple[Retargeter, ...] = (
        ScaledJointSpaceRetargeter(
            canonical_models,
            target_builder,
            slave_ee_frame=SLAVE_FRAMES["wrist"],
        ),
        BoundedDLSIKRetargeter(
            canonical_models,
            target_builder,
            slave_ee_frame=SLAVE_FRAMES["wrist"],
        ),
        TaskPriorityIKRetargeter(
            canonical_models,
            target_builder,
            slave_ee_frame=SLAVE_FRAMES["wrist"],
        ),
    )
    return (
        {baseline.name: baseline for baseline in baselines},
        SEWRetargeterAdapter(mapper),
    )