diff --git a/docs/source/_static/dvrk-needle-pass.gif b/docs/source/_static/dvrk-needle-pass.gif new file mode 100644 index 000000000..208ff2b90 --- /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 new file mode 100644 index 000000000..ae1bb7534 --- /dev/null +++ b/docs/source/references/retargeting/dvrk.rst @@ -0,0 +1,207 @@ +.. SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +.. SPDX-License-Identifier: Apache-2.0 + +dVRK PSM retargeters +==================== + +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 +----------- + +.. 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 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 + + 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 + + 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, + DVRKPSMGripperConfig, + DVRKPSMGripperRetargeter, + ) + + 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. + +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 +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 d67113c80..6c78e87ed 100644 --- a/docs/source/references/retargeting/index.rst +++ b/docs/source/references/retargeting/index.rst @@ -55,6 +55,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 @@ -338,4 +346,5 @@ and :doc:`Contributing Guide <../../getting_started/contributing>` for details. sharpa so101 + dvrk joint_space diff --git a/src/core/python/pyproject.toml.in b/src/core/python/pyproject.toml.in index 1946d2808..0f3d261ec 100644 --- a/src/core/python/pyproject.toml.in +++ b/src/core/python/pyproject.toml.in @@ -51,6 +51,7 @@ packages = [ "isaacteleop.retargeters", "isaacteleop.retargeters.G1", "isaacteleop.retargeters.SO101", + "isaacteleop.retargeters.DVRK", "isaacteleop.retargeters.joint_space", "isaacteleop.retargeting_engine_ui", "isaacteleop.teleop_session_manager", diff --git a/src/core/retargeting_engine_tests/python/CMakeLists.txt b/src/core/retargeting_engine_tests/python/CMakeLists.txt index 5feb90623..8f85db74d 100644 --- a/src/core/retargeting_engine_tests/python/CMakeLists.txt +++ b/src/core/retargeting_engine_tests/python/CMakeLists.txt @@ -56,6 +56,7 @@ add_test( COMMAND uv run --python ${ISAAC_TELEOP_PYTHON_VERSION} --extra dev mypy --config-file pyproject.toml "${CMAKE_SOURCE_DIR}/src/core/retargeting_engine/python" + "${CMAKE_SOURCE_DIR}/src/retargeters/DVRK" WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" ) diff --git a/src/core/retargeting_engine_tests/python/test_dvrk_psm_retargeters.py b/src/core/retargeting_engine_tests/python/test_dvrk_psm_retargeters.py new file mode 100644 index 000000000..bd8f7e7da --- /dev/null +++ b/src/core/retargeting_engine_tests/python/test_dvrk_psm_retargeters.py @@ -0,0 +1,1206 @@ +# 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.""" + +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, + normalise_quaternion_xyzw, + 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) + 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.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): + """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 + ) + 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): + """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_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 + ) + 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_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) + 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) + + @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) + 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) + + 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"), + ( + ((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, + ) diff --git a/src/retargeters/CMakeLists.txt b/src/retargeters/CMakeLists.txt index ba1c06784..80ffa2c81 100644 --- a/src/retargeters/CMakeLists.txt +++ b/src/retargeters/CMakeLists.txt @@ -12,6 +12,8 @@ if(BUILD_PYTHON_BINDINGS) "${CMAKE_BINARY_DIR}/python_package/$/isaacteleop/retargeters/G1/__pycache__" COMMAND ${CMAKE_COMMAND} -E rm -rf "${CMAKE_BINARY_DIR}/python_package/$/isaacteleop/retargeters/SO101/__pycache__" + COMMAND ${CMAKE_COMMAND} -E rm -rf + "${CMAKE_BINARY_DIR}/python_package/$/isaacteleop/retargeters/DVRK/__pycache__" COMMAND ${CMAKE_COMMAND} -E rm -rf "${CMAKE_BINARY_DIR}/python_package/$/isaacteleop/retargeters/joint_space/__pycache__" COMMAND ${CMAKE_COMMAND} -E rm -f diff --git a/src/retargeters/DVRK/__init__.py b/src/retargeters/DVRK/__init__.py new file mode 100644 index 000000000..1152b6fbf --- /dev/null +++ b/src/retargeters/DVRK/__init__.py @@ -0,0 +1,30 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# 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/retargeters/DVRK/control.py b/src/retargeters/DVRK/control.py new file mode 100644 index 000000000..ad685d7f9 --- /dev/null +++ b/src/retargeters/DVRK/control.py @@ -0,0 +1,625 @@ +# 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. + +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 +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) + 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 + 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 scaled / scaled_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 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.asarray( + np.minimum(np.maximum(position, workspace_lower), workspace_upper), + dtype=np.float64, + ) + + +@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_float32_output_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_float32_output_vector( + self._config.workspace_lower, length=3, name="workspace_lower" + ) + 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): + 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 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 within the float32 range.""" + return _as_float32_output_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 + 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 + 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 + 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 + 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 + + # 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._cancel_opening() + return self.targets + + +__all__ = [ + "DVRKPSMCartesianClutchConfig", + "DVRKPSMCartesianClutchStateMachine", + "DVRKPSMJawIntentConfig", + "DVRKPSMJawIntentStateMachine", +] diff --git a/src/retargeters/DVRK/psm_retargeter.py b/src/retargeters/DVRK/psm_retargeter.py new file mode 100644 index 000000000..2838cc8cf --- /dev/null +++ b/src/retargeters/DVRK/psm_retargeter.py @@ -0,0 +1,300 @@ +# 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). + +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. 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. 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 + +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/src/retargeters/__init__.py b/src/retargeters/__init__.py index ae3a9de19..530154411 100644 --- a/src/retargeters/__init__.py +++ b/src/retargeters/__init__.py @@ -18,6 +18,8 @@ - GripperRetargeter: Pinch-based gripper control - SO101ClutchRetargeter: Clutch-rebased absolute EE pose for the SO-101 5-DOF arm - 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 - JointStateRetargeter: Generic joint-space device (leader arm, exoskeleton) -> joint or EE action - SharpaHandRetargeter: Pinocchio/Pink IK-based retargeting for Sharpa hand - SharpaBiManualRetargeter: Bimanual version of SharpaHandRetargeter @@ -112,6 +114,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, + ), # .joint_space (generic joint-space devices: leader arms, exoskeletons, ...) "JointStateRetargeter": ( ".joint_space.joint_state_retargeter", @@ -217,6 +240,11 @@ def __getattr__(name: str): # SO-101 5-DOF arm retargeters "SO101ClutchRetargeter", "SO101GripperRetargeter", + # dVRK PSM retargeters + "DVRKPSMClutchConfig", + "DVRKPSMClutchRetargeter", + "DVRKPSMGripperConfig", + "DVRKPSMGripperRetargeter", # Generic joint-space device retargeters (leader arms, exoskeletons, ...) "JointStateRetargeter", "JointStateRetargeterConfig",