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(