From b7aa1dd3aa734edb3ebedcf3a8e09318080fb9f1 Mon Sep 17 00:00:00 2001 From: cristhian aguilera Date: Thu, 2 Apr 2026 14:07:28 -0300 Subject: [PATCH] add TOOL_TYPE offset to tcp_pose for vacuum and gripper Apply Z-axis tool offset when publishing tcp_pose so the output reflects the actual tool tip. Adds TOOL_TYPE (none/vacuum/gripper) and TOOL_OFFSET_MM env vars with standard Lite6 accessory presets. Bump version to 0.1.2. --- README.md | 4 +++- dataflow.yml | 2 ++ dora_ulite6/main.py | 44 +++++++++++++++++++++++++++++++++++++------- pyproject.toml | 2 +- uv.lock | 2 +- 5 files changed, 44 insertions(+), 10 deletions(-) diff --git a/README.md b/README.md index bd13ff2..efc0393 100644 --- a/README.md +++ b/README.md @@ -40,6 +40,8 @@ nodes: | `API_HOST` | `0.0.0.0` | API server host | | `API_PORT` | `8080` | API server port | | `VACUUM_ENABLED` | `false` | Enable vacuum gripper controls | +| `TOOL_TYPE` | `none` | Mounted tool: `none`, `vacuum`, `gripper` | +| `TOOL_OFFSET_MM` | `0.0` | Additional Z-offset in mm added on top of the tool preset | | `INIT_MODE` | `none` | Startup positioning: `home`, `pose`, or `none` | | `INIT_X/Y/Z` | `300/0/250` | Initial pose coordinates (mm) | | `INIT_ROLL/PITCH/YAW` | `180/0/0` | Initial pose orientation (deg) | @@ -103,7 +105,7 @@ curl -X POST http://localhost:8080/api/move_to_pose \ - `status` - JSON: `{ok, action, message, timestamp_ns}` - `joint_pose` - 6 joint angles in degrees -- `tcp_pose` - `[x, y, z, roll, pitch, yaw]` in mm/deg +- `tcp_pose` - `[x, y, z, roll, pitch, yaw]` in mm/deg — reflects tool tip when `TOOL_TYPE` is set ## Dora Inputs diff --git a/dataflow.yml b/dataflow.yml index 1a7e0a0..ee68ccc 100644 --- a/dataflow.yml +++ b/dataflow.yml @@ -15,3 +15,5 @@ nodes: API_HOST: "0.0.0.0" API_PORT: "9000" VACUUM_ENABLED: "true" + TOOL_TYPE: "vacuum" + TOOL_OFFSET_MM: "0.0" diff --git a/dora_ulite6/main.py b/dora_ulite6/main.py index b93b7fa..c90ad41 100644 --- a/dora_ulite6/main.py +++ b/dora_ulite6/main.py @@ -67,6 +67,20 @@ class ULite6Helper: "yaw": pos[5], } + def get_tool_tcp_pose(self, offset_mm: float) -> Dict[str, float]: + pos = self.get_current_position() + if offset_mm == 0.0: + return pos + T = self.get_pose_matrix(tcp_offset_mm=offset_mm) + return { + "x": T[0, 3] * 1000.0, + "y": T[1, 3] * 1000.0, + "z": T[2, 3] * 1000.0, + "roll": pos["roll"], + "pitch": pos["pitch"], + "yaw": pos["yaw"], + } + def get_pose_matrix(self, tcp_offset_mm: float = 0.0) -> np.ndarray: pos = self.get_current_position() R = Rotation.from_euler( @@ -154,7 +168,12 @@ class MoveToPoseRequest(BaseModel): def create_api( - helper: ULite6Helper, default_speed: float, default_units: str, vacuum_enabled: bool + helper: ULite6Helper, + default_speed: float, + default_units: str, + vacuum_enabled: bool, + tool_type: str, + tool_offset_mm: float, ) -> FastAPI: """Create FastAPI application with robot control endpoints.""" app = FastAPI( @@ -348,7 +367,7 @@ def create_api(
-

TCP Pose (mm, deg)

+

TCP Pose (mm, deg)

X
--
Y
--
@@ -528,6 +547,9 @@ def create_api( updateVacuumStatus(); setInterval(updateVacuumStatus, 1000); } + if (config.tool_type && config.tool_type !== 'none') { + $('tcp-pose-title').textContent = 'TCP Pose — ' + config.tool_type + ' (mm, deg)'; + } } async function updateVacuumStatus() { @@ -586,9 +608,9 @@ def create_api( @app.get("/api/pose") def get_pose(): - """Get current TCP pose [x, y, z, roll, pitch, yaw].""" + """Get current TCP pose [x, y, z, roll, pitch, yaw] with tool offset applied.""" try: - return helper.get_current_position() + return helper.get_tool_tcp_pose(tool_offset_mm) except Exception as e: raise HTTPException(status_code=500, detail=str(e)) @@ -698,7 +720,11 @@ def create_api( @app.get("/api/config") def get_config(): """Get API configuration.""" - return {"vacuum_enabled": vacuum_enabled} + return { + "vacuum_enabled": vacuum_enabled, + "tool_type": tool_type, + "tool_offset_mm": tool_offset_mm, + } return app @@ -801,6 +827,10 @@ def main() -> None: api_port = int(os.getenv("API_PORT", "8080")) vacuum_enabled = os.getenv("VACUUM_ENABLED", "false").lower() in ("true", "1", "yes") + _TOOL_OFFSET_PRESETS: Dict[str, float] = {"none": 0.0, "vacuum": 61.0, "gripper": 65.0} + tool_type = os.getenv("TOOL_TYPE", "none").lower() + tool_offset_mm = _TOOL_OFFSET_PRESETS.get(tool_type, 0.0) + float(os.getenv("TOOL_OFFSET_MM", "0.0")) + # Initial position settings init_mode = os.getenv("INIT_MODE", "none").lower() # "home", "pose", or "none" init_x = float(os.getenv("INIT_X", "300.0")) @@ -839,7 +869,7 @@ def main() -> None: _log("Skipping initial position (INIT_MODE=none)") # Create and start FastAPI server in background thread - app = create_api(helper, default_speed, default_units, vacuum_enabled) + app = create_api(helper, default_speed, default_units, vacuum_enabled, tool_type, tool_offset_mm) api_thread = threading.Thread( target=run_uvicorn, args=(app, api_host, api_port), daemon=True ) @@ -854,7 +884,7 @@ def main() -> None: if event["id"] == "tick": try: joints = helper.get_joint_angles_deg() - tcp_pose = helper.get_current_position() + tcp_pose = helper.get_tool_tcp_pose(tool_offset_mm) _send_joint_pose(node, joints) _send_tcp_pose(node, tcp_pose) except Exception as exc: diff --git a/pyproject.toml b/pyproject.toml index 6a36d76..90ce414 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,6 +1,6 @@ [project] name = "dora-ulite6" -version = "0.1.1" +version = "0.1.2" license = "MIT" authors = [ { name = "Dora" } diff --git a/uv.lock b/uv.lock index 5d189bf..d70effd 100644 --- a/uv.lock +++ b/uv.lock @@ -78,7 +78,7 @@ wheels = [ [[package]] name = "dora-ulite6" -version = "0.1.0" +version = "0.1.1" source = { virtual = "." } dependencies = [ { name = "dora-rs" },