# -*- 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_fallback = float(np.linalg.norm(nref_tilde)) < 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, "reach_clipped": clip_region != "none", "clip_region": clip_region, "master_reach": d_m, "slave_reach": d_s, "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"], "master_reach": target["master_reach"], "slave_reach": target["slave_reach"], "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