From 5489edb2ff47e29bf76e57f478ee1be72ef0ee1c Mon Sep 17 00:00:00 2001 From: Chris von Csefalvay Date: Mon, 13 Jul 2026 05:55:27 -0600 Subject: [PATCH 1/6] feat(retargeters): add dVRK PSM XR teleoperation Signed-off-by: Chris von Csefalvay --- docs/source/references/retargeting/dvrk.rst | 150 +++ docs/source/references/retargeting/index.rst | 9 + .../isaaccapture/retargeters/__init__.py | 28 + .../isaacteleop/retargeters/DVRK/__init__.py | 30 + .../isaacteleop/retargeters/DVRK/control.py | 601 +++++++++ .../retargeters/DVRK/psm_retargeter.py | 300 +++++ .../test_dvrk_psm_retargeters.py | 1073 +++++++++++++++++ 7 files changed, 2191 insertions(+) create mode 100644 docs/source/references/retargeting/dvrk.rst create mode 100644 src/python/isaacteleop/retargeters/DVRK/__init__.py create mode 100644 src/python/isaacteleop/retargeters/DVRK/control.py create mode 100644 src/python/isaacteleop/retargeters/DVRK/psm_retargeter.py create mode 100644 tests/python/core/retargeting_engine/test_dvrk_psm_retargeters.py diff --git a/docs/source/references/retargeting/dvrk.rst b/docs/source/references/retargeting/dvrk.rst new file mode 100644 index 0000000000..131e1bfab4 --- /dev/null +++ b/docs/source/references/retargeting/dvrk.rst @@ -0,0 +1,150 @@ +.. SPDX-FileCopyrightText: Copyright (c) 2026 Chris von Csefalvay (HCLTech) +.. SPDX-License-Identifier: Apache-2.0 + +dVRK PSM retargeters +==================== + +The dVRK Patient Side Manipulator (PSM) retargeters map XR controllers to one or +two simulated PSMs. They convert controller state into absolute tool-pose and +paired-jaw targets. Isaac Lab reads those targets, uses the live articulation +Jacobian and joint state, and solves differential IK. + +They are not a physical dVRK driver or force-feedback system, and are not +intended for clinical workflow or surgical validation. + +At a glance +----------- + +.. list-table:: + :header-rows: 1 + :widths: 30 26 44 + + * - Retargeter + - Output + - Contract + * - ``DVRKPSMClutchRetargeter`` + - ``ee_pose``: 7-D ``[x, y, z, qx, qy, qz, qw]`` + - Updates an absolute pose while squeeze is held. Release holds the last + target; the next valid squeezed sample re-bases without moving it. + * - ``DVRKPSMGripperRetargeter`` + - ``jaw_targets``: 2-D ``[jaw_1, jaw_2]`` [rad] + - Maps latched trigger movement to two mirrored PSM jaw targets. The + output is not a scalar closedness value, and releasing the arm clutch + does not change it. + +The clutch accepts a sample only when controller tracking is valid and every +pose value is finite. If a pose sample is missing or invalid, the retargeter +holds the last safe target and clears its controller origin. The next valid +sample with squeeze above threshold captures a fresh origin without moving the +tool. An inactive teleoperation session holds both pose and jaw targets. + +Controller mapping +------------------ + +OpenXR's *grip pose* is the tracked controller-handle transform. It is not the +side grip button, which is called ``squeeze`` in the input packet. The pose and +squeeze control tool motion; the index trigger controls the jaws. + +For each PSM: + +.. code-block:: text + + controller grip pose in shared reference frame + -> squeeze deadman + origin-rebased absolute tool pose + -> six-DOF DLS IK at psm_tool_tip_link + + controller trigger + -> [psm_tool_gripper1_joint, psm_tool_gripper2_joint] + +These retargeters do not consume face buttons, thumbsticks, thumbstick clicks, +or menu buttons. A task or hosting XR application may bind those inputs +separately. + +The squeeze threshold defaults to ``0.5``. The trigger value captured at +squeeze engagement is neutral. The first jaw movement requires at least +``0.05`` travel from that value. Positive travel past the threshold starts +closing immediately. Negative travel must remain at least ``0.05`` below its +reference for ``0.1`` seconds before opening begins. Once opening is active, +further release opens the jaws continuously. Releasing squeeze, losing +tracking, or stopping the session cancels a pending opening and holds the last +target. Travel is capped at the configured physical endpoints: + +.. code-block:: text + + open endpoint: [-0.50, +0.50] rad + closed endpoint: [-0.09, +0.09] rad + +Those are library defaults, not a claim about every PSM asset. Pass the +articulation's measured open and grip targets through +``DVRKPSMGripperConfig`` when the simulator uses different endpoints. + +``initial_closedness`` selects the reset target: ``0`` is open and ``1`` is +closed. It does not inspect contact or attach an object. + +Reference frames +^^^^^^^^^^^^^^^^ + +``DVRKPSMClutchConfig`` does not prescribe a reference frame, but +``home_reference_T_ee``, controller input, and workspace bounds must all use the +same one. This can be a PSM base frame for a single arm or a shared world frame +for a bimanual setup. The home pose must lie within the workspace bounds so +the first clutch engagement remains continuous with the reset pose instead of +clipping to another target. + +For a bimanual setup, express both controller streams and home poses in one +world frame. Convert each target to that PSM's base frame immediately before +its IK solve. Reusing one base transform for independently placed PSMs is +incorrect. + +Use the retargeters from Python +------------------------------- + +.. code-block:: python + + from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource + from isaacteleop.retargeters import ( + DVRKPSMClutchConfig, + DVRKPSMClutchRetargeter, + DVRKPSMGripperConfig, + DVRKPSMGripperRetargeter, + ) + + left_pose = DVRKPSMClutchRetargeter( + DVRKPSMClutchConfig( + input_device=ControllersSource.LEFT, + home_reference_T_ee=left_home_world_T_ee, + workspace_lower=(-0.50, -0.40, -0.10), + workspace_upper=(0.50, 0.40, 0.60), + clutch_threshold=0.5, + ), + name="left_psm_pose", + ) + left_jaws = DVRKPSMGripperRetargeter( + DVRKPSMGripperConfig(input_device=ControllersSource.LEFT), + name="left_psm_jaws", + ) + +Create one pose retargeter and one jaw retargeter for each PSM. Concatenate the +outputs for a bimanual action in this order: + +.. code-block:: text + + left xyzxyzw, left jaws, right xyzxyzw, right jaws + +Use ``DVRKPSMClutchRetargeter.OUTPUT_POSE`` and +``DVRKPSMGripperRetargeter.OUTPUT_JAW_TARGETS`` when wiring those output ports. +These names are part of the public retargeter API, so callers do not need to +import implementation modules. + +Validation +---------- + +Tests that do not require Isaac Sim cover jaw endpoints, time-based opening +intent, jaw hold across clutch release and re-engagement, Cartesian clutch +behaviour, continuous motion, workspace bounds, invalid tracking, and malformed +samples. Fixed-trace tests also compare the public retargeter wrappers with the +shared NumPy kernels: + +.. code-block:: console + + $ ctest --test-dir build -R retargeting_test_dvrk_psm_retargeters --output-on-failure diff --git a/docs/source/references/retargeting/index.rst b/docs/source/references/retargeting/index.rst index a5071bc6f9..f8cef24cb4 100644 --- a/docs/source/references/retargeting/index.rst +++ b/docs/source/references/retargeting/index.rst @@ -68,6 +68,14 @@ Available Retargeters ``SO101GripperRetargeter`` maps the trigger to a proportional jaw closedness in ``[0, 1]``. See :doc:`so101` for the full setup. +.. dropdown:: DVRKPSMClutchRetargeter / DVRKPSMGripperRetargeter + + Simulated dVRK Patient Side Manipulator teleoperation primitives. The clutch + maps controller grip pose to a squeeze-deadman, re-clutched 7-D tool target; + the trigger maps to two mirrored PSM jaw targets. The simulator integration + owns the per-arm IK solve. See :doc:`dvrk` for the controller, frame, and + safety contracts. + .. dropdown:: JointStateRetargeter Maps a name-keyed joint-state input (from ``JointStateSource``) to an action for a generic @@ -379,5 +387,6 @@ See the :doc:`Contributing Guide <../../development/contributing>` for details. sharpa so101 + dvrk joint_space wuji diff --git a/src/python/isaaccapture/retargeters/__init__.py b/src/python/isaaccapture/retargeters/__init__.py index 518fb9b2bf..b8df06cca0 100644 --- a/src/python/isaaccapture/retargeters/__init__.py +++ b/src/python/isaaccapture/retargeters/__init__.py @@ -30,6 +30,8 @@ re-latches BOTH home position and orientation on every engage, base-frame left-composed, no fixed offset - SO101GripperRetargeter: Proportional (analog) jaw closedness for the SO-101 gripper + - DVRKPSMClutchRetargeter: No-jump absolute tool-pose clutch for a dVRK PSM + - DVRKPSMGripperRetargeter: Intent-latched paired dVRK PSM jaw targets - WujiHandRetargeter: Retargeting for the Wuji hand via wuji_sdk.retargeting - JointStateRetargeter: Generic joint-space device (leader arm, exoskeleton) -> joint or EE action - EePoseRateLimiter / JointRateLimiter: Safety-harness velocity bounds for EE-pose / joint streams @@ -226,6 +228,27 @@ "SO101GripperRetargeter", None, ), + # .DVRK (dVRK PSM: Cartesian clutch and paired jaw targets) + "DVRKPSMClutchConfig": ( + ".DVRK.psm_retargeter", + "DVRKPSMClutchConfig", + None, + ), + "DVRKPSMClutchRetargeter": ( + ".DVRK.psm_retargeter", + "DVRKPSMClutchRetargeter", + None, + ), + "DVRKPSMGripperConfig": ( + ".DVRK.psm_retargeter", + "DVRKPSMGripperConfig", + None, + ), + "DVRKPSMGripperRetargeter": ( + ".DVRK.psm_retargeter", + "DVRKPSMGripperRetargeter", + None, + ), # .wuji_hand_retargeter (requires wuji extra: wuji-sdk[retarget], # which transitively pulls nlopt, pinocchio, scipy) "WujiHandRetargeter": ( @@ -407,6 +430,11 @@ def __getattr__(name: str): # SO-101 5-DOF arm retargeters "SO101ClutchRetargeter", "SO101GripperRetargeter", + # dVRK PSM retargeters + "DVRKPSMClutchConfig", + "DVRKPSMClutchRetargeter", + "DVRKPSMGripperConfig", + "DVRKPSMGripperRetargeter", # Wuji hand retargeters (require wuji extra: wuji-sdk[retarget]) "WujiHandRetargeter", "WujiHandRetargeterConfig", diff --git a/src/python/isaacteleop/retargeters/DVRK/__init__.py b/src/python/isaacteleop/retargeters/DVRK/__init__.py new file mode 100644 index 0000000000..5c269788aa --- /dev/null +++ b/src/python/isaacteleop/retargeters/DVRK/__init__.py @@ -0,0 +1,30 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 Chris von Csefalvay (HCLTech) +# SPDX-License-Identifier: Apache-2.0 + +"""dVRK Patient Side Manipulator retargeters.""" + +import importlib as _importlib + +_RETARGETER_EXPORTS = { + "DVRKPSMClutchConfig", + "DVRKPSMClutchRetargeter", + "DVRKPSMGripperConfig", + "DVRKPSMGripperRetargeter", +} + + +def __getattr__(name: str): + """Load engine-dependent retargeter wrappers only when requested.""" + if name not in _RETARGETER_EXPORTS: + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") + value = getattr(_importlib.import_module(".psm_retargeter", __package__), name) + globals()[name] = value + return value + + +__all__ = [ + "DVRKPSMClutchConfig", + "DVRKPSMClutchRetargeter", + "DVRKPSMGripperConfig", + "DVRKPSMGripperRetargeter", +] diff --git a/src/python/isaacteleop/retargeters/DVRK/control.py b/src/python/isaacteleop/retargeters/DVRK/control.py new file mode 100644 index 0000000000..e66cc4825a --- /dev/null +++ b/src/python/isaacteleop/retargeters/DVRK/control.py @@ -0,0 +1,601 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 Chris von Csefalvay (HCLTech) +# SPDX-License-Identifier: Apache-2.0 + +"""Simulator-independent control state for a dVRK PSM. + +This module intentionally depends only on NumPy. Isaac Teleop retargeters and +lightweight simulator consumers can therefore share the exact same Cartesian +clutch and jaw-intent semantics without importing one another's runtime. +""" + +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np + +DEFAULT_JAW_OPEN = (-0.50, 0.50) +DEFAULT_JAW_CLOSED = (-0.09, 0.09) +DEFAULT_CLUTCH_THRESHOLD = 0.50 +DEFAULT_TRIGGER_DEADZONE = 0.05 +DEFAULT_OPENING_INTENT_DURATION_S = 0.10 + +# Conservative I4H PSM-base-frame defaults. They guard a target stream; they +# do not replace consumer-side articulation limits or collision checking. +DEFAULT_HOME_POSITION = (0.0, 0.0, 0.18) +DEFAULT_WORKSPACE_LOWER = (-0.16, -0.14, 0.06) +DEFAULT_WORKSPACE_UPPER = (0.16, 0.14, 0.28) +IDENTITY_QUATERNION_XYZW = (0.0, 0.0, 0.0, 1.0) +MIN_QUATERNION_NORM = 1e-6 + + +def _as_vector(values: object, *, length: int, name: str) -> np.ndarray: + """Return a finite vector, accepting NumPy and DLPack producers.""" + if hasattr(values, "__dlpack__"): + values = np.from_dlpack(values) # type: ignore[arg-type] + vector = np.asarray(values, dtype=np.float64) + if vector.shape != (length,) or not np.all(np.isfinite(vector)): + raise ValueError(f"{name} must be a finite vector with shape ({length},)") + return vector.copy() + + +def normalise_quaternion_xyzw(quaternion: object) -> np.ndarray | None: + """Return a finite unit scalar-last quaternion, or ``None`` if unsafe.""" + try: + vector = _as_vector(quaternion, length=4, name="quaternion") + except (TypeError, ValueError): + return None + norm = float(np.linalg.norm(vector)) + if norm < MIN_QUATERNION_NORM: + return None + return vector / norm + + +def quat_mul_xyzw(left: np.ndarray, right: np.ndarray) -> np.ndarray: + """Return the Hamilton product of two scalar-last quaternions.""" + lx, ly, lz, lw = left + rx, ry, rz, rw = right + return np.array( + [ + lw * rx + lx * rw + ly * rz - lz * ry, + lw * ry - lx * rz + ly * rw + lz * rx, + lw * rz + lx * ry - ly * rx + lz * rw, + lw * rw - lx * rx - ly * ry - lz * rz, + ], + dtype=np.float64, + ) + + +def quat_conjugate_xyzw(quaternion: np.ndarray) -> np.ndarray: + """Return the conjugate of a scalar-last unit quaternion.""" + return np.array( + [-quaternion[0], -quaternion[1], -quaternion[2], quaternion[3]], + dtype=np.float64, + ) + + +def rotation_matrix_to_quat_xyzw(rotation: object) -> np.ndarray | None: + """Convert a proper 3-D rotation matrix to a unit scalar-last quaternion.""" + rotation = np.asarray(rotation, dtype=np.float64) + if rotation.shape != (3, 3) or not np.all(np.isfinite(rotation)): + return None + if ( + not np.allclose(rotation.T @ rotation, np.eye(3), atol=1e-5) + or np.linalg.det(rotation) <= 0.0 + ): + return None + trace = float(np.trace(rotation)) + if trace > 0.0: + scale = 2.0 * np.sqrt(trace + 1.0) + quaternion = np.array( + [ + (rotation[2, 1] - rotation[1, 2]) / scale, + (rotation[0, 2] - rotation[2, 0]) / scale, + (rotation[1, 0] - rotation[0, 1]) / scale, + 0.25 * scale, + ], + dtype=np.float64, + ) + else: + axis = int(np.argmax(np.diag(rotation))) + if axis == 0: + scale = 2.0 * np.sqrt( + 1.0 + rotation[0, 0] - rotation[1, 1] - rotation[2, 2] + ) + quaternion = np.array( + [ + 0.25 * scale, + (rotation[0, 1] + rotation[1, 0]) / scale, + (rotation[0, 2] + rotation[2, 0]) / scale, + (rotation[2, 1] - rotation[1, 2]) / scale, + ], + dtype=np.float64, + ) + elif axis == 1: + scale = 2.0 * np.sqrt( + 1.0 + rotation[1, 1] - rotation[0, 0] - rotation[2, 2] + ) + quaternion = np.array( + [ + (rotation[0, 1] + rotation[1, 0]) / scale, + 0.25 * scale, + (rotation[1, 2] + rotation[2, 1]) / scale, + (rotation[0, 2] - rotation[2, 0]) / scale, + ], + dtype=np.float64, + ) + else: + scale = 2.0 * np.sqrt( + 1.0 + rotation[2, 2] - rotation[0, 0] - rotation[1, 1] + ) + quaternion = np.array( + [ + (rotation[0, 2] + rotation[2, 0]) / scale, + (rotation[1, 2] + rotation[2, 1]) / scale, + 0.25 * scale, + (rotation[1, 0] - rotation[0, 1]) / scale, + ], + dtype=np.float64, + ) + return normalise_quaternion_xyzw(quaternion) + + +def rebased_position( + controller_position: np.ndarray, + controller_origin: np.ndarray, + home_position: np.ndarray, + translation_scale: float, +) -> np.ndarray: + """Apply an absolute controller delta to a pose captured at engagement.""" + return home_position + translation_scale * (controller_position - controller_origin) + + +def clip_workspace( + position: np.ndarray, workspace_lower: np.ndarray, workspace_upper: np.ndarray +) -> np.ndarray: + """Clip a Cartesian target to configured reference-frame bounds.""" + return np.minimum(np.maximum(position, workspace_lower), workspace_upper) + + +@dataclass(frozen=True) +class DVRKPSMCartesianClutchConfig: + """Configuration for simulator-independent clutch-rebased Cartesian pose. + + Home, controller samples, and workspace bounds must share one reference + frame. Orientations are scalar-last ``xyzw`` quaternions. The calibration + ``orientation_offset`` is conjugated around controller-relative rotation; + it does not change the configured home orientation at engagement. + """ + + home_position: tuple[float, float, float] = DEFAULT_HOME_POSITION + home_orientation: tuple[float, float, float, float] = IDENTITY_QUATERNION_XYZW + workspace_lower: tuple[float, float, float] = DEFAULT_WORKSPACE_LOWER + workspace_upper: tuple[float, float, float] = DEFAULT_WORKSPACE_UPPER + translation_scale: float = 1.0 + orientation_offset: tuple[float, float, float, float] = IDENTITY_QUATERNION_XYZW + clutch_threshold: float = DEFAULT_CLUTCH_THRESHOLD + + +class DVRKPSMCartesianClutchStateMachine: + """Produce a bounded absolute tool pose from one controller clutch stream. + + The first valid squeezed sample captures a controller origin and emits the + exact held pose. Subsequent samples apply absolute translation and + orientation deltas from that origin. Unclutch, tracking loss, invalid + samples, and inactive sessions hold the exact last pose and require a fresh + origin on the next engagement. Differential IK remains a consumer concern. + """ + + def __init__(self, config: DVRKPSMCartesianClutchConfig | None = None) -> None: + self._config = config or DVRKPSMCartesianClutchConfig() + self._configured_home = _as_vector( + self._config.home_position, length=3, name="home_position" + ) + home_orientation = normalise_quaternion_xyzw(self._config.home_orientation) + if home_orientation is None: + raise ValueError( + "home_orientation must be a non-zero finite xyzw quaternion" + ) + self._configured_home_orientation = home_orientation + self._workspace_lower = _as_vector( + self._config.workspace_lower, length=3, name="workspace_lower" + ) + self._workspace_upper = _as_vector( + self._config.workspace_upper, length=3, name="workspace_upper" + ) + if np.any(self._workspace_lower > self._workspace_upper): + raise ValueError("workspace_lower must not exceed workspace_upper") + if not np.array_equal( + self._configured_home, + clip_workspace( + self._configured_home, + self._workspace_lower, + self._workspace_upper, + ), + ): + raise ValueError("home_position must lie within the configured workspace") + if ( + not np.isfinite(self._config.translation_scale) + or self._config.translation_scale <= 0.0 + ): + raise ValueError("translation_scale must be finite and positive") + self._translation_scale = float(self._config.translation_scale) + orientation_offset = normalise_quaternion_xyzw(self._config.orientation_offset) + if orientation_offset is None: + raise ValueError( + "orientation_offset must be a non-zero finite xyzw quaternion" + ) + self._orientation_offset = orientation_offset + if not np.isfinite(self._config.clutch_threshold) or not ( + 0.0 <= self._config.clutch_threshold <= 1.0 + ): + raise ValueError("clutch_threshold must be finite and in [0, 1]") + + self._last_pose = np.concatenate( + [self._configured_home, self._configured_home_orientation] + ).astype(np.float32) + self._engaged = False + self._controller_origin: np.ndarray | None = None + self._controller_orientation_origin: np.ndarray | None = None + self._pose_at_engagement: np.ndarray | None = None + + @property + def pose(self) -> np.ndarray: + """Return a copy of the exact currently held absolute pose.""" + return self._last_pose.copy() + + @property + def engaged(self) -> bool: + """Whether a valid controller origin is currently captured.""" + return self._engaged + + def reset(self) -> np.ndarray: + """Restore configured home and require a fresh controller origin.""" + self._last_pose = np.concatenate( + [self._configured_home, self._configured_home_orientation] + ).astype(np.float32) + self._disengage() + return self.pose + + def _disengage(self) -> None: + self._engaged = False + self._controller_origin = None + self._controller_orientation_origin = None + self._pose_at_engagement = None + + def step( + self, + *, + controller_position: object | None, + controller_orientation: object | None, + squeeze: float | None, + tracking_valid: bool, + session_active: bool, + ) -> np.ndarray: + """Advance one controller sample and return the absolute held pose.""" + try: + squeeze_value = float(squeeze) if squeeze is not None else None + except (TypeError, ValueError): + squeeze_value = None + if ( + not session_active + or not tracking_valid + or squeeze_value is None + or not np.isfinite(squeeze_value) + or squeeze_value < self._config.clutch_threshold + ): + self._disengage() + return self.pose + + try: + position = _as_vector( + controller_position, length=3, name="controller_position" + ) + except (TypeError, ValueError): + self._disengage() + return self.pose + orientation = normalise_quaternion_xyzw(controller_orientation) + if orientation is None: + self._disengage() + return self.pose + + if not self._engaged: + self._engaged = True + self._controller_origin = position + self._controller_orientation_origin = orientation + self._pose_at_engagement = self._last_pose.astype(np.float64) + return self.pose + + assert self._controller_origin is not None + assert self._controller_orientation_origin is not None + assert self._pose_at_engagement is not None + target_position = clip_workspace( + rebased_position( + position, + self._controller_origin, + self._pose_at_engagement[:3], + self._translation_scale, + ), + self._workspace_lower, + self._workspace_upper, + ) + controller_relative_orientation = quat_mul_xyzw( + quat_conjugate_xyzw(self._controller_orientation_origin), orientation + ) + calibrated_relative_orientation = quat_mul_xyzw( + quat_mul_xyzw(self._orientation_offset, controller_relative_orientation), + quat_conjugate_xyzw(self._orientation_offset), + ) + target_orientation = normalise_quaternion_xyzw( + quat_mul_xyzw( + self._pose_at_engagement[3:7], calibrated_relative_orientation + ) + ) + if target_orientation is None: + self._disengage() + return self.pose + + self._last_pose = np.concatenate([target_position, target_orientation]).astype( + np.float32 + ) + return self.pose + + +def closedness_to_jaw_targets( + closedness: float, jaw_open: np.ndarray, jaw_closed: np.ndarray +) -> np.ndarray: + """Interpolate paired jaw targets between configured physical endpoints.""" + return jaw_open + float(np.clip(closedness, 0.0, 1.0)) * (jaw_closed - jaw_open) + + +def _as_jaw_vector(values: object, *, name: str) -> np.ndarray: + """Return a finite two-element jaw vector.""" + return _as_vector(values, length=2, name=name) + + +@dataclass(frozen=True) +class DVRKPSMJawIntentConfig: + """Configuration for :class:`DVRKPSMJawIntentStateMachine`. + + ``jaw_open`` and ``jaw_closed`` are the physical articulation endpoints; + every emitted target is interpolated between them. ``initial_closedness`` + selects the exact reset target on that segment (zero is open, one is + closed). The squeeze threshold is intentionally shared with the arm + clutch so releasing the arm deadman never changes the jaw command. + + Trigger motion is relative to the value captured on each squeeze + engagement. This avoids a jump when the operator re-clutches with the + index trigger at a different position. Positive trigger travel closes + immediately, but negative travel must exceed ``trigger_deadband`` and + remain present for ``opening_intent_duration_s`` before it opens the jaws. + That short time gate rejects the common natural-unclutch sequence in which + the index trigger relaxes one or two samples before squeeze. The pending + opening is cancelled if squeeze or tracking disappears. + """ + + jaw_open: tuple[float, float] = DEFAULT_JAW_OPEN + jaw_closed: tuple[float, float] = DEFAULT_JAW_CLOSED + initial_closedness: float = 0.0 + clutch_threshold: float = DEFAULT_CLUTCH_THRESHOLD + trigger_deadband: float = DEFAULT_TRIGGER_DEADZONE + opening_intent_duration_s: float = DEFAULT_OPENING_INTENT_DURATION_S + + +class DVRKPSMJawIntentStateMachine: + """Turn trigger/clutch samples into bounded, latched PSM jaw targets. + + Call :meth:`step` with elapsed seconds to keep opening-intent timing + independent of render or physics frame rate. Missing/invalid tracking, an + inactive session, or a released squeeze freezes the exact last target and + re-arms trigger-reference capture for the next engagement. + + The state machine never inspects contacts and never creates an attachment; + it only represents operator jaw intent. + """ + + _DIRECTION_EPSILON = 1e-9 + + def __init__(self, config: DVRKPSMJawIntentConfig | None = None) -> None: + self._config = config or DVRKPSMJawIntentConfig() + self._jaw_open = _as_jaw_vector(self._config.jaw_open, name="jaw_open") + self._jaw_closed = _as_jaw_vector(self._config.jaw_closed, name="jaw_closed") + if not ( + self._jaw_open[0] < self._jaw_closed[0] <= 0.0 + and self._jaw_open[1] > self._jaw_closed[1] >= 0.0 + ): + raise ValueError( + "dVRK jaw endpoints must move inward toward zero in " + "[gripper1, gripper2] order" + ) + if not np.isfinite(self._config.initial_closedness) or not ( + 0.0 <= self._config.initial_closedness <= 1.0 + ): + raise ValueError("initial_closedness must be finite and in [0, 1]") + if not np.isfinite(self._config.clutch_threshold) or not ( + 0.0 <= self._config.clutch_threshold <= 1.0 + ): + raise ValueError("clutch_threshold must be finite and in [0, 1]") + if not np.isfinite(self._config.trigger_deadband) or not ( + 0.0 <= self._config.trigger_deadband < 1.0 + ): + raise ValueError("trigger_deadband must be finite and in [0, 1)") + if not np.isfinite(self._config.opening_intent_duration_s) or ( + self._config.opening_intent_duration_s < 0.0 + ): + raise ValueError( + "opening_intent_duration_s must be finite and non-negative" + ) + + self._initial_closedness = float(self._config.initial_closedness) + self._closedness = self._initial_closedness + self._targets = closedness_to_jaw_targets( + self._closedness, self._jaw_open, self._jaw_closed + ).astype(np.float32) + self._engaged = False + self._interaction_started = False + self._last_trigger: float | None = None + self._opening_anchor_trigger: float | None = None + self._opening_elapsed_s = 0.0 + self._opening_active = False + + @property + def targets(self) -> np.ndarray: + """Return a copy of the exact currently latched jaw targets.""" + return self._targets.copy() + + @property + def closedness(self) -> float: + """Return current scalar jaw closedness in ``[0, 1]``.""" + return self._closedness + + def reset(self) -> np.ndarray: + """Restore the configured initial target and clear engagement state.""" + self._closedness = self._initial_closedness + self._targets = closedness_to_jaw_targets( + self._closedness, self._jaw_open, self._jaw_closed + ).astype(np.float32) + self._disengage() + return self.targets + + def _disengage(self) -> None: + """Hold target while requiring fresh trigger-reference capture.""" + self._engaged = False + self._interaction_started = False + self._last_trigger = None + self._cancel_opening() + + def _cancel_opening(self) -> None: + self._opening_anchor_trigger = None + self._opening_elapsed_s = 0.0 + self._opening_active = False + + def _set_closedness(self, value: float) -> None: + self._closedness = float(np.clip(value, 0.0, 1.0)) + self._targets = closedness_to_jaw_targets( + self._closedness, self._jaw_open, self._jaw_closed + ).astype(np.float32) + + def step( + self, + *, + trigger: float | None, + squeeze: float | None, + tracking_valid: bool, + session_active: bool, + dt_seconds: float, + ) -> np.ndarray: + """Advance one input sample and return the exact jaw target to emit. + + ``dt_seconds`` is elapsed wall/graph time since the previous sample. + A non-finite or negative duration is rejected because silently counting + it could bypass the deliberate-opening guard. + """ + try: + elapsed_seconds = float(dt_seconds) + except (TypeError, ValueError) as error: + raise ValueError("dt_seconds must be finite and non-negative") from error + if not np.isfinite(elapsed_seconds) or elapsed_seconds < 0.0: + raise ValueError("dt_seconds must be finite and non-negative") + + try: + trigger_value = float(trigger) if trigger is not None else None + squeeze_value = float(squeeze) if squeeze is not None else None + except (TypeError, ValueError): + trigger_value = None + squeeze_value = None + if ( + not session_active + or not tracking_valid + or trigger_value is None + or squeeze_value is None + or not np.isfinite(trigger_value) + or not np.isfinite(squeeze_value) + or squeeze_value < self._config.clutch_threshold + ): + # This also cancels a not-yet-committed opening, which is the key + # protection for trigger-up immediately followed by squeeze-up. + self._disengage() + return self.targets + + trigger_value = float(np.clip(trigger_value, 0.0, 1.0)) + if not self._engaged: + self._engaged = True + self._interaction_started = False + self._last_trigger = trigger_value + self._cancel_opening() + return self.targets + + assert self._last_trigger is not None + trigger_delta = trigger_value - self._last_trigger + + if not self._interaction_started: + # Reference capture makes re-clutch a no-op until the operator + # moves the index trigger far enough to be unambiguous. + if trigger_delta >= self._config.trigger_deadband: + self._interaction_started = True + self._cancel_opening() + self._set_closedness(self._closedness + trigger_delta) + self._last_trigger = trigger_value + return self.targets + if trigger_delta <= -self._config.trigger_deadband: + if self._opening_anchor_trigger is None: + self._opening_anchor_trigger = self._last_trigger + self._opening_elapsed_s += elapsed_seconds + if self._opening_elapsed_s >= self._config.opening_intent_duration_s: + opening_delta = trigger_value - self._opening_anchor_trigger + self._interaction_started = True + self._opening_active = True + self._set_closedness(self._closedness + opening_delta) + self._last_trigger = trigger_value + return self.targets + + self._cancel_opening() + return self.targets + + if trigger_delta > self._DIRECTION_EPSILON: + # Closing is immediately safe but remains physically bounded by + # the configured endpoint interpolation. + self._cancel_opening() + self._set_closedness(self._closedness + trigger_delta) + self._last_trigger = trigger_value + return self.targets + + if trigger_delta < -self._DIRECTION_EPSILON: + if self._opening_active: + self._set_closedness(self._closedness + trigger_delta) + self._last_trigger = trigger_value + return self.targets + if self._opening_anchor_trigger is None: + self._opening_anchor_trigger = self._last_trigger + if ( + trigger_value + > self._opening_anchor_trigger - self._config.trigger_deadband + ): + self._cancel_opening() + return self.targets + self._opening_elapsed_s += elapsed_seconds + if self._opening_elapsed_s >= self._config.opening_intent_duration_s: + opening_delta = trigger_value - self._opening_anchor_trigger + self._opening_active = True + self._set_closedness(self._closedness + opening_delta) + self._last_trigger = trigger_value + return self.targets + + # A held-lower trigger still constitutes sustained opening intent even + # though no new analogue delta arrived in this particular sample. + if self._opening_anchor_trigger is not None and not self._opening_active: + self._opening_elapsed_s += elapsed_seconds + if self._opening_elapsed_s >= self._config.opening_intent_duration_s: + opening_delta = trigger_value - self._opening_anchor_trigger + self._opening_active = True + self._set_closedness(self._closedness + opening_delta) + self._last_trigger = trigger_value + return self.targets + + +__all__ = [ + "DVRKPSMCartesianClutchConfig", + "DVRKPSMCartesianClutchStateMachine", + "DVRKPSMJawIntentConfig", + "DVRKPSMJawIntentStateMachine", +] diff --git a/src/python/isaacteleop/retargeters/DVRK/psm_retargeter.py b/src/python/isaacteleop/retargeters/DVRK/psm_retargeter.py new file mode 100644 index 0000000000..eaed853ca8 --- /dev/null +++ b/src/python/isaacteleop/retargeters/DVRK/psm_retargeter.py @@ -0,0 +1,300 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 Chris von Csefalvay (HCLTech) +# SPDX-License-Identifier: Apache-2.0 + +"""XR-controller retargeters for a dVRK Patient Side Manipulator (PSM). + +The PSM has six task-space arm joints and two mechanically coupled jaw joints. +This module owns the XR-facing contracts only: + +* :class:`DVRKPSMClutchRetargeter` emits a clipped, absolute 7-D pose in a + caller-selected reference frame only while the XR squeeze (deadman) is + held. It captures the controller origin at engagement, so the target does + not jump when the operator re-clutches. It wraps the simulator-independent + :class:`DVRKPSMCartesianClutchStateMachine` shared with other consumers. +* :class:`DVRKPSMGripperRetargeter` maps the analog trigger to the paired PSM + jaw targets in radians. It wraps :class:`DVRKPSMJawIntentStateMachine`, a + simulator-independent input-state kernel that preserves jaw intent across + arm clutch and tracking transitions. + +Differential IK deliberately remains in the simulator integration, where the +live articulation Jacobian, current joint state, and robot-specific limits are +available. The Isaac Lab dVRK examples consume this module's outputs with a +six-DOF DLS controller targeting ``psm_tool_tip_link``. + +Frame contract +-------------- +The controller stream, home pose, and workspace must use one shared reference +frame. For one PSM a caller may choose its base frame. The bimanual Isaac Lab +example instead keeps both streams in the Isaac world frame, then transforms +each target into that arm's base frame immediately before DLS IK. This keeps +one XR stream valid for two independently placed PSM bases. +""" + +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np + +from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource +from isaacteleop.retargeting_engine.interface import BaseRetargeter, RetargeterIOType +from isaacteleop.retargeting_engine.interface.execution_events import ExecutionState +from isaacteleop.retargeting_engine.interface.retargeter_core_types import RetargeterIO +from isaacteleop.retargeting_engine.interface.tensor_group_type import ( + OptionalType, + TensorGroupType, +) +from isaacteleop.retargeting_engine.tensor_types import ( + ControllerInput, + ControllerInputIndex, + DLDataType, + NDArrayType, +) + +from .control import ( + DEFAULT_CLUTCH_THRESHOLD as _DEFAULT_CLUTCH_THRESHOLD, + DEFAULT_JAW_CLOSED as _DEFAULT_JAW_CLOSED, + DEFAULT_JAW_OPEN as _DEFAULT_JAW_OPEN, + DEFAULT_OPENING_INTENT_DURATION_S as _DEFAULT_OPENING_INTENT_DURATION_S, + DEFAULT_TRIGGER_DEADZONE as _TRIGGER_DEADZONE, + DEFAULT_WORKSPACE_LOWER as _DEFAULT_WORKSPACE_LOWER, + DEFAULT_WORKSPACE_UPPER as _DEFAULT_WORKSPACE_UPPER, + DVRKPSMCartesianClutchConfig, + DVRKPSMCartesianClutchStateMachine, + DVRKPSMJawIntentConfig, + DVRKPSMJawIntentStateMachine, + rotation_matrix_to_quat_xyzw, +) + + +@dataclass(frozen=True) +class DVRKPSMClutchConfig: + """Configuration for an absolute-pose PSM clutch. + + ``home_reference_T_ee`` and all workspace coordinates are expressed in one + shared command reference frame. The owning simulator must reset the PSM + to the same physical home pose before the first engagement. The + ``orientation_offset`` is a scalar-last calibration rotation conjugated + around the controller's relative rotation; this preserves the configured + tool orientation on the squeeze-latching frame. + """ + + home_reference_T_ee: np.ndarray + input_device: str = ControllersSource.RIGHT + workspace_lower: tuple[float, float, float] = _DEFAULT_WORKSPACE_LOWER + workspace_upper: tuple[float, float, float] = _DEFAULT_WORKSPACE_UPPER + translation_scale: float = 1.0 + orientation_offset: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0) + clutch_threshold: float = _DEFAULT_CLUTCH_THRESHOLD + + +class DVRKPSMClutchRetargeter(BaseRetargeter): + """Emit a workspace-bounded absolute PSM tool-tip target with clutch rebasing. + + The Quest controller's analog squeeze is the deadman clutch. The first + valid squeezed frame after entering ``RUNNING`` latches the controller + origin. The tool target then follows controller displacement from that + origin. Releasing squeeze preserves the last target and re-arms the + origin, so a later squeeze re-clutches at the current PSM target without + teleporting. The trigger is deliberately not read here: the paired-jaw + retargeter owns that relative, intent-latched gripper command. + """ + + OUTPUT_POSE = "ee_pose" + """Output group carrying the 7-D PSM tool-tip pose command.""" + + def __init__(self, config: DVRKPSMClutchConfig, name: str) -> None: + self._input_device = config.input_device + transform = np.asarray(config.home_reference_T_ee, dtype=np.float64) + if transform.shape != (4, 4) or not np.all(np.isfinite(transform)): + raise ValueError("home_reference_T_ee must be a finite (4, 4) matrix") + if not np.allclose(transform[3], (0.0, 0.0, 0.0, 1.0), atol=1e-8): + raise ValueError( + "home_reference_T_ee must be a homogeneous transform with " + "bottom row [0, 0, 0, 1]" + ) + home_position = transform[:3, 3].copy() + home_orientation = rotation_matrix_to_quat_xyzw(transform[:3, :3]) + if home_orientation is None: + raise ValueError( + "home_reference_T_ee must contain a proper rotation matrix" + ) + self._clutch_state = DVRKPSMCartesianClutchStateMachine( + DVRKPSMCartesianClutchConfig( + home_position=tuple(home_position), + home_orientation=tuple(home_orientation), + workspace_lower=config.workspace_lower, + workspace_upper=config.workspace_upper, + translation_scale=config.translation_scale, + orientation_offset=config.orientation_offset, + clutch_threshold=config.clutch_threshold, + ) + ) + super().__init__(name=name) + + def input_spec(self) -> RetargeterIOType: + """Require the optional controller input for the selected PSM side.""" + return {self._input_device: OptionalType(ControllerInput())} + + def output_spec(self) -> RetargeterIOType: + """Emit one 7-D ``[x, y, z, qx, qy, qz, qw]`` absolute pose.""" + return { + self.OUTPUT_POSE: TensorGroupType( + self.OUTPUT_POSE, + [ + NDArrayType( + "pose", shape=(7,), dtype=DLDataType.FLOAT, dtype_bits=32 + ) + ], + ) + } + + def reset_target_state(self) -> None: + """Restore the configured home and require a fresh clutch origin. + + Simulator integrations should call this after an environment reset or + while discarding inactive-session outputs. The next valid squeezed + sample then captures its current controller pose and emits the exact + configured home, preventing pre-start controller motion from becoming + a first-frame target jump. + """ + self._clutch_state.reset() + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + """Adapt one controller tensor sample to the shared clutch kernel.""" + if context.execution_events.reset: + self.reset_target_state() + + output = outputs[self.OUTPUT_POSE] + controller = inputs[self._input_device] + if controller.is_none: + output[0] = self._clutch_state.step( + controller_position=None, + controller_orientation=None, + squeeze=None, + tracking_valid=False, + session_active=( + context.execution_events.execution_state == ExecutionState.RUNNING + ), + ) + return + output[0] = self._clutch_state.step( + controller_position=controller[ControllerInputIndex.GRIP_POSITION], + controller_orientation=controller[ControllerInputIndex.GRIP_ORIENTATION], + squeeze=controller[ControllerInputIndex.SQUEEZE_VALUE], + tracking_valid=bool(controller[ControllerInputIndex.GRIP_IS_VALID]), + session_active=( + context.execution_events.execution_state == ExecutionState.RUNNING + ), + ) + + +@dataclass(frozen=True) +class DVRKPSMGripperConfig: + """Configuration for one intent-latched pair of native PSM jaws.""" + + input_device: str = ControllersSource.RIGHT + jaw_open: tuple[float, float] = _DEFAULT_JAW_OPEN + jaw_closed: tuple[float, float] = _DEFAULT_JAW_CLOSED + initial_closedness: float = 0.0 + clutch_threshold: float = _DEFAULT_CLUTCH_THRESHOLD + trigger_deadband: float = _TRIGGER_DEADZONE + opening_intent_duration_s: float = _DEFAULT_OPENING_INTENT_DURATION_S + + +class DVRKPSMGripperRetargeter(BaseRetargeter): + """Map an analog trigger to the two coupled dVRK PSM jaw targets. + + The I4H PSM model uses ``psm_tool_gripper1_joint`` and + ``psm_tool_gripper2_joint``. The emitted vector is ordered exactly as that + pair: trigger travel toward pressed closes the jaws and travel toward + released opens them, bounded by ``[-0.50, +0.50]`` and + ``[-0.09, +0.09]`` rad by default. The trigger reference is captured at + squeeze engagement, so arm unclutch, tracking loss, and re-clutch hold the + exact last safe target. See :class:`DVRKPSMJawIntentStateMachine` for the + deliberate-opening guard. + """ + + OUTPUT_JAW_TARGETS = "jaw_targets" + """Output group carrying the two PSM gripper-joint targets in radians.""" + + def __init__(self, config: DVRKPSMGripperConfig, name: str) -> None: + self._input_device = config.input_device + self._jaw_intent = DVRKPSMJawIntentStateMachine( + DVRKPSMJawIntentConfig( + jaw_open=config.jaw_open, + jaw_closed=config.jaw_closed, + initial_closedness=config.initial_closedness, + clutch_threshold=config.clutch_threshold, + trigger_deadband=config.trigger_deadband, + opening_intent_duration_s=config.opening_intent_duration_s, + ) + ) + self._last_graph_time_ns: int | None = None + super().__init__(name=name) + + def input_spec(self) -> RetargeterIOType: + """Require the optional selected controller with an analog trigger.""" + return {self._input_device: OptionalType(ControllerInput())} + + def output_spec(self) -> RetargeterIOType: + """Emit the two ordered PSM gripper-joint targets in radians.""" + return { + self.OUTPUT_JAW_TARGETS: TensorGroupType( + self.OUTPUT_JAW_TARGETS, + [ + NDArrayType( + "jaws", shape=(2,), dtype=DLDataType.FLOAT, dtype_bits=32 + ) + ], + ) + } + + def reset_target_state(self) -> None: + """Restore configured initial jaw intent and re-arm trigger capture.""" + self._jaw_intent.reset() + self._last_graph_time_ns = None + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + """Advance the shared jaw-intent kernel from one graph-time sample.""" + if context.execution_events.reset: + self.reset_target_state() + + graph_time_ns = int(context.graph_time.real_time_ns) + if self._last_graph_time_ns is None: + dt_seconds = 0.0 + self._last_graph_time_ns = graph_time_ns + elif graph_time_ns <= self._last_graph_time_ns: + # A repeated or regressed clock sample contributes no intent time. + # Keep the prior high-water mark so the next normal sample cannot + # count the regressed interval a second time. + dt_seconds = 0.0 + else: + dt_seconds = (graph_time_ns - self._last_graph_time_ns) / 1_000_000_000.0 + self._last_graph_time_ns = graph_time_ns + + output = outputs[self.OUTPUT_JAW_TARGETS] + controller = inputs[self._input_device] + if controller.is_none: + output[0] = self._jaw_intent.step( + trigger=None, + squeeze=None, + tracking_valid=False, + session_active=( + context.execution_events.execution_state == ExecutionState.RUNNING + ), + dt_seconds=dt_seconds, + ) + return + + trigger = float(controller[ControllerInputIndex.TRIGGER_VALUE]) + squeeze = float(controller[ControllerInputIndex.SQUEEZE_VALUE]) + output[0] = self._jaw_intent.step( + trigger=trigger, + squeeze=squeeze, + tracking_valid=bool(controller[ControllerInputIndex.GRIP_IS_VALID]), + session_active=( + context.execution_events.execution_state == ExecutionState.RUNNING + ), + dt_seconds=dt_seconds, + ) diff --git a/tests/python/core/retargeting_engine/test_dvrk_psm_retargeters.py b/tests/python/core/retargeting_engine/test_dvrk_psm_retargeters.py new file mode 100644 index 0000000000..b22379d9e4 --- /dev/null +++ b/tests/python/core/retargeting_engine/test_dvrk_psm_retargeters.py @@ -0,0 +1,1073 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 Chris von Csefalvay (HCLTech) +# SPDX-License-Identifier: Apache-2.0 + +"""Sim-free tests for the dVRK PSM clutch and paired-jaw retargeters.""" + +import math + +import numpy as np +import pytest +import isaacteleop.retargeters as retargeters + +from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource +from isaacteleop.retargeting_engine.interface import ( + ComputeContext, + ExecutionEvents, + ExecutionState, + OptionalTensorGroup, + TensorGroup, +) +from isaacteleop.retargeting_engine.interface.retargeter_core_types import GraphTime +from isaacteleop.retargeting_engine.interface.tensor_group_type import ( + OptionalTensorGroupType, +) +from isaacteleop.retargeting_engine.tensor_types import ( + ControllerInput, + ControllerInputIndex, +) +from isaacteleop.retargeters import ( + DVRKPSMClutchConfig, + DVRKPSMClutchRetargeter, + DVRKPSMGripperConfig, + DVRKPSMGripperRetargeter, +) +from isaacteleop.retargeters.DVRK.control import ( + DVRKPSMCartesianClutchConfig, + DVRKPSMCartesianClutchStateMachine, + DVRKPSMJawIntentConfig, + DVRKPSMJawIntentStateMachine, + closedness_to_jaw_targets, + quat_mul_xyzw, + rebased_position, +) + +_IDENTITY_QUAT = np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32) +_JAW_OPEN = np.array([-0.50, 0.50], dtype=np.float64) +_JAW_CLOSED = np.array([-0.09, 0.09], dtype=np.float64) +_POSE_OUTPUT = DVRKPSMClutchRetargeter.OUTPUT_POSE +_JAW_OUTPUT = DVRKPSMGripperRetargeter.OUTPUT_JAW_TARGETS + + +def test_public_package_surface_contains_only_supported_dvrk_types() -> None: + supported = { + "DVRKPSMClutchConfig", + "DVRKPSMClutchRetargeter", + "DVRKPSMGripperConfig", + "DVRKPSMGripperRetargeter", + } + internal = { + "DVRKPSMCartesianClutchConfig", + "DVRKPSMCartesianClutchStateMachine", + "DVRKPSMJawIntentConfig", + "DVRKPSMJawIntentStateMachine", + } + + assert supported <= set(retargeters.__all__) + assert internal.isdisjoint(retargeters.__all__) + assert all(hasattr(retargeters, name) for name in supported) + assert all(not hasattr(retargeters, name) for name in internal) + assert DVRKPSMClutchRetargeter.OUTPUT_POSE == "ee_pose" + assert DVRKPSMGripperRetargeter.OUTPUT_JAW_TARGETS == "jaw_targets" + + +def test_clutch_config_requires_an_explicit_home_transform() -> None: + with pytest.raises(TypeError, match="home_reference_T_ee"): + DVRKPSMClutchConfig() # type: ignore[call-arg] + + +def _make_context( + *, + reset: bool = False, + state: ExecutionState = ExecutionState.RUNNING, + real_time_s: float = 0.0, +) -> ComputeContext: + """Build one retargeter compute context.""" + time_ns = round(real_time_s * 1_000_000_000) + return ComputeContext( + graph_time=GraphTime(sim_time_ns=time_ns, real_time_ns=time_ns), + execution_events=ExecutionEvents(reset=reset, execution_state=state), + ) + + +def _build_io(retargeter): + """Construct empty input/output tensor groups from a retargeter contract.""" + inputs = {} + for key, value in retargeter.input_spec().items(): + inputs[key] = ( + OptionalTensorGroup(value) + if isinstance(value, OptionalTensorGroupType) + else TensorGroup(value) + ) + outputs = {} + for key, value in retargeter.output_spec().items(): + outputs[key] = ( + OptionalTensorGroup(value) + if isinstance(value, OptionalTensorGroupType) + else TensorGroup(value) + ) + return inputs, outputs + + +def _make_controller( + *, + grip_pos=(0.0, 0.0, 0.0), + grip_ori=_IDENTITY_QUAT, + trigger: float = 0.0, + squeeze: float = 1.0, + grip_is_valid: bool = True, +) -> TensorGroup: + """Build a present XR controller sample for a retargeter frame.""" + controller = TensorGroup(ControllerInput()) + controller[ControllerInputIndex.GRIP_POSITION] = np.asarray( + grip_pos, dtype=np.float32 + ) + controller[ControllerInputIndex.GRIP_ORIENTATION] = np.asarray( + grip_ori, dtype=np.float32 + ) + controller[ControllerInputIndex.GRIP_IS_VALID] = grip_is_valid + controller[ControllerInputIndex.AIM_ORIENTATION] = _IDENTITY_QUAT + controller[ControllerInputIndex.AIM_IS_VALID] = True + controller[ControllerInputIndex.SQUEEZE_VALUE] = float(squeeze) + controller[ControllerInputIndex.TRIGGER_VALUE] = float(trigger) + return controller + + +def _make_home_transform(position, rotation: np.ndarray | None = None) -> np.ndarray: + """Build a PSM base-to-tool home transform with identity orientation.""" + transform = np.eye(4, dtype=np.float64) + if rotation is not None: + transform[:3, :3] = rotation + transform[:3, 3] = np.asarray(position, dtype=np.float64) + return transform + + +def _quat_xyzw_z(angle: float) -> np.ndarray: + """Build a scalar-last quaternion for a rotation about the Z axis.""" + return np.array((0.0, 0.0, math.sin(angle / 2.0), math.cos(angle / 2.0))) + + +def _quat_xyzw_x(angle: float) -> np.ndarray: + """Build a scalar-last quaternion for a rotation about the X axis.""" + return np.array((math.sin(angle / 2.0), 0.0, 0.0, math.cos(angle / 2.0))) + + +def _rotation_matrix_z(angle: float) -> np.ndarray: + """Build a 3-D Z-axis rotation matrix for a configured tool home.""" + return np.array( + ( + (math.cos(angle), -math.sin(angle), 0.0), + (math.sin(angle), math.cos(angle), 0.0), + (0.0, 0.0, 1.0), + ) + ) + + +def _read_vector(outputs, key: str) -> np.ndarray: + """Read an emitted DLPack vector into a float64 array for comparisons.""" + return np.asarray(np.from_dlpack(outputs[key][0]), dtype=np.float64) + + +class TestDVRKPSMGripperMath: + """Pure trigger-to-paired-jaw mapping checks.""" + + def test_trigger_endpoints_match_i4h_psm_joint_targets(self): + """Released/open and pressed/closed commands land on I4H jaw endpoints.""" + np.testing.assert_allclose( + closedness_to_jaw_targets(0.0, _JAW_OPEN, _JAW_CLOSED), + _JAW_OPEN, + ) + np.testing.assert_allclose( + closedness_to_jaw_targets(1.0, _JAW_OPEN, _JAW_CLOSED), + _JAW_CLOSED, + ) + + def test_mapping_is_continuous_and_mirror_coupled(self): + """A half trigger produces a bounded pair that moves symmetrically toward zero.""" + targets = closedness_to_jaw_targets(0.5, _JAW_OPEN, _JAW_CLOSED) + assert _JAW_OPEN[0] < targets[0] < _JAW_CLOSED[0] + assert _JAW_CLOSED[1] < targets[1] < _JAW_OPEN[1] + assert targets[0] == pytest.approx(-targets[1], abs=1e-8) + + +class TestDVRKPSMGripperRetargeter: + """End-to-end paired-jaw commands through BaseRetargeter.compute.""" + + @staticmethod + def _compute(retargeter, *, time_s: float, **controller_kwargs) -> np.ndarray: + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller(**controller_kwargs) + retargeter.compute(inputs, outputs, _make_context(real_time_s=time_s)) + return _read_vector(outputs, _JAW_OUTPUT) + + def test_engagement_captures_trigger_then_full_travel_closes_both_jaws(self): + """Engagement is no-op; deliberate press emits bounded articulation-order targets.""" + retargeter = DVRKPSMGripperRetargeter(DVRKPSMGripperConfig(), name="jaws") + engaged = self._compute(retargeter, time_s=0.00, trigger=0.0, squeeze=1.0) + np.testing.assert_allclose(engaged, _JAW_OPEN, atol=1e-6) + + closed = self._compute(retargeter, time_s=0.02, trigger=1.0, squeeze=1.0) + np.testing.assert_allclose(closed, _JAW_CLOSED, atol=1e-6) + + def test_configured_initial_hold_can_use_a_validated_tighter_endpoint(self): + """A task can start at its physical grasp cap without an initial trigger jump.""" + validated_closed = (-0.032, 0.032) + retargeter = DVRKPSMGripperRetargeter( + DVRKPSMGripperConfig( + jaw_closed=validated_closed, + initial_closedness=1.0, + ), + name="jaws", + ) + initial = self._compute(retargeter, time_s=0.00, trigger=0.63, squeeze=1.0) + np.testing.assert_allclose(initial, validated_closed, atol=1e-7) + + # A different trigger position after re-clutch remains a no-op until + # deliberate post-clutch travel occurs. + self._compute(retargeter, time_s=0.02, trigger=0.10, squeeze=0.0) + reclutched = self._compute(retargeter, time_s=0.04, trigger=0.10, squeeze=1.0) + np.testing.assert_array_equal(reclutched, initial) + + def test_explicit_target_reset_restores_configured_hold_before_reengagement(self): + """An environment reset restores the configured initial jaw target.""" + configured_hold = (-0.030, 0.030) + retargeter = DVRKPSMGripperRetargeter( + DVRKPSMGripperConfig( + jaw_open=(-0.25, 0.25), + jaw_closed=configured_hold, + initial_closedness=1.0, + ), + name="left_jaws", + ) + initial = self._compute(retargeter, time_s=0.00, trigger=1.0) + np.testing.assert_allclose(initial, configured_hold, atol=1e-8) + self._compute(retargeter, time_s=0.05, trigger=0.0) + opened = self._compute(retargeter, time_s=0.10, trigger=0.0) + assert not np.array_equal(opened, initial) + + retargeter.reset_target_state() + reset_engagement = self._compute(retargeter, time_s=0.11, trigger=0.0) + np.testing.assert_allclose(reset_engagement, configured_hold, atol=1e-8) + + def test_wrapper_times_deliberate_opening_from_graph_time(self): + """Opening guard uses elapsed graph time rather than an assumed frame rate.""" + retargeter = DVRKPSMGripperRetargeter( + DVRKPSMGripperConfig(initial_closedness=1.0), name="jaws" + ) + closed = self._compute(retargeter, time_s=0.000, trigger=1.0) + np.testing.assert_array_equal( + self._compute(retargeter, time_s=0.020, trigger=0.4), closed + ) + np.testing.assert_array_equal( + self._compute(retargeter, time_s=0.099, trigger=0.4), closed + ) + opened = self._compute(retargeter, time_s=0.100, trigger=0.4) + assert not np.array_equal(opened, closed) + + def test_regressed_graph_time_cannot_bypass_opening_duration(self): + """A clock regression contributes zero time and does not move the high-water mark.""" + retargeter = DVRKPSMGripperRetargeter( + DVRKPSMGripperConfig(initial_closedness=1.0), name="jaws" + ) + closed = self._compute(retargeter, time_s=1.00, trigger=1.0) + np.testing.assert_array_equal( + self._compute(retargeter, time_s=0.50, trigger=0.4), closed + ) + np.testing.assert_array_equal( + self._compute(retargeter, time_s=1.05, trigger=0.4), closed + ) + opened = self._compute(retargeter, time_s=1.10, trigger=0.4) + assert not np.array_equal(opened, closed) + + def test_dropped_frame_holds_and_reset_restores_configured_initial_target(self): + """Tracking loss holds exactly; reset restores the configured initial target.""" + retargeter = DVRKPSMGripperRetargeter(DVRKPSMGripperConfig(), name="jaws") + self._compute(retargeter, time_s=0.00, trigger=0.0) + closed = self._compute(retargeter, time_s=0.02, trigger=1.0) + + dropped_inputs, dropped_outputs = _build_io(retargeter) + retargeter.compute( + dropped_inputs, dropped_outputs, _make_context(real_time_s=0.04) + ) + np.testing.assert_allclose( + _read_vector(dropped_outputs, _JAW_OUTPUT), + closed, + atol=1e-6, + ) + + reset_inputs, reset_outputs = _build_io(retargeter) + retargeter.compute( + reset_inputs, + reset_outputs, + _make_context(reset=True, real_time_s=0.06), + ) + np.testing.assert_allclose( + _read_vector(reset_outputs, _JAW_OUTPUT), _JAW_OPEN, atol=1e-6 + ) + + def test_nonfinite_trigger_holds_last_safe_jaw_target(self): + """A malformed analog sample cannot emit NaN joint targets.""" + retargeter = DVRKPSMGripperRetargeter(DVRKPSMGripperConfig(), name="jaws") + self._compute(retargeter, time_s=0.00, trigger=0.0) + closed = self._compute(retargeter, time_s=0.02, trigger=1.0) + + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller(trigger=math.nan) + retargeter.compute(inputs, outputs, _make_context(real_time_s=0.04)) + emitted = _read_vector(outputs, _JAW_OUTPUT) + assert np.all(np.isfinite(emitted)) + np.testing.assert_allclose(emitted, closed, atol=1e-6) + + def test_stopped_session_holds_the_last_jaw_target(self): + """A session stop freezes jaws as well as the Cartesian PSM target.""" + retargeter = DVRKPSMGripperRetargeter(DVRKPSMGripperConfig(), name="jaws") + self._compute(retargeter, time_s=0.00, trigger=0.0) + closed = self._compute(retargeter, time_s=0.02, trigger=1.0) + + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller(trigger=0.0) + retargeter.compute( + inputs, + outputs, + _make_context(state=ExecutionState.STOPPED, real_time_s=0.04), + ) + np.testing.assert_allclose( + _read_vector(outputs, _JAW_OUTPUT), closed, atol=1e-6 + ) + + +class TestDVRKPSMJawIntentStateMachine: + """Pure jaw-intent behaviour shared by device adapters.""" + + @staticmethod + def _kernel(*, initial_closedness: float = 0.65): + return DVRKPSMJawIntentStateMachine( + DVRKPSMJawIntentConfig( + jaw_open=tuple(_JAW_OPEN), + jaw_closed=tuple(_JAW_CLOSED), + initial_closedness=initial_closedness, + trigger_deadband=0.05, + opening_intent_duration_s=0.08, + ) + ) + + @staticmethod + def _step( + kernel, + *, + trigger: float | None = 0.8, + squeeze: float | None = 1.0, + valid: bool = True, + active: bool = True, + dt: float = 0.02, + ) -> np.ndarray: + return kernel.step( + trigger=trigger, + squeeze=squeeze, + tracking_valid=valid, + session_active=active, + dt_seconds=dt, + ) + + def test_initial_target_is_exact_and_first_engagement_is_no_op(self): + kernel = self._kernel(initial_closedness=0.65) + expected = closedness_to_jaw_targets(0.65, _JAW_OPEN, _JAW_CLOSED) + np.testing.assert_allclose(kernel.targets, expected, atol=1e-6) + np.testing.assert_allclose( + self._step(kernel, trigger=0.73), expected, atol=1e-6 + ) + + @pytest.mark.parametrize( + ("jaw_open", "jaw_closed"), + ( + ((-0.50, 0.50), (-0.60, 0.09)), + ((-0.50, 0.50), (-0.09, 0.60)), + ((0.10, 0.50), (0.05, 0.09)), + ((-0.50, -0.10), (-0.09, -0.05)), + ), + ) + def test_invalid_dvrk_jaw_endpoint_direction_is_rejected( + self, jaw_open, jaw_closed + ): + with pytest.raises(ValueError, match="must move inward toward zero"): + DVRKPSMJawIntentStateMachine( + DVRKPSMJawIntentConfig(jaw_open=jaw_open, jaw_closed=jaw_closed) + ) + + @pytest.mark.parametrize("field", ("trigger", "squeeze")) + def test_malformed_analog_sample_holds_and_rearms(self, field): + kernel = self._kernel(initial_closedness=0.8) + held = self._step(kernel, trigger=0.4) + sample = {field: "not-a-number"} + np.testing.assert_array_equal(self._step(kernel, **sample), held) + np.testing.assert_array_equal(self._step(kernel, trigger=0.9), held) + + @pytest.mark.parametrize( + "transition", + ( + {"squeeze": 0.0}, + {"valid": False}, + {"active": False}, + {"trigger": None}, + ), + ) + def test_unclutch_tracking_loss_and_inactive_session_hold_exact_target( + self, transition + ): + kernel = self._kernel(initial_closedness=0.2) + self._step(kernel, trigger=0.2) + moved = self._step(kernel, trigger=0.7) + sample = {"trigger": 0.0, **transition} + held = self._step(kernel, **sample) + np.testing.assert_array_equal(held, moved) + + def test_reclutch_captures_new_reference_and_requires_fresh_interaction(self): + kernel = self._kernel(initial_closedness=0.7) + initial = self._step(kernel, trigger=0.9) + self._step(kernel, trigger=0.2, squeeze=0.0) + + # Re-clutching at a very different index-trigger position is a no-op. + np.testing.assert_array_equal(self._step(kernel, trigger=0.2), initial) + np.testing.assert_array_equal(self._step(kernel, trigger=0.23), initial) + + # Deliberate travel beyond the deadband resumes relative control. + moved = self._step(kernel, trigger=0.4) + assert kernel.closedness == pytest.approx(0.9) + assert not np.array_equal(moved, initial) + + def test_short_trigger_release_then_squeeze_release_never_opens(self): + kernel = self._kernel(initial_closedness=1.0) + closed = self._step(kernel, trigger=1.0) + + # Index release starts first, as it commonly does in a natural + # unclutch, but remains shorter than the deliberate-opening duration. + np.testing.assert_array_equal( + self._step(kernel, trigger=0.2, squeeze=1.0, dt=0.02), closed + ) + np.testing.assert_array_equal( + self._step(kernel, trigger=0.2, squeeze=0.0, dt=0.02), closed + ) + + # Re-clutch at the relaxed trigger position also preserves the hold. + np.testing.assert_array_equal( + self._step(kernel, trigger=0.2, squeeze=1.0, dt=0.50), closed + ) + + def test_deliberate_opening_uses_elapsed_time_not_frame_count(self): + for time_steps in ((0.02, 0.02, 0.04), (0.079, 0.001)): + kernel = self._kernel(initial_closedness=1.0) + closed = self._step(kernel, trigger=1.0, dt=0.0) + for dt in time_steps[:-1]: + np.testing.assert_array_equal( + self._step(kernel, trigger=0.4, dt=dt), closed + ) + opened = self._step(kernel, trigger=0.4, dt=time_steps[-1]) + assert kernel.closedness == pytest.approx(0.4) + assert not np.array_equal(opened, closed) + + def test_closing_is_immediate_but_capped_at_configured_endpoint(self): + kernel = self._kernel(initial_closedness=0.4) + self._step(kernel, trigger=0.0) + kernel.step( + trigger=4.0, + squeeze=1.0, + tracking_valid=True, + session_active=True, + dt_seconds=0.01, + ) + assert kernel.closedness == 1.0 + np.testing.assert_allclose(kernel.targets, _JAW_CLOSED, atol=1e-6) + + def test_reset_restores_initial_target_and_rearms_reference_capture(self): + kernel = self._kernel(initial_closedness=0.35) + self._step(kernel, trigger=0.0) + self._step(kernel, trigger=0.6) + reset = kernel.reset() + expected = closedness_to_jaw_targets(0.35, _JAW_OPEN, _JAW_CLOSED) + np.testing.assert_allclose(reset, expected, atol=1e-6) + np.testing.assert_allclose(self._step(kernel, trigger=0.9), expected, atol=1e-6) + + +class TestDVRKPSMClutchMath: + """Pure absolute-pose clutch calculations.""" + + def test_rebased_position_applies_scaled_controller_delta(self): + """The latching frame is home and later controller motion is scaled from it.""" + home = np.array([0.02, 0.00, 0.18], dtype=np.float64) + origin = np.array([0.31, -0.12, 0.44], dtype=np.float64) + delta = np.array([0.04, -0.01, 0.03], dtype=np.float64) + np.testing.assert_allclose(rebased_position(origin, origin, home, 1.5), home) + np.testing.assert_allclose( + rebased_position(origin + delta, origin, home, 1.5), home + 1.5 * delta + ) + + +class TestDVRKPSMCartesianClutchStateMachine: + """Pure Cartesian clutch behaviour shared by every simulator consumer.""" + + @staticmethod + def _kernel( + *, + home_position=(0.02, 0.00, 0.18), + home_orientation=(0.0, 0.0, 0.0, 1.0), + ) -> DVRKPSMCartesianClutchStateMachine: + return DVRKPSMCartesianClutchStateMachine( + DVRKPSMCartesianClutchConfig( + home_position=home_position, + home_orientation=home_orientation, + workspace_lower=(-0.10, -0.10, 0.08), + workspace_upper=(0.10, 0.10, 0.24), + translation_scale=1.0, + ) + ) + + @staticmethod + def _step( + kernel, + *, + position=(0.30, -0.20, 0.50), + orientation=_IDENTITY_QUAT, + squeeze: float | None = 1.0, + valid: bool = True, + active: bool = True, + ) -> np.ndarray: + return kernel.step( + controller_position=position, + controller_orientation=orientation, + squeeze=squeeze, + tracking_valid=valid, + session_active=active, + ) + + def test_engagement_and_reclutch_are_no_jump_absolute_deltas(self): + kernel = self._kernel() + origin = np.array((0.30, -0.20, 0.50), dtype=np.float64) + home = kernel.pose + + np.testing.assert_array_equal(self._step(kernel, position=origin), home) + moved = self._step( + kernel, + position=origin + (0.03, 0.01, -0.02), + orientation=_quat_xyzw_z(0.30), + ) + np.testing.assert_allclose(moved[:3], home[:3] + (0.03, 0.01, -0.02), atol=1e-6) + np.testing.assert_allclose(moved[3:], _quat_xyzw_z(0.30), atol=1e-6) + + # Repositioning while unclutched cannot move either pose component. + np.testing.assert_array_equal( + self._step( + kernel, + position=(9.0, 9.0, 9.0), + orientation=_quat_xyzw_z(-1.0), + squeeze=0.0, + ), + moved, + ) + new_origin = np.array((-0.8, 0.4, 1.2), dtype=np.float64) + np.testing.assert_array_equal( + self._step( + kernel, + position=new_origin, + orientation=_quat_xyzw_z(-0.6), + ), + moved, + ) + resumed = self._step( + kernel, + position=new_origin + (0.01, -0.02, 0.01), + orientation=_quat_xyzw_z(-0.4), + ) + np.testing.assert_allclose( + resumed[:3], moved[:3] + (0.01, -0.02, 0.01), atol=1e-6 + ) + expected_orientation = quat_mul_xyzw( + moved[3:].astype(np.float64), _quat_xyzw_z(0.2) + ) + np.testing.assert_allclose(resumed[3:], expected_orientation, atol=1e-6) + + @pytest.mark.parametrize( + ("bad_position", "bad_orientation"), + ( + ((math.nan, 0.0, 0.0), _IDENTITY_QUAT), + ((0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 0.0)), + ((0.0, 0.0, 0.0), (math.nan, 0.0, 0.0, 1.0)), + ), + ) + def test_invalid_pose_holds_exactly_and_requires_fresh_origin( + self, bad_position, bad_orientation + ): + kernel = self._kernel() + origin = np.array((0.30, -0.20, 0.50), dtype=np.float64) + self._step(kernel, position=origin) + moved = self._step(kernel, position=origin + (0.02, 0.0, 0.0)) + + invalid = self._step(kernel, position=bad_position, orientation=bad_orientation) + np.testing.assert_array_equal(invalid, moved) + assert not kernel.engaged + + # Recovery only captures a new origin; stale deltas cannot leak across + # the invalid interval. + np.testing.assert_array_equal( + self._step(kernel, position=(1.0, 1.0, 1.0)), moved + ) + + @pytest.mark.parametrize( + "transition", + ( + {"squeeze": 0.0}, + {"valid": False}, + {"active": False}, + ), + ) + def test_unclutch_tracking_loss_and_inactive_session_hold_exact_pose( + self, transition + ): + kernel = self._kernel() + origin = np.array((0.30, -0.20, 0.50), dtype=np.float64) + self._step(kernel, position=origin) + moved = self._step( + kernel, + position=origin + (0.02, -0.01, 0.01), + orientation=_quat_xyzw_z(0.2), + ) + held = self._step(kernel, position=(8.0, 8.0, 8.0), **transition) + np.testing.assert_array_equal(held, moved) + assert not kernel.engaged + + def test_reset_is_deterministic_and_rearms_origin_capture(self): + home_orientation = _quat_xyzw_z(0.35) + kernel = self._kernel(home_orientation=tuple(home_orientation)) + home = kernel.pose + origin = np.array((0.30, -0.20, 0.50), dtype=np.float64) + self._step(kernel, position=origin) + self._step(kernel, position=origin + (0.03, 0.0, 0.0)) + np.testing.assert_array_equal(kernel.reset(), home) + assert not kernel.engaged + np.testing.assert_array_equal( + self._step(kernel, position=(1.0, 1.0, 1.0)), home + ) + + def test_left_and_right_instances_are_isolated(self): + left = self._kernel(home_position=(-0.04, 0.00, 0.18)) + right = self._kernel(home_position=(0.04, 0.00, 0.18)) + left_origin = np.array((-0.30, 0.10, 0.40), dtype=np.float64) + right_origin = np.array((0.30, 0.10, 0.40), dtype=np.float64) + self._step(left, position=left_origin) + self._step(right, position=right_origin) + right_home = right.pose + + left_moved = self._step(left, position=left_origin + (0.03, 0.0, 0.0)) + np.testing.assert_allclose(left_moved[:3], (-0.01, 0.00, 0.18), atol=1e-6) + np.testing.assert_array_equal(right.pose, right_home) + + right_moved = self._step( + right, + position=right_origin + (0.0, -0.02, 0.01), + orientation=_quat_xyzw_z(-0.2), + ) + np.testing.assert_allclose(right_moved[:3], (0.04, -0.02, 0.19), atol=1e-6) + np.testing.assert_array_equal(left.pose, left_moved) + + +class TestDVRKPSMClutchRetargeter: + """End-to-end PSM clutch output and resilience checks.""" + + @staticmethod + def _retargeter() -> DVRKPSMClutchRetargeter: + config = DVRKPSMClutchConfig( + home_reference_T_ee=_make_home_transform((0.02, 0.00, 0.18)), + workspace_lower=(-0.10, -0.10, 0.08), + workspace_upper=(0.10, 0.10, 0.24), + orientation_offset=(0.0, 0.0, 0.0, 1.0), + ) + return DVRKPSMClutchRetargeter(config, name="ee_pose") + + def test_output_contract_is_absolute_7d_pose(self): + """The DLS integration receives one position-plus-xyzw target vector.""" + pose_type = self._retargeter().output_spec()[_POSE_OUTPUT].types[0] + assert pose_type.shape == (7,) + + def test_wrapper_matches_shared_kernel_on_fixed_controller_trace(self): + """Retargeting-engine adaptation cannot diverge from the reusable kernel.""" + home_position = (0.02, 0.00, 0.18) + workspace_lower = (-0.10, -0.10, 0.08) + workspace_upper = (0.10, 0.10, 0.24) + orientation_offset = tuple(_quat_xyzw_x(0.5)) + wrapper = DVRKPSMClutchRetargeter( + DVRKPSMClutchConfig( + home_reference_T_ee=_make_home_transform(home_position), + workspace_lower=workspace_lower, + workspace_upper=workspace_upper, + translation_scale=1.25, + orientation_offset=orientation_offset, + ), + name="ee_pose", + ) + kernel = DVRKPSMCartesianClutchStateMachine( + DVRKPSMCartesianClutchConfig( + home_position=home_position, + workspace_lower=workspace_lower, + workspace_upper=workspace_upper, + translation_scale=1.25, + orientation_offset=orientation_offset, + ) + ) + + origin_orientation = _quat_xyzw_z(-0.4) + moved_orientation = quat_mul_xyzw(origin_orientation, _quat_xyzw_z(0.3)) + recovered_orientation = _quat_xyzw_z(0.7) + trace = ( + { + "position": (0.30, -0.20, 0.50), + "orientation": origin_orientation, + }, + { + "position": (0.33, -0.19, 0.48), + "orientation": moved_orientation, + }, + { + "position": (math.nan, 0.0, 0.0), + "orientation": _IDENTITY_QUAT, + }, + { + "position": (-0.6, 0.4, 1.0), + "orientation": recovered_orientation, + }, + { + "position": (-0.59, 0.38, 1.01), + "orientation": quat_mul_xyzw(recovered_orientation, _quat_xyzw_z(-0.2)), + }, + { + "position": (8.0, 8.0, 8.0), + "orientation": _quat_xyzw_z(1.0), + "squeeze": 0.0, + }, + { + "position": (0.2, 0.3, 0.4), + "orientation": _quat_xyzw_z(-0.2), + "active": False, + }, + { + "position": (0.9, 0.8, 0.7), + "orientation": _quat_xyzw_z(0.6), + "reset": True, + }, + ) + + for index, sample in enumerate(trace): + position = np.asarray(sample["position"], dtype=np.float32) + orientation = np.asarray(sample["orientation"], dtype=np.float32) + squeeze = float(sample.get("squeeze", 1.0)) + active = bool(sample.get("active", True)) + reset = bool(sample.get("reset", False)) + + inputs, outputs = _build_io(wrapper) + inputs[ControllersSource.RIGHT] = _make_controller( + grip_pos=position, + grip_ori=orientation, + squeeze=squeeze, + ) + wrapper.compute( + inputs, + outputs, + _make_context( + reset=reset, + state=( + ExecutionState.RUNNING if active else ExecutionState.STOPPED + ), + ), + ) + if reset: + kernel.reset() + expected = kernel.step( + controller_position=position, + controller_orientation=orientation, + squeeze=squeeze, + tracking_valid=True, + session_active=active, + ) + np.testing.assert_array_equal( + _read_vector(outputs, _POSE_OUTPUT), + expected.astype(np.float64), + err_msg=f"trace sample {index}", + ) + + def test_out_of_workspace_configured_home_is_rejected(self): + """A reset pose outside the target guard would violate no-jump engagement.""" + with pytest.raises(ValueError, match="must lie within"): + DVRKPSMClutchRetargeter( + DVRKPSMClutchConfig( + home_reference_T_ee=_make_home_transform((0.20, 0.0, 0.18)), + workspace_lower=(-0.10, -0.10, 0.08), + workspace_upper=(0.10, 0.10, 0.24), + ), + name="ee_pose", + ) + + def test_non_homogeneous_configured_home_is_rejected(self): + """A finite 4x4 array is not a pose unless its projective row is canonical.""" + transform = _make_home_transform((0.02, 0.0, 0.18)) + transform[3] = (0.0, 0.0, 0.25, 1.0) + with pytest.raises(ValueError, match="homogeneous transform"): + DVRKPSMClutchRetargeter( + DVRKPSMClutchConfig(home_reference_T_ee=transform), + name="ee_pose", + ) + + def test_engage_motion_workspace_clamp_and_reclutch_are_no_jump(self): + """Engage, clamp, stop, and re-engage preserve a bounded continuous target.""" + retargeter = self._retargeter() + controller_origin = np.array([0.30, -0.20, 0.50], dtype=np.float64) + + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=controller_origin) + retargeter.compute(inputs, outputs, _make_context()) + np.testing.assert_allclose( + _read_vector(outputs, _POSE_OUTPUT)[:3], + (0.02, 0.00, 0.18), + atol=1e-6, + ) + + # This position would exceed every configured bound without the Cartesian guard. + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller( + grip_pos=controller_origin + np.array((1.0, -1.0, 1.0)) + ) + retargeter.compute(inputs, outputs, _make_context()) + held = _read_vector(outputs, _POSE_OUTPUT) + np.testing.assert_allclose(held[:3], (0.10, -0.10, 0.24), atol=1e-6) + + # Repositioning the hand while stopped cannot move the virtual tool. + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=(9.0, 9.0, 9.0)) + retargeter.compute(inputs, outputs, _make_context(state=ExecutionState.STOPPED)) + np.testing.assert_allclose(_read_vector(outputs, _POSE_OUTPUT), held, atol=1e-6) + + # The next running frame merely latches its new controller origin; it does not jump. + next_origin = np.array((1.0, 1.0, 1.0), dtype=np.float64) + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=next_origin) + retargeter.compute(inputs, outputs, _make_context()) + np.testing.assert_allclose(_read_vector(outputs, _POSE_OUTPUT), held, atol=1e-6) + + # A small fresh delta now starts from the previous target rather than reset home. + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller( + grip_pos=next_origin + np.array((-0.02, 0.01, -0.01)) + ) + retargeter.compute(inputs, outputs, _make_context()) + np.testing.assert_allclose( + _read_vector(outputs, _POSE_OUTPUT)[:3], + (0.08, -0.09, 0.23), + atol=1e-6, + ) + + def test_continuous_squeeze_uses_controller_delta_not_integrated_delta(self): + """Two frames while held follow absolute displacement from one clutch origin.""" + retargeter = self._retargeter() + origin = np.array((0.30, -0.20, 0.50), dtype=np.float64) + home = np.array((0.02, 0.00, 0.18), dtype=np.float64) + + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=origin, squeeze=1.0) + retargeter.compute(inputs, outputs, _make_context()) + + first_delta = np.array((0.03, 0.01, -0.01), dtype=np.float64) + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller( + grip_pos=origin + first_delta, squeeze=1.0 + ) + retargeter.compute(inputs, outputs, _make_context()) + np.testing.assert_allclose( + _read_vector(outputs, _POSE_OUTPUT)[:3], + home + first_delta, + atol=1e-6, + ) + + # This is an absolute controller location relative to ``origin``; + # the first delta must not be applied a second time. + second_delta = np.array((0.04, -0.02, 0.02), dtype=np.float64) + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller( + grip_pos=origin + second_delta, squeeze=1.0 + ) + retargeter.compute(inputs, outputs, _make_context()) + np.testing.assert_allclose( + _read_vector(outputs, _POSE_OUTPUT)[:3], + home + second_delta, + atol=1e-6, + ) + + def test_squeeze_is_deadman_clutch_and_reengagement_is_no_jump(self): + """Release freezes the PSM; squeeze again latches a fresh origin at the held pose.""" + retargeter = self._retargeter() + origin = np.array((0.30, -0.20, 0.50), dtype=np.float64) + + # Merely tracking a controller does not command motion before squeeze. + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=origin, squeeze=0.0) + retargeter.compute(inputs, outputs, _make_context()) + np.testing.assert_allclose( + _read_vector(outputs, _POSE_OUTPUT)[:3], + (0.02, 0.00, 0.18), + atol=1e-6, + ) + assert not retargeter._clutch_state.engaged + + # Squeeze latches, then a hand displacement moves the virtual tool. + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=origin, squeeze=1.0) + retargeter.compute(inputs, outputs, _make_context()) + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller( + grip_pos=origin + (0.03, 0.0, 0.0), squeeze=1.0 + ) + retargeter.compute(inputs, outputs, _make_context()) + held = _read_vector(outputs, _POSE_OUTPUT) + np.testing.assert_allclose(held[:3], (0.05, 0.00, 0.18), atol=1e-6) + + # Releasing while moving cannot change the target. + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller( + grip_pos=(9.0, 9.0, 9.0), squeeze=0.0 + ) + retargeter.compute(inputs, outputs, _make_context()) + np.testing.assert_allclose(_read_vector(outputs, _POSE_OUTPUT), held, atol=1e-6) + + # Squeezing at the new hand pose only latches it. Motion resumes from + # the held PSM target, not from the configured reset home. + new_origin = np.array((1.0, 1.0, 1.0), dtype=np.float64) + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller( + grip_pos=new_origin, squeeze=1.0 + ) + retargeter.compute(inputs, outputs, _make_context()) + np.testing.assert_allclose(_read_vector(outputs, _POSE_OUTPUT), held, atol=1e-6) + + def test_orientation_is_relative_to_squeeze_latch(self): + """Squeeze holds the configured orientation, then follows controller-relative rotation.""" + home_angle = 0.4 + controller_angle = -0.8 + home_rotation = _rotation_matrix_z(home_angle) + retargeter = DVRKPSMClutchRetargeter( + DVRKPSMClutchConfig( + home_reference_T_ee=_make_home_transform( + (0.02, 0.00, 0.18), home_rotation + ), + workspace_lower=(-0.10, -0.10, 0.08), + workspace_upper=(0.10, 0.10, 0.24), + ), + name="ee_pose", + ) + origin_orientation = _quat_xyzw_z(controller_angle) + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller( + grip_pos=(0.30, -0.20, 0.50), + grip_ori=origin_orientation, + squeeze=1.0, + ) + retargeter.compute(inputs, outputs, _make_context()) + home_quaternion = _quat_xyzw_z(home_angle) + np.testing.assert_allclose( + _read_vector(outputs, _POSE_OUTPUT)[3:], home_quaternion, atol=1e-6 + ) + + relative_rotation = _quat_xyzw_z(0.3) + current_orientation = quat_mul_xyzw(origin_orientation, relative_rotation) + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller( + grip_pos=(0.30, -0.20, 0.50), + grip_ori=current_orientation, + squeeze=1.0, + ) + retargeter.compute(inputs, outputs, _make_context()) + expected = quat_mul_xyzw(home_quaternion, relative_rotation) + np.testing.assert_allclose( + _read_vector(outputs, _POSE_OUTPUT)[3:], expected, atol=1e-6 + ) + + def test_invalid_or_dropped_tracking_holds_a_finite_last_pose(self): + """A bad validity flag, NaN position, or absent sample never poisons downstream IK.""" + retargeter = self._retargeter() + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=(0.3, 0.2, 0.5)) + retargeter.compute(inputs, outputs, _make_context()) + initial = _read_vector(outputs, _POSE_OUTPUT) + + for controller in ( + _make_controller(grip_pos=(9.0, 9.0, 9.0), grip_is_valid=False), + _make_controller(grip_pos=(math.nan, 0.0, 0.0)), + ): + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = controller + retargeter.compute(inputs, outputs, _make_context()) + emitted = _read_vector(outputs, _POSE_OUTPUT) + assert np.all(np.isfinite(emitted)) + np.testing.assert_allclose(emitted, initial, atol=1e-6) + + inputs, outputs = _build_io(retargeter) + retargeter.compute(inputs, outputs, _make_context()) + np.testing.assert_allclose( + _read_vector(outputs, _POSE_OUTPUT), initial, atol=1e-6 + ) + + def test_malformed_pose_rearms_clutch_before_the_next_valid_frame(self): + """A malformed valid-flagged sample cannot leave a stale clutch origin armed.""" + retargeter = self._retargeter() + origin = np.array((0.30, -0.20, 0.50), dtype=np.float64) + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=origin) + retargeter.compute(inputs, outputs, _make_context()) + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller( + grip_pos=origin + (0.03, 0.0, 0.0) + ) + retargeter.compute(inputs, outputs, _make_context()) + held = _read_vector(outputs, _POSE_OUTPUT) + + # The source claims tracking is valid but sends malformed coordinates. + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller( + grip_pos=(math.nan, 0.0, 0.0) + ) + retargeter.compute(inputs, outputs, _make_context()) + np.testing.assert_allclose(_read_vector(outputs, _POSE_OUTPUT), held, atol=1e-6) + + # A distant recovered controller pose only establishes a fresh origin. + recovered_origin = np.array((1.0, 1.0, 1.0), dtype=np.float64) + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=recovered_origin) + retargeter.compute(inputs, outputs, _make_context()) + np.testing.assert_allclose(_read_vector(outputs, _POSE_OUTPUT), held, atol=1e-6) + + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller( + grip_pos=recovered_origin + (0.01, 0.0, 0.0) + ) + retargeter.compute(inputs, outputs, _make_context()) + np.testing.assert_allclose( + _read_vector(outputs, _POSE_OUTPUT)[:3], + held[:3] + (0.01, 0.0, 0.0), + atol=1e-6, + ) + + def test_reset_restores_configured_home(self): + """An episode reset discards a prior re-clutch target and starts from configured home.""" + retargeter = self._retargeter() + origin = np.array((0.3, 0.2, 0.5), dtype=np.float64) + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=origin) + retargeter.compute(inputs, outputs, _make_context()) + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller( + grip_pos=origin + (0.03, 0.0, 0.0) + ) + retargeter.compute(inputs, outputs, _make_context()) + + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=(1.0, 1.0, 1.0)) + retargeter.compute(inputs, outputs, _make_context(reset=True)) + np.testing.assert_allclose( + _read_vector(outputs, _POSE_OUTPUT)[:3], + (0.02, 0.00, 0.18), + atol=1e-6, + ) From 3a6a1cc804352f522ef8181c089b5ff17223baff Mon Sep 17 00:00:00 2001 From: Chris von Csefalvay Date: Mon, 13 Jul 2026 07:17:09 -0600 Subject: [PATCH 2/6] fix(retargeters): harden dVRK state handling Signed-off-by: Chris von Csefalvay --- docs/source/references/retargeting/dvrk.rst | 104 ++++++++----- .../isaacteleop/retargeters/DVRK/control.py | 72 ++++++--- .../retargeters/DVRK/psm_retargeter.py | 12 +- .../core/retargeting_engine/CMakeLists.txt | 1 + .../test_dvrk_psm_retargeters.py | 143 +++++++++++++++++- 5 files changed, 263 insertions(+), 69 deletions(-) diff --git a/docs/source/references/retargeting/dvrk.rst b/docs/source/references/retargeting/dvrk.rst index 131e1bfab4..caa43bea2f 100644 --- a/docs/source/references/retargeting/dvrk.rst +++ b/docs/source/references/retargeting/dvrk.rst @@ -4,13 +4,17 @@ dVRK PSM retargeters ==================== +The da Vinci Research Kit (dVRK) is a telerobotics research platform built from +first-generation da Vinci surgical-system components and an open control stack. +See the `dVRK Research Wiki `_ +and the `JHU dVRK software wiki +`_ for the hardware, +software, and research community. + The dVRK Patient Side Manipulator (PSM) retargeters map XR controllers to one or two simulated PSMs. They convert controller state into absolute tool-pose and -paired-jaw targets. Isaac Lab reads those targets, uses the live articulation -Jacobian and joint state, and solves differential IK. - -They are not a physical dVRK driver or force-feedback system, and are not -intended for clinical workflow or surgical validation. +paired-jaw targets. A simulator integration can read those targets, use its +live articulation Jacobian and joint state, and solve differential IK. At a glance ----------- @@ -63,11 +67,12 @@ separately. The squeeze threshold defaults to ``0.5``. The trigger value captured at squeeze engagement is neutral. The first jaw movement requires at least ``0.05`` travel from that value. Positive travel past the threshold starts -closing immediately. Negative travel must remain at least ``0.05`` below its -reference for ``0.1`` seconds before opening begins. Once opening is active, -further release opens the jaws continuously. Releasing squeeze, losing -tracking, or stopping the session cancels a pending opening and holds the last -target. Travel is capped at the configured physical endpoints: +closing immediately. Negative travel must first be observed at least ``0.05`` +below its reference, then remain there for ``0.1`` seconds before opening +begins. Once opening is active, further release opens the jaws continuously. +Releasing squeeze, losing tracking, or stopping the session cancels a pending +opening and holds the last target. Travel is capped at the configured physical +endpoints: .. code-block:: text @@ -101,7 +106,10 @@ Use the retargeters from Python .. code-block:: python + import numpy as np + from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource + from isaacteleop.retargeting_engine.interface import OutputCombiner from isaacteleop.retargeters import ( DVRKPSMClutchConfig, DVRKPSMClutchRetargeter, @@ -109,35 +117,61 @@ Use the retargeters from Python DVRKPSMGripperRetargeter, ) - left_pose = DVRKPSMClutchRetargeter( - DVRKPSMClutchConfig( - input_device=ControllersSource.LEFT, - home_reference_T_ee=left_home_world_T_ee, - workspace_lower=(-0.50, -0.40, -0.10), - workspace_upper=(0.50, 0.40, 0.60), - clutch_threshold=0.5, - ), - name="left_psm_pose", - ) - left_jaws = DVRKPSMGripperRetargeter( - DVRKPSMGripperConfig(input_device=ControllersSource.LEFT), - name="left_psm_jaws", - ) - -Create one pose retargeter and one jaw retargeter for each PSM. Concatenate the -outputs for a bimanual action in this order: - -.. code-block:: text - - left xyzxyzw, left jaws, right xyzxyzw, right jaws - -Use ``DVRKPSMClutchRetargeter.OUTPUT_POSE`` and + def build_right_psm_pipeline() -> OutputCombiner: + controllers = ControllersSource(name="controllers") + + # The home transform, raw controller grip pose, and workspace are all + # expressed in this example's shared tracking reference frame. + home_reference_T_ee = np.eye(4, dtype=np.float64) + home_reference_T_ee[:3, 3] = (0.0, 0.0, 0.18) + + pose = DVRKPSMClutchRetargeter( + DVRKPSMClutchConfig( + input_device=ControllersSource.RIGHT, + home_reference_T_ee=home_reference_T_ee, + workspace_lower=(-0.16, -0.14, 0.06), + workspace_upper=(0.16, 0.14, 0.28), + clutch_threshold=0.5, + ), + name="right_psm_pose", + ) + jaws = DVRKPSMGripperRetargeter( + DVRKPSMGripperConfig(input_device=ControllersSource.RIGHT), + name="right_psm_jaws", + ) + + right_controller = controllers.output(ControllersSource.RIGHT) + connected_pose = pose.connect( + {ControllersSource.RIGHT: right_controller} + ) + connected_jaws = jaws.connect( + {ControllersSource.RIGHT: right_controller} + ) + + return OutputCombiner( + { + "ee_pose": connected_pose.output( + DVRKPSMClutchRetargeter.OUTPUT_POSE + ), + "jaw_targets": connected_jaws.output( + DVRKPSMGripperRetargeter.OUTPUT_JAW_TARGETS + ), + } + ) + + + pipeline = build_right_psm_pipeline() + +Create one pose retargeter and one jaw retargeter for each PSM. Each instance +publishes an independent named output; the downstream consumer chooses any +packing and ordering required by its own action contract. Use +``DVRKPSMClutchRetargeter.OUTPUT_POSE`` and ``DVRKPSMGripperRetargeter.OUTPUT_JAW_TARGETS`` when wiring those output ports. These names are part of the public retargeter API, so callers do not need to import implementation modules. -Validation ----------- +Validate +-------- Tests that do not require Isaac Sim cover jaw endpoints, time-based opening intent, jaw hold across clutch release and re-engagement, Cartesian clutch diff --git a/src/python/isaacteleop/retargeters/DVRK/control.py b/src/python/isaacteleop/retargeters/DVRK/control.py index e66cc4825a..a0f99e7f1d 100644 --- a/src/python/isaacteleop/retargeters/DVRK/control.py +++ b/src/python/isaacteleop/retargeters/DVRK/control.py @@ -27,28 +27,41 @@ DEFAULT_WORKSPACE_UPPER = (0.16, 0.14, 0.28) IDENTITY_QUATERNION_XYZW = (0.0, 0.0, 0.0, 1.0) MIN_QUATERNION_NORM = 1e-6 +FLOAT32_MAX = float(np.finfo(np.float32).max) def _as_vector(values: object, *, length: int, name: str) -> np.ndarray: """Return a finite vector, accepting NumPy and DLPack producers.""" if hasattr(values, "__dlpack__"): - values = np.from_dlpack(values) # type: ignore[arg-type] + values = np.from_dlpack(values) vector = np.asarray(values, dtype=np.float64) if vector.shape != (length,) or not np.all(np.isfinite(vector)): raise ValueError(f"{name} must be a finite vector with shape ({length},)") return vector.copy() +def _as_float32_output_vector(values: object, *, length: int, name: str) -> np.ndarray: + """Return a finite vector whose values lie within the finite float32 range.""" + vector = _as_vector(values, length=length, name=name) + if np.any(np.abs(vector) > FLOAT32_MAX): + raise ValueError(f"{name} must lie within the finite float32 range") + return vector + + def normalise_quaternion_xyzw(quaternion: object) -> np.ndarray | None: """Return a finite unit scalar-last quaternion, or ``None`` if unsafe.""" try: vector = _as_vector(quaternion, length=4, name="quaternion") except (TypeError, ValueError): return None - norm = float(np.linalg.norm(vector)) - if norm < MIN_QUATERNION_NORM: + scale = float(np.max(np.abs(vector))) + if scale == 0.0: + return None + scaled = vector / scale + scaled_norm = float(np.linalg.norm(scaled)) + if not np.isfinite(scaled_norm) or scale < MIN_QUATERNION_NORM / scaled_norm: return None - return vector / norm + return scaled / scaled_norm def quat_mul_xyzw(left: np.ndarray, right: np.ndarray) -> np.ndarray: @@ -147,14 +160,20 @@ def rebased_position( translation_scale: float, ) -> np.ndarray: """Apply an absolute controller delta to a pose captured at engagement.""" - return home_position + translation_scale * (controller_position - controller_origin) + return np.asarray( + home_position + translation_scale * (controller_position - controller_origin), + dtype=np.float64, + ) def clip_workspace( position: np.ndarray, workspace_lower: np.ndarray, workspace_upper: np.ndarray ) -> np.ndarray: """Clip a Cartesian target to configured reference-frame bounds.""" - return np.minimum(np.maximum(position, workspace_lower), workspace_upper) + return np.asarray( + np.minimum(np.maximum(position, workspace_lower), workspace_upper), + dtype=np.float64, + ) @dataclass(frozen=True) @@ -188,7 +207,7 @@ class DVRKPSMCartesianClutchStateMachine: def __init__(self, config: DVRKPSMCartesianClutchConfig | None = None) -> None: self._config = config or DVRKPSMCartesianClutchConfig() - self._configured_home = _as_vector( + self._configured_home = _as_float32_output_vector( self._config.home_position, length=3, name="home_position" ) home_orientation = normalise_quaternion_xyzw(self._config.home_orientation) @@ -197,10 +216,10 @@ def __init__(self, config: DVRKPSMCartesianClutchConfig | None = None) -> None: "home_orientation must be a non-zero finite xyzw quaternion" ) self._configured_home_orientation = home_orientation - self._workspace_lower = _as_vector( + self._workspace_lower = _as_float32_output_vector( self._config.workspace_lower, length=3, name="workspace_lower" ) - self._workspace_upper = _as_vector( + self._workspace_upper = _as_float32_output_vector( self._config.workspace_upper, length=3, name="workspace_upper" ) if np.any(self._workspace_lower > self._workspace_upper): @@ -231,7 +250,9 @@ def __init__(self, config: DVRKPSMCartesianClutchConfig | None = None) -> None: ): raise ValueError("clutch_threshold must be finite and in [0, 1]") - self._last_pose = np.concatenate( + # Keep this explicit: NumPy's chained concatenate/astype stubs otherwise + # leave the attribute as Any under the repository's mypy configuration. + self._last_pose: np.ndarray = np.concatenate( [self._configured_home, self._configured_home_orientation] ).astype(np.float32) self._engaged = False @@ -345,12 +366,15 @@ def closedness_to_jaw_targets( closedness: float, jaw_open: np.ndarray, jaw_closed: np.ndarray ) -> np.ndarray: """Interpolate paired jaw targets between configured physical endpoints.""" - return jaw_open + float(np.clip(closedness, 0.0, 1.0)) * (jaw_closed - jaw_open) + return np.asarray( + jaw_open + float(np.clip(closedness, 0.0, 1.0)) * (jaw_closed - jaw_open), + dtype=np.float64, + ) def _as_jaw_vector(values: object, *, name: str) -> np.ndarray: - """Return a finite two-element jaw vector.""" - return _as_vector(values, length=2, name=name) + """Return a finite two-element jaw vector within the float32 range.""" + return _as_float32_output_vector(values, length=2, name=name) @dataclass(frozen=True) @@ -540,7 +564,10 @@ def step( if trigger_delta <= -self._config.trigger_deadband: if self._opening_anchor_trigger is None: self._opening_anchor_trigger = self._last_trigger - self._opening_elapsed_s += elapsed_seconds + else: + # The interval preceding the first qualifying sample is + # not evidence that opening intent was already present. + self._opening_elapsed_s += elapsed_seconds if self._opening_elapsed_s >= self._config.opening_intent_duration_s: opening_delta = trigger_value - self._opening_anchor_trigger self._interaction_started = True @@ -567,13 +594,16 @@ def step( return self.targets if self._opening_anchor_trigger is None: self._opening_anchor_trigger = self._last_trigger + else: + # Start timing at the first observed threshold crossing. A + # delayed frame must not count the unobserved preceding gap. + self._opening_elapsed_s += elapsed_seconds if ( trigger_value > self._opening_anchor_trigger - self._config.trigger_deadband ): self._cancel_opening() return self.targets - self._opening_elapsed_s += elapsed_seconds if self._opening_elapsed_s >= self._config.opening_intent_duration_s: opening_delta = trigger_value - self._opening_anchor_trigger self._opening_active = True @@ -581,15 +611,11 @@ def step( self._last_trigger = trigger_value return self.targets - # A held-lower trigger still constitutes sustained opening intent even - # though no new analogue delta arrived in this particular sample. + # While opening is pending, _last_trigger remains the pre-opening + # anchor. A zero delta therefore means the trigger returned to that + # anchor, rather than remaining held lower, so pending intent must end. if self._opening_anchor_trigger is not None and not self._opening_active: - self._opening_elapsed_s += elapsed_seconds - if self._opening_elapsed_s >= self._config.opening_intent_duration_s: - opening_delta = trigger_value - self._opening_anchor_trigger - self._opening_active = True - self._set_closedness(self._closedness + opening_delta) - self._last_trigger = trigger_value + self._cancel_opening() return self.targets diff --git a/src/python/isaacteleop/retargeters/DVRK/psm_retargeter.py b/src/python/isaacteleop/retargeters/DVRK/psm_retargeter.py index eaed853ca8..3c9e3deefd 100644 --- a/src/python/isaacteleop/retargeters/DVRK/psm_retargeter.py +++ b/src/python/isaacteleop/retargeters/DVRK/psm_retargeter.py @@ -18,16 +18,16 @@ Differential IK deliberately remains in the simulator integration, where the live articulation Jacobian, current joint state, and robot-specific limits are -available. The Isaac Lab dVRK examples consume this module's outputs with a -six-DOF DLS controller targeting ``psm_tool_tip_link``. +available. A simulator consumer can use this module's outputs with a six-DOF +DLS controller targeting its PSM tool-tip link. Frame contract -------------- The controller stream, home pose, and workspace must use one shared reference -frame. For one PSM a caller may choose its base frame. The bimanual Isaac Lab -example instead keeps both streams in the Isaac world frame, then transforms -each target into that arm's base frame immediately before DLS IK. This keeps -one XR stream valid for two independently placed PSM bases. +frame. For one PSM a caller may choose its base frame. A bimanual integration +can instead keep both streams in a shared world frame, then transform each +target into that arm's base frame immediately before DLS IK. This keeps one XR +stream valid for two independently placed PSM bases. """ from __future__ import annotations diff --git a/tests/python/core/retargeting_engine/CMakeLists.txt b/tests/python/core/retargeting_engine/CMakeLists.txt index 7ec300ff2b..96a8b52876 100644 --- a/tests/python/core/retargeting_engine/CMakeLists.txt +++ b/tests/python/core/retargeting_engine/CMakeLists.txt @@ -61,6 +61,7 @@ add_test( COMMAND uv run --python ${ISAAC_TELEOP_PYTHON_VERSION} --extra dev mypy --config-file pyproject.toml "${CMAKE_SOURCE_DIR}/src/python/isaaccapture/retargeting_engine" + "${CMAKE_SOURCE_DIR}/src/python/isaacteleop/retargeters/DVRK" WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" ) diff --git a/tests/python/core/retargeting_engine/test_dvrk_psm_retargeters.py b/tests/python/core/retargeting_engine/test_dvrk_psm_retargeters.py index b22379d9e4..2a132f5154 100644 --- a/tests/python/core/retargeting_engine/test_dvrk_psm_retargeters.py +++ b/tests/python/core/retargeting_engine/test_dvrk_psm_retargeters.py @@ -37,6 +37,7 @@ DVRKPSMJawIntentConfig, DVRKPSMJawIntentStateMachine, closedness_to_jaw_targets, + normalise_quaternion_xyzw, quat_mul_xyzw, rebased_position, ) @@ -240,12 +241,17 @@ def test_explicit_target_reset_restores_configured_hold_before_reengagement(self ) initial = self._compute(retargeter, time_s=0.00, trigger=1.0) np.testing.assert_allclose(initial, configured_hold, atol=1e-8) - self._compute(retargeter, time_s=0.05, trigger=0.0) - opened = self._compute(retargeter, time_s=0.10, trigger=0.0) + np.testing.assert_array_equal( + self._compute(retargeter, time_s=0.05, trigger=0.0), initial + ) + np.testing.assert_array_equal( + self._compute(retargeter, time_s=0.10, trigger=0.0), initial + ) + opened = self._compute(retargeter, time_s=0.151, trigger=0.0) assert not np.array_equal(opened, initial) retargeter.reset_target_state() - reset_engagement = self._compute(retargeter, time_s=0.11, trigger=0.0) + reset_engagement = self._compute(retargeter, time_s=0.16, trigger=0.0) np.testing.assert_allclose(reset_engagement, configured_hold, atol=1e-8) def test_wrapper_times_deliberate_opening_from_graph_time(self): @@ -260,7 +266,10 @@ def test_wrapper_times_deliberate_opening_from_graph_time(self): np.testing.assert_array_equal( self._compute(retargeter, time_s=0.099, trigger=0.4), closed ) - opened = self._compute(retargeter, time_s=0.100, trigger=0.4) + np.testing.assert_array_equal( + self._compute(retargeter, time_s=0.100, trigger=0.4), closed + ) + opened = self._compute(retargeter, time_s=0.121, trigger=0.4) assert not np.array_equal(opened, closed) def test_regressed_graph_time_cannot_bypass_opening_duration(self): @@ -452,10 +461,16 @@ def test_short_trigger_release_then_squeeze_release_never_opens(self): self._step(kernel, trigger=0.2, squeeze=1.0, dt=0.50), closed ) - def test_deliberate_opening_uses_elapsed_time_not_frame_count(self): + def test_deliberate_opening_uses_observed_time_not_frame_count(self): for time_steps in ((0.02, 0.02, 0.04), (0.079, 0.001)): kernel = self._kernel(initial_closedness=1.0) closed = self._step(kernel, trigger=1.0, dt=0.0) + + # A long gap before opening intent is first observed contributes + # no duration: the trigger state during that gap is unknown. + np.testing.assert_array_equal( + self._step(kernel, trigger=0.4, dt=10.0), closed + ) for dt in time_steps[:-1]: np.testing.assert_array_equal( self._step(kernel, trigger=0.4, dt=dt), closed @@ -464,6 +479,34 @@ def test_deliberate_opening_uses_elapsed_time_not_frame_count(self): assert kernel.closedness == pytest.approx(0.4) assert not np.array_equal(opened, closed) + def test_reversing_after_closing_starts_a_fresh_observed_opening_timer(self): + kernel = self._kernel(initial_closedness=0.5) + self._step(kernel, trigger=0.4, dt=0.0) + closed = self._step(kernel, trigger=0.8, dt=0.02) + assert kernel.closedness == pytest.approx(0.9) + + # This exercises the post-interaction direction-reversal path. The + # first lower sample starts, but cannot satisfy, the time gate. + np.testing.assert_array_equal(self._step(kernel, trigger=0.6, dt=10.0), closed) + opened = self._step(kernel, trigger=0.6, dt=0.08) + assert kernel.closedness == pytest.approx(0.7) + assert not np.array_equal(opened, closed) + + def test_returning_to_anchor_cancels_pending_opening_after_closing(self): + kernel = self._kernel(initial_closedness=0.5) + self._step(kernel, trigger=0.4, dt=0.0) + closed = self._step(kernel, trigger=0.8, dt=0.02) + self._step(kernel, trigger=0.6, dt=0.0) + + # Returning to the pre-opening anchor cannot mature a zero-motion + # intent, regardless of how long it remains there. + np.testing.assert_array_equal(self._step(kernel, trigger=0.8, dt=1.0), closed) + np.testing.assert_array_equal(self._step(kernel, trigger=0.6, dt=0.0), closed) + + opened = self._step(kernel, trigger=0.6, dt=0.08) + assert kernel.closedness == pytest.approx(0.7) + assert not np.array_equal(opened, closed) + def test_closing_is_immediate_but_capped_at_configured_endpoint(self): kernel = self._kernel(initial_closedness=0.4) self._step(kernel, trigger=0.0) @@ -486,10 +529,42 @@ def test_reset_restores_initial_target_and_rearms_reference_capture(self): np.testing.assert_allclose(reset, expected, atol=1e-6) np.testing.assert_allclose(self._step(kernel, trigger=0.9), expected, atol=1e-6) + @pytest.mark.parametrize( + ("config", "field"), + ( + ( + DVRKPSMJawIntentConfig( + jaw_open=(-1e40, 1e40), + jaw_closed=(-0.09, 0.09), + ), + "jaw_open", + ), + ( + DVRKPSMJawIntentConfig( + jaw_open=(-0.50, 0.50), + jaw_closed=(-1e40, 1e40), + ), + "jaw_closed", + ), + ), + ) + def test_finite_jaw_endpoints_outside_float32_range_are_rejected( + self, config, field + ): + with pytest.raises( + ValueError, match=f"{field} must lie within the finite float32 range" + ): + DVRKPSMJawIntentStateMachine(config) + class TestDVRKPSMClutchMath: """Pure absolute-pose clutch calculations.""" + def test_large_finite_quaternion_normalises_without_overflow(self): + quaternion = normalise_quaternion_xyzw((1e308, 1e308, 1e308, 1e308)) + assert quaternion is not None + np.testing.assert_allclose(quaternion, (0.5, 0.5, 0.5, 0.5), atol=1e-12) + def test_rebased_position_applies_scaled_controller_delta(self): """The latching frame is home and later controller motion is scaled from it.""" home = np.array([0.02, 0.00, 0.18], dtype=np.float64) @@ -584,6 +659,64 @@ def test_engagement_and_reclutch_are_no_jump_absolute_deltas(self): ) np.testing.assert_allclose(resumed[3:], expected_orientation, atol=1e-6) + def test_non_commuting_orientation_offset_maps_z_rotation_to_negative_y(self): + quarter_turn_about_x = _quat_xyzw_x(math.pi / 2.0) + kernel = DVRKPSMCartesianClutchStateMachine( + DVRKPSMCartesianClutchConfig( + home_position=(0.02, 0.00, 0.18), + workspace_lower=(-0.10, -0.10, 0.08), + workspace_upper=(0.10, 0.10, 0.24), + orientation_offset=tuple(quarter_turn_about_x), + ) + ) + origin = np.array((0.30, -0.20, 0.50), dtype=np.float64) + self._step(kernel, position=origin, orientation=_IDENTITY_QUAT) + + angle = 0.4 + pose = self._step( + kernel, + position=origin, + orientation=_quat_xyzw_z(angle), + ) + + # Rx(pi/2) Rz(angle) Rx(-pi/2) is a rotation of -angle about Y. + expected = np.array( + (0.0, -math.sin(angle / 2.0), 0.0, math.cos(angle / 2.0)), + dtype=np.float64, + ) + np.testing.assert_allclose(pose[3:], expected, atol=1e-6) + + @pytest.mark.parametrize( + ("config", "field"), + ( + ( + DVRKPSMCartesianClutchConfig( + home_position=(1e40, 0.0, 0.18), + ), + "home_position", + ), + ( + DVRKPSMCartesianClutchConfig( + workspace_lower=(-1e40, -0.14, 0.06), + ), + "workspace_lower", + ), + ( + DVRKPSMCartesianClutchConfig( + workspace_upper=(1e40, 0.14, 0.28), + ), + "workspace_upper", + ), + ), + ) + def test_finite_pose_configuration_outside_float32_range_is_rejected( + self, config, field + ): + with pytest.raises( + ValueError, match=f"{field} must lie within the finite float32 range" + ): + DVRKPSMCartesianClutchStateMachine(config) + @pytest.mark.parametrize( ("bad_position", "bad_orientation"), ( From bd480eed0b119aa3fadb485ebf11ea3a742d8aaf Mon Sep 17 00:00:00 2001 From: Chris von Csefalvay Date: Mon, 13 Jul 2026 13:16:57 -0600 Subject: [PATCH 3/6] Fixes autospdx oopsie Signed-off-by: Chris von Csefalvay --- docs/source/references/retargeting/dvrk.rst | 2 +- src/python/isaacteleop/retargeters/DVRK/__init__.py | 2 +- src/python/isaacteleop/retargeters/DVRK/control.py | 2 +- src/python/isaacteleop/retargeters/DVRK/psm_retargeter.py | 2 +- .../python/core/retargeting_engine/test_dvrk_psm_retargeters.py | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/docs/source/references/retargeting/dvrk.rst b/docs/source/references/retargeting/dvrk.rst index caa43bea2f..998d7b9616 100644 --- a/docs/source/references/retargeting/dvrk.rst +++ b/docs/source/references/retargeting/dvrk.rst @@ -1,4 +1,4 @@ -.. SPDX-FileCopyrightText: Copyright (c) 2026 Chris von Csefalvay (HCLTech) +.. SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. .. SPDX-License-Identifier: Apache-2.0 dVRK PSM retargeters diff --git a/src/python/isaacteleop/retargeters/DVRK/__init__.py b/src/python/isaacteleop/retargeters/DVRK/__init__.py index 5c269788aa..1152b6fbff 100644 --- a/src/python/isaacteleop/retargeters/DVRK/__init__.py +++ b/src/python/isaacteleop/retargeters/DVRK/__init__.py @@ -1,4 +1,4 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 Chris von Csefalvay (HCLTech) +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 """dVRK Patient Side Manipulator retargeters.""" diff --git a/src/python/isaacteleop/retargeters/DVRK/control.py b/src/python/isaacteleop/retargeters/DVRK/control.py index a0f99e7f1d..29833e051e 100644 --- a/src/python/isaacteleop/retargeters/DVRK/control.py +++ b/src/python/isaacteleop/retargeters/DVRK/control.py @@ -1,4 +1,4 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 Chris von Csefalvay (HCLTech) +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 """Simulator-independent control state for a dVRK PSM. diff --git a/src/python/isaacteleop/retargeters/DVRK/psm_retargeter.py b/src/python/isaacteleop/retargeters/DVRK/psm_retargeter.py index 3c9e3deefd..2838cc8cf7 100644 --- a/src/python/isaacteleop/retargeters/DVRK/psm_retargeter.py +++ b/src/python/isaacteleop/retargeters/DVRK/psm_retargeter.py @@ -1,4 +1,4 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 Chris von Csefalvay (HCLTech) +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 """XR-controller retargeters for a dVRK Patient Side Manipulator (PSM). diff --git a/tests/python/core/retargeting_engine/test_dvrk_psm_retargeters.py b/tests/python/core/retargeting_engine/test_dvrk_psm_retargeters.py index 2a132f5154..bd8f7e7dac 100644 --- a/tests/python/core/retargeting_engine/test_dvrk_psm_retargeters.py +++ b/tests/python/core/retargeting_engine/test_dvrk_psm_retargeters.py @@ -1,4 +1,4 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 Chris von Csefalvay (HCLTech) +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 """Sim-free tests for the dVRK PSM clutch and paired-jaw retargeters.""" From ffa972b524fa0203b2e10441eb543cd8264079ea Mon Sep 17 00:00:00 2001 From: Chris von Csefalvay Date: Mon, 20 Jul 2026 15:09:21 -0600 Subject: [PATCH 4/6] add dVRK overview and needle-pass animation Brief context of embodiment, ECM/PSM roles and simulation, add GIF of bimanual needle passing teleop run sub 5 megs. Signed-off-by: Chris von Csefalvay --- docs/source/_static/dvrk-needle-pass.gif | 3 ++ docs/source/references/retargeting/dvrk.rst | 35 +++++++++++++++++---- 2 files changed, 32 insertions(+), 6 deletions(-) create mode 100644 docs/source/_static/dvrk-needle-pass.gif diff --git a/docs/source/_static/dvrk-needle-pass.gif b/docs/source/_static/dvrk-needle-pass.gif new file mode 100644 index 0000000000..208ff2b90f --- /dev/null +++ b/docs/source/_static/dvrk-needle-pass.gif @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3f54ceff34ad8050b6e91903ffb9ee6f63e2597da815bc531d7968061196f81b +size 4698098 diff --git a/docs/source/references/retargeting/dvrk.rst b/docs/source/references/retargeting/dvrk.rst index 998d7b9616..ae1bb75344 100644 --- a/docs/source/references/retargeting/dvrk.rst +++ b/docs/source/references/retargeting/dvrk.rst @@ -4,18 +4,41 @@ dVRK PSM retargeters ==================== -The da Vinci Research Kit (dVRK) is a telerobotics research platform built from -first-generation da Vinci surgical-system components and an open control stack. -See the `dVRK Research Wiki `_ -and the `JHU dVRK software wiki -`_ for the hardware, -software, and research community. +The da Vinci Research Kit (dVRK) fills a role in surgical robotics similar to +the :doc:`SO-101 ` in general manipulation: it +provides a shared research embodiment built from repurposed first-generation +da Vinci hardware, with open controllers and software. The patient-side +architecture combines an Endoscopic Camera Manipulator (ECM) with up to three +Patient Side Manipulators (PSMs), which carry instruments such as needle drivers +and forceps. + +Physical dVRK systems are scarce and are primarily available to eligible +non-profit education and research institutions as non-clinical research +platforms. Simulation broadens access to the PSM embodiment for teleoperation, +demonstration collection, policy development, and reproducible evaluation. +dVRK research setups are typically bimanual, with two PSMs coordinating in a +shared workspace. Collaborative manipulation tasks therefore abound, +including passing a needle from one instrument to the other, as shown below. +See the `Intuitive Foundation dVRK programme +`_, the `dVRK Research Wiki +`_, and the `JHU dVRK +software wiki `_ for +the hardware, software, and research community. The dVRK Patient Side Manipulator (PSM) retargeters map XR controllers to one or two simulated PSMs. They convert controller state into absolute tool-pose and paired-jaw targets. A simulator integration can read those targets, use its live articulation Jacobian and joint state, and solve differential IK. +.. figure:: ../../_static/dvrk-needle-pass.gif + :alt: Real dVRK PSM teleoperation passing a surgical needle in Isaac Lab + :width: 720px + :align: center + :class: no-image-zoom + + A real bimanual teleoperation run of the Isaac Lab needle-passing task using + the same dVRK tool-pose and jaw-target contract. + At a glance ----------- From 298a6dfe1c69af9fd7c0df879726ebb7054cf8b6 Mon Sep 17 00:00:00 2001 From: Chris von Csefalvay Date: Thu, 10 Sep 2026 19:15:15 -0600 Subject: [PATCH 5/6] fix(dvrk): align rotation frames and clutch boundaries Apply controller rotation deltas in the shared reference frame and require squeeze to exceed the threshold for both pose and jaw control. Accept rounded home rotations and add regression coverage for calibration, re-clutching, threshold boundaries and zero-deadband jaw behaviour. Signed-off-by: Chris von Csefalvay --- docs/source/references/retargeting/dvrk.rst | 22 +- .../isaacteleop/retargeters/DVRK/control.py | 33 +- .../retargeters/DVRK/psm_retargeter.py | 17 +- .../test_dvrk_psm_retargeters.py | 375 ++++++++++++++++-- 4 files changed, 387 insertions(+), 60 deletions(-) diff --git a/docs/source/references/retargeting/dvrk.rst b/docs/source/references/retargeting/dvrk.rst index ae1bb75344..ba1fe89fa3 100644 --- a/docs/source/references/retargeting/dvrk.rst +++ b/docs/source/references/retargeting/dvrk.rst @@ -87,8 +87,11 @@ These retargeters do not consume face buttons, thumbsticks, thumbstick clicks, or menu buttons. A task or hosting XR application may bind those inputs separately. -The squeeze threshold defaults to ``0.5``. The trigger value captured at -squeeze engagement is neutral. The first jaw movement requires at least +The squeeze threshold defaults to ``0.5`` and must be finite and in ``[0, 1)``. +Both retargeters engage only when squeeze strictly exceeds the threshold. +Squeeze at or below the threshold holds the last target, including zero squeeze +with a zero threshold. The trigger value captured at squeeze engagement is +neutral. The first jaw movement requires at least ``0.05`` travel from that value. Positive travel past the threshold starts closing immediately. Negative travel must first be observed at least ``0.05`` below its reference, then remain there for ``0.1`` seconds before opening @@ -119,6 +122,21 @@ for a bimanual setup. The home pose must lie within the workspace bounds so the first clutch engagement remains continuous with the reset pose instead of clipping to another target. +Translation and rotation follow the shared reference axes by default, regardless +of the home orientation or the controller orientation at engagement. The +rotation delta is ``q_now * inverse(q_origin)`` and acts on the left of the tool +orientation captured at engagement. Re-clutching captures fresh controller +and tool references without changing the held target. + +``orientation_offset`` optionally remaps the rotation axes by conjugating this +delta: ``offset * delta * inverse(offset)``. It leaves translation unchanged +and is therefore not a transform between tracking and robot reference frames. +Express the controller input in the chosen shared reference frame before +passing it to the retargeter. + +Home rotation matrices allow an absolute orthonormality error of ``1e-4`` for +rounded configuration values and must have a positive determinant. + For a bimanual setup, express both controller streams and home poses in one world frame. Convert each target to that PSM's base frame immediately before its IK solve. Reusing one base transform for independently placed PSMs is diff --git a/src/python/isaacteleop/retargeters/DVRK/control.py b/src/python/isaacteleop/retargeters/DVRK/control.py index 29833e051e..1d28f55897 100644 --- a/src/python/isaacteleop/retargeters/DVRK/control.py +++ b/src/python/isaacteleop/retargeters/DVRK/control.py @@ -28,6 +28,8 @@ IDENTITY_QUATERNION_XYZW = (0.0, 0.0, 0.0, 1.0) MIN_QUATERNION_NORM = 1e-6 FLOAT32_MAX = float(np.finfo(np.float32).max) +# Allow small orthonormality errors from rounded configuration literals. +_ROTATION_ATOL = 1e-4 def _as_vector(values: object, *, length: int, name: str) -> np.ndarray: @@ -93,7 +95,7 @@ def rotation_matrix_to_quat_xyzw(rotation: object) -> np.ndarray | None: if rotation.shape != (3, 3) or not np.all(np.isfinite(rotation)): return None if ( - not np.allclose(rotation.T @ rotation, np.eye(3), atol=1e-5) + not np.allclose(rotation.T @ rotation, np.eye(3), atol=_ROTATION_ATOL, rtol=0.0) or np.linalg.det(rotation) <= 0.0 ): return None @@ -180,10 +182,11 @@ def clip_workspace( class DVRKPSMCartesianClutchConfig: """Configuration for simulator-independent clutch-rebased Cartesian pose. - Home, controller samples, and workspace bounds must share one reference - frame. Orientations are scalar-last ``xyzw`` quaternions. The calibration - ``orientation_offset`` is conjugated around controller-relative rotation; - it does not change the configured home orientation at engagement. + Home, controller samples and workspace bounds share one reference frame. + Orientations are scalar-last ``xyzw`` quaternions. ``orientation_offset`` + remaps the reference-frame rotation delta by conjugation, leaving translation + unchanged. Engagement preserves the held tool orientation. Squeeze must + exceed ``clutch_threshold``, which is finite and in ``[0, 1)``. """ home_position: tuple[float, float, float] = DEFAULT_HOME_POSITION @@ -246,9 +249,9 @@ def __init__(self, config: DVRKPSMCartesianClutchConfig | None = None) -> None: ) self._orientation_offset = orientation_offset if not np.isfinite(self._config.clutch_threshold) or not ( - 0.0 <= self._config.clutch_threshold <= 1.0 + 0.0 <= self._config.clutch_threshold < 1.0 ): - raise ValueError("clutch_threshold must be finite and in [0, 1]") + raise ValueError("clutch_threshold must be finite and in [0, 1)") # Keep this explicit: NumPy's chained concatenate/astype stubs otherwise # leave the attribute as Any under the repository's mypy configuration. @@ -303,7 +306,7 @@ def step( or not tracking_valid or squeeze_value is None or not np.isfinite(squeeze_value) - or squeeze_value < self._config.clutch_threshold + or squeeze_value <= self._config.clutch_threshold ): self._disengage() return self.pose @@ -340,8 +343,9 @@ def step( self._workspace_lower, self._workspace_upper, ) + # Spatial deltas act on the left so both channels follow reference axes. controller_relative_orientation = quat_mul_xyzw( - quat_conjugate_xyzw(self._controller_orientation_origin), orientation + orientation, quat_conjugate_xyzw(self._controller_orientation_origin) ) calibrated_relative_orientation = quat_mul_xyzw( quat_mul_xyzw(self._orientation_offset, controller_relative_orientation), @@ -349,7 +353,7 @@ def step( ) target_orientation = normalise_quaternion_xyzw( quat_mul_xyzw( - self._pose_at_engagement[3:7], calibrated_relative_orientation + calibrated_relative_orientation, self._pose_at_engagement[3:7] ) ) if target_orientation is None: @@ -385,7 +389,8 @@ class DVRKPSMJawIntentConfig: every emitted target is interpolated between them. ``initial_closedness`` selects the exact reset target on that segment (zero is open, one is closed). The squeeze threshold is intentionally shared with the arm - clutch so releasing the arm deadman never changes the jaw command. + clutch so releasing the arm deadman never changes the jaw command. Squeeze + must exceed the finite ``clutch_threshold`` in ``[0, 1)`` to engage. Trigger motion is relative to the value captured on each squeeze engagement. This avoids a jump when the operator re-clutches with the @@ -436,9 +441,9 @@ def __init__(self, config: DVRKPSMJawIntentConfig | None = None) -> None: ): raise ValueError("initial_closedness must be finite and in [0, 1]") if not np.isfinite(self._config.clutch_threshold) or not ( - 0.0 <= self._config.clutch_threshold <= 1.0 + 0.0 <= self._config.clutch_threshold < 1.0 ): - raise ValueError("clutch_threshold must be finite and in [0, 1]") + raise ValueError("clutch_threshold must be finite and in [0, 1)") if not np.isfinite(self._config.trigger_deadband) or not ( 0.0 <= self._config.trigger_deadband < 1.0 ): @@ -534,7 +539,7 @@ def step( or squeeze_value is None or not np.isfinite(trigger_value) or not np.isfinite(squeeze_value) - or squeeze_value < self._config.clutch_threshold + or squeeze_value <= self._config.clutch_threshold ): # This also cancels a not-yet-committed opening, which is the key # protection for trigger-up immediately followed by squeeze-up. diff --git a/src/python/isaacteleop/retargeters/DVRK/psm_retargeter.py b/src/python/isaacteleop/retargeters/DVRK/psm_retargeter.py index 2838cc8cf7..49c852c959 100644 --- a/src/python/isaacteleop/retargeters/DVRK/psm_retargeter.py +++ b/src/python/isaacteleop/retargeters/DVRK/psm_retargeter.py @@ -23,8 +23,9 @@ Frame contract -------------- -The controller stream, home pose, and workspace must use one shared reference -frame. For one PSM a caller may choose its base frame. A bimanual integration +The controller stream, home pose and workspace must use one shared reference +frame. Translation and uncalibrated rotation follow that frame's axes. For +one PSM a caller may choose its base frame. A bimanual integration can instead keep both streams in a shared world frame, then transform each target into that arm's base frame immediately before DLS IK. This keeps one XR stream valid for two independently placed PSM bases. @@ -74,9 +75,10 @@ class DVRKPSMClutchConfig: ``home_reference_T_ee`` and all workspace coordinates are expressed in one shared command reference frame. The owning simulator must reset the PSM to the same physical home pose before the first engagement. The - ``orientation_offset`` is a scalar-last calibration rotation conjugated - around the controller's relative rotation; this preserves the configured - tool orientation on the squeeze-latching frame. + ``orientation_offset`` is a scalar-last rotation that remaps the reference- + frame rotation delta by conjugation. It leaves translation unchanged and + preserves the held tool orientation at engagement. Squeeze must exceed + the finite ``clutch_threshold`` in ``[0, 1)`` to engage. """ home_reference_T_ee: np.ndarray @@ -191,7 +193,10 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N @dataclass(frozen=True) class DVRKPSMGripperConfig: - """Configuration for one intent-latched pair of native PSM jaws.""" + """Configuration for one intent-latched pair of native PSM jaws. + + Squeeze must exceed the finite ``clutch_threshold`` in ``[0, 1)`` to engage. + """ input_device: str = ControllersSource.RIGHT jaw_open: tuple[float, float] = _DEFAULT_JAW_OPEN diff --git a/tests/python/core/retargeting_engine/test_dvrk_psm_retargeters.py b/tests/python/core/retargeting_engine/test_dvrk_psm_retargeters.py index bd8f7e7dac..1db48e1fa9 100644 --- a/tests/python/core/retargeting_engine/test_dvrk_psm_retargeters.py +++ b/tests/python/core/retargeting_engine/test_dvrk_psm_retargeters.py @@ -8,6 +8,7 @@ import numpy as np import pytest import isaacteleop.retargeters as retargeters +from scipy.spatial.transform import Rotation from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource from isaacteleop.retargeting_engine.interface import ( @@ -40,6 +41,7 @@ normalise_quaternion_xyzw, quat_mul_xyzw, rebased_position, + rotation_matrix_to_quat_xyzw, ) _IDENTITY_QUAT = np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32) @@ -47,6 +49,21 @@ _JAW_CLOSED = np.array([-0.09, 0.09], dtype=np.float64) _POSE_OUTPUT = DVRKPSMClutchRetargeter.OUTPUT_POSE _JAW_OUTPUT = DVRKPSMGripperRetargeter.OUTPUT_JAW_TARGETS +_ROUNDED_HOME_ROTATION = np.array( + ( + (0.90673, -0.27703, 0.31794), + (0.37771, 0.86881, -0.32018), + (-0.18754, 0.41041, 0.89241), + ) +) +_INVALID_HOME_ROTATIONS = ( + pytest.param(np.diag((1.001, 1.0, 1.0)), id="scale"), + pytest.param( + np.array(((1.0, 0.001, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0))), + id="shear", + ), + pytest.param(np.diag((-1.0, 1.0, 1.0)), id="reflection"), +) def test_public_package_surface_contains_only_supported_dvrk_types() -> None: @@ -152,22 +169,36 @@ def _quat_xyzw_x(angle: float) -> np.ndarray: return np.array((math.sin(angle / 2.0), 0.0, 0.0, math.cos(angle / 2.0))) -def _rotation_matrix_z(angle: float) -> np.ndarray: - """Build a 3-D Z-axis rotation matrix for a configured tool home.""" - return np.array( - ( - (math.cos(angle), -math.sin(angle), 0.0), - (math.sin(angle), math.cos(angle), 0.0), - (0.0, 0.0, 1.0), - ) - ) - - def _read_vector(outputs, key: str) -> np.ndarray: """Read an emitted DLPack vector into a float64 array for comparisons.""" return np.asarray(np.from_dlpack(outputs[key][0]), dtype=np.float64) +@pytest.mark.parametrize("threshold", (-0.01, 1.0, math.nan, math.inf, -math.inf)) +@pytest.mark.parametrize( + ("config_type", "consumer_type"), + ( + (DVRKPSMCartesianClutchConfig, DVRKPSMCartesianClutchStateMachine), + (DVRKPSMJawIntentConfig, DVRKPSMJawIntentStateMachine), + (DVRKPSMClutchConfig, DVRKPSMClutchRetargeter), + (DVRKPSMGripperConfig, DVRKPSMGripperRetargeter), + ), +) +def test_clutch_threshold_must_be_finite_and_in_half_open_unit_interval( + threshold, config_type, consumer_type +): + config_kwargs = {"clutch_threshold": threshold} + if config_type is DVRKPSMClutchConfig: + config_kwargs["home_reference_T_ee"] = _make_home_transform((0.02, 0.0, 0.18)) + consumer_kwargs = ( + {"name": "test"} + if consumer_type in (DVRKPSMClutchRetargeter, DVRKPSMGripperRetargeter) + else {} + ) + with pytest.raises(ValueError, match="clutch_threshold"): + consumer_type(config_type(**config_kwargs), **consumer_kwargs) + + class TestDVRKPSMGripperMath: """Pure trigger-to-paired-jaw mapping checks.""" @@ -209,6 +240,37 @@ def test_engagement_captures_trigger_then_full_travel_closes_both_jaws(self): closed = self._compute(retargeter, time_s=0.02, trigger=1.0, squeeze=1.0) np.testing.assert_allclose(closed, _JAW_CLOSED, atol=1e-6) + @pytest.mark.parametrize("threshold", (0.0, 0.5)) + def test_squeeze_threshold_holds_and_reengages_without_a_jaw_jump(self, threshold): + retargeter = DVRKPSMGripperRetargeter( + DVRKPSMGripperConfig(clutch_threshold=threshold), name="jaws" + ) + initial = self._compute(retargeter, time_s=0.0, trigger=0.0, squeeze=threshold) + np.testing.assert_array_equal( + self._compute(retargeter, time_s=0.02, trigger=1.0, squeeze=threshold), + initial, + ) + engaged_squeeze = threshold + 1e-6 + np.testing.assert_array_equal( + self._compute( + retargeter, time_s=0.04, trigger=0.2, squeeze=engaged_squeeze + ), + initial, + ) + held = self._compute( + retargeter, time_s=0.06, trigger=0.7, squeeze=engaged_squeeze + ) + assert not np.array_equal(held, initial) + for time_s, squeeze in ( + (0.08, threshold), + (0.10, 0.0), + (0.12, engaged_squeeze), + ): + np.testing.assert_array_equal( + self._compute(retargeter, time_s=time_s, trigger=0.0, squeeze=squeeze), + held, + ) + def test_configured_initial_hold_can_use_a_validated_tighter_endpoint(self): """A task can start at its physical grasp cap without an initial trigger jump.""" validated_closed = (-0.032, 0.032) @@ -385,6 +447,52 @@ def test_initial_target_is_exact_and_first_engagement_is_no_op(self): self._step(kernel, trigger=0.73), expected, atol=1e-6 ) + @pytest.mark.parametrize("threshold", (0.0, 0.5)) + def test_squeeze_must_exceed_threshold_and_reclutch_captures_trigger( + self, threshold + ): + kernel = DVRKPSMJawIntentStateMachine( + DVRKPSMJawIntentConfig(initial_closedness=0.2, clutch_threshold=threshold) + ) + initial = kernel.targets + for trigger in (0.0, 1.0): + np.testing.assert_array_equal( + self._step(kernel, trigger=trigger, squeeze=threshold), initial + ) + + engaged_squeeze = threshold + 1e-6 + np.testing.assert_array_equal( + self._step(kernel, trigger=0.2, squeeze=engaged_squeeze), initial + ) + held = self._step(kernel, trigger=0.7, squeeze=engaged_squeeze) + assert kernel.closedness == pytest.approx(0.7) + for squeeze, trigger in ((threshold, 0.0), (0.0, 1.0)): + np.testing.assert_array_equal( + self._step(kernel, trigger=trigger, squeeze=squeeze, dt=1.0), held + ) + np.testing.assert_array_equal( + self._step(kernel, trigger=0.0, squeeze=engaged_squeeze), held + ) + self._step(kernel, trigger=0.1, squeeze=engaged_squeeze) + assert kernel.closedness == pytest.approx(0.8) + + def test_zero_deadband_stationary_trigger_preserves_hold_and_opening_dwell(self): + kernel = DVRKPSMJawIntentStateMachine( + DVRKPSMJawIntentConfig( + initial_closedness=0.7, + trigger_deadband=0.0, + opening_intent_duration_s=0.08, + ) + ) + held = self._step(kernel, trigger=0.8, dt=0.0) + for _ in range(3): + np.testing.assert_array_equal(self._step(kernel, trigger=0.8, dt=1.0), held) + np.testing.assert_array_equal(self._step(kernel, trigger=0.4, dt=1.0), held) + np.testing.assert_array_equal(self._step(kernel, trigger=0.4, dt=0.07), held) + opened = self._step(kernel, trigger=0.4, dt=0.011) + assert kernel.closedness == pytest.approx(0.3) + assert not np.array_equal(opened, held) + @pytest.mark.parametrize( ("jaw_open", "jaw_closed"), ( @@ -565,6 +673,21 @@ def test_large_finite_quaternion_normalises_without_overflow(self): assert quaternion is not None np.testing.assert_allclose(quaternion, (0.5, 0.5, 0.5, 0.5), atol=1e-12) + def test_five_decimal_rotation_matrix_is_accepted(self): + quaternion = rotation_matrix_to_quat_xyzw(_ROUNDED_HOME_ROTATION) + assert quaternion is not None + np.testing.assert_allclose(np.linalg.norm(quaternion), 1.0, atol=1e-12) + np.testing.assert_allclose( + Rotation.from_quat(quaternion).as_matrix(), + _ROUNDED_HOME_ROTATION, + atol=1e-5, + rtol=0.0, + ) + + @pytest.mark.parametrize("rotation", _INVALID_HOME_ROTATIONS) + def test_improper_rotation_matrix_is_rejected(self, rotation): + assert rotation_matrix_to_quat_xyzw(rotation) is None + def test_rebased_position_applies_scaled_controller_delta(self): """The latching frame is home and later controller motion is scaled from it.""" home = np.array([0.02, 0.00, 0.18], dtype=np.float64) @@ -613,6 +736,109 @@ def _step( session_active=active, ) + @pytest.mark.parametrize("threshold", (0.0, 0.5)) + def test_squeeze_must_exceed_threshold_and_reclutch_captures_pose(self, threshold): + kernel = DVRKPSMCartesianClutchStateMachine( + DVRKPSMCartesianClutchConfig(clutch_threshold=threshold) + ) + home = kernel.pose + origin = np.array((0.3, -0.2, 0.5)) + for position in (origin, origin + (0.03, 0.0, 0.0)): + np.testing.assert_array_equal( + self._step(kernel, position=position, squeeze=threshold), home + ) + assert not kernel.engaged + + engaged_squeeze = threshold + 1e-6 + np.testing.assert_array_equal( + self._step(kernel, position=origin, squeeze=engaged_squeeze), home + ) + assert kernel.engaged + held = self._step( + kernel, position=origin + (0.03, 0.0, 0.0), squeeze=engaged_squeeze + ) + np.testing.assert_allclose(held[:3], home[:3] + (0.03, 0.0, 0.0), atol=1e-6) + for squeeze in (threshold, 0.0): + np.testing.assert_array_equal( + self._step( + kernel, + position=(1.0, 1.0, 1.0), + orientation=_quat_xyzw_x(0.8), + squeeze=squeeze, + ), + held, + ) + assert not kernel.engaged + np.testing.assert_array_equal( + self._step(kernel, position=origin, squeeze=engaged_squeeze), held + ) + resumed = self._step( + kernel, position=origin + (0.01, 0.0, 0.0), squeeze=engaged_squeeze + ) + np.testing.assert_allclose(resumed[:3], held[:3] + (0.01, 0.0, 0.0), atol=1e-6) + + @pytest.mark.parametrize( + ("home_axis", "home_degrees", "origin_axis", "origin_degrees"), + (("x", 180, "y", 60), ("y", 60, "x", 90), ("x", 0, "x", 90)), + ) + def test_rotation_follows_reference_axes_with_rotated_home_and_controller( + self, home_axis, home_degrees, origin_axis, origin_degrees + ): + home = Rotation.from_euler(home_axis, home_degrees, degrees=True) + origin = Rotation.from_euler(origin_axis, origin_degrees, degrees=True) + delta = Rotation.from_euler("z", 30, degrees=True) + kernel = self._kernel(home_orientation=tuple(home.as_quat())) + home_pose = kernel.pose + np.testing.assert_array_equal( + self._step(kernel, orientation=origin.as_quat()), home_pose + ) + pose = self._step(kernel, orientation=(delta * origin).as_quat()) + np.testing.assert_allclose( + Rotation.from_quat(pose[3:]).as_matrix(), + delta.as_matrix() @ home.as_matrix(), + atol=1e-6, + ) + + @pytest.mark.parametrize("offset_degrees", (0, 90)) + def test_reference_rotation_and_calibration_survive_rotated_reclutch( + self, offset_degrees + ): + home = Rotation.from_euler("y", 60, degrees=True) + origin = Rotation.from_euler("x", 90, degrees=True) + offset = Rotation.from_euler("x", offset_degrees, degrees=True) + kernel = DVRKPSMCartesianClutchStateMachine( + DVRKPSMCartesianClutchConfig( + home_orientation=tuple(home.as_quat()), + orientation_offset=tuple(offset.as_quat()), + ) + ) + self._step(kernel, orientation=origin.as_quat()) + delta = Rotation.from_euler("z", 30, degrees=True) + held = self._step(kernel, orientation=(delta * origin).as_quat()) + offset_matrix = offset.as_matrix() + held_rotation = ( + offset_matrix @ delta.as_matrix() @ offset_matrix.T @ home.as_matrix() + ) + np.testing.assert_allclose( + Rotation.from_quat(held[3:]).as_matrix(), held_rotation, atol=1e-6 + ) + + new_origin = Rotation.from_euler("zy", (-70, 40), degrees=True) + np.testing.assert_array_equal( + self._step(kernel, orientation=new_origin.as_quat(), squeeze=0.0), held + ) + np.testing.assert_array_equal( + self._step(kernel, orientation=new_origin.as_quat()), held + ) + new_delta = Rotation.from_euler("y", -20, degrees=True) + resumed = self._step(kernel, orientation=(new_delta * new_origin).as_quat()) + np.testing.assert_allclose( + Rotation.from_quat(resumed[3:]).as_matrix(), + offset_matrix @ new_delta.as_matrix() @ offset_matrix.T @ held_rotation, + atol=1e-6, + ) + np.testing.assert_array_equal(resumed[:3], held[:3]) + def test_engagement_and_reclutch_are_no_jump_absolute_deltas(self): kernel = self._kernel() origin = np.array((0.30, -0.20, 0.50), dtype=np.float64) @@ -814,6 +1040,13 @@ def _retargeter() -> DVRKPSMClutchRetargeter: ) return DVRKPSMClutchRetargeter(config, name="ee_pose") + @staticmethod + def _compute(retargeter, **controller_kwargs) -> np.ndarray: + inputs, outputs = _build_io(retargeter) + inputs[ControllersSource.RIGHT] = _make_controller(**controller_kwargs) + retargeter.compute(inputs, outputs, _make_context()) + return _read_vector(outputs, _POSE_OUTPUT) + def test_output_contract_is_absolute_7d_pose(self): """The DLS integration receives one position-plus-xyzw target vector.""" pose_type = self._retargeter().output_spec()[_POSE_OUTPUT].types[0] @@ -946,6 +1179,69 @@ def test_non_homogeneous_configured_home_is_rejected(self): name="ee_pose", ) + @pytest.mark.parametrize("rotation", _INVALID_HOME_ROTATIONS) + def test_improper_configured_home_rotation_is_rejected(self, rotation): + with pytest.raises(ValueError, match="proper rotation matrix"): + DVRKPSMClutchRetargeter( + DVRKPSMClutchConfig( + home_reference_T_ee=_make_home_transform( + (0.02, 0.0, 0.18), rotation + ) + ), + name="ee_pose", + ) + + def test_five_decimal_home_rotation_is_accepted_without_an_engagement_jump(self): + retargeter = DVRKPSMClutchRetargeter( + DVRKPSMClutchConfig( + home_reference_T_ee=_make_home_transform( + (0.02, 0.0, 0.18), _ROUNDED_HOME_ROTATION + ) + ), + name="ee_pose", + ) + home = self._compute(retargeter, squeeze=0.0) + np.testing.assert_allclose( + Rotation.from_quat(home[3:]).as_matrix(), + _ROUNDED_HOME_ROTATION, + atol=1e-5, + rtol=0.0, + ) + np.testing.assert_array_equal( + self._compute(retargeter, grip_ori=_quat_xyzw_x(0.8)), home + ) + + @pytest.mark.parametrize("threshold", (0.0, 0.5)) + def test_squeeze_threshold_holds_and_reengages_without_a_pose_jump(self, threshold): + retargeter = DVRKPSMClutchRetargeter( + DVRKPSMClutchConfig( + home_reference_T_ee=_make_home_transform((0.02, 0.0, 0.18)), + clutch_threshold=threshold, + ), + name="ee_pose", + ) + origin = np.array((0.3, -0.2, 0.5)) + home = self._compute(retargeter, grip_pos=origin, squeeze=threshold) + np.testing.assert_array_equal( + self._compute( + retargeter, grip_pos=origin + (0.03, 0.0, 0.0), squeeze=threshold + ), + home, + ) + engaged_squeeze = threshold + 1e-6 + np.testing.assert_array_equal( + self._compute(retargeter, grip_pos=origin, squeeze=engaged_squeeze), home + ) + held = self._compute( + retargeter, grip_pos=origin + (0.03, 0.0, 0.0), squeeze=engaged_squeeze + ) + np.testing.assert_allclose(held[:3], home[:3] + (0.03, 0.0, 0.0), atol=1e-6) + for squeeze in (threshold, 0.0, engaged_squeeze): + np.testing.assert_array_equal( + self._compute(retargeter, grip_pos=(1.0, 1.0, 1.0), squeeze=squeeze), + held, + ) + def test_engage_motion_workspace_clamp_and_reclutch_are_no_jump(self): """Engage, clamp, stop, and re-engage preserve a bounded continuous target.""" retargeter = self._retargeter() @@ -1076,46 +1372,49 @@ def test_squeeze_is_deadman_clutch_and_reengagement_is_no_jump(self): retargeter.compute(inputs, outputs, _make_context()) np.testing.assert_allclose(_read_vector(outputs, _POSE_OUTPUT), held, atol=1e-6) - def test_orientation_is_relative_to_squeeze_latch(self): - """Squeeze holds the configured orientation, then follows controller-relative rotation.""" - home_angle = 0.4 - controller_angle = -0.8 - home_rotation = _rotation_matrix_z(home_angle) + @pytest.mark.parametrize( + ("home_axis", "home_degrees", "offset_degrees"), + (("x", 180, 0), ("y", 60, 0), ("y", 60, 90)), + ) + def test_rotation_uses_reference_axes_with_rotated_home_and_controller( + self, home_axis, home_degrees, offset_degrees + ): + home = Rotation.from_euler(home_axis, home_degrees, degrees=True) + origin = Rotation.from_euler("x", 90, degrees=True) + offset = Rotation.from_euler("x", offset_degrees, degrees=True) retargeter = DVRKPSMClutchRetargeter( DVRKPSMClutchConfig( home_reference_T_ee=_make_home_transform( - (0.02, 0.00, 0.18), home_rotation + (0.02, 0.00, 0.18), home.as_matrix() ), - workspace_lower=(-0.10, -0.10, 0.08), - workspace_upper=(0.10, 0.10, 0.24), + orientation_offset=tuple(offset.as_quat()), ), name="ee_pose", ) - origin_orientation = _quat_xyzw_z(controller_angle) - inputs, outputs = _build_io(retargeter) - inputs[ControllersSource.RIGHT] = _make_controller( - grip_pos=(0.30, -0.20, 0.50), - grip_ori=origin_orientation, - squeeze=1.0, + origin_position = np.array((0.3, -0.2, 0.5)) + latched = self._compute( + retargeter, grip_pos=origin_position, grip_ori=origin.as_quat() ) - retargeter.compute(inputs, outputs, _make_context()) - home_quaternion = _quat_xyzw_z(home_angle) np.testing.assert_allclose( - _read_vector(outputs, _POSE_OUTPUT)[3:], home_quaternion, atol=1e-6 + Rotation.from_quat(latched[3:]).as_matrix(), home.as_matrix(), atol=1e-6 ) - relative_rotation = _quat_xyzw_z(0.3) - current_orientation = quat_mul_xyzw(origin_orientation, relative_rotation) - inputs, outputs = _build_io(retargeter) - inputs[ControllersSource.RIGHT] = _make_controller( - grip_pos=(0.30, -0.20, 0.50), - grip_ori=current_orientation, - squeeze=1.0, + delta = Rotation.from_euler("z", 30, degrees=True) + moved = self._compute( + retargeter, + grip_pos=origin_position + (0.01, 0.02, -0.01), + grip_ori=(delta * origin).as_quat(), + ) + np.testing.assert_allclose( + Rotation.from_quat(moved[3:]).as_matrix(), + offset.as_matrix() + @ delta.as_matrix() + @ offset.as_matrix().T + @ home.as_matrix(), + atol=1e-6, ) - retargeter.compute(inputs, outputs, _make_context()) - expected = quat_mul_xyzw(home_quaternion, relative_rotation) np.testing.assert_allclose( - _read_vector(outputs, _POSE_OUTPUT)[3:], expected, atol=1e-6 + moved[:3], latched[:3] + (0.01, 0.02, -0.01), atol=1e-6 ) def test_invalid_or_dropped_tracking_holds_a_finite_last_pose(self): From ec67bb534bff771c6c90eb8ab2c8a2fae9f381ff Mon Sep 17 00:00:00 2001 From: Chris von Csefalvay Date: Wed, 21 Oct 2026 19:00:23 +0000 Subject: [PATCH 6/6] fix(dvrk): migrate retargeters to isaaccapture Move the dVRK package into the canonical Python tree and update imports, docs and the mypy target for the package rename. Signed-off-by: Chris von Csefalvay --- docs/source/references/retargeting/dvrk.rst | 6 +++--- .../retargeters/DVRK/__init__.py | 0 .../retargeters/DVRK/control.py | 0 .../retargeters/DVRK/psm_retargeter.py | 12 ++++++------ .../core/retargeting_engine/CMakeLists.txt | 2 +- .../test_dvrk_psm_retargeters.py | 16 ++++++++-------- 6 files changed, 18 insertions(+), 18 deletions(-) rename src/python/{isaacteleop => isaaccapture}/retargeters/DVRK/__init__.py (100%) rename src/python/{isaacteleop => isaaccapture}/retargeters/DVRK/control.py (100%) rename src/python/{isaacteleop => isaaccapture}/retargeters/DVRK/psm_retargeter.py (96%) diff --git a/docs/source/references/retargeting/dvrk.rst b/docs/source/references/retargeting/dvrk.rst index ba1fe89fa3..6188e9bfea 100644 --- a/docs/source/references/retargeting/dvrk.rst +++ b/docs/source/references/retargeting/dvrk.rst @@ -149,9 +149,9 @@ Use the retargeters from Python import numpy as np - from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource - from isaacteleop.retargeting_engine.interface import OutputCombiner - from isaacteleop.retargeters import ( + from isaaccapture.retargeting_engine.deviceio_source_nodes import ControllersSource + from isaaccapture.retargeting_engine.interface import OutputCombiner + from isaaccapture.retargeters import ( DVRKPSMClutchConfig, DVRKPSMClutchRetargeter, DVRKPSMGripperConfig, diff --git a/src/python/isaacteleop/retargeters/DVRK/__init__.py b/src/python/isaaccapture/retargeters/DVRK/__init__.py similarity index 100% rename from src/python/isaacteleop/retargeters/DVRK/__init__.py rename to src/python/isaaccapture/retargeters/DVRK/__init__.py diff --git a/src/python/isaacteleop/retargeters/DVRK/control.py b/src/python/isaaccapture/retargeters/DVRK/control.py similarity index 100% rename from src/python/isaacteleop/retargeters/DVRK/control.py rename to src/python/isaaccapture/retargeters/DVRK/control.py diff --git a/src/python/isaacteleop/retargeters/DVRK/psm_retargeter.py b/src/python/isaaccapture/retargeters/DVRK/psm_retargeter.py similarity index 96% rename from src/python/isaacteleop/retargeters/DVRK/psm_retargeter.py rename to src/python/isaaccapture/retargeters/DVRK/psm_retargeter.py index 49c852c959..a9ada6b32d 100644 --- a/src/python/isaacteleop/retargeters/DVRK/psm_retargeter.py +++ b/src/python/isaaccapture/retargeters/DVRK/psm_retargeter.py @@ -37,15 +37,15 @@ import numpy as np -from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource -from isaacteleop.retargeting_engine.interface import BaseRetargeter, RetargeterIOType -from isaacteleop.retargeting_engine.interface.execution_events import ExecutionState -from isaacteleop.retargeting_engine.interface.retargeter_core_types import RetargeterIO -from isaacteleop.retargeting_engine.interface.tensor_group_type import ( +from isaaccapture.retargeting_engine.deviceio_source_nodes import ControllersSource +from isaaccapture.retargeting_engine.interface import BaseRetargeter, RetargeterIOType +from isaaccapture.retargeting_engine.interface.execution_events import ExecutionState +from isaaccapture.retargeting_engine.interface.retargeter_core_types import RetargeterIO +from isaaccapture.retargeting_engine.interface.tensor_group_type import ( OptionalType, TensorGroupType, ) -from isaacteleop.retargeting_engine.tensor_types import ( +from isaaccapture.retargeting_engine.tensor_types import ( ControllerInput, ControllerInputIndex, DLDataType, diff --git a/tests/python/core/retargeting_engine/CMakeLists.txt b/tests/python/core/retargeting_engine/CMakeLists.txt index 96a8b52876..37eaccfca8 100644 --- a/tests/python/core/retargeting_engine/CMakeLists.txt +++ b/tests/python/core/retargeting_engine/CMakeLists.txt @@ -61,7 +61,7 @@ add_test( COMMAND uv run --python ${ISAAC_TELEOP_PYTHON_VERSION} --extra dev mypy --config-file pyproject.toml "${CMAKE_SOURCE_DIR}/src/python/isaaccapture/retargeting_engine" - "${CMAKE_SOURCE_DIR}/src/python/isaacteleop/retargeters/DVRK" + "${CMAKE_SOURCE_DIR}/src/python/isaaccapture/retargeters/DVRK" WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" ) diff --git a/tests/python/core/retargeting_engine/test_dvrk_psm_retargeters.py b/tests/python/core/retargeting_engine/test_dvrk_psm_retargeters.py index 1db48e1fa9..a4d195a07a 100644 --- a/tests/python/core/retargeting_engine/test_dvrk_psm_retargeters.py +++ b/tests/python/core/retargeting_engine/test_dvrk_psm_retargeters.py @@ -7,32 +7,32 @@ import numpy as np import pytest -import isaacteleop.retargeters as retargeters +import isaaccapture.retargeters as retargeters from scipy.spatial.transform import Rotation -from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource -from isaacteleop.retargeting_engine.interface import ( +from isaaccapture.retargeting_engine.deviceio_source_nodes import ControllersSource +from isaaccapture.retargeting_engine.interface import ( ComputeContext, ExecutionEvents, ExecutionState, OptionalTensorGroup, TensorGroup, ) -from isaacteleop.retargeting_engine.interface.retargeter_core_types import GraphTime -from isaacteleop.retargeting_engine.interface.tensor_group_type import ( +from isaaccapture.retargeting_engine.interface.retargeter_core_types import GraphTime +from isaaccapture.retargeting_engine.interface.tensor_group_type import ( OptionalTensorGroupType, ) -from isaacteleop.retargeting_engine.tensor_types import ( +from isaaccapture.retargeting_engine.tensor_types import ( ControllerInput, ControllerInputIndex, ) -from isaacteleop.retargeters import ( +from isaaccapture.retargeters import ( DVRKPSMClutchConfig, DVRKPSMClutchRetargeter, DVRKPSMGripperConfig, DVRKPSMGripperRetargeter, ) -from isaacteleop.retargeters.DVRK.control import ( +from isaaccapture.retargeters.DVRK.control import ( DVRKPSMCartesianClutchConfig, DVRKPSMCartesianClutchStateMachine, DVRKPSMJawIntentConfig,