"""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), )