From 7deef914fb601973ebbcd05ef3f4bbc2389030da Mon Sep 17 00:00:00 2001 From: spirosperos Date: Thu, 28 May 2026 17:25:36 +0200 Subject: [PATCH 1/6] api router --- docs/architecture.md | 268 +++++++++++++++++++++++++++++++++++++++++++ teleop/__init__.py | 5 +- 2 files changed, 272 insertions(+), 1 deletion(-) create mode 100644 docs/architecture.md diff --git a/docs/architecture.md b/docs/architecture.md new file mode 100644 index 0000000..332aa7f --- /dev/null +++ b/docs/architecture.md @@ -0,0 +1,268 @@ +# Architecture + +This document summarizes the current architecture plan based only on the decisions made so far. + +## System Diagram + +```mermaid +flowchart TB + subgraph CAM["Cameras"] + ZED["ZED Mini
Stereo main camera"] + GCL["Left gripper camera"] + GCR["Right gripper camera"] + end + + subgraph BE["Backend"] + APP["Teleop Pro backend
Python / FastAPI / Uvicorn
Discovery, role assignment, session state
Works with 1 or 2 arms"] + + subgraph MEDIA["Media subsystem"] + CAP["Camera capture / preparation
Capture path open"] + MTX["MediaMTX
Multi-consumer stream distribution"] + end + + subgraph SAFE["Safety backend (modular)"] + SAFEV1["v1: TCP safety filter
Configurable no-go boxes
Input: desired TCP position"] + SAFEFUT["Future option
MoveIt"] + end + + subgraph CTRL["Robot control backend (modular)"] + CTRLV1["v1: Pinocchio / JacobiRobot
IK + regularization"] + CTRLFUT["Future options
MoveIt, qmotion"] + end + + REC["Recording module
Synchronized dataset
LeRobot-compatible episodes later"] + end + + subgraph UI["UI"] + VR["VR headset browser
WebXR operator UI
True bimanual control"] + end + + subgraph ROB["Robots"] + RL["Left robot arm + gripper"] + RR["Right robot arm + gripper"] + end + + TV["Showroom TV / browser
Spectator UI"] + + ZED --> CAP + GCL --> CAP + GCR --> CAP + + CAP --> MTX + MTX -->|WebRTC| VR + MTX -->|WebRTC| TV + + APP -->|HTTPS| VR + VR -->|WebSocket| APP + + APP -->|evaluate target| SAFEV1 + APP --> REC + APP -->|send allowed target| CTRLV1 + CTRLV1 --> RL + CTRLV1 --> RR +``` + +## System Overview + +- The backend is the system shell and owns startup, discovery, role assignment, session state, recording coordination, safety, and robot control coordination. +- The system must work with either one arm or two arms. +- The operator uses a browser in a VR headset. +- The VR headset is used for first-person viewing and bimanual controller input. +- Head motion is not used for robot control. +- The showroom TV is another stream consumer. +- The backend owns the media subsystem. Cameras connect into the backend, and the backend makes those streams available through MediaMTX. + +## Module Boundaries + +- `Backend shell` + - Libraries: `Python`, `FastAPI`, `Uvicorn` + - Responsibilities: startup, discovery, role assignment, session state, UI serving, protocol coordination +- `Media subsystem` + - Technology: `MediaMTX` + - Browser streaming protocol: `WebRTC` + - Covered so far: main stereo ZED stream, optional gripper camera streams, multiple consumers + - Open: exact camera capture path and exact ingest path into MediaMTX +- `UI` + - Technology: `WebXR` + - Optional frontend stack discussed: `React`, `Zustand`, `Immer`, `UIKit` + - Covered so far: operator UI in headset, simple status/control UI, recording start/stop, optional gripper camera views +- `Safety backend` + - v1 backend: custom TCP-position safety filter with multiple configurable no-go boxes + - Future optional backend: `MoveIt` + - Covered so far: a desired TCP position is checked against configured boxes and the safety backend returns a decision to the backend shell +- `Robot control backend` + - v1 backend: `Pinocchio` with the current [JacobiRobot](../teleop/utils/jacobi_robot.py) module + - Future optional backends: `MoveIt`, `qmotion` + - Covered so far: keep the current approach, improve it iteratively, do not fundamentally replace it in v1 +- `Recording` + - Covered so far: synchronized recording owned by this repo, start/stop from UI, target is LeRobot-compatible episodes later + +## Protocols And Libraries + +- `HTTPS` + - Backend serves the browser UI +- `WebSocket` + - Browser-to-backend control and status path + - Covered so far: controller poses, button state, recording commands, general app communication +- `WebRTC` + - Media delivery from backend media subsystem to VR headset browser and TV/browser +- `MediaMTX` + - Multi-consumer media distribution +- `WebXR` + - Browser-side VR presentation and controller input +- `Pinocchio` + - Current robot control / IK backend +- `FastAPI` and `Uvicorn` + - Current backend foundation +- `In-process Python calls` + - Backend shell, safety backend, robot control backend, and recorder are currently planned as modules inside the same Python process + - Communication between these backend modules is planned as normal Python class method calls, not as network calls + +## Backend Internal API + +For the `Teleop Pro backend -> safety backend -> robot control backend` path, the current v1 direction is: + +- The safety backend is a Python class selected by configuration. +- The robot control backend is a Python class selected by configuration. +- The backend shell owns both objects and orchestrates the call order. +- The safety backend should not directly talk to the robot controller. +- Instead, the safety backend should return a decision to the backend shell, and the backend shell should decide whether to call the robot control backend. + +This keeps the safety backend modular and makes it easier to replace the v1 safety module with a future `MoveIt` backend. + +### Proposed v1 module split + +- `TeleopBackend` + - owns session state + - receives operator commands from the browser + - constructs target commands for each arm + - calls the safety backend + - if allowed, calls the robot control backend + - sends status/errors back to the UI +- `SafetyBackend` + - receives a desired target for one arm + - checks the target TCP position against configured no-go boxes + - returns allow/block information and optional adjusted target data +- `RobotControlBackend` + - receives an allowed target + - converts the target into robot motion using the selected control backend + - sends commands to the robot driver/API + +### Proposed v1 data flow + +```text +browser command +-> TeleopBackend +-> SafetyBackend.evaluate_target(...) +-> SafetyResult +-> if allowed: RobotControlBackend.send_target(...) +-> robot +``` + +### Proposed v1 Python-style interface + +```python +from dataclasses import dataclass +from typing import Literal + + +ArmId = Literal["left", "right"] + + +@dataclass +class PoseTarget: + arm: ArmId + tcp_pose: object + gripper_command: str | None = None + timestamp_ms: int | None = None + + +@dataclass +class SafetyResult: + allowed: bool + target: PoseTarget | None + reason: str | None = None + violated_zone_ids: list[str] | None = None + + +class SafetyBackend: + def evaluate_target(self, target: PoseTarget) -> SafetyResult: + raise NotImplementedError + + +class RobotControlBackend: + def send_target(self, target: PoseTarget) -> None: + raise NotImplementedError +``` + +The exact pose type is still open. The current decision is only that the safety API operates on a desired TCP target before the control backend sends robot commands. + +### Proposed v1 safety backend configuration + +The v1 safety backend can be constructed from: + +- backend type, for example `basic` now and `moveit` later +- a list of configured no-go boxes +- per-arm enable/disable settings if needed later + +Example shape: + +```python +basic_safety = SafetyBackendBasic( + boxes=[ + { + "id": "basket_keepout", + "frame": "world", + "min": [0.40, -0.10, 0.15], + "max": [0.55, 0.10, 0.35], + } + ] +) +``` + +The exact box schema is still open. The important current decision is that v1 uses multiple configurable Cartesian no-go boxes checked against TCP position. + +## Control Sequence + +The current intended control sequence is: + +1. The browser sends operator input to the backend over `WebSocket`. +2. The backend converts that input into a desired per-arm TCP target. +3. The backend calls the selected safety backend with that target. +4. The safety backend returns a `SafetyResult`. +5. If the result is allowed, the backend calls the selected robot control backend. +6. The robot control backend computes and sends the robot command. +7. The backend can send status or rejection reasons back to the UI. + +This is different from letting the safety module directly forward commands to the controller. The backend shell should remain the orchestrator. + +## Current v1 Decisions + +- Use `MediaMTX` for browser-facing media distribution. +- Keep media as part of the backend architecture. +- Keep `Pinocchio` / `JacobiRobot` as the v1 robot control backend. +- Make the robot control backend modular so that `MoveIt` or `qmotion` can be used later. +- Build a separate v1 safety module that filters desired TCP positions against multiple configurable no-go boxes. +- Make the safety module modular so that `MoveIt` can be used later. +- The safety backend and robot control backend are planned as in-process Python modules selected by configuration. +- The backend shell owns orchestration. The safety backend returns decisions; it does not directly own controller dispatch. +- Record synchronized session data in this repo. LeRobot-compatible episode details are deferred. + +## JacobiRobot Notes + +- The current IK implementation lives in [teleop/utils/jacobi_robot.py](../teleop/utils/jacobi_robot.py). +- The IK regularization is used to avoid singularities. +- This module should be improved through several iterations inside this project. +- The current direction is to avoid a fundamental rewrite, because the module is already the result of many failed iterations and has proven to work in practice. + +## Open Questions + +- Exact camera capture path for the ZED Mini and gripper cameras +- Exact ingest path from backend capture into MediaMTX +- Exact low-level robot driver / API path +- Exact control/data APIs +- Exact update rates and frequencies +- Exact format for the safety box configuration +- Exact behavior when a target is invalid beyond allow/block status +- Exact LeRobot-compatible episode schema details +- Whether the spectator UI is separate from the operator UI or shares the same frontend diff --git a/teleop/__init__.py b/teleop/__init__.py index eb36af4..efd88e6 100644 --- a/teleop/__init__.py +++ b/teleop/__init__.py @@ -4,7 +4,7 @@ import logging from typing import Callable, List import uvicorn -from fastapi import FastAPI, WebSocket, WebSocketDisconnect +from fastapi import APIRouter, FastAPI, WebSocket, WebSocketDisconnect from fastapi.responses import FileResponse from fastapi.staticfiles import StaticFiles import transforms3d as t3d @@ -192,6 +192,9 @@ def __init__( logging.getLogger("uvicorn.access").setLevel(logging.WARNING) self.__setup_routes() + def include_router(self, router: APIRouter, **kwargs) -> None: + self.__app.include_router(router, **kwargs) + def set_pose(self, pose: np.ndarray) -> None: """ Set the current pose of the end-effector. From 6edefce16cd23dd9b97bc40ddde60b38666d43f1 Mon Sep 17 00:00:00 2001 From: spirosperos <30575489+spirosperos@users.noreply.github.com> Date: Thu, 28 May 2026 17:27:22 +0200 Subject: [PATCH 2/6] Delete docs/architecture.md --- docs/architecture.md | 268 ------------------------------------------- 1 file changed, 268 deletions(-) delete mode 100644 docs/architecture.md diff --git a/docs/architecture.md b/docs/architecture.md deleted file mode 100644 index 332aa7f..0000000 --- a/docs/architecture.md +++ /dev/null @@ -1,268 +0,0 @@ -# Architecture - -This document summarizes the current architecture plan based only on the decisions made so far. - -## System Diagram - -```mermaid -flowchart TB - subgraph CAM["Cameras"] - ZED["ZED Mini
Stereo main camera"] - GCL["Left gripper camera"] - GCR["Right gripper camera"] - end - - subgraph BE["Backend"] - APP["Teleop Pro backend
Python / FastAPI / Uvicorn
Discovery, role assignment, session state
Works with 1 or 2 arms"] - - subgraph MEDIA["Media subsystem"] - CAP["Camera capture / preparation
Capture path open"] - MTX["MediaMTX
Multi-consumer stream distribution"] - end - - subgraph SAFE["Safety backend (modular)"] - SAFEV1["v1: TCP safety filter
Configurable no-go boxes
Input: desired TCP position"] - SAFEFUT["Future option
MoveIt"] - end - - subgraph CTRL["Robot control backend (modular)"] - CTRLV1["v1: Pinocchio / JacobiRobot
IK + regularization"] - CTRLFUT["Future options
MoveIt, qmotion"] - end - - REC["Recording module
Synchronized dataset
LeRobot-compatible episodes later"] - end - - subgraph UI["UI"] - VR["VR headset browser
WebXR operator UI
True bimanual control"] - end - - subgraph ROB["Robots"] - RL["Left robot arm + gripper"] - RR["Right robot arm + gripper"] - end - - TV["Showroom TV / browser
Spectator UI"] - - ZED --> CAP - GCL --> CAP - GCR --> CAP - - CAP --> MTX - MTX -->|WebRTC| VR - MTX -->|WebRTC| TV - - APP -->|HTTPS| VR - VR -->|WebSocket| APP - - APP -->|evaluate target| SAFEV1 - APP --> REC - APP -->|send allowed target| CTRLV1 - CTRLV1 --> RL - CTRLV1 --> RR -``` - -## System Overview - -- The backend is the system shell and owns startup, discovery, role assignment, session state, recording coordination, safety, and robot control coordination. -- The system must work with either one arm or two arms. -- The operator uses a browser in a VR headset. -- The VR headset is used for first-person viewing and bimanual controller input. -- Head motion is not used for robot control. -- The showroom TV is another stream consumer. -- The backend owns the media subsystem. Cameras connect into the backend, and the backend makes those streams available through MediaMTX. - -## Module Boundaries - -- `Backend shell` - - Libraries: `Python`, `FastAPI`, `Uvicorn` - - Responsibilities: startup, discovery, role assignment, session state, UI serving, protocol coordination -- `Media subsystem` - - Technology: `MediaMTX` - - Browser streaming protocol: `WebRTC` - - Covered so far: main stereo ZED stream, optional gripper camera streams, multiple consumers - - Open: exact camera capture path and exact ingest path into MediaMTX -- `UI` - - Technology: `WebXR` - - Optional frontend stack discussed: `React`, `Zustand`, `Immer`, `UIKit` - - Covered so far: operator UI in headset, simple status/control UI, recording start/stop, optional gripper camera views -- `Safety backend` - - v1 backend: custom TCP-position safety filter with multiple configurable no-go boxes - - Future optional backend: `MoveIt` - - Covered so far: a desired TCP position is checked against configured boxes and the safety backend returns a decision to the backend shell -- `Robot control backend` - - v1 backend: `Pinocchio` with the current [JacobiRobot](../teleop/utils/jacobi_robot.py) module - - Future optional backends: `MoveIt`, `qmotion` - - Covered so far: keep the current approach, improve it iteratively, do not fundamentally replace it in v1 -- `Recording` - - Covered so far: synchronized recording owned by this repo, start/stop from UI, target is LeRobot-compatible episodes later - -## Protocols And Libraries - -- `HTTPS` - - Backend serves the browser UI -- `WebSocket` - - Browser-to-backend control and status path - - Covered so far: controller poses, button state, recording commands, general app communication -- `WebRTC` - - Media delivery from backend media subsystem to VR headset browser and TV/browser -- `MediaMTX` - - Multi-consumer media distribution -- `WebXR` - - Browser-side VR presentation and controller input -- `Pinocchio` - - Current robot control / IK backend -- `FastAPI` and `Uvicorn` - - Current backend foundation -- `In-process Python calls` - - Backend shell, safety backend, robot control backend, and recorder are currently planned as modules inside the same Python process - - Communication between these backend modules is planned as normal Python class method calls, not as network calls - -## Backend Internal API - -For the `Teleop Pro backend -> safety backend -> robot control backend` path, the current v1 direction is: - -- The safety backend is a Python class selected by configuration. -- The robot control backend is a Python class selected by configuration. -- The backend shell owns both objects and orchestrates the call order. -- The safety backend should not directly talk to the robot controller. -- Instead, the safety backend should return a decision to the backend shell, and the backend shell should decide whether to call the robot control backend. - -This keeps the safety backend modular and makes it easier to replace the v1 safety module with a future `MoveIt` backend. - -### Proposed v1 module split - -- `TeleopBackend` - - owns session state - - receives operator commands from the browser - - constructs target commands for each arm - - calls the safety backend - - if allowed, calls the robot control backend - - sends status/errors back to the UI -- `SafetyBackend` - - receives a desired target for one arm - - checks the target TCP position against configured no-go boxes - - returns allow/block information and optional adjusted target data -- `RobotControlBackend` - - receives an allowed target - - converts the target into robot motion using the selected control backend - - sends commands to the robot driver/API - -### Proposed v1 data flow - -```text -browser command --> TeleopBackend --> SafetyBackend.evaluate_target(...) --> SafetyResult --> if allowed: RobotControlBackend.send_target(...) --> robot -``` - -### Proposed v1 Python-style interface - -```python -from dataclasses import dataclass -from typing import Literal - - -ArmId = Literal["left", "right"] - - -@dataclass -class PoseTarget: - arm: ArmId - tcp_pose: object - gripper_command: str | None = None - timestamp_ms: int | None = None - - -@dataclass -class SafetyResult: - allowed: bool - target: PoseTarget | None - reason: str | None = None - violated_zone_ids: list[str] | None = None - - -class SafetyBackend: - def evaluate_target(self, target: PoseTarget) -> SafetyResult: - raise NotImplementedError - - -class RobotControlBackend: - def send_target(self, target: PoseTarget) -> None: - raise NotImplementedError -``` - -The exact pose type is still open. The current decision is only that the safety API operates on a desired TCP target before the control backend sends robot commands. - -### Proposed v1 safety backend configuration - -The v1 safety backend can be constructed from: - -- backend type, for example `basic` now and `moveit` later -- a list of configured no-go boxes -- per-arm enable/disable settings if needed later - -Example shape: - -```python -basic_safety = SafetyBackendBasic( - boxes=[ - { - "id": "basket_keepout", - "frame": "world", - "min": [0.40, -0.10, 0.15], - "max": [0.55, 0.10, 0.35], - } - ] -) -``` - -The exact box schema is still open. The important current decision is that v1 uses multiple configurable Cartesian no-go boxes checked against TCP position. - -## Control Sequence - -The current intended control sequence is: - -1. The browser sends operator input to the backend over `WebSocket`. -2. The backend converts that input into a desired per-arm TCP target. -3. The backend calls the selected safety backend with that target. -4. The safety backend returns a `SafetyResult`. -5. If the result is allowed, the backend calls the selected robot control backend. -6. The robot control backend computes and sends the robot command. -7. The backend can send status or rejection reasons back to the UI. - -This is different from letting the safety module directly forward commands to the controller. The backend shell should remain the orchestrator. - -## Current v1 Decisions - -- Use `MediaMTX` for browser-facing media distribution. -- Keep media as part of the backend architecture. -- Keep `Pinocchio` / `JacobiRobot` as the v1 robot control backend. -- Make the robot control backend modular so that `MoveIt` or `qmotion` can be used later. -- Build a separate v1 safety module that filters desired TCP positions against multiple configurable no-go boxes. -- Make the safety module modular so that `MoveIt` can be used later. -- The safety backend and robot control backend are planned as in-process Python modules selected by configuration. -- The backend shell owns orchestration. The safety backend returns decisions; it does not directly own controller dispatch. -- Record synchronized session data in this repo. LeRobot-compatible episode details are deferred. - -## JacobiRobot Notes - -- The current IK implementation lives in [teleop/utils/jacobi_robot.py](../teleop/utils/jacobi_robot.py). -- The IK regularization is used to avoid singularities. -- This module should be improved through several iterations inside this project. -- The current direction is to avoid a fundamental rewrite, because the module is already the result of many failed iterations and has proven to work in practice. - -## Open Questions - -- Exact camera capture path for the ZED Mini and gripper cameras -- Exact ingest path from backend capture into MediaMTX -- Exact low-level robot driver / API path -- Exact control/data APIs -- Exact update rates and frequencies -- Exact format for the safety box configuration -- Exact behavior when a target is invalid beyond allow/block status -- Exact LeRobot-compatible episode schema details -- Whether the spectator UI is separate from the operator UI or shares the same frontend From 6fd70314b883e59ce04a01aafd960ff57302e96d Mon Sep 17 00:00:00 2001 From: spirosperos Date: Fri, 5 Jun 2026 14:48:32 +0200 Subject: [PATCH 3/6] offset user orientation --- teleop/__init__.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/teleop/__init__.py b/teleop/__init__.py index efd88e6..3c01168 100644 --- a/teleop/__init__.py +++ b/teleop/__init__.py @@ -157,6 +157,7 @@ def __init__( natural_phone_orientation_euler=None, natural_phone_position=None, frontend_dir=None, + offset_user_orientation=0, ): self.__logger = logging.getLogger("teleop") self.__logger.setLevel(logging.INFO) @@ -180,6 +181,7 @@ def __init__( t3d.euler.euler2mat(*natural_phone_orientation_euler), [1, 1, 1], ) + self.__offset_user_orientation = t3d.affines.compose([0, 0, 0], t3d.euler.euler2mat(0, 0, offset_user_orientation), [1, 1, 1]) if frontend_dir is None: frontend_dir = THIS_DIR @@ -244,7 +246,7 @@ def __update(self, message): received_pose[:3, :3] = received_pose[:3, :3] @ np.linalg.inv( TF_RUB2FLU[:3, :3] ) - received_pose = received_pose @ self.__natural_phone_pose + received_pose = self.__offset_user_orientation @ received_pose @ self.__natural_phone_pose # Pose jump protection if self.__previous_received_pose is not None: From ef7e984924580b96dde3708432fc613292c8ff87 Mon Sep 17 00:00:00 2001 From: spirosperos Date: Wed, 17 Jun 2026 18:09:37 +0200 Subject: [PATCH 4/6] servo to joint positions using ruckig --- pyproject.toml | 2 +- teleop/utils/jacobi_robot.py | 137 ++++++++++++++++++++++++- teleop/utils/jacobi_robot_ros.py | 28 +++++ tests/test_jacobi_robot_joint_servo.py | 96 +++++++++++++++++ 4 files changed, 261 insertions(+), 2 deletions(-) create mode 100644 tests/test_jacobi_robot_joint_servo.py diff --git a/pyproject.toml b/pyproject.toml index c60e14c..8528840 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -25,7 +25,7 @@ Source = "https://github.com/SpesRobotics/teleop" Tracker = "https://github.com/SpesRobotics/teleop/issues" [project.optional-dependencies] -utils = ["pin"] +utils = ["pin", "ruckig"] [tool.setuptools.packages.find] include = ["teleop*"] diff --git a/teleop/utils/jacobi_robot.py b/teleop/utils/jacobi_robot.py index 22b316f..f2e1e76 100644 --- a/teleop/utils/jacobi_robot.py +++ b/teleop/utils/jacobi_robot.py @@ -1,6 +1,7 @@ import numpy as np import pinocchio as pin import matplotlib.pyplot as plt +import ruckig from typing import List, Tuple @@ -48,6 +49,8 @@ def __init__( min_angular_vel: float = 0.1, linear_gain: float = 20.0, angular_gain: float = 12.0, + max_joint_acc: float = 10.0, + max_joint_jerk: float = 100.0, ): """ Initialize the Pinocchio robot with servo control capabilities. @@ -67,6 +70,7 @@ def __init__( # Robot state self.q = pin.neutral(self.model) # Joint positions self.dq = np.zeros(self.model.nv) # Joint velocities + self.ddq = np.zeros(self.model.nv) # Joint accelerations # Joint limits self.q_min = self.model.lowerPositionLimit @@ -79,6 +83,8 @@ def __init__( self.max_linear_acc = max_linear_acc self.max_angular_acc = max_angular_acc self.max_joint_vel = max_joint_vel + self.max_joint_acc = max_joint_acc + self.max_joint_jerk = max_joint_jerk self.min_linear_vel = min_linear_vel self.min_angular_vel = min_angular_vel self.linear_gain = linear_gain @@ -324,10 +330,95 @@ def servo_to_pose( reached = position_error < linear_tol and orientation_error < angular_tol return reached + def servo_to_joint_positions( + self, + target_joint_positions, + dt: float = 0.01, + joint_tol: float = 1e-4, + max_joint_vel: float = None, + max_joint_acc: float = None, + max_joint_jerk: float = None, + ) -> bool: + """ + Move one jerk-limited Ruckig step toward target joint positions. + + Args: + target_joint_positions: Dict of joint names to positions, or a vector + ordered like ``self.q``. + dt: Time step for online trajectory generation. + joint_tol: Joint-space convergence tolerance. + max_joint_vel: Optional per-joint velocity cap. Defaults to the robot's + configured ``max_joint_vel``. + max_joint_acc: Optional per-joint acceleration cap. Defaults to the + robot's configured ``max_joint_acc``. + max_joint_jerk: Optional per-joint jerk cap. Defaults to the robot's + configured ``max_joint_jerk``. + + Returns: + bool: True if the target joint positions are reached after this step. + """ + if dt <= 0: + raise ValueError("dt must be greater than zero.") + if joint_tol < 0: + raise ValueError("joint_tol must be non-negative.") + + target_q = self.__target_joint_positions_to_vector(target_joint_positions) + + velocity_cap = self.max_joint_vel if max_joint_vel is None else max_joint_vel + if velocity_cap <= 0: + raise ValueError("max_joint_vel must be greater than zero.") + + acceleration_cap = ( + self.max_joint_acc if max_joint_acc is None else max_joint_acc + ) + if acceleration_cap <= 0: + raise ValueError("max_joint_acc must be greater than zero.") + + jerk_cap = self.max_joint_jerk if max_joint_jerk is None else max_joint_jerk + if jerk_cap <= 0: + raise ValueError("max_joint_jerk must be greater than zero.") + + self.__validate_joint_position_limits(target_q, joint_tol) + + max_velocity = self.__joint_velocity_limits(velocity_cap) + dofs = len(self.q) + otg = ruckig.Ruckig(dofs, dt) + ruckig_input = ruckig.InputParameter(dofs) + ruckig_output = ruckig.OutputParameter(dofs) + + ruckig_input.current_position = self.q.tolist() + ruckig_input.current_velocity = self.dq.tolist() + ruckig_input.current_acceleration = self.ddq.tolist() + ruckig_input.target_position = target_q.tolist() + ruckig_input.target_velocity = np.zeros(dofs).tolist() + ruckig_input.target_acceleration = np.zeros(dofs).tolist() + ruckig_input.max_velocity = max_velocity.tolist() + ruckig_input.max_acceleration = np.full(dofs, acceleration_cap).tolist() + ruckig_input.max_jerk = np.full(dofs, jerk_cap).tolist() + + try: + result = otg.update(ruckig_input, ruckig_output) + except Exception as error: + raise ValueError("Failed to calculate Ruckig joint trajectory.") from error + + if result not in (ruckig.Result.Working, ruckig.Result.Finished): + raise ValueError(f"Ruckig failed to calculate joint trajectory: {result}") + + self.q = np.asarray(ruckig_output.new_position, dtype=float) + self.dq = np.asarray(ruckig_output.new_velocity, dtype=float) + self.ddq = np.asarray(ruckig_output.new_acceleration, dtype=float) + + remaining_error = target_q - self.q + return bool(np.all(np.abs(remaining_error) <= joint_tol)) + def update_state(self, joint_velocities: np.ndarray, dt: float = 0.01): """Update robot state with given joint velocities.""" + previous_dq = self.dq.copy() + # Store velocities self.dq = joint_velocities.copy() + if dt > 0: + self.ddq = (self.dq - previous_dq) / dt # Integrate to get new joint positions self.q = self.q + self.dq * dt @@ -463,7 +554,6 @@ def set_joint_position(self, joint_name: str, position: float): joint_index = self.__get_joint_index(joint_name) if joint_index < 0 or joint_index >= self.model.njoints: raise ValueError(f"Joint '{joint_name}' not found in model.") - print(f"Setting joint '{joint_name}' to position {position:.3f}") self.q[joint_index] = position def get_joint_names(self) -> List[str]: @@ -482,6 +572,51 @@ def get_joint_velocity(self, joint_name: str) -> float: raise ValueError(f"Joint '{joint_name}' not found in model.") return self.dq[joint_index] + def __target_joint_positions_to_vector(self, target_joint_positions) -> np.ndarray: + if isinstance(target_joint_positions, dict): + target_q = self.q.copy() + for joint_name, position in target_joint_positions.items(): + joint_index = self.__get_joint_index(joint_name) + if joint_index < 0 or joint_index >= self.model.nq: + raise ValueError(f"Joint '{joint_name}' not found in model.") + target_q[joint_index] = position + else: + target_q = np.asarray(target_joint_positions, dtype=float) + if target_q.shape != self.q.shape: + raise ValueError( + "target_joint_positions must have shape " + f"{self.q.shape}, got {target_q.shape}" + ) + + if not np.all(np.isfinite(target_q)): + raise ValueError("target_joint_positions must be finite.") + + return target_q + + def __joint_velocity_limits(self, velocity_cap: float) -> np.ndarray: + max_velocity = np.full(len(self.q), velocity_cap, dtype=float) + for i in range(len(max_velocity)): + if i < len(self.dq_max) and self.dq_max[i] > 0: + max_velocity[i] = min(max_velocity[i], self.dq_max[i]) + return max_velocity + + def __validate_joint_position_limits( + self, target_q: np.ndarray, joint_tol: float + ) -> None: + lower_limit_mask = np.isfinite(self.q_min) + below_limits = ( + target_q[lower_limit_mask] < self.q_min[lower_limit_mask] - joint_tol + ) + if np.any(below_limits): + raise ValueError("target_joint_positions are below the robot joint limits.") + + upper_limit_mask = np.isfinite(self.q_max) + above_limits = ( + target_q[upper_limit_mask] > self.q_max[upper_limit_mask] + joint_tol + ) + if np.any(above_limits): + raise ValueError("target_joint_positions are above the robot joint limits.") + def __compute_regularized_jacobian_pinv(self, J: np.ndarray) -> np.ndarray: """ Compute regularized pseudo-inverse of Jacobian with multiple regularization terms. diff --git a/teleop/utils/jacobi_robot_ros.py b/teleop/utils/jacobi_robot_ros.py index 18ae8f7..d817681 100644 --- a/teleop/utils/jacobi_robot_ros.py +++ b/teleop/utils/jacobi_robot_ros.py @@ -25,10 +25,13 @@ def __init__( max_linear_acc: float = 3.0, max_angular_acc: float = 6.0, max_joint_vel: float = 5.0, + max_joint_acc: float = 10.0, + max_joint_jerk: float = 100.0, min_linear_vel: float = 0.03, min_angular_vel: float = 0.1, linear_gain: float = 5.0, angular_gain: float = 1.0, + ): """ Initialize ROS 2-enabled Jacobian robot. @@ -50,6 +53,8 @@ def __init__( min_angular_vel: Minimum angular velocity linear_gain: Linear gain for control angular_gain: Angular gain for control + max_joint_acc: Maximum joint acceleration + max_joint_jerk: Maximum joint jerk """ self.node = node @@ -68,6 +73,8 @@ def __init__( min_angular_vel, linear_gain, angular_gain, + max_joint_acc, + max_joint_jerk, ) self.joint_states_received = False @@ -205,6 +212,27 @@ def servo_to_pose(self, target_pose: np.ndarray, dt: float = 0.1) -> bool: self.__send_joint_trajectory_topic(duration=dt) return reached + def servo_to_joint_positions( + self, + target_joint_positions, + dt: float = 0.1, + joint_tol: float = 1e-4, + max_joint_vel: float = None, + max_joint_acc: float = None, + max_joint_jerk: float = None, + ) -> bool: + reached = super().servo_to_joint_positions( + target_joint_positions, + dt=dt, + joint_tol=joint_tol, + max_joint_vel=max_joint_vel, + max_joint_acc=max_joint_acc, + max_joint_jerk=max_joint_jerk, + ) + + self.__send_joint_trajectory_topic(duration=dt) + return reached + def reset_joint_states(self, blocked: bool = True): """Reset the robot state.""" self.joint_states_received = False diff --git a/tests/test_jacobi_robot_joint_servo.py b/tests/test_jacobi_robot_joint_servo.py new file mode 100644 index 0000000..881bb17 --- /dev/null +++ b/tests/test_jacobi_robot_joint_servo.py @@ -0,0 +1,96 @@ +import unittest +from pathlib import Path + +import numpy as np + +try: + from teleop.utils.jacobi_robot import JacobiRobot +except ImportError as error: + JacobiRobot = None + JACOBI_IMPORT_ERROR = error +else: + JACOBI_IMPORT_ERROR = None + + +class TestJacobiRobotJointServo(unittest.TestCase): + def setUp(self): + if JacobiRobot is None: + self.skipTest( + f"JacobiRobot dependencies are unavailable: {JACOBI_IMPORT_ERROR}" + ) + + urdf_path = ( + Path(__file__).resolve().parents[1] + / "teleop" + / "utils" + / "lite6.urdf" + ) + self.robot = JacobiRobot(str(urdf_path), ee_link="link6") + + def test_servo_to_joint_positions_steps_toward_named_targets(self): + target = { + "joint1": 0.2, + "joint2": 0.0, + "joint3": 0.0, + "joint4": 0.0, + "joint5": 0.0, + "joint6": 0.0, + } + + reached = self.robot.servo_to_joint_positions( + target, + dt=0.1, + max_joint_vel=1.0, + max_joint_acc=10.0, + max_joint_jerk=100.0, + ) + + self.assertFalse(reached) + self.assertGreater(self.robot.get_joint_position("joint1"), 0.0) + self.assertLess(self.robot.get_joint_position("joint1"), 0.1) + self.assertLessEqual(abs(self.robot.get_joint_velocity("joint1")), 1.0) + self.assertLessEqual(abs(self.robot.ddq[0]), 10.0) + + for _ in range(10): + reached = self.robot.servo_to_joint_positions( + target, + dt=0.1, + max_joint_vel=1.0, + max_joint_acc=10.0, + max_joint_jerk=100.0, + ) + if reached: + break + + self.assertTrue(reached) + self.assertAlmostEqual(self.robot.get_joint_position("joint1"), 0.2) + + def test_servo_to_joint_positions_accepts_vector_targets(self): + target = np.zeros_like(self.robot.q) + target[0] = 0.05 + + reached = self.robot.servo_to_joint_positions( + target, + dt=0.1, + max_joint_vel=1.0, + max_joint_acc=10.0, + max_joint_jerk=100.0, + ) + + for _ in range(10): + if reached: + break + reached = self.robot.servo_to_joint_positions( + target, + dt=0.1, + max_joint_vel=1.0, + max_joint_acc=10.0, + max_joint_jerk=100.0, + ) + + self.assertTrue(reached) + self.assertAlmostEqual(self.robot.get_joint_position("joint1"), 0.05) + + +if __name__ == "__main__": + unittest.main() From 9ceb916ab54122ec7d9649d094d0730c60fa7dd3 Mon Sep 17 00:00:00 2001 From: spirosperos Date: Thu, 18 Jun 2026 15:49:14 +0200 Subject: [PATCH 5/6] fix pose accumulation --- teleop/__init__.py | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/teleop/__init__.py b/teleop/__init__.py index 3c01168..7473698 100644 --- a/teleop/__init__.py +++ b/teleop/__init__.py @@ -158,6 +158,7 @@ def __init__( natural_phone_position=None, frontend_dir=None, offset_user_orientation=0, + current_pose_provider=None, ): self.__logger = logging.getLogger("teleop") self.__logger.setLevel(logging.INFO) @@ -171,6 +172,8 @@ def __init__( self.__previous_received_pose = None self.__callbacks = [] self.__pose = np.eye(4) + self.__previous_move = False + self.__current_pose_provider = current_pose_provider if natural_phone_orientation_euler is None: natural_phone_orientation_euler = [0, math.radians(-45), 0] @@ -227,6 +230,9 @@ def __update(self, message): position = message["position"] orientation = message["orientation"] scale = message.get("scale", 1.0) + #Detect rising edge of move signal + move_started = move and not self.__previous_move + self.__previous_move = move position = np.array([position["x"], position["y"], position["z"]]) quat = np.array( @@ -238,7 +244,13 @@ def __update(self, message): self.__absolute_pose_init = None self.__notify_subscribers(self.__pose, message) return - + if move_started and self.__current_pose_provider is not None: # Rising edge + try: + current_pose = self.__current_pose_provider() + self.__pose = np.array(current_pose, dtype=float, copy=True) + except Exception: + self.__logger.warning("Failed to get current pose, using last known pose") + received_pose_rub = t3d.affines.compose( position, t3d.quaternions.quat2mat(quat), [1, 1, 1] ) From c1984d882328165412eff45d5270bea3dd59fb13 Mon Sep 17 00:00:00 2001 From: spirosperos Date: Thu, 9 Jul 2026 18:07:10 +0200 Subject: [PATCH 6/6] Mount multiple frontends --- teleop/__init__.py | 105 +++++++++++++++++++++++++++++++++++++++------ 1 file changed, 92 insertions(+), 13 deletions(-) diff --git a/teleop/__init__.py b/teleop/__init__.py index 7473698..180223a 100644 --- a/teleop/__init__.py +++ b/teleop/__init__.py @@ -2,7 +2,7 @@ import math import socket import logging -from typing import Callable, List +from typing import Callable, List, NamedTuple import uvicorn from fastapi import APIRouter, FastAPI, WebSocket, WebSocketDisconnect from fastapi.responses import FileResponse @@ -15,6 +15,11 @@ THIS_DIR = os.path.dirname(os.path.realpath(__file__)) +class FrontendMount(NamedTuple): + path: str + directory: str + + def get_local_ip(): try: # Connect to an external address (doesn't actually send data) @@ -147,7 +152,9 @@ class Teleop: Args: host (str, optional): The host IP address. Defaults to "0.0.0.0". port (int, optional): The port number. Defaults to 4443. - frontend_dir (str, optional): The directory containing the frontend assets. Defaults to THIS_DIR. + frontend_dir (str | list[tuple[str, str]], optional): The directory + containing the frontend assets, or a list of route prefix and + frontend directory pairs. Defaults to THIS_DIR. """ def __init__( @@ -186,9 +193,7 @@ def __init__( ) self.__offset_user_orientation = t3d.affines.compose([0, 0, 0], t3d.euler.euler2mat(0, 0, offset_user_orientation), [1, 1, 1]) - if frontend_dir is None: - frontend_dir = THIS_DIR - self.__frontend_dir = frontend_dir + self.__frontend_mounts = self.__normalize_frontend_mounts(frontend_dir) self.__app = FastAPI() self.__manager = ConnectionManager() @@ -200,6 +205,49 @@ def __init__( def include_router(self, router: APIRouter, **kwargs) -> None: self.__app.include_router(router, **kwargs) + def __normalize_frontend_mounts(self, frontend_dir): + if frontend_dir is None: + frontend_dir = THIS_DIR + + if isinstance(frontend_dir, (str, os.PathLike)): + return [FrontendMount("/", os.fspath(frontend_dir))] + + mounts = [] + seen_paths = set() + for mount in frontend_dir: + try: + path, directory = mount + except (TypeError, ValueError): + raise ValueError( + "frontend_dir entries must be (path, directory) pairs" + ) from None + + path = self.__normalize_frontend_path(path) + if path in seen_paths: + raise ValueError(f"Duplicate frontend mount path: {path}") + + seen_paths.add(path) + mounts.append(FrontendMount(path, os.fspath(directory))) + + if not mounts: + raise ValueError("frontend_dir mount list cannot be empty") + + return mounts + + @staticmethod + def __normalize_frontend_path(path): + path = str(path).strip() + if not path: + raise ValueError("Frontend mount path cannot be empty") + + if not path.startswith("/"): + path = f"/{path}" + + if path != "/": + path = path.rstrip("/") + + return path + def set_pose(self, pose: np.ndarray) -> None: """ Set the current pose of the end-effector. @@ -302,14 +350,8 @@ def __update(self, message): self.__notify_subscribers(self.__pose, message) def __setup_routes(self): - # Mount static files directory - assets_dir = os.path.join(self.__frontend_dir, "assets") - self.__app.mount("/assets", StaticFiles(directory=assets_dir), name="assets") - - @self.__app.get("/") - async def index(): - self.__logger.debug("Serving the index.html file") - return FileResponse(os.path.join(self.__frontend_dir, "index.html")) + for mount_index, mount in enumerate(self.__frontend_mounts): + self.__mount_frontend(mount, mount_index) @self.__app.websocket("/ws") async def websocket_endpoint(websocket: WebSocket): @@ -331,6 +373,43 @@ async def websocket_endpoint(websocket: WebSocket): self.__manager.disconnect(websocket) self.__logger.info("Client disconnected") + def __mount_frontend(self, mount: FrontendMount, mount_index: int): + assets_dir = os.path.join(mount.directory, "assets") + assets_path = "/assets" if mount.path == "/" else f"{mount.path}/assets" + self.__app.mount( + assets_path, + StaticFiles(directory=assets_dir), + name=f"frontend_assets_{mount_index}", + ) + + async def index(): + self.__logger.debug( + f"Serving {mount.path} frontend index.html from {mount.directory}" + ) + return FileResponse(os.path.join(mount.directory, "index.html")) + + if mount.path == "/": + self.__app.add_api_route("/", index, methods=["GET"]) + return + + self.__app.add_api_route(mount.path, index, methods=["GET"]) + self.__app.add_api_route(f"{mount.path}/", index, methods=["GET"]) + + @self.__app.get(f"{mount.path}/{{path:path}}") + async def prefixed_spa_index(path: str): + requested_path = os.path.abspath(os.path.join(mount.directory, path)) + frontend_root = os.path.abspath(mount.directory) + if ( + os.path.commonpath([frontend_root, requested_path]) == frontend_root + and os.path.isfile(requested_path) + ): + return FileResponse(requested_path) + + self.__logger.debug( + f"Serving {mount.path} frontend index.html from {mount.directory}" + ) + return FileResponse(os.path.join(mount.directory, "index.html")) + def run(self) -> None: """ Runs the teleop server. This method is blocking.