teleopro-core 0.1.0__tar.gz

teleopro_core-0.1.0/LICENSE

@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

teleopro_core-0.1.0/MANIFEST.in

@@ -0,0 +1 @@
+include LICENSE README.md package.xml resource/teleopro_core

teleopro_core-0.1.0/PKG-INFO

@@ -0,0 +1,101 @@
+Metadata-Version: 2.4
+Name: teleopro_core
+Version: 0.1.0
+Summary: Device- and robot-agnostic teleoperation mapping, no ROS required.
+Home-page: https://github.com/ShifeiDuan/teleopro
+Maintainer: Shifei Duan
+Maintainer-email: sdua078@aucklanduni.ac.nz
+License: Apache-2.0
+Project-URL: Source, https://github.com/ShifeiDuan/teleopro
+Project-URL: Issues, https://github.com/ShifeiDuan/teleopro/issues
+Keywords: teleoperation,robotics,ros2,xr,spacemouse
+Classifier: Development Status :: 3 - Alpha
+Classifier: Intended Audience :: Developers
+Classifier: Intended Audience :: Science/Research
+Classifier: Operating System :: OS Independent
+Classifier: Programming Language :: Python :: 3
+Classifier: Topic :: Scientific/Engineering
+Classifier: Topic :: Software Development :: Libraries :: Python Modules
+Requires-Python: >=3.8
+Description-Content-Type: text/markdown
+License-File: LICENSE
+Requires-Dist: numpy
+Provides-Extra: test
+Requires-Dist: pytest; extra == "test"
+Dynamic: classifier
+Dynamic: description
+Dynamic: description-content-type
+Dynamic: home-page
+Dynamic: keywords
+Dynamic: license
+Dynamic: license-file
+Dynamic: maintainer
+Dynamic: maintainer-email
+Dynamic: project-url
+Dynamic: provides-extra
+Dynamic: requires-dist
+Dynamic: requires-python
+Dynamic: summary
+
+# teleopro_core
+
+Teleoperate any robot with any input device, in pure Python (numpy only, no
+ROS). It turns the motion of a device (XR headset, tracker, mocap, ...) into a
+target pose for the robot, with clutch, speed limits and input-loss handling.
+
+ROS users: use the `teleopro_ros` package, which wraps this library with TF.
+
+## Install
+
+```bash
+pip install "git+https://github.com/ShifeiDuan/teleopro#subdirectory=teleopro_core"
+```
+
+## Usage
+
+```python
+import time
+from teleopro_core import Teleop, as_pos_quat
+
+teleop = Teleop(translation_scale=0.5, max_lin_vel=0.1)
+
+teleop.engage()                       # e.g. when a button is pressed
+while True:
+    device_pose = my_device.get_pose()       # ((x, y, z), (qx, qy, qz, qw)) or 4x4, None if not tracked
+    robot_pose = my_robot.get_tcp_pose()     # same formats, in the robot base frame
+    target = teleop.update(time.monotonic(), device_pose, robot_pose)
+    if target is not None:
+        my_robot.move_to(*as_pos_quat(target))
+    time.sleep(0.01)
+```
+
+- On `engage()` the target starts at the current robot pose, then follows the
+  device's motion since that moment. `disengage()` holds it still (clutch).
+- If the device is not tracked (`None`) or its `input_stamp` is older than
+  `input_timeout`, the target holds; when tracking returns it continues from
+  where it stopped, without jumping.
+- Before the first engage, `update()` returns the robot pose latched once.
+
+Velocity devices (keyboard, SpaceMouse) use `update_twist()` instead:
+
+```python
+teleop = Teleop(twist_frame='tool')      # or 'base'
+target = teleop.update_twist(time.monotonic(), ((vx, vy, vz), (wx, wy, wz)), robot_pose)
+```
+
+## Parameters
+
+| Parameter | Default | Meaning |
+|---|---|---|
+| `align_rpy` | `(0, 0, 0)` | Rotation from device axes to robot axes |
+| `translation_scale`, `rotation_scale` | `1.0` | Motion scaling |
+| `max_lin_vel`, `max_ang_vel` | `0.5`, `1.0` | Target speed limits (m/s, rad/s), 0 = off |
+| `dof_mask` | `(1, 1, 1, 1, 1, 1)` | `[x, y, z, roll, pitch, yaw]`; 0 freezes a DoF |
+| `workspace_min`, `workspace_max` | `None` | `[x, y, z]` box the target stays in |
+| `input_timeout` | `0.2` | Seconds before a stale `input_stamp` counts as lost |
+| `twist_frame` | `'base'` | `'base'` or `'tool'`, frame of the twist in `update_twist()` |
+| `logger` | `logging` | Anything with `.debug()` / `.info()` / `.warning()` |
+
+## License
+
+Apache-2.0

teleopro_core-0.1.0/README.md

@@ -0,0 +1,62 @@
+# teleopro_core
+
+Teleoperate any robot with any input device, in pure Python (numpy only, no
+ROS). It turns the motion of a device (XR headset, tracker, mocap, ...) into a
+target pose for the robot, with clutch, speed limits and input-loss handling.
+
+ROS users: use the `teleopro_ros` package, which wraps this library with TF.
+
+## Install
+
+```bash
+pip install "git+https://github.com/ShifeiDuan/teleopro#subdirectory=teleopro_core"
+```
+
+## Usage
+
+```python
+import time
+from teleopro_core import Teleop, as_pos_quat
+
+teleop = Teleop(translation_scale=0.5, max_lin_vel=0.1)
+
+teleop.engage()                       # e.g. when a button is pressed
+while True:
+    device_pose = my_device.get_pose()       # ((x, y, z), (qx, qy, qz, qw)) or 4x4, None if not tracked
+    robot_pose = my_robot.get_tcp_pose()     # same formats, in the robot base frame
+    target = teleop.update(time.monotonic(), device_pose, robot_pose)
+    if target is not None:
+        my_robot.move_to(*as_pos_quat(target))
+    time.sleep(0.01)
+```
+
+- On `engage()` the target starts at the current robot pose, then follows the
+  device's motion since that moment. `disengage()` holds it still (clutch).
+- If the device is not tracked (`None`) or its `input_stamp` is older than
+  `input_timeout`, the target holds; when tracking returns it continues from
+  where it stopped, without jumping.
+- Before the first engage, `update()` returns the robot pose latched once.
+
+Velocity devices (keyboard, SpaceMouse) use `update_twist()` instead:
+
+```python
+teleop = Teleop(twist_frame='tool')      # or 'base'
+target = teleop.update_twist(time.monotonic(), ((vx, vy, vz), (wx, wy, wz)), robot_pose)
+```
+
+## Parameters
+
+| Parameter | Default | Meaning |
+|---|---|---|
+| `align_rpy` | `(0, 0, 0)` | Rotation from device axes to robot axes |
+| `translation_scale`, `rotation_scale` | `1.0` | Motion scaling |
+| `max_lin_vel`, `max_ang_vel` | `0.5`, `1.0` | Target speed limits (m/s, rad/s), 0 = off |
+| `dof_mask` | `(1, 1, 1, 1, 1, 1)` | `[x, y, z, roll, pitch, yaw]`; 0 freezes a DoF |
+| `workspace_min`, `workspace_max` | `None` | `[x, y, z]` box the target stays in |
+| `input_timeout` | `0.2` | Seconds before a stale `input_stamp` counts as lost |
+| `twist_frame` | `'base'` | `'base'` or `'tool'`, frame of the twist in `update_twist()` |
+| `logger` | `logging` | Anything with `.debug()` / `.info()` / `.warning()` |
+
+## License
+
+Apache-2.0

teleopro_core-0.1.0/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>teleopro_core</name>
+  <version>0.1.0</version>
+  <description>
+    Device- and robot-agnostic teleoperation mapping in pure Python (numpy only).
+    Usable with or without ROS.
+  </description>
+  <maintainer email="sdua078@aucklanduni.ac.nz">Shifei Duan</maintainer>
+  <license>Apache-2.0</license>
+
+  <exec_depend>python3-numpy</exec_depend>
+
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

teleopro_core-0.1.0/setup.cfg

@@ -0,0 +1,4 @@
+[egg_info]
+tag_build = 
+tag_date = 0
+

teleopro_core-0.1.0/setup.py

@@ -0,0 +1,47 @@
+import os
+
+from setuptools import find_packages, setup
+
+package_name = 'teleopro_core'
+
+# colcon --symlink-install runs setup.py from the build directory
+readme = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'README.md')
+long_description = ''
+if os.path.exists(readme):
+    with open(readme, encoding='utf-8') as f:
+        long_description = f.read()
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['numpy'],
+    python_requires='>=3.8',
+    zip_safe=True,
+    maintainer='Shifei Duan',
+    maintainer_email='sdua078@aucklanduni.ac.nz',
+    description='Device- and robot-agnostic teleoperation mapping, no ROS required.',
+    long_description=long_description,
+    long_description_content_type='text/markdown',
+    license='Apache-2.0',
+    url='https://github.com/ShifeiDuan/teleopro',
+    project_urls={
+        'Source': 'https://github.com/ShifeiDuan/teleopro',
+        'Issues': 'https://github.com/ShifeiDuan/teleopro/issues',
+    },
+    keywords=['teleoperation', 'robotics', 'ros2', 'xr', 'spacemouse'],
+    classifiers=[
+        'Development Status :: 3 - Alpha',
+        'Intended Audience :: Developers',
+        'Intended Audience :: Science/Research',
+        'Operating System :: OS Independent',
+        'Programming Language :: Python :: 3',
+        'Topic :: Scientific/Engineering',
+        'Topic :: Software Development :: Libraries :: Python Modules',
+    ],
+    extras_require={'test': ['pytest']},
+)

teleopro_core-0.1.0/teleopro_core/__init__.py

@@ -0,0 +1,6 @@
+"""Teleoperation mapping for any device and robot."""
+
+from teleopro_core.mapping import TeleopMapper
+from teleopro_core.teleop import as_matrix, as_pos_quat, Teleop
+
+__all__ = ['Teleop', 'TeleopMapper', 'as_matrix', 'as_pos_quat']

teleopro_core-0.1.0/teleopro_core/mapping.py

@@ -0,0 +1,150 @@
+"""Mapping from input pose or velocity to a robot target pose."""
+
+import numpy as np
+
+from teleopro_core.transforms import make_transform, matrix_to_rotvec, rotvec_to_matrix
+
+TWIST_FRAMES = ('base', 'tool')
+
+
+class TeleopMapper:
+    """Maps input motion onto a robot target pose.
+
+    Pose input applies the input motion since engage to the reference pose:
+
+        p = p_ref + s_t * R_align * (p_in - p_in0)
+        R = R_align * dR * R_align^T * R_ref,  dR = R_in * R_in0^T
+
+    Twist input integrates the velocity, expressed in the robot base or tool frame.
+
+    Args:
+        align: 3x3 rotation of the input axes in the robot base.
+        translation_scale: Translation scale factor.
+        rotation_scale: Rotation scale factor.
+        dof_mask: [x, y, z, roll, pitch, yaw], 0 locks the axis.
+        max_lin_vel: Linear velocity limit in m/s, 0 for none.
+        max_ang_vel: Angular velocity limit in rad/s, 0 for none.
+        workspace_min: Lower position bound [x, y, z], or None.
+        workspace_max: Upper position bound [x, y, z], or None.
+    """
+
+    def __init__(self, align=None, translation_scale=1.0, rotation_scale=1.0,
+                 dof_mask=(1, 1, 1, 1, 1, 1), max_lin_vel=0.0, max_ang_vel=0.0,
+                 workspace_min=None, workspace_max=None):
+        if len(dof_mask) != 6:
+            raise ValueError('dof_mask must have 6 entries [x, y, z, roll, pitch, yaw]')
+        self.align = np.eye(3) if align is None else np.asarray(align, dtype=float)
+        self.translation_scale = float(translation_scale)
+        self.rotation_scale = float(rotation_scale)
+        self.dof_mask = np.asarray(dof_mask, dtype=float)
+        self.max_lin_vel = float(max_lin_vel)
+        self.max_ang_vel = float(max_ang_vel)
+        self.workspace_min = None if workspace_min is None else np.asarray(workspace_min, float)
+        self.workspace_max = None if workspace_max is None else np.asarray(workspace_max, float)
+
+        self._input_ref = None
+        self._target_ref = None
+        self._last_target = None
+
+    @property
+    def engaged(self):
+        return self._target_ref is not None
+
+    @property
+    def last_target(self):
+        return None if self._last_target is None else self._last_target.copy()
+
+    def engage(self, T_input, T_target_ref):
+        """Set the reference poses.
+
+        Args:
+            T_input: 4x4 input pose in the input base, or None for twist input.
+            T_target_ref: 4x4 starting target pose in the robot base.
+        """
+        self._input_ref = None if T_input is None else np.array(T_input, dtype=float)
+        self._target_ref = np.array(T_target_ref, dtype=float)
+        self._last_target = self._target_ref.copy()
+
+    def disengage(self):
+        self._input_ref = None
+        self._target_ref = None
+
+    def update(self, T_input, dt):
+        """Compute the target pose from an input pose.
+
+        Args:
+            T_input: 4x4 input pose in the input base.
+            dt: Time since the last update in seconds.
+
+        Returns:
+            4x4 target pose in the robot base.
+        """
+        if self._input_ref is None:
+            raise RuntimeError('update() called without an input reference')
+        T_in = np.asarray(T_input, dtype=float)
+        T0 = self._input_ref
+        R_a = self.align
+        dp = self.translation_scale * (R_a @ (T_in[:3, 3] - T0[:3, 3]))
+        # Rotation vector in robot base
+        w = self.rotation_scale * (R_a @ matrix_to_rotvec(T_in[:3, :3] @ T0[:3, :3].T))
+        dp *= self.dof_mask[:3]
+        w *= self.dof_mask[3:]
+        T_desired = make_transform(rotvec_to_matrix(w) @ self._target_ref[:3, :3],
+                                   self._target_ref[:3, 3] + dp)
+        return self._finish(T_desired, dt)
+
+    def integrate(self, linear, angular, dt, frame='base'):
+        """Compute the target pose from an input velocity.
+
+        Args:
+            linear: Linear velocity [x, y, z] in m/s.
+            angular: Angular velocity [x, y, z] in rad/s.
+            dt: Time since the last update in seconds.
+            frame: 'base' or 'tool', the frame the velocity is expressed in.
+
+        Returns:
+            4x4 target pose in the robot base.
+        """
+        if not self.engaged:
+            raise RuntimeError('integrate() called while not engaged')
+        if frame not in TWIST_FRAMES:
+            raise ValueError(f'frame must be one of {TWIST_FRAMES}, got {frame!r}')
+        T_prev = self._last_target
+        v = self.translation_scale * (self.align @ np.asarray(linear, dtype=float))
+        w = self.rotation_scale * (self.align @ np.asarray(angular, dtype=float))
+        if frame == 'tool':
+            v = T_prev[:3, :3] @ v
+            w = T_prev[:3, :3] @ w
+        v *= self.dof_mask[:3]
+        w *= self.dof_mask[3:]
+        T_desired = make_transform(rotvec_to_matrix(w * dt) @ T_prev[:3, :3],
+                                   T_prev[:3, 3] + v * dt)
+        return self._finish(T_desired, dt)
+
+    def _finish(self, T_desired, dt):
+        if self.workspace_min is not None:
+            T_desired[:3, 3] = np.maximum(T_desired[:3, 3], self.workspace_min)
+        if self.workspace_max is not None:
+            T_desired[:3, 3] = np.minimum(T_desired[:3, 3], self.workspace_max)
+        T_out = self._limit_rate(self._last_target, T_desired, dt)
+        self._last_target = T_out
+        return T_out.copy()
+
+    def _limit_rate(self, T_prev, T_next, dt):
+        if dt <= 0.0:
+            return T_next
+        p_prev, p_next = T_prev[:3, 3], T_next[:3, 3]
+        dp = p_next - p_prev
+        if self.max_lin_vel > 0.0:
+            max_step = self.max_lin_vel * dt
+            n = np.linalg.norm(dp)
+            if n > max_step:
+                dp *= max_step / n
+        R = T_next[:3, :3]
+        if self.max_ang_vel > 0.0:
+            w = matrix_to_rotvec(T_next[:3, :3] @ T_prev[:3, :3].T)
+            max_step = self.max_ang_vel * dt
+            n = np.linalg.norm(w)
+            if n > max_step:
+                R = rotvec_to_matrix(w * max_step / n) @ T_prev[:3, :3]
+        return make_transform(R, p_prev + dp)

teleopro_core-0.1.0/teleopro_core/teleop.py

@@ -0,0 +1,186 @@
+"""Teleoperation session with engage, input loss and re-anchoring."""
+
+import logging
+
+import numpy as np
+
+from teleopro_core.mapping import TeleopMapper, TWIST_FRAMES
+from teleopro_core.transforms import make_transform, matrix_to_quat, quat_to_matrix, rpy_to_matrix
+
+
+def as_matrix(pose):
+    """Convert a pose to a 4x4 matrix.
+
+    Args:
+        pose: 4x4 matrix or ((x, y, z), (qx, qy, qz, qw)).
+    """
+    if isinstance(pose, np.ndarray) and pose.shape == (4, 4):
+        return pose.astype(float)
+    xyz, quat = pose
+    return make_transform(quat_to_matrix(quat), xyz)
+
+
+def as_pos_quat(T):
+    """Convert a 4x4 matrix to ((x, y, z), (qx, qy, qz, qw))."""
+    return tuple(float(v) for v in T[:3, 3]), tuple(float(v) for v in matrix_to_quat(T[:3, :3]))
+
+
+class Teleop:
+    """Teleoperation session.
+
+    Args:
+        align_rpy: Rotation of the input axes in the robot base (roll, pitch, yaw).
+        translation_scale: Translation scale factor.
+        rotation_scale: Rotation scale factor.
+        max_lin_vel: Linear velocity limit in m/s, 0 for none.
+        max_ang_vel: Angular velocity limit in rad/s, 0 for none.
+        dof_mask: [x, y, z, roll, pitch, yaw], 0 locks the axis.
+        workspace_min: Lower position bound [x, y, z], or None.
+        workspace_max: Upper position bound [x, y, z], or None.
+        input_timeout: Input older than this many seconds is treated as lost.
+        twist_frame: 'base' or 'tool', the frame twist input is expressed in.
+        logger: Logger with debug(), info() and warning(). Defaults to logging.
+    """
+
+    def __init__(self, align_rpy=(0.0, 0.0, 0.0), translation_scale=1.0,
+                 rotation_scale=1.0, max_lin_vel=0.5, max_ang_vel=1.0,
+                 dof_mask=(1, 1, 1, 1, 1, 1), workspace_min=None, workspace_max=None,
+                 input_timeout=0.2, twist_frame='base', logger=None):
+        if twist_frame not in TWIST_FRAMES:
+            raise ValueError(f'twist_frame must be one of {TWIST_FRAMES}, got {twist_frame!r}')
+        self.mapper = TeleopMapper(
+            align=rpy_to_matrix(*align_rpy),
+            translation_scale=translation_scale,
+            rotation_scale=rotation_scale,
+            dof_mask=dof_mask,
+            max_lin_vel=max_lin_vel,
+            max_ang_vel=max_ang_vel,
+            workspace_min=workspace_min,
+            workspace_max=workspace_max,
+        )
+        self.input_timeout = float(input_timeout)
+        self.twist_frame = twist_frame
+        self.log = logger if logger is not None else logging.getLogger('teleopro_core')
+
+        self._want_engaged = False
+        self._input_stale = False
+        self._needs_reanchor = False
+        self._waiting_logged = False
+        self._last_lost_log = None
+        self._hold_pose = None
+        self._last_now = None
+
+    @property
+    def engaged(self):
+        return self.mapper.engaged
+
+    @property
+    def needs_robot_pose(self):
+        """Whether the next update() uses robot_pose."""
+        if self.mapper.engaged:
+            return False
+        return self._want_engaged or self._hold_pose is None
+
+    def engage(self):
+        """Start following the input."""
+        self._want_engaged = True
+
+    def disengage(self):
+        """Stop following the input and hold the target."""
+        self._want_engaged = False
+        if self.mapper.engaged:
+            self._hold_pose = self.mapper.last_target
+            self.mapper.disengage()
+            self.log.info('Teleop disengaged')
+
+    def update(self, now, input_pose=None, robot_pose=None, input_stamp=None):
+        """Advance one step with pose input.
+
+        Args:
+            now: Current time in seconds.
+            input_pose: Input pose in the input base, or None if not tracked.
+            robot_pose: Current robot pose in the robot base.
+            input_stamp: Time of input_pose in seconds, or None.
+
+        Returns:
+            4x4 target pose in the robot base, or None if no robot pose yet.
+        """
+        dt = self._step_time(now)
+        T_in = None if input_pose is None else as_matrix(input_pose)
+        T_robot = None if robot_pose is None else as_matrix(robot_pose)
+
+        stale = T_in is None or self._is_stale(now, input_stamp)
+        if stale != self._input_stale:
+            self._input_stale = stale
+            if stale and self.mapper.engaged:
+                self._needs_reanchor = True
+                # Throttled, tracking can flicker
+                if self._last_lost_log is None or now - self._last_lost_log >= 5.0:
+                    self.log.warning('Input lost, holding target')
+                    self._last_lost_log = now
+
+        if self._want_engaged and not stale:
+            if not self.mapper.engaged:
+                self._try_engage(T_in, T_robot)
+            elif self._needs_reanchor:
+                # Reset references after input loss
+                self.mapper.engage(T_in, self.mapper.last_target)
+                self._needs_reanchor = False
+                self.log.debug('Input back, re-anchored')
+            else:
+                return self.mapper.update(T_in, dt)
+
+        if self.mapper.engaged:
+            return self.mapper.last_target
+        return self._hold(T_robot)
+
+    def update_twist(self, now, twist=None, robot_pose=None, twist_stamp=None):
+        """Advance one step with twist input.
+
+        Args:
+            now: Current time in seconds.
+            twist: ((vx, vy, vz), (wx, wy, wz)) in m/s and rad/s, or None.
+            robot_pose: Current robot pose in the robot base.
+            twist_stamp: Time of twist in seconds, or None.
+
+        Returns:
+            4x4 target pose in the robot base, or None if no robot pose yet.
+        """
+        dt = self._step_time(now)
+        T_robot = None if robot_pose is None else as_matrix(robot_pose)
+
+        if self._want_engaged and not self.mapper.engaged:
+            self._try_engage(None, T_robot)
+
+        if self.mapper.engaged:
+            # Stop when the twist is missing or stale
+            if twist is None or self._is_stale(now, twist_stamp):
+                twist = ((0.0, 0.0, 0.0), (0.0, 0.0, 0.0))
+            linear, angular = twist
+            return self.mapper.integrate(linear, angular, dt, self.twist_frame)
+        return self._hold(T_robot)
+
+    def _step_time(self, now):
+        dt = 0.0 if self._last_now is None else now - self._last_now
+        self._last_now = now
+        return dt
+
+    def _is_stale(self, now, stamp):
+        return (stamp is not None and self.input_timeout > 0.0
+                and now - stamp > self.input_timeout)
+
+    def _try_engage(self, T_in, T_robot):
+        if T_robot is None:
+            if not self._waiting_logged:
+                self.log.warning('Waiting for robot pose to engage')
+                self._waiting_logged = True
+            return
+        self.mapper.engage(T_in, T_robot)
+        self._waiting_logged = False
+        self.log.info('Teleop engaged')
+
+    def _hold(self, T_robot):
+        # Hold the first robot pose until engaged
+        if self._hold_pose is None and T_robot is not None:
+            self._hold_pose = T_robot.copy()
+        return None if self._hold_pose is None else self._hold_pose.copy()

teleopro_core-0.1.0/teleopro_core/transforms.py

@@ -0,0 +1,87 @@
+"""SE(3) helpers. Quaternions are (x, y, z, w)."""
+
+import numpy as np
+
+
+def quat_to_matrix(q):
+    x, y, z, w = q
+    n = x * x + y * y + z * z + w * w
+    if n < 1e-12:
+        return np.eye(3)
+    s = 2.0 / n
+    return np.array([
+        [1 - s * (y * y + z * z), s * (x * y - z * w), s * (x * z + y * w)],
+        [s * (x * y + z * w), 1 - s * (x * x + z * z), s * (y * z - x * w)],
+        [s * (x * z - y * w), s * (y * z + x * w), 1 - s * (x * x + y * y)],
+    ])
+
+
+def matrix_to_quat(R):
+    tr = np.trace(R)
+    if tr > 0:
+        s = 2.0 * np.sqrt(tr + 1.0)
+        w = 0.25 * s
+        x = (R[2, 1] - R[1, 2]) / s
+        y = (R[0, 2] - R[2, 0]) / s
+        z = (R[1, 0] - R[0, 1]) / s
+    elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
+        s = 2.0 * np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2])
+        w = (R[2, 1] - R[1, 2]) / s
+        x = 0.25 * s
+        y = (R[0, 1] + R[1, 0]) / s
+        z = (R[0, 2] + R[2, 0]) / s
+    elif R[1, 1] > R[2, 2]:
+        s = 2.0 * np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2])
+        w = (R[0, 2] - R[2, 0]) / s
+        x = (R[0, 1] + R[1, 0]) / s
+        y = 0.25 * s
+        z = (R[1, 2] + R[2, 1]) / s
+    else:
+        s = 2.0 * np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1])
+        w = (R[1, 0] - R[0, 1]) / s
+        x = (R[0, 2] + R[2, 0]) / s
+        y = (R[1, 2] + R[2, 1]) / s
+        z = 0.25 * s
+    q = np.array([x, y, z, w])
+    return q / np.linalg.norm(q)
+
+
+def rpy_to_matrix(roll, pitch, yaw):
+    """Fixed-axis roll, pitch, yaw to rotation matrix (URDF convention)."""
+    cr, sr = np.cos(roll), np.sin(roll)
+    cp, sp = np.cos(pitch), np.sin(pitch)
+    cy, sy = np.cos(yaw), np.sin(yaw)
+    return np.array([
+        [cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
+        [sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr],
+        [-sp, cp * sr, cp * cr],
+    ])
+
+
+def rotvec_to_matrix(v):
+    theta = np.linalg.norm(v)
+    if theta < 1e-12:
+        return np.eye(3)
+    k = v / theta
+    K = np.array([[0, -k[2], k[1]], [k[2], 0, -k[0]], [-k[1], k[0], 0]])
+    return np.eye(3) + np.sin(theta) * K + (1 - np.cos(theta)) * (K @ K)
+
+
+def matrix_to_rotvec(R):
+    q = matrix_to_quat(R)
+    if q[3] < 0:
+        q = -q
+    s = np.linalg.norm(q[:3])
+    if s < 1e-12:
+        return np.zeros(3)
+    theta = 2.0 * np.arctan2(s, q[3])
+    return q[:3] / s * theta
+
+
+def make_transform(R=None, p=None):
+    T = np.eye(4)
+    if R is not None:
+        T[:3, :3] = R
+    if p is not None:
+        T[:3, 3] = p
+    return T

teleopro_core-0.1.0/teleopro_core.egg-info/PKG-INFO

@@ -0,0 +1,101 @@
+Metadata-Version: 2.4
+Name: teleopro_core
+Version: 0.1.0
+Summary: Device- and robot-agnostic teleoperation mapping, no ROS required.
+Home-page: https://github.com/ShifeiDuan/teleopro
+Maintainer: Shifei Duan
+Maintainer-email: sdua078@aucklanduni.ac.nz
+License: Apache-2.0
+Project-URL: Source, https://github.com/ShifeiDuan/teleopro
+Project-URL: Issues, https://github.com/ShifeiDuan/teleopro/issues
+Keywords: teleoperation,robotics,ros2,xr,spacemouse
+Classifier: Development Status :: 3 - Alpha
+Classifier: Intended Audience :: Developers
+Classifier: Intended Audience :: Science/Research
+Classifier: Operating System :: OS Independent
+Classifier: Programming Language :: Python :: 3
+Classifier: Topic :: Scientific/Engineering
+Classifier: Topic :: Software Development :: Libraries :: Python Modules
+Requires-Python: >=3.8
+Description-Content-Type: text/markdown
+License-File: LICENSE
+Requires-Dist: numpy
+Provides-Extra: test
+Requires-Dist: pytest; extra == "test"
+Dynamic: classifier
+Dynamic: description
+Dynamic: description-content-type
+Dynamic: home-page
+Dynamic: keywords
+Dynamic: license
+Dynamic: license-file
+Dynamic: maintainer
+Dynamic: maintainer-email
+Dynamic: project-url
+Dynamic: provides-extra
+Dynamic: requires-dist
+Dynamic: requires-python
+Dynamic: summary
+
+# teleopro_core
+
+Teleoperate any robot with any input device, in pure Python (numpy only, no
+ROS). It turns the motion of a device (XR headset, tracker, mocap, ...) into a
+target pose for the robot, with clutch, speed limits and input-loss handling.
+
+ROS users: use the `teleopro_ros` package, which wraps this library with TF.
+
+## Install
+
+```bash
+pip install "git+https://github.com/ShifeiDuan/teleopro#subdirectory=teleopro_core"
+```
+
+## Usage
+
+```python
+import time
+from teleopro_core import Teleop, as_pos_quat
+
+teleop = Teleop(translation_scale=0.5, max_lin_vel=0.1)
+
+teleop.engage()                       # e.g. when a button is pressed
+while True:
+    device_pose = my_device.get_pose()       # ((x, y, z), (qx, qy, qz, qw)) or 4x4, None if not tracked
+    robot_pose = my_robot.get_tcp_pose()     # same formats, in the robot base frame
+    target = teleop.update(time.monotonic(), device_pose, robot_pose)
+    if target is not None:
+        my_robot.move_to(*as_pos_quat(target))
+    time.sleep(0.01)
+```
+
+- On `engage()` the target starts at the current robot pose, then follows the
+  device's motion since that moment. `disengage()` holds it still (clutch).
+- If the device is not tracked (`None`) or its `input_stamp` is older than
+  `input_timeout`, the target holds; when tracking returns it continues from
+  where it stopped, without jumping.
+- Before the first engage, `update()` returns the robot pose latched once.
+
+Velocity devices (keyboard, SpaceMouse) use `update_twist()` instead:
+
+```python
+teleop = Teleop(twist_frame='tool')      # or 'base'
+target = teleop.update_twist(time.monotonic(), ((vx, vy, vz), (wx, wy, wz)), robot_pose)
+```
+
+## Parameters
+
+| Parameter | Default | Meaning |
+|---|---|---|
+| `align_rpy` | `(0, 0, 0)` | Rotation from device axes to robot axes |
+| `translation_scale`, `rotation_scale` | `1.0` | Motion scaling |
+| `max_lin_vel`, `max_ang_vel` | `0.5`, `1.0` | Target speed limits (m/s, rad/s), 0 = off |
+| `dof_mask` | `(1, 1, 1, 1, 1, 1)` | `[x, y, z, roll, pitch, yaw]`; 0 freezes a DoF |
+| `workspace_min`, `workspace_max` | `None` | `[x, y, z]` box the target stays in |
+| `input_timeout` | `0.2` | Seconds before a stale `input_stamp` counts as lost |
+| `twist_frame` | `'base'` | `'base'` or `'tool'`, frame of the twist in `update_twist()` |
+| `logger` | `logging` | Anything with `.debug()` / `.info()` / `.warning()` |
+
+## License
+
+Apache-2.0

teleopro_core-0.1.0/teleopro_core.egg-info/SOURCES.txt

@@ -0,0 +1,18 @@
+LICENSE
+MANIFEST.in
+README.md
+package.xml
+setup.py
+resource/teleopro_core
+teleopro_core/__init__.py
+teleopro_core/mapping.py
+teleopro_core/teleop.py
+teleopro_core/transforms.py
+teleopro_core.egg-info/PKG-INFO
+teleopro_core.egg-info/SOURCES.txt
+teleopro_core.egg-info/dependency_links.txt
+teleopro_core.egg-info/requires.txt
+teleopro_core.egg-info/top_level.txt
+teleopro_core.egg-info/zip-safe
+test/test_mapping.py
+test/test_teleop.py

teleopro_core-0.1.0/teleopro_core.egg-info/dependency_links.txt

@@ -0,0 +1 @@
+

teleopro_core-0.1.0/teleopro_core.egg-info/requires.txt

@@ -0,0 +1,4 @@
+numpy
+
+[test]
+pytest

teleopro_core-0.1.0/teleopro_core.egg-info/top_level.txt

@@ -0,0 +1 @@
+teleopro_core

teleopro_core-0.1.0/teleopro_core.egg-info/zip-safe

@@ -0,0 +1 @@
+

teleopro_core-0.1.0/test/test_mapping.py

@@ -0,0 +1,109 @@
+import numpy as np
+import pytest
+
+from teleopro_core.mapping import TeleopMapper
+from teleopro_core.transforms import (
+    make_transform, matrix_to_quat, matrix_to_rotvec, quat_to_matrix,
+    rotvec_to_matrix, rpy_to_matrix)
+
+
+def pose(xyz=(0, 0, 0), rpy=(0, 0, 0)):
+    return make_transform(rpy_to_matrix(*rpy), xyz)
+
+
+ROBOT_REF = pose((0.4, 0.1, 0.3), (np.pi, 0, 0.3))
+INPUT_REF = pose((1.0, 2.0, 1.5), (0.2, -0.1, 1.0))
+
+
+def test_quat_roundtrip():
+    for rpy in [(0, 0, 0), (np.pi, 0, 0), (0.3, -1.2, 2.9), (0, np.pi, 0)]:
+        R = rpy_to_matrix(*rpy)
+        assert np.allclose(quat_to_matrix(matrix_to_quat(R)), R)
+
+
+def test_rotvec_roundtrip():
+    for v in [(0, 0, 0), (0.1, 0.2, -0.3), (0, 0, 3.0)]:
+        assert np.allclose(matrix_to_rotvec(rotvec_to_matrix(np.array(v))), v)
+
+
+def test_engage_does_not_jump():
+    m = TeleopMapper()
+    m.engage(INPUT_REF, ROBOT_REF)
+    assert np.allclose(m.update(INPUT_REF, 0.01), ROBOT_REF)
+
+
+def test_relative_translation_identity_alignment():
+    m = TeleopMapper()
+    m.engage(INPUT_REF, ROBOT_REF)
+    moved = INPUT_REF.copy()
+    moved[:3, 3] += [0.05, -0.02, 0.01]
+    out = m.update(moved, 0.01)
+    assert np.allclose(out[:3, 3], ROBOT_REF[:3, 3] + [0.05, -0.02, 0.01])
+    assert np.allclose(out[:3, :3], ROBOT_REF[:3, :3])
+
+
+def test_relative_alignment_rotates_motion():
+    # Input +x maps to robot +y
+    m = TeleopMapper(align=rpy_to_matrix(0, 0, np.pi / 2))
+    m.engage(INPUT_REF, ROBOT_REF)
+    moved = INPUT_REF.copy()
+    moved[:3, 3] += [0.1, 0, 0]
+    out = m.update(moved, 0.0)
+    assert np.allclose(out[:3, 3] - ROBOT_REF[:3, 3], [0, 0.1, 0])
+
+
+def test_relative_rotation_about_input_axis():
+    m = TeleopMapper(align=rpy_to_matrix(0, 0, np.pi / 2))
+    m.engage(INPUT_REF, ROBOT_REF)
+    # Rotation about input x maps to robot y
+    moved = make_transform(rpy_to_matrix(0.3, 0, 0) @ INPUT_REF[:3, :3], INPUT_REF[:3, 3])
+    out = m.update(moved, 0.0)
+    delta = matrix_to_rotvec(out[:3, :3] @ ROBOT_REF[:3, :3].T)
+    assert np.allclose(delta, [0, 0.3, 0])
+
+
+def test_scales():
+    m = TeleopMapper(translation_scale=0.5, rotation_scale=2.0)
+    m.engage(INPUT_REF, ROBOT_REF)
+    moved = make_transform(rpy_to_matrix(0, 0, 0.1) @ INPUT_REF[:3, :3],
+                           INPUT_REF[:3, 3] + [0.2, 0, 0])
+    out = m.update(moved, 0.0)
+    assert np.allclose(out[:3, 3] - ROBOT_REF[:3, 3], [0.1, 0, 0])
+    assert np.allclose(matrix_to_rotvec(out[:3, :3] @ ROBOT_REF[:3, :3].T), [0, 0, 0.2])
+
+
+def test_dof_mask_planar():
+    m = TeleopMapper(dof_mask=[1, 1, 0, 0, 0, 1])
+    m.engage(INPUT_REF, ROBOT_REF)
+    moved = make_transform(rpy_to_matrix(0.2, 0, 0.1) @ INPUT_REF[:3, :3],
+                           INPUT_REF[:3, 3] + [0.1, 0.1, 0.1])
+    out = m.update(moved, 0.0)
+    assert np.allclose(out[:3, 3] - ROBOT_REF[:3, 3], [0.1, 0.1, 0])
+    w = matrix_to_rotvec(out[:3, :3] @ ROBOT_REF[:3, :3].T)
+    assert abs(w[0]) < 1e-9 and abs(w[1]) < 1e-9 and abs(w[2]) > 0.05
+
+
+def test_velocity_limit():
+    m = TeleopMapper(max_lin_vel=1.0, max_ang_vel=1.0)
+    m.engage(INPUT_REF, ROBOT_REF)
+    moved = make_transform(rpy_to_matrix(0, 0, 1.0) @ INPUT_REF[:3, :3],
+                           INPUT_REF[:3, 3] + [1.0, 0, 0])
+    out = m.update(moved, 0.01)
+    assert np.isclose(np.linalg.norm(out[:3, 3] - ROBOT_REF[:3, 3]), 0.01)
+    assert np.isclose(np.linalg.norm(matrix_to_rotvec(out[:3, :3] @ ROBOT_REF[:3, :3].T)), 0.01)
+
+
+def test_workspace_clamp():
+    m = TeleopMapper(workspace_min=[0, -0.2, 0.2], workspace_max=[0.5, 0.2, 0.6])
+    m.engage(INPUT_REF, ROBOT_REF)
+    moved = INPUT_REF.copy()
+    moved[:3, 3] += [1.0, -1.0, 1.0]
+    out = m.update(moved, 0.0)
+    assert np.allclose(out[:3, 3], [0.5, -0.2, 0.6])
+
+
+def test_invalid_args():
+    with pytest.raises(ValueError):
+        TeleopMapper(dof_mask=[1, 1, 1])
+    with pytest.raises(RuntimeError):
+        TeleopMapper().update(INPUT_REF, 0.01)

teleopro_core-0.1.0/test/test_teleop.py

@@ -0,0 +1,172 @@
+import numpy as np
+
+from teleopro_core import as_matrix, as_pos_quat, Teleop
+from teleopro_core.transforms import make_transform, matrix_to_rotvec, rpy_to_matrix
+
+ROBOT = make_transform(rpy_to_matrix(np.pi, 0, 0), [0.4, 0.0, 0.4])
+DEVICE = make_transform(np.eye(3), [1.0, 2.0, 1.0])
+
+
+def moved(dx):
+    T = DEVICE.copy()
+    T[0, 3] += dx
+    return T
+
+
+def make(**kw):
+    kw.setdefault('max_lin_vel', 0.0)
+    kw.setdefault('max_ang_vel', 0.0)
+    return Teleop(**kw)
+
+
+def test_pose_formats_roundtrip():
+    xyz, quat = as_pos_quat(ROBOT)
+    assert np.allclose(as_matrix((xyz, quat)), ROBOT)
+
+
+def test_holds_robot_pose_until_engaged():
+    t = make()
+    assert t.update(0.0, DEVICE, None) is None
+    assert np.allclose(t.update(0.01, DEVICE, ROBOT), ROBOT)
+    # Target stays latched
+    other = ROBOT.copy()
+    other[2, 3] -= 0.05
+    assert np.allclose(t.update(0.02, moved(0.1), other), ROBOT)
+    assert not t.engaged
+
+
+def test_engage_follows_device():
+    t = make()
+    t.engage()
+    assert np.allclose(t.update(0.0, DEVICE, ROBOT), ROBOT)
+    assert t.engaged
+    out = t.update(0.01, moved(0.05))
+    assert np.allclose(out[:3, 3], ROBOT[:3, 3] + [0.05, 0, 0])
+
+
+def test_engage_waits_for_robot_pose():
+    t = make()
+    t.engage()
+    assert t.needs_robot_pose
+    assert t.update(0.0, DEVICE, None) is None
+    assert not t.engaged
+    t.update(0.01, DEVICE, ROBOT)
+    assert t.engaged and not t.needs_robot_pose
+
+
+def test_disengage_holds_last_target():
+    t = make()
+    t.engage()
+    t.update(0.0, DEVICE, ROBOT)
+    last = t.update(0.01, moved(0.05))
+    t.disengage()
+    assert np.allclose(t.update(0.02, moved(0.3), ROBOT), last)
+
+
+def test_reengage_starts_from_robot_pose():
+    t = make()
+    t.engage()
+    t.update(0.0, DEVICE, ROBOT)
+    t.update(0.01, moved(0.05))
+    t.disengage()
+    t.engage()
+    assert np.allclose(t.update(0.02, moved(0.2), ROBOT), ROBOT)
+
+
+def test_input_lost_holds_then_reanchors():
+    t = make()
+    t.engage()
+    t.update(0.0, DEVICE, ROBOT)
+    last = t.update(0.01, moved(0.05))
+    assert np.allclose(t.update(0.02, None), last)              # lost
+    assert np.allclose(t.update(0.03, moved(0.5)), last)        # re-anchored
+    out = t.update(0.04, moved(0.52))                            # following
+    assert np.allclose(out[:3, 3], last[:3, 3] + [0.02, 0, 0])
+
+
+def test_stale_stamp_counts_as_lost():
+    t = make(input_timeout=0.2)
+    t.engage()
+    t.update(0.0, DEVICE, ROBOT, input_stamp=0.0)
+    last = t.update(0.1, moved(0.05), input_stamp=0.1)
+    # Stale stamp
+    assert np.allclose(t.update(0.5, moved(0.3), input_stamp=0.1), last)
+
+
+def test_accepts_xyz_quat_input():
+    t = make()
+    t.engage()
+    t.update(0.0, as_pos_quat(DEVICE), as_pos_quat(ROBOT))
+    out = t.update(0.01, ((1.05, 2.0, 1.0), (0, 0, 0, 1)))
+    assert np.allclose(out[:3, 3], ROBOT[:3, 3] + [0.05, 0, 0])
+
+
+# Twist input
+
+ZERO = (0.0, 0.0, 0.0)
+
+
+def test_twist_holds_until_engaged():
+    t = make()
+    assert np.allclose(t.update_twist(0.0, ((0.1, 0, 0), ZERO), ROBOT), ROBOT)
+    assert np.allclose(t.update_twist(1.0, ((0.1, 0, 0), ZERO), ROBOT), ROBOT)
+
+
+def test_twist_base_frame():
+    t = make()
+    t.engage()
+    t.update_twist(0.0, (ZERO, ZERO), ROBOT)
+    out = t.update_twist(0.5, ((0.1, 0, 0.2), ZERO))
+    assert np.allclose(out[:3, 3], ROBOT[:3, 3] + [0.05, 0, 0.1])
+    assert np.allclose(out[:3, :3], ROBOT[:3, :3])
+
+
+def test_twist_tool_frame():
+    # ROBOT tool is flipped about x: tool +z points along base -z
+    t = make(twist_frame='tool')
+    t.engage()
+    t.update_twist(0.0, (ZERO, ZERO), ROBOT)
+    out = t.update_twist(0.5, ((0, 0, 0.1), ZERO))
+    assert np.allclose(out[:3, 3], ROBOT[:3, 3] + [0, 0, -0.05])
+
+
+def test_twist_rotation():
+    t = make()
+    t.engage()
+    t.update_twist(0.0, (ZERO, ZERO), ROBOT)
+    out = t.update_twist(1.0, (ZERO, (0, 0, 0.2)))
+    w = matrix_to_rotvec(out[:3, :3] @ ROBOT[:3, :3].T)
+    assert np.allclose(w, [0, 0, 0.2])
+
+
+def test_twist_stale_stops():
+    t = make(input_timeout=0.2)
+    t.engage()
+    t.update_twist(0.0, (ZERO, ZERO), ROBOT, twist_stamp=0.0)
+    last = t.update_twist(0.1, ((0.1, 0, 0), ZERO), twist_stamp=0.1)
+    assert np.allclose(t.update_twist(1.0, ((0.1, 0, 0), ZERO), twist_stamp=0.1), last)
+    assert np.allclose(t.update_twist(1.1, None), last)
+
+
+def test_twist_dof_mask_and_scale():
+    t = make(dof_mask=[1, 0, 1, 0, 0, 1], translation_scale=2.0)
+    t.engage()
+    t.update_twist(0.0, (ZERO, ZERO), ROBOT)
+    out = t.update_twist(0.5, ((0.1, 0.1, 0), (0.3, 0, 0)))
+    assert np.allclose(out[:3, 3], ROBOT[:3, 3] + [0.1, 0, 0])
+    assert np.allclose(out[:3, :3], ROBOT[:3, :3])
+
+
+def test_twist_disengage_holds():
+    t = make()
+    t.engage()
+    t.update_twist(0.0, (ZERO, ZERO), ROBOT)
+    last = t.update_twist(0.5, ((0.1, 0, 0), ZERO))
+    t.disengage()
+    assert np.allclose(t.update_twist(1.0, ((0.1, 0, 0), ZERO), ROBOT), last)
+
+
+def test_invalid_twist_frame():
+    import pytest
+    with pytest.raises(ValueError):
+        Teleop(twist_frame='world')