diff --git a/.env.example b/.env.example index 3a41e68..e1d402c 100644 --- a/.env.example +++ b/.env.example @@ -2,11 +2,14 @@ # NODE_STATUS_UPDATE_INTERVAL=2.0 # NODE_STATE_UPDATE_INTERVAL=2.0 -# NODE_NODE_NAME=null -# NODE_NODE_ID=null -# NODE_NODE_TYPE=null +# NODE_NAME=null +# NODE_ID=null +# NODE_TYPE=null # NODE_MODULE_NAME=null # NODE_MODULE_VERSION=null +# NODE_ENABLE_REGISTRY_RESOLUTION=true +# NODE_LAB_URL=null +# NODE_REGISTRY_LOCK_TIMEOUT=60.0 # NODE_URL="http://127.0.0.1:2000/" # NODE_UVICORN_KWARGS={"limit_concurrency":10} # NODE_ENABLE_RATE_LIMITING=true diff --git a/.gitignore b/.gitignore index 98dc9f5..fe7dd8f 100644 --- a/.gitignore +++ b/.gitignore @@ -161,5 +161,10 @@ cython_debug/ #.idea/ .vscode -.DS_Store .madsci/ + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 95ff59e..2a7a3d1 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -22,6 +22,7 @@ repos: # Run the linter. - id: ruff args: [--fix] + exclude: '\.ipynb$' # Exclude Jupyter Notebooks from linting. # Run the formatter. - id: ruff-format - repo: https://github.com/jag-k/pydantic-settings-export @@ -31,3 +32,4 @@ repos: entry: pdm run pydantic-settings-export language: system files: ^src/pf400_rest_node\.py$ + exclude: '\.ipynb$' # Exclude Jupyter Notebooks from all hooks. diff --git a/docs/Configuration.md b/docs/Configuration.md index 7655cca..0ae8b32 100644 --- a/docs/Configuration.md +++ b/docs/Configuration.md @@ -12,11 +12,14 @@ Configuration for the pf400 node module. |------------------------------------|--------------------------|----------------------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------------|----------------------------| | `NODE_STATUS_UPDATE_INTERVAL` | `number` \| `NoneType` | `2.0` | The interval in seconds at which the node should update its status. | `2.0` | | `NODE_STATE_UPDATE_INTERVAL` | `number` \| `NoneType` | `2.0` | The interval in seconds at which the node should update its state. | `2.0` | -| `NODE_NODE_NAME` | `string` \| `NoneType` | `null` | Name for this node. If not set, defaults to the class name. | `null` | -| `NODE_NODE_ID` | `string` \| `NoneType` | `null` | Unique ID for this node. If not set, a new ULID is generated. | `null` | -| `NODE_NODE_TYPE` | `NodeType` \| `NoneType` | `null` | The type of thing this node provides an interface for. | `null` | +| `NODE_NAME` \| `NODE_NAME` | `string` \| `NoneType` | `null` | Name for this node. If not set, defaults to the class name. | `null` | +| `NODE_ID` \| `NODE_ID` | `string` \| `NoneType` | `null` | Unique ID for this node. If not set, a new ULID is generated. | `null` | +| `NODE_TYPE` \| `NODE_TYPE` | `NodeType` \| `NoneType` | `null` | The type of thing this node provides an interface for. | `null` | | `NODE_MODULE_NAME` | `string` \| `NoneType` | `null` | Name of the node module implementation. | `null` | | `NODE_MODULE_VERSION` | `string` \| `NoneType` | `null` | Version of the node module implementation. | `null` | +| `NODE_ENABLE_REGISTRY_RESOLUTION` | `boolean` | `true` | When true, resolve node_id from the ID Registry at startup for stable identity across restarts. | `true` | +| `NODE_LAB_URL` | `AnyUrl` \| `NoneType` | `null` | Lab Manager URL for distributed registry coordination. | `null` | +| `NODE_REGISTRY_LOCK_TIMEOUT` | `number` | `60.0` | Seconds to retry registry lock acquisition on contention at startup. Should be at least 2x the lock TTL (30s) to survive ungraceful container restarts. | `60.0` | | `NODE_URL` \| `NODE_URL` | `AnyUrl` | `"http://127.0.0.1:2000/"` | The URL used to communicate with the node. This is the base URL for the REST API. | `"http://127.0.0.1:2000/"` | | `NODE_UVICORN_KWARGS` | `object` | `{"limit_concurrency":10}` | Configuration for the Uvicorn server that runs the REST API. By default, sets limit_concurrency=10 to protect against connection exhaustion attacks. | `{"limit_concurrency":10}` | | `NODE_ENABLE_RATE_LIMITING` | `boolean` | `true` | Enable rate limiting middleware for the REST API. | `true` | diff --git a/pdm.lock b/pdm.lock index 0a4e22a..f8577dd 100644 --- a/pdm.lock +++ b/pdm.lock @@ -427,10 +427,11 @@ files = [ [[package]] name = "httpcore" version = "1.0.9" -summary = "" +requires_python = ">=3.8" +summary = "A minimal low-level HTTP client." dependencies = [ "certifi", - "h11", + "h11>=0.16", ] files = [ {file = "httpcore-1.0.9-py3-none-any.whl", hash = "sha256:2d400746a40668fc9dec9810239072b40b4484b640a8c38fd654a024c7a1bf55"}, @@ -469,11 +470,12 @@ files = [ [[package]] name = "httpx" version = "0.28.1" -summary = "" +requires_python = ">=3.8" +summary = "The next generation HTTP client." dependencies = [ "anyio", "certifi", - "httpcore", + "httpcore==1.*", "idna", ] files = [ @@ -513,72 +515,27 @@ files = [ [[package]] name = "ipykernel" -version = "7.2.0" -summary = "" +version = "7.1.0" +requires_python = ">=3.10" +summary = "IPython Kernel for Jupyter" dependencies = [ - "appnope; sys_platform == \"darwin\"", - "comm", - "debugpy", - "ipython==8.38.0; python_full_version < \"3.11\"", - "ipython==9.10.0; python_full_version == \"3.11.*\"", - "ipython==9.11.0; python_full_version >= \"3.12\"", - "jupyter-client", - "jupyter-core", - "matplotlib-inline", - "nest-asyncio", - "packaging", - "psutil", - "pyzmq", - "tornado", - "traitlets", + "appnope>=0.1.2; platform_system == \"Darwin\"", + "comm>=0.1.1", + "debugpy>=1.6.5", + "ipython>=7.23.1", + "jupyter-client>=8.0.0", + "jupyter-core!=5.0.*,>=4.12", + "matplotlib-inline>=0.1", + "nest-asyncio>=1.4", + "packaging>=22", + "psutil>=5.7", + "pyzmq>=25", + "tornado>=6.2", + "traitlets>=5.4.0", ] files = [ - {file = "ipykernel-7.2.0-py3-none-any.whl", hash = "sha256:3bbd4420d2b3cc105cbdf3756bfc04500b1e52f090a90716851f3916c62e1661"}, - {file = "ipykernel-7.2.0.tar.gz", hash = "sha256:18ed160b6dee2cbb16e5f3575858bc19d8f1fe6046a9a680c708494ce31d909e"}, -] - -[[package]] -name = "ipython" -version = "8.38.0" -summary = "" -dependencies = [ - "colorama; python_full_version < \"3.11\" and sys_platform == \"win32\"", - "decorator; python_full_version < \"3.11\"", - "exceptiongroup; python_full_version < \"3.11\"", - "jedi; python_full_version < \"3.11\"", - "matplotlib-inline; python_full_version < \"3.11\"", - "pexpect; python_full_version < \"3.11\" and (sys_platform != \"emscripten\" and sys_platform != \"win32\")", - "prompt-toolkit; python_full_version < \"3.11\"", - "pygments; python_full_version < \"3.11\"", - "stack-data; python_full_version < \"3.11\"", - "traitlets; python_full_version < \"3.11\"", - "typing-extensions; python_full_version < \"3.11\"", -] -files = [ - {file = "ipython-8.38.0-py3-none-any.whl", hash = "sha256:750162629d800ac65bb3b543a14e7a74b0e88063eac9b92124d4b2aa3f6d8e86"}, - {file = "ipython-8.38.0.tar.gz", hash = "sha256:9cfea8c903ce0867cc2f23199ed8545eb741f3a69420bfcf3743ad1cec856d39"}, -] - -[[package]] -name = "ipython" -version = "9.10.0" -summary = "" -dependencies = [ - "colorama; python_full_version == \"3.11.*\" and sys_platform == \"win32\"", - "decorator; python_full_version == \"3.11.*\"", - "ipython-pygments-lexers; python_full_version == \"3.11.*\"", - "jedi; python_full_version == \"3.11.*\"", - "matplotlib-inline; python_full_version == \"3.11.*\"", - "pexpect; python_full_version == \"3.11.*\" and (sys_platform != \"emscripten\" and sys_platform != \"win32\")", - "prompt-toolkit; python_full_version == \"3.11.*\"", - "pygments; python_full_version == \"3.11.*\"", - "stack-data; python_full_version == \"3.11.*\"", - "traitlets; python_full_version == \"3.11.*\"", - "typing-extensions; python_full_version == \"3.11.*\"", -] -files = [ - {file = "ipython-9.10.0-py3-none-any.whl", hash = "sha256:c6ab68cc23bba8c7e18e9b932797014cc61ea7fd6f19de180ab9ba73e65ee58d"}, - {file = "ipython-9.10.0.tar.gz", hash = "sha256:cd9e656be97618a0676d058134cd44e6dc7012c0e5cb36a9ce96a8c904adaf77"}, + {file = "ipykernel-7.1.0-py3-none-any.whl", hash = "sha256:763b5ec6c5b7776f6a8d7ce09b267693b4e5ce75cb50ae696aaefb3c85e1ea4c"}, + {file = "ipykernel-7.1.0.tar.gz", hash = "sha256:58a3fc88533d5930c3546dc7eac66c6d288acde4f801e2001e65edc5dc9cf0db"}, ] [[package]] @@ -591,7 +548,7 @@ dependencies = [ "ipython-pygments-lexers; python_full_version >= \"3.12\"", "jedi; python_full_version >= \"3.12\"", "matplotlib-inline; python_full_version >= \"3.12\"", - "pexpect; python_full_version >= \"3.12\" and (sys_platform != \"emscripten\" and sys_platform != \"win32\")", + "pexpect; (sys_platform != \"emscripten\" and sys_platform != \"win32\") and python_full_version >= \"3.12\"", "prompt-toolkit; python_full_version >= \"3.12\"", "pygments; python_full_version >= \"3.12\"", "stack-data; python_full_version >= \"3.12\"", @@ -669,70 +626,73 @@ files = [ [[package]] name = "madsci-client" -version = "0.7.0" -summary = "" +version = "0.8.0" +requires_python = ">=3.10.0" +summary = "The Modular Autonomous Discovery for Science (MADSci) Python Clients." dependencies = [ - "click", - "httpx", + "click>=8.1.7", + "httpx>=0.25.0", "madsci-common", - "minio", - "opentelemetry-api", - "rich", - "structlog", - "trogon", + "minio>=7.1.0", + "opentelemetry-api>=1.20.0", + "rich>=13.0.0", + "structlog>=24.1.0", + "trogon>=0.6.0", ] files = [ - {file = "madsci_client-0.7.0-py3-none-any.whl", hash = "sha256:aea70545929af0c07b46fcd35f639fc728be3cab78cd2049d5dfe7d8244f79d9"}, - {file = "madsci_client-0.7.0.tar.gz", hash = "sha256:d707383f607da7e16ea2e7a5ba42e6af6bc0b9beb355e8811f2a4e4cef21739b"}, + {file = "madsci_client-0.8.0-py3-none-any.whl", hash = "sha256:a8619e9375d4e2effe7934da8e7a447c43bd9e2be63f90fbd1ec5096b781639e"}, + {file = "madsci_client-0.8.0.tar.gz", hash = "sha256:db6ce05d5e9cf7d9e85844e1eb825962842784c82424bf2097eb3258a14bf9eb"}, ] [[package]] name = "madsci-common" -version = "0.7.0" -summary = "" +version = "0.8.0" +requires_python = ">=3.10.0" +summary = "The Modular Autonomous Discovery for Science (MADSci) Common Definitions and Utilities." dependencies = [ - "aenum", - "classy-fastapi", - "click", - "fastapi", - "httpx", - "multiprocess", - "opentelemetry-api", - "psycopg2-binary", - "pydantic", - "pydantic-extra-types", - "pydantic-settings", - "pydantic-settings-export", - "pymongo", - "python-dotenv", - "python-multipart", - "python-ulid[pydantic]", - "pyyaml", - "regex", - "requests", - "rich", - "semver", - "simpleeval", - "sqlmodel", - "uvicorn[standard]", -] -files = [ - {file = "madsci_common-0.7.0-py3-none-any.whl", hash = "sha256:4eb988c97709d55baa07d24a708e42567de6c75f9713b54e188bff000903adf8"}, - {file = "madsci_common-0.7.0.tar.gz", hash = "sha256:0bf3f155c3de19b85090ef2928ef4eb157405f096484fe7c27b5cc45a16daab6"}, + "PyYAML>=6.0.2", + "aenum>=3.1.15", + "classy-fastapi>=0.7.0", + "click>=8.1.0", + "fastapi>=0.115.4", + "httpx>=0.28.1", + "multiprocess>=0.70.17", + "opentelemetry-api>=1.20.0", + "psycopg2-binary>=2.9.0", + "pydantic-extra-types>=2.10.2", + "pydantic-settings-export>=1.0.2", + "pydantic-settings>=2.9.1", + "pydantic>=2.10", + "pymongo>=4.10.1", + "python-dotenv>=1.0.1", + "python-multipart>=0.0.17", + "python-ulid[pydantic]>=3.0.0", + "regex>=2025.7.34", + "requests>=2.32.3", + "rich>=13.0.0", + "semver>=3.0.4", + "simpleeval>=1.0.0", + "sqlmodel>=0.0.22", + "uvicorn[standard]>=0.32.0", +] +files = [ + {file = "madsci_common-0.8.0-py3-none-any.whl", hash = "sha256:f87d11b2554d242a33d6666faccd935f95e19d5454ce36a75fc4785e82e1ea6d"}, + {file = "madsci_common-0.8.0.tar.gz", hash = "sha256:26a4eccbf0194327e3c1fba9464e751346334e919b86a9d029ee4196f9800676"}, ] [[package]] name = "madsci-node-module" -version = "0.7.0" -summary = "" +version = "0.8.0" +requires_python = ">=3.10.0" +summary = "The Modular Autonomous Discovery for Science (MADSci) Node Module Helper Classes." dependencies = [ "madsci-client", "madsci-common", "regex", ] files = [ - {file = "madsci_node_module-0.7.0-py3-none-any.whl", hash = "sha256:744cc1cbf9e4eb698f4709f93039439d693788c9a4ce5d9bf0f022b2784c1a43"}, - {file = "madsci_node_module-0.7.0.tar.gz", hash = "sha256:1bdbf319c56de287e97feb605bc05308195323318db3a22f73081b4ddf21bc67"}, + {file = "madsci_node_module-0.8.0-py3-none-any.whl", hash = "sha256:cc014e5ce451d8a9572a41d2c85dcd50776eb2ac93d2d388b61be742c3e257bc"}, + {file = "madsci_node_module-0.8.0.tar.gz", hash = "sha256:22a1cc577db4730e8b9627652595ef4d3a12f8f09fc4216374e81073eb2e08f0"}, ] [[package]] @@ -948,7 +908,8 @@ files = [ [[package]] name = "psycopg2-binary" version = "2.9.11" -summary = "" +requires_python = ">=3.9" +summary = "psycopg2 - Python-PostgreSQL Database Adapter" files = [ {file = "psycopg2-binary-2.9.11.tar.gz", hash = "sha256:b6aed9e096bf63f9e75edf2581aa9a7e7186d97ab5c177aa6c87797cd591236c"}, {file = "psycopg2_binary-2.9.11-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:d6fe6b47d0b42ce1c9f1fa3e35bb365011ca22e39db37074458f27921dca40f2"}, @@ -984,6 +945,17 @@ files = [ {file = "psycopg2_binary-2.9.11-cp312-cp312-musllinux_1_2_riscv64.whl", hash = "sha256:edcb3aeb11cb4bf13a2af3c53a15b3d612edeb6409047ea0b5d6a21a9d744b34"}, {file = "psycopg2_binary-2.9.11-cp312-cp312-musllinux_1_2_x86_64.whl", hash = "sha256:62b6d93d7c0b61a1dd6197d208ab613eb7dcfdcca0a49c42ceb082257991de9d"}, {file = "psycopg2_binary-2.9.11-cp312-cp312-win_amd64.whl", hash = "sha256:b33fabeb1fde21180479b2d4667e994de7bbf0eec22832ba5d9b5e4cf65b6c6d"}, + {file = "psycopg2_binary-2.9.11-cp39-cp39-macosx_10_9_x86_64.whl", hash = "sha256:20e7fb94e20b03dcc783f76c0865f9da39559dcc0c28dd1a3fce0d01902a6b9c"}, + {file = "psycopg2_binary-2.9.11-cp39-cp39-macosx_11_0_arm64.whl", hash = "sha256:4bdab48575b6f870f465b397c38f1b415520e9879fdf10a53ee4f49dcbdf8a21"}, + {file = "psycopg2_binary-2.9.11-cp39-cp39-manylinux2014_aarch64.manylinux_2_17_aarch64.whl", hash = "sha256:9d3a9edcfbe77a3ed4bc72836d466dfce4174beb79eda79ea155cc77237ed9e8"}, + {file = "psycopg2_binary-2.9.11-cp39-cp39-manylinux2014_ppc64le.manylinux_2_17_ppc64le.whl", hash = "sha256:44fc5c2b8fa871ce7f0023f619f1349a0aa03a0857f2c96fbc01c657dcbbdb49"}, + {file = "psycopg2_binary-2.9.11-cp39-cp39-manylinux2014_x86_64.manylinux_2_17_x86_64.whl", hash = "sha256:9c55460033867b4622cda1b6872edf445809535144152e5d14941ef591980edf"}, + {file = "psycopg2_binary-2.9.11-cp39-cp39-manylinux_2_38_riscv64.manylinux_2_39_riscv64.whl", hash = "sha256:2d11098a83cca92deaeaed3d58cfd150d49b3b06ee0d0852be466bf87596899e"}, + {file = "psycopg2_binary-2.9.11-cp39-cp39-musllinux_1_2_aarch64.whl", hash = "sha256:691c807d94aecfbc76a14e1408847d59ff5b5906a04a23e12a89007672b9e819"}, + {file = "psycopg2_binary-2.9.11-cp39-cp39-musllinux_1_2_ppc64le.whl", hash = "sha256:8b81627b691f29c4c30a8f322546ad039c40c328373b11dff7490a3e1b517855"}, + {file = "psycopg2_binary-2.9.11-cp39-cp39-musllinux_1_2_riscv64.whl", hash = "sha256:b637d6d941209e8d96a072d7977238eea128046effbf37d1d8b2c0764750017d"}, + {file = "psycopg2_binary-2.9.11-cp39-cp39-musllinux_1_2_x86_64.whl", hash = "sha256:41360b01c140c2a03d346cec3280cf8a71aa07d94f3b1509fa0161c366af66b4"}, + {file = "psycopg2_binary-2.9.11-cp39-cp39-win_amd64.whl", hash = "sha256:875039274f8a2361e5207857899706da840768e2a775bf8c65e82f60b197df02"}, ] [[package]] @@ -1846,7 +1818,7 @@ dependencies = [ "python-dotenv", "pyyaml", "uvicorn==0.41.0", - "uvloop; platform_python_implementation != \"PyPy\" and (sys_platform != \"cygwin\" and sys_platform != \"win32\")", + "uvloop; (sys_platform != \"cygwin\" and sys_platform != \"win32\") and platform_python_implementation != \"PyPy\"", "watchfiles", "websockets", ] diff --git a/scripts/custom_tcs_server/Custom.gpl b/scripts/custom_tcs_server/Custom.gpl new file mode 100644 index 0000000..ddbdb6d --- /dev/null +++ b/scripts/custom_tcs_server/Custom.gpl @@ -0,0 +1,411 @@ +#Region "Module CustomCommands" +' Copyright (c) 2012 by Precise Automation Inc. All rights reserved. +' ======================================================================= +' Permission is granted to customers of Precise Automation to use this +' software for any purpose, including commercial applications, and to +' alter it and redistribute it freely, so long as this notice is included +' with any modified or unmodified version of this software. +' +' This software is provided "as is," without warranty of any kind, express or +' implied. In no event shall Precise Automation be held liable for any +' direct, indirect, incidental, special or consequential damages arising +' out of the use of or inability to use this software. +' ======================================================================= + +Module CustomCommands + +' Custom commands for RPL Argonne National Laboratory +' +' Commands implemented: +' +' JointToCart +' Forward kinematics (FK): converts joint angles to a Cartesian location. +' Rail offset is added to X so the result matches wherec world coordinates. +' Returns: X Y Z yaw pitch roll +' +' CartToJoint +' Inverse kinematics (IK): converts a Cartesian location to joint angles. +' Robot configuration is read from the robot automatically. +' Rail is passed explicitly and subtracted from X before IK. +' Returns: j1 j2 j3 j4 j5 rail +' +' RotateLoc +' Rotates the end effector by rotation_deg around the Z axis (yaw) at the +' given joint location. Internally runs FK, applies the rotation, then runs +' IK to compute the new joint angles. Useful for switching between narrow +' and wide microplate orientations without saving duplicate locations. +' Returns: j1 j2 j3 j4 j5 rail +' +' EnableCompliance +' Enables horizontal force compliance on the robot joints (XY plane). +' The arm will float/comply to horizontal reaction forces during motion. +' reduced_pct (0-100) sets a bias torque as a percentage of the last used +' position control torque to prevent axes from drifting or twitching. +' 0 = fully free (no bias torque), 100 = full holding torque (no compliance). +' Typical values: 0-20 for most pick/place operations. +' Always call DisableCompliance after the operation is complete. +' Returns: 0 on success, error code and message on failure +' +' DisableCompliance +' Disables horizontal force compliance and returns joints to normal +' position control mode. +' Returns: 0 on success, error code and message on failure + + Private Const Version As String = "RPL Custom Module 2.1" + Public Custom_global_value As Integer + Public Custom_robot_array(N_ROB) As Integer + +#End Region +#Region "Init" +' Init -- Module Initialization + + Public Sub Init() + Custom_global_value = 123 + Command.AddPlugin(Version, "CustomCommands") + End Sub + +#End Region +#Region "Hook_InitCommand" +' Hook_InitCommand -- Initialization routine called when a command object is created + + Public Sub Hook_InitCommand(ByVal Cmd As Command, ByRef Reply As String) + + If Cmd.Rob.RobNum = 0 Then + GoTo _exit ' No robot + End If + + Custom_robot_array(Cmd.Rob.RobNum) = Cmd.Rob.RobNum + 1000 + + _exit: + Return + End Sub + +#End Region +#Region "Cmd_JointToCart" +' Cmd_JointToCart -- Convert joint angles to Cartesian location (Forward Kinematics, FK) +' +' SYNOPSIS: +' JointToCart +' +' INPUTS: +' j1 Tower Z joint angle in degrees +' j2..j5 Remaining arm joint angles in degrees +' rail Rail position in mm (j6) +' +' OUTPUTS: +' Reply: "0 X Y Z yaw pitch roll" on success +' "-error_code, message" on failure +' +' NOTE: X includes the rail offset so the result matches wherec world coordinates. +' +' EXAMPLE: +' wherej -> 0 176.539 1.4 177.05 537.273 70.803 -967.609 +' Send: JointToCart 176.539 1.4 177.05 537.273 70.803 -967.609 +' Receive: 0 -811.497 24.550 176.539 -4.277 90.000 180.000 + + Public Sub Cmd_JointToCart(ByVal Cmd As Command, ByRef Reply As String) + + Dim joint_loc As New Location + Dim cart_loc As New Location + Dim rail_pos As Double + + If Cmd.Nparm <> 6 Then + Reply = "-1, JointToCart requires 6 parameters: j1 j2 j3 j4 j5 rail" + Return + End If + + Cmd.StringtoDouble(0) + + ' Build joint angle location from the 5 arm joints + joint_loc.Angles( _ + Cmd.cData(1), _ + Cmd.cData(2), _ + Cmd.cData(3), _ + Cmd.cData(4), _ + Cmd.cData(5) _ + ) + + rail_pos = Cmd.cData(6) + + ' Select robot so KineSol can use its kinematic model + Robot.Selected = Cmd.Rob.RobNum + + ' Forward kinematics: convert joint angles to Cartesian + ' mode=1 ignores conversion errors (joint out of range, etc.) + cart_loc = joint_loc.KineSol(1) + + ' Add rail offset to X to match wherec world coordinates + cart_loc.X = cart_loc.X + rail_pos + + Reply = "0 " & _ + Format(cart_loc.X, "0.000") & " " & _ + Format(cart_loc.Y, "0.000") & " " & _ + Format(cart_loc.Z, "0.000") & " " & _ + Format(cart_loc.Yaw, "0.000") & " " & _ + Format(cart_loc.Pitch, "0.000") & " " & _ + Format(cart_loc.Roll, "0.000") + + End Sub + +#End Region +#Region "Cmd_CartToJoint" +' Cmd_CartToJoint -- Convert Cartesian location to joint angles (Inverse Kinematics, IK) +' +' SYNOPSIS: +' CartToJoint +' +' INPUTS: +' X Y Z Cartesian position in mm (world coordinates, including rail offset) +' yaw pitch roll Orientation in degrees +' rail Rail position in mm (j6 from wherej), used to remove rail offset from X +' +' NOTE: Robot configuration is read automatically from Robot.Where.Config. +' Rail is subtracted from X before IK so the kinematic model works in arm-local space. +' Rail is returned unchanged as the last value in the output. +' +' OUTPUTS: +' Reply: "0 j1 j2 j3 j4 j5 rail" on success +' "-error_code, message" on failure +' +' EXAMPLE: +' wherec -> 0 434.606 365.118 357.127 -4.839 90 180 1 +' wherej -> 0 357.127 13.669 84.86 616.632 70.785 40.839 +' Send: CartToJoint 434.606 365.118 357.127 -4.839 90 180 40.839 +' Receive: 0 357.127 13.669 84.860 616.632 70.785 40.839 + + Public Sub Cmd_CartToJoint(ByVal Cmd As Command, ByRef Reply As String) + + Dim cart_loc As New Location + Dim joint_loc As New Location + Dim rail_pos As Double + + If Cmd.Nparm <> 7 Then + Reply = "-1, CartToJoint requires 7 parameters: X Y Z yaw pitch roll rail" + Return + End If + + Cmd.StringtoDouble(0) + + rail_pos = Cmd.cData(7) + + ' Build Cartesian location by setting each property individually + ' Subtract rail offset from X so KineSol works in arm-local coordinates + cart_loc.X = Cmd.cData(1) - rail_pos + cart_loc.Y = Cmd.cData(2) + cart_loc.Z = Cmd.cData(3) + cart_loc.Yaw = Cmd.cData(4) + cart_loc.Pitch = Cmd.cData(5) + cart_loc.Roll = Cmd.cData(6) + + ' Read config from the robot's current position + cart_loc.Config = Robot.Where.Config + + ' Select robot so KineSol can use its kinematic model + Robot.Selected = Cmd.Rob.RobNum + + ' Build a 5-axis joint hint from the robot's current position + ' This is passed to KineSol so the solver stays close to the current + ' configuration and avoids jumping to equivalent but distant solutions + ' for multi-turn axes like j4. Rail axis is excluded as KineSol only + ' works on the 5 arm axes. + Dim hint_loc As New Location + hint_loc.Angles( _ + Robot.WhereAngles.Angle(1), _ + Robot.WhereAngles.Angle(2), _ + Robot.WhereAngles.Angle(3), _ + Robot.WhereAngles.Angle(4), _ + Robot.WhereAngles.Angle(5) _ + ) + + ' Inverse kinematics: convert Cartesian to joint angles + ' Pass hint so solver stays close to current configuration + joint_loc = cart_loc.KineSol(1, hint_loc) + + Reply = "0 " & _ + Format(joint_loc.Angle(1), "0.000") & " " & _ + Format(joint_loc.Angle(2), "0.000") & " " & _ + Format(joint_loc.Angle(3), "0.000") & " " & _ + Format(joint_loc.Angle(4), "0.000") & " " & _ + Format(joint_loc.Angle(5), "0.000") & " " & _ + Format(rail_pos, "0.000") + + End Sub + +#End Region +#Region "Cmd_RotateLoc" +' Cmd_RotateLoc -- Rotate end effector at a joint location by a given angle (FK + IK) +' +' Internally this command runs forward kinematics (FK) to convert the joint angles +' to Cartesian space, applies the rotation to the yaw axis, then runs inverse +' kinematics (IK) to return the new joint angles. This allows switching between +' narrow and wide microplate orientations without saving duplicate locations. +' +' SYNOPSIS: +' RotateLoc +' +' INPUTS: +' j1 Tower Z joint angle in degrees +' j2..j5 Remaining arm joint angles in degrees +' rail Rail position in mm (j6) +' rotation_deg Yaw rotation to apply in degrees (e.g. 90 or -90) +' +' OUTPUTS: +' Reply: "0 j1 j2 j3 j4 j5 rail" on success +' "-error_code, message" on failure +' +' EXAMPLE: +' wherej -> 0 176.539 1.4 177.05 537.273 70.803 -967.609 +' Send: RotateLoc 176.539 1.4 177.05 537.273 70.803 -967.609 90 +' Receive: 0 176.539 1.400 177.050 447.273 70.803 -967.609 + + Public Sub Cmd_RotateLoc(ByVal Cmd As Command, ByRef Reply As String) + + Dim joint_loc As New Location + Dim cart_loc As New Location + Dim result_loc As New Location + Dim rail_pos As Double + Dim rot_deg As Double + Dim new_yaw As Double + + If Cmd.Nparm <> 7 Then + Reply = "-1, RotateLoc requires 7 parameters: j1 j2 j3 j4 j5 rail rotation_deg" + Return + End If + + Cmd.StringtoDouble(0) + + ' Build joint angle location from the 5 arm joints + joint_loc.Angles( _ + Cmd.cData(1), _ + Cmd.cData(2), _ + Cmd.cData(3), _ + Cmd.cData(4), _ + Cmd.cData(5) _ + ) + + rail_pos = Cmd.cData(6) + rot_deg = Cmd.cData(7) + + ' Select robot so KineSol can use its kinematic model + Robot.Selected = Cmd.Rob.RobNum + + ' Step 1: Forward kinematics, convert joint angles to Cartesian + cart_loc = joint_loc.KineSol(1) + + ' Add rail offset to X to work in world coordinates + cart_loc.X = cart_loc.X + rail_pos + + ' Step 2: Apply yaw rotation and normalize to -180 to 180 range + new_yaw = cart_loc.Yaw + rot_deg + If new_yaw > 180.0 Then + new_yaw = new_yaw - 360.0 + End If + If new_yaw < -180.0 Then + new_yaw = new_yaw + 360.0 + End If + cart_loc.Yaw = new_yaw + + ' Step 3: Inverse kinematics, convert rotated Cartesian back to joint angles + ' Subtract rail offset from X so KineSol works in arm-local coordinates + ' Pass input joint angles as hint so solver stays close to original configuration + ' This prevents j4 (and other multi-turn axes) from jumping to an equivalent + ' but physically distant solution + cart_loc.X = cart_loc.X - rail_pos + cart_loc.Config = Robot.Where.Config + result_loc = cart_loc.KineSol(1, joint_loc) + + Reply = "0 " & _ + Format(result_loc.Angle(1), "0.000") & " " & _ + Format(result_loc.Angle(2), "0.000") & " " & _ + Format(result_loc.Angle(3), "0.000") & " " & _ + Format(result_loc.Angle(4), "0.000") & " " & _ + Format(result_loc.Angle(5), "0.000") & " " & _ + Format(rail_pos, "0.000") + + End Sub + +#Region "Cmd_EnableCompliance" +' Cmd_EnableCompliance -- Enable horizontal force compliance on robot joints +' +' Enables AC_HorizCompliance2 mode which allows the horizontal arm axes to +' float and comply to reaction forces while other axes continue to be driven +' normally. Use this before descending into a pick or place location where +' the plate may be slightly misaligned or stuck. +' +' The reduced_pct parameter sets a bias torque as a percentage of the last +' used position control torque to prevent axes from drifting or twitching +' when compliance is first enabled: +' 0 = fully free, no bias torque (maximum compliance) +' 100 = full holding torque (effectively no compliance) +' 10-20 is recommended for most pick/place operations +' +' Requires the XY Compliance license to be installed on the controller. +' +' SYNOPSIS: +' EnableCompliance +' +' INPUTS: +' reduced_pct Bias torque percentage 0-100 +' +' OUTPUTS: +' Reply: "0" on success +' "-error_code, message" on failure + + Public Sub Cmd_EnableCompliance(ByVal Cmd As Command, ByRef Reply As String) + + Dim exc1 As Exception + Dim reduced_pct As Double + + If Cmd.Nparm <> 1 Then + Reply = "-1, EnableCompliance requires 1 parameter: reduced_pct (0-100)" + Return + End If + + Cmd.StringtoDouble(0) + reduced_pct = Cmd.cData(1) + + If reduced_pct < 0 Or reduced_pct > 100 Then + Reply = "-1, EnableCompliance: reduced_pct must be between 0 and 100" + Return + End If + + exc1 = AC_HorizCompliance2(Cmd.Rob.RobNum, True, reduced_pct) + If exc1.ErrorCode < 0 Then + Reply = CStr(exc1.ErrorCode) & ", " & AC_ErrorMessage(exc1) + Else + Reply = "0" + End If + + End Sub + +#End Region +#Region "Cmd_DisableCompliance" +' Cmd_DisableCompliance -- Disable horizontal force compliance on robot joints +' +' Disables AC_HorizCompliance2 mode and returns the horizontal arm axes to +' normal position control mode. Always call this after EnableCompliance +' once the pick or place operation is complete. +' +' SYNOPSIS: +' DisableCompliance +' +' OUTPUTS: +' Reply: "0" on success +' "-error_code, message" on failure + + Public Sub Cmd_DisableCompliance(ByVal Cmd As Command, ByRef Reply As String) + + Dim exc1 As Exception + + exc1 = AC_HorizCompliance2(Cmd.Rob.RobNum, False, 0) + If exc1.ErrorCode < 0 Then + Reply = CStr(exc1.ErrorCode) & ", " & AC_ErrorMessage(exc1) + Else + Reply = "0" + End If + + End Sub + +#End Region + +End Module diff --git a/scripts/custom_tcs_server/TCS_Instructions.md b/scripts/custom_tcs_server/TCS_Instructions.md new file mode 100644 index 0000000..78524dd --- /dev/null +++ b/scripts/custom_tcs_server/TCS_Instructions.md @@ -0,0 +1,152 @@ +# Deploying Custom.gpl to the PF400 TCS Server + +This document describes how to update the PF400 robot's TCS server with a new version of `Custom.gpl`. +`Custom.gpl` adds custom commands to the TCS server for kinematics, force compliance, and height detection. + +## Prerequisites + +- Network access to `rplpf400.cels.anl.gov` +- `lftp` or FileZilla installed on your machine +- The robot's TCS server running on port 10100 + +## Steps + +### 1. Back up the existing file (first time only) + +```bash +lftp ftp://rplpf400.cels.anl.gov +cd /flash/projects/Tcp_cmd_server_pa +get Custom.gpl -o Custom.gpl.bak +quit +``` + +### 2. Upload the new Custom.gpl + +**Via lftp:** + +```bash +lftp ftp://rplpf400.cels.anl.gov +set ftp:passive-mode off +cd /flash/projects/Tcp_cmd_server_pa +put path/to/Custom.gpl +quit +``` + +**Via FileZilla:** + +- Host: `rplpf400.cels.anl.gov`, Port: `21`, leave username and password blank +- Navigate to `/flash/projects/Tcp_cmd_server_pa` in the right panel +- Drag `Custom.gpl` from your local machine into that folder + +### 3. Reload the project on the robot + +- Open the robot web interface at `http://rplpf400.cels.anl.gov` +- Navigate to **Setup > Load/Compile Project** +- Select `Tcp_cmd_server_pa` and click **Load** +- Wait for the confirmation that the project compiled and started successfully + +### 4. Verify the deployment + +Connect via telnet and check that the new module version appears: + +```bash +telnet rplpf400.cels.anl.gov 10100 +``` + +Then send: + +``` +version +``` + +Expected output should include `RPL Custom Module 2.1`. If you see an older version number the project did not reload correctly — repeat step 3. + +### 5. Test the commands + +With the robot powered and attached, run a quick sanity check for each command group. + +**Kinematics:** + +``` +wherej +``` + +Take the 6 joint values from the response (strip the leading `0`) and run: + +``` +JointToCart +``` + +Compare the output against `wherec` — X, Y, Z, yaw, pitch, roll should match within rounding (~0.01mm). + +**Force Compliance** (requires XY Compliance license): + +``` +EnableCompliance 0 +DisableCompliance +``` + +Both should return `0`. When compliance is enabled the horizontal arm axes should feel loose when pushed by hand. + +**Height Detection** (requires Z Height Detection license): + +Position the gripper at least 20mm above a surface, then: + +``` +HeightDetect 2 -400 -15 +``` + +Should return `0 ` rather than a license error. + +## Rolling Back + +If the new `Custom.gpl` causes issues, restore the backup and reload: + +```bash +lftp ftp://rplpf400.cels.anl.gov +set ftp:passive-mode off +cd /flash/projects/Tcp_cmd_server_pa +put Custom.gpl.bak Custom.gpl +quit +``` + +Then reload the project following step 3 above. + +## Custom Commands Reference + +All responses are prefixed with `0` on success (standard TCS status code) or a negative error code and message on failure. + +### Kinematics + +| Command | Arguments | Returns | Description | +|---|---|---|---| +| `JointToCart` | `j1 j2 j3 j4 j5 rail` | `X Y Z yaw pitch roll` | Forward kinematics (FK): convert joint angles to Cartesian coordinates. Rail offset is added to X to match world coordinates. | +| `CartToJoint` | `X Y Z yaw pitch roll rail` | `j1 j2 j3 j4 j5 rail` | Inverse kinematics (IK): convert Cartesian coordinates to joint angles. Rail is subtracted from X before IK and returned unchanged. | +| `RotateLoc` | `j1 j2 j3 j4 j5 rail rotation_deg` | `j1 j2 j3 j4 j5 rail` | Rotate end effector yaw by rotation_deg degrees. Internally runs FK, applies rotation, then IK. Use to switch between narrow and wide plate orientations. | + +### Force Compliance + +Requires XY Compliance license installed on the controller. + +| Command | Arguments | Returns | Description | +|---|---|---|---| +| `EnableCompliance` | `bias_torque_pct` | `0` | Enable horizontal force compliance. `bias_torque_pct` (0-100) sets a bias torque percentage to prevent axis drift. 0 = fully free, typical values 0-20. | +| `DisableCompliance` | none | `0` | Disable horizontal force compliance and return to normal position control. Always call after EnableCompliance. | + +### Height Detection + +Requires Z Height Detection license installed on the controller. Position the gripper at least 20mm above the surface before calling. + +| Command | Arguments | Returns | Description | +|---|---|---|---| +| `HeightDetect` | `mode search_limit_mm max_force_n` | `detected_Z_mm` | Detect surface height. mode: 1=quick (0.5mm), 2=thorough (0.3mm). search_limit_mm: max downward travel (negative). max_force_n: contact force limit (negative). | + +## License Requirements + +| Feature | Required License | +|---|---| +| Kinematics (JointToCart, CartToJoint, RotateLoc) | GPL license (already installed) | +| Force Compliance (EnableCompliance, DisableCompliance) | XY Compliance | +| Height Detection (HeightDetect) | Z Height Detection | + +To check installed licenses, navigate to **Utilities > Controller Options** in the robot web interface. diff --git a/src/pf400_interface/pf400.py b/src/pf400_interface/pf400.py index 23befa5..f10ed1b 100644 --- a/src/pf400_interface/pf400.py +++ b/src/pf400_interface/pf400.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -"""Driver code for the PF400 robot arm.""" +"""Interface code for the PF400 robot arm.""" import copy import telnetlib @@ -20,11 +20,10 @@ Pf400ConnectionError, Pf400ResponseError, ) -from pf400_interface.pf400_kinematics import KINEMATICS -class PF400(KINEMATICS): - """Main Driver Class for the PF400 Robot Arm.""" +class PF400: + """Main Interface Class for the PF400 Robot Arm.""" slow_motion_profile = 1 fast_motion_profile = 2 @@ -40,6 +39,8 @@ class PF400(KINEMATICS): safe_left_boundary = -350.0 safe_right_boundary = 350.0 + default_bias_torque_pct: int = 50 + gripper_clearance_height = 110.0 default_approach_height = 15.0 default_approach_vector: typing.ClassVar[list] = [ default_approach_height, @@ -49,7 +50,7 @@ class PF400(KINEMATICS): 0.0, 0.0, ] - + default_lid_height = 7.0 movement_state = 0 robot_connection = None @@ -79,9 +80,7 @@ def __init__( - Programs are sent to the 10x00 port (first robot port: 10100). - A program sent to robot will be executed immediately unless there is a prior operation running on the robot. - If a second motion command is sent while the referenced robot is moving, the second command is blocked and will not reply until the first motion is complete. - """ - super().__init__() # PF400 kinematics self.logger = logger or EventClient() self.host = host self.port = port @@ -92,31 +91,23 @@ def __init__( self.gripper_resource_id = gripper_resource_id self.command_lock = Lock() self.status_lock = Lock() - # Initialize robot self.connect() - self.configure_robot() - # Plate variables + self._configure_robot() - # Initialize neutral_joints as an instance attribute self.neutral_joints = [ 400.0, - 1.400, - 177.101, - 537.107, + 1.650, + 177.662, + -179.494, self.gripper_close_narrow, 0.0, ] self.set_gripper_open() self.set_gripper_close() - self.logger.warn( - "HARD CODED ROTATION LOCATION IS DEPRECATED, USE rotation_deck WITH TRANSFER METHOD INSTEAD" - ) def connect(self) -> None: - """ - Description: Create a streaming socket to send string commands to the robot using telnetlib3. - """ + """Create a streaming socket to send string commands to the robot using telnetlib.""" try: self.robot_connection = telnetlib.Telnet(self.host, self.port, 5) # noqa: S312 self.status_connection = telnetlib.Telnet(self.host, self.status_port, 5) # noqa: S312 @@ -125,11 +116,11 @@ def connect(self) -> None: err_message=f"Failed to connect using telnetlib: {e}" ) from e - def configure_robot(self) -> None: + def _configure_robot(self) -> None: """Configures the robot by setting the mode and selecting the robot ID.""" - self.send_robot_command(f"mode {self.mode}") + self.send_command(f"mode {self.mode}") self.send_status_command(f"mode {self.mode}") - self.send_robot_command(f"selectRobot {self.robot_id}") + self.send_command(f"selectRobot {self.robot_id}") self.send_status_command(f"selectRobot {self.robot_id}") def disconnect(self) -> None: @@ -141,7 +132,7 @@ def disconnect(self) -> None: self.status_connection.close() self.status_connection = None - def send_robot_command(self, command: str) -> str: + def send_command(self, command: str) -> str: """ Sends a command to the robot and return the response. @@ -156,8 +147,7 @@ def send_robot_command(self, command: str) -> str: Returns: str: The response received from the robot. - - Raises: + Raises: Pf400ConnectionError: If no connection to the robot can be established. Pf400CommandError: If an AttributeError occurs during command execution. """ @@ -172,10 +162,10 @@ def send_robot_command(self, command: str) -> str: .rstrip("\r\n") ) if response != "" and response in ERROR_CODES: - self.handle_error_output(response) + self._handle_error_output(response) if response in OUTPUT_CODES: self.logger.log_debug(response) - self.await_movement_completion() + self._await_movement_completion() return response except AttributeError as e: raise Pf400CommandError(err_message="Attribute Error") from e @@ -184,10 +174,6 @@ def send_status_command(self, command: str) -> str: """ Sends a status command to the PF400 device and returns the response. - This method ensures thread-safe access using a lock, establishes a connection if needed, - writes the command to the status writer, and reads the response. It handles error and output - codes appropriately, logging or raising exceptions as necessary. - Args: command (str): The command string to send to the PF400 device. @@ -209,98 +195,70 @@ def send_status_command(self, command: str) -> str: .rstrip("\r\n") ) if response != "" and response in ERROR_CODES: - self.handle_error_output(response) + self._handle_error_output(response) if response in OUTPUT_CODES: self.logger.log_debug(response) return response except AttributeError as e: raise Pf400CommandError(err_message="Attribute Error") from e - def handle_error_output(self, output: str) -> None: - """ - Description: Handles the error message output - """ + def _parse_response(self, response: str) -> list[float]: + """Parse a TCS response string into a list of floats, stripping the leading status code.""" + parts = response.split(" ") + return [float(x) for x in parts[1:]] + + def _handle_error_output(self, output: str) -> None: + """Handles the error message output.""" response = Pf400ResponseError.from_error_code(output) self.logger.log_error(response) raise response def enable_power(self) -> str: - """ - Description: Enables the power on the robot - """ - return self.send_robot_command("hp 1 -1") + """Enables the power on the robot.""" + return self.send_command("hp 1 -1") def disable_power(self) -> str: - """ - Description: Disables the power on the robot - """ - return self.send_robot_command("hp 0") + """Disables the power on the robot.""" + return self.send_command("hp 0") - def split_response(self, response: str) -> list[str]: - """ - Description: Splits the response string into a list of strings. - Parameters: - - response: The response string to be split. - Returns: A list of strings. - """ + def _split_response(self, response: str) -> list[str]: + """Splits the response string into a list of strings.""" return response.split(" ") if response else [] def check_powered(self) -> bool: - """ - Description: Checks whether the robot power is on or off. - Returns: bool indicating whether the robot is powered on. - """ - self.power_state = self.split_response(self.send_status_command("hp"))[1] + """Checks whether the robot power is on or off.""" + self.power_state = self._split_response(self.send_status_command("hp"))[1] return self.power_state == "1" def check_attached(self) -> bool: - """ - Description: Checks whether the robot is attached or not. - Returns: bool indicating whether the robot is attached. - """ - self.attach_state = self.split_response(self.send_robot_command("attach"))[1] + """Checks whether the robot is attached or not.""" + self.attach_state = self._split_response(self.send_command("attach"))[1] return self.attach_state == "1" def check_homed(self) -> bool: - """ - Description: Checks whether the robot is homed or not. - Returns: bool indicating whether the robot is homed. - """ - self.home_state = self.split_response(self.send_status_command("pd 2800"))[1] + """Checks whether the robot is homed or not.""" + self.home_state = self._split_response(self.send_status_command("pd 2800"))[1] return self.home_state == "1" def check_system_state(self) -> str: - """ - Description: Checks the global system state code - Returns: The system state code as a string. - """ - self.system_state = self.send_robot_command("sysState") + """Checks the global system state code.""" + self.system_state = self.send_command("sysState") return self.system_state def attach_robot(self) -> str: - """ - Description: Attach to the robot to enable motion commands. - """ - return self.send_robot_command("attach 1") + """Attach to the robot to enable motion commands.""" + return self.send_command("attach 1") def detach_robot(self) -> str: - """ - Description: Detach from the robot to disable motion commands. - """ - return self.send_robot_command("attach 0") + """Detach from the robot to disable motion commands.""" + return self.send_command("attach 0") def home_robot(self) -> str: - """ - Description: Homes robot joints. Homing takes around 15 seconds. - """ - - return self.send_robot_command("home") + """Homes robot joints. Homing takes around 15 seconds.""" + return self.send_command("home") def initialize_robot(self) -> None: - """ - Description: Initializes the robot by calling enable_power, attach_robot, home_robot, set_profile functions and checks the robot state to find out if the initialization was successful - """ - + """Initializes the robot by calling enable_power, attach_robot, home_robot, set_profile functions.""" self.check_state() retry_count = 0 while self.power_state != "1" and retry_count < 5: @@ -327,70 +285,55 @@ def initialize_robot(self) -> None: self.get_robot_movement_state() def get_robot_movement_state(self) -> int: - """Checks the movement state of the robot - States: 0 = Power off - 1 = Stopped - 2 = Acceleration - 3 = Deceleration + """Checks the movement state of the robot. + + States: 0 = Power off, 1 = Stopped, 2 = Acceleration, 3 = Deceleration """ movement_state = self.send_status_command("state") self.movement_state = int(float(movement_state.split(" ")[1])) return self.movement_state - def await_movement_completion(self) -> None: - """Waits until the robot has finished moving""" + def _await_movement_completion(self) -> None: + """Waits until the robot has finished moving.""" while True: if self.get_robot_movement_state() <= 1: return time.sleep(0.1) def check_state(self) -> int: - """ - Description: Checks the various state values of the robot and returns False if any of the states are not initialized correctly. - """ - + """Checks the various state values of the robot.""" try: is_powered = self.check_powered() is_attached = self.check_attached() is_homed = self.check_homed() system_state = self.check_system_state() - system_state_ok = self.split_response(system_state)[1] == "21" + system_state_ok = self._split_response(system_state)[1] == "21" return is_powered and is_attached and is_homed and system_state_ok except Exception as e: self.logger.log_info(f"Exception during state check: {e}") return False def get_joint_states(self) -> list[float]: - """ - Description: Locates the robot and returns the joint locations for all 6 joints. - """ - states = self.send_robot_command("wherej") + """Locates the robot and returns the joint locations for all 6 joints.""" + states = self.send_command("wherej") joints = states.split(" ") joints = joints[1:] return [float(x) for x in joints] def get_cartesian_coordinates(self) -> list[float]: - """ - Description: This function finds the current cartesian coordinates and angles of the robot. - Return: A float array with x/y/z yaw/pitch/roll - """ - coordinates = self.send_robot_command("whereC") + """Returns the current Cartesian coordinates of the robot as [X, Y, Z, yaw, pitch, roll].""" + coordinates = self.send_command("whereC") coordinates_list = coordinates.split(" ") coordinates_list = coordinates_list[1:-1] return [float(x) for x in coordinates_list] - def get_gripper_position(self) -> float: + def get_gripper_state(self) -> float: """Returns the current position of the gripper.""" joint_angles = self.get_joint_states() return joint_angles[4] def set_profile(self, profile_dict: Optional[dict] = None) -> str: - """ - Description: Sets and saves the motion profiles (defined in robot data) to the robot. - If user defines a custom profile, this profile will saved onto motion profile 4 on the robot - Parameters: - - profile_dict: Custom motion profile - """ + """Sets and saves the motion profiles to the robot.""" if profile_dict is None: profile1 = "Profile 1" for value in MOTION_PROFILES[0].values(): @@ -401,16 +344,14 @@ def set_profile(self, profile_dict: Optional[dict] = None) -> str: profile3 = "Profile 3" for value in MOTION_PROFILES[2].values(): profile3 += " " + str(value) - - self.send_robot_command(profile1) - self.send_robot_command(profile2) - out_msg = self.send_robot_command(profile3) - + self.send_command(profile1) + self.send_command(profile2) + out_msg = self.send_command(profile3) elif len(profile_dict) == 8: profile4 = "Profile 4" for value in profile_dict.values(): profile4 += " " + str(value) - out_msg = self.send_robot_command(profile4) + out_msg = self.send_command(profile4) else: raise Exception( f"Motion profile takes 8 arguments, {len(profile_dict)} where given" @@ -429,33 +370,21 @@ def gripper_close(self) -> int: def set_gripper_open(self, gripper_length: Optional[int] = None) -> None: """Configure the definition of gripper open.""" - self.send_robot_command(f"GripOpenPos {gripper_length or self.gripper_open}") + self.send_command(f"GripOpenPos {gripper_length or self.gripper_open}") def set_gripper_close(self, gripper_length: Optional[int] = None) -> None: """Configure the definition of gripper close.""" - self.send_robot_command(f"GripClosePos {gripper_length or self.gripper_close}") + self.send_command(f"GripClosePos {gripper_length or self.gripper_close}") def grab_plate( self, width: Optional[int] = None, speed: int = 100, force: int = 10 ) -> bool: - """ - Description: - Grabs the plate by applying additional force - Parameters: - - width: Plate width, in mm. Should be accurate to within about 1 mm. - - speed: Percent speed to open fingers. 1 to 100. - - Force: Maximum gripper squeeze force, in Nt. - A positive value indicates the fingers must close to grasp. - A negative value indicates the fingers must open to grasp. - Returns: - True if the plate was successfully grabbed, False otherwise. - """ + """Grabs the plate by applying additional force.""" if width is None: width = self.gripper_close - grab_plate_status = self.send_robot_command( + grab_plate_status = self.send_command( f"GraspPlate {width} {speed} {force}" ).split(" ") - if grab_plate_status[1] == "0": return False if grab_plate_status[1] == "-1": @@ -468,23 +397,12 @@ def grab_plate( ) def release_plate(self, width: Optional[int] = None, speed: int = 100) -> bool: - """ - Description: - Release the plate - Parameters: - - width: Open width, in mm. Larger than the widest corners of the plates. - If None, uses the default gripper_open value based on grip_wide setting. - - speed: Percent speed to open fingers. 1 to 100. - Returns: - True if the gripper successfully opened to the target width, False otherwise. - """ + """Release the plate.""" if width is None: width = self.gripper_open - - release_plate_status = self.send_robot_command( - f"ReleasePlate {width} {speed}" - ).split(" ") - + release_plate_status = self.send_command(f"ReleasePlate {width} {speed}").split( + " " + ) if release_plate_status[0] != "0": self.logger.log_error( f"Unexpected response from ReleasePlate: {release_plate_status[0]}" @@ -492,80 +410,212 @@ def release_plate(self, width: Optional[int] = None, speed: int = 100) -> bool: raise Pf400ResponseError( f"Unexpected response from ReleasePlate command: {release_plate_status[0]}." ) - - # Verify gripper opened to target width - current_gripper_position = self.get_gripper_position() - + current_gripper_position = self.get_gripper_state() if abs(current_gripper_position - width) <= 5: return True - self.logger.log_error( f"Gripper failed to open to target width. Expected: {width}, Got: {current_gripper_position}" ) return False def open_gripper(self, gripper_length: Optional[int] = None) -> float: - """Opens the gripper""" + """Opens the gripper.""" self.set_gripper_open(gripper_length=gripper_length) - self.send_robot_command("gripper 1") - return self.get_gripper_position() + self.send_command("gripper 1") + return self.get_gripper_state() def close_gripper(self, gripper_length: Optional[int] = None) -> float: - """Closes the gripper""" + """Closes the gripper.""" self.set_gripper_close(gripper_length=gripper_length) - self.send_robot_command("gripper 2") - return self.get_gripper_position() + self.send_command("gripper 2") + return self.get_gripper_state() + + # ------------------------------------------------------------------------- + # Kinematics -- implemented via custom TCS server commands (Custom.gpl) + # ------------------------------------------------------------------------- + + def joint_to_cart(self, joint_states: list[float]) -> list[float]: + """Forward kinematics (FK): convert joint angles to Cartesian coordinates. + + Calls the JointToCart custom TCS command which uses the robot's internal + KineSol method. The rail offset is handled automatically inside the command. - def set_plate_rotation( - self, joint_states: list[float], rotation_degree: float = 0 + Args: + joint_states: 6 joint values [j1, j2, j3, j4, j5, rail] + + Returns: + Cartesian coordinates as [X, Y, Z, yaw, pitch, roll] + """ + j1, j2, j3, j4, j5, rail = joint_states + response = self.send_command(f"JointToCart {j1} {j2} {j3} {j4} {j5} {rail}") + return self._parse_response(response) + + def cart_to_joint( + self, cartesian_coordinates: list[float], rail: float ) -> list[float]: + """Inverse kinematics (IK): convert Cartesian coordinates to joint angles. + + Calls the CartToJoint custom TCS command which uses the robot's internal + KineSol method. The rail position must be passed explicitly so the command + can subtract it from X before running IK, then return it as j6. + + Args: + cartesian_coordinates: [X, Y, Z, yaw, pitch, roll] in world coordinates + rail: Rail position in mm (j6 from wherej) + + Returns: + Joint angles as [j1, j2, j3, j4, j5, rail] """ - Description: - Parameters: - - joint_states: - - rotation_degree: - Note: If the rotation requires changing the "Quadrant" on the coordinate plane, - inverse kinematics calculation will be calculated wrong! + x, y, z, yaw, pitch, roll = cartesian_coordinates + response = self.send_command( + f"CartToJoint {x} {y} {z} {yaw} {pitch} {roll} {rail}" + ) + return self._parse_response(response) + + def rotate_yaw(self, joint_states: list[float], rotation_deg: float) -> list[float]: + """Rotate the end effector yaw at a given joint location. + + Calls the RotateLoc custom TCS command which internally runs FK, applies + the yaw rotation, then runs IK to return the new joint angles. Use this + to switch between narrow and wide microplate orientations without saving + duplicate locations. + + Args: + joint_states: 6 joint values [j1, j2, j3, j4, j5, rail] + rotation_deg: Yaw rotation to apply in degrees, typically 90 or -90 + + Returns: + New joint angles as [j1, j2, j3, j4, j5, rail] """ - cartesian_coordinates, phi_angle, rail_pos = self.forward_kinematics( - joint_states + j1, j2, j3, j4, j5, rail = joint_states + response = self.send_command( + f"RotateLoc {j1} {j2} {j3} {j4} {j5} {rail} {rotation_deg}" ) - # Fixing the orientation offset here - if rotation_degree == -90: # Yaw 90 to 0 degrees: - cartesian_coordinates[1] += 4 - cartesian_coordinates[0] += 29 - elif rotation_degree == 90: - cartesian_coordinates[1] -= 4 - cartesian_coordinates[0] -= 29 - - if cartesian_coordinates[1] < 0: - # Location is on the right side of the robot - cartesian_coordinates[3] += rotation_degree - elif cartesian_coordinates[1] > 0 and joint_states[1]: - cartesian_coordinates[3] -= rotation_degree - - return self.inverse_kinematics(cartesian_coordinates, phi_angle, rail_pos) + return self._parse_response(response) - def check_incorrect_plate_orientation( - self, goal_location: list[float], goal_rotation: list[float] - ) -> list[float]: + def move_with_rotation( + self, + joint_states: list[float], + rotation_deg: float, + profile: int = 2, + ) -> str: + """Move the end effector to a location with a yaw rotation applied. + + Computes the rotated Cartesian location using FK and the given rotation, + then moves to it using MoveC (straight line Cartesian motion). This avoids + IK ambiguity by letting the robot's internal motion controller handle the + joint configuration along the straight line path. + + Args: + joint_states: 6 joint values [j1, j2, j3, j4, j5, rail] + rotation_deg: Yaw rotation to apply in degrees, typically 90 or -90 + profile: Motion profile index, defaults to fast profile (2) + + Returns: + Robot response string + """ + cart = self.joint_to_cart(joint_states) + cart[3] += rotation_deg + # Normalize yaw to -180 to 180 + if cart[3] > 180: + cart[3] -= 360 + elif cart[3] < -180: + cart[3] += 360 + return self.move_cartesian(cart, profile=profile) + + # ------------------------------------------------------------------------- + # Force Compliance -- implemented via custom TCS server commands (Custom.gpl) + # Requires XY Compliance license on the controller. + # ------------------------------------------------------------------------- + + def enable_compliance(self) -> str: + """Enable horizontal force compliance on the robot joints. + + Allows the horizontal arm axes to float and comply to reaction forces + while other axes continue to be driven normally. Use before descending + into a pick or place location where the plate may be slightly misaligned + or stuck. Always call disable_compliance() after the operation. + + Args: + bias_torque_pct: Bias torque as a percentage of last used position control + torque (0-100). 0 = fully free (maximum compliance), 100 = full + holding torque (no compliance). Typical values: 0-20 for most + pick/place operations. + + Returns: + Robot response string """ - Description: Fixes plate rotation on the goal location if it was recorded with an incorrect orientation. - Parameters: - - goal_location - - goal_rotation - Return: - goal_location: - - New goal location if the incorrect orientation was found. - - Same goal location if there orientation was correct. + return self.send_command(f"EnableCompliance {self.default_bias_torque_pct}") + + def disable_compliance(self) -> str: + """Disable horizontal force compliance and return to normal position control. + + Always call this after enable_compliance() once the pick or place + operation is complete. + + Returns: + Robot response string """ - # This will fix plate rotation on the goal location if it was recorded with an incorrect orientation - cartesian_goal, _phi_source, _rail_source = self.forward_kinematics( - goal_location + return self.send_command("DisableCompliance") + + # ------------------------------------------------------------------------- + # Height Detection -- implemented via TCS PARobot Auto Center module + # Requires Z Height Detection license on the controller. + # ------------------------------------------------------------------------- + + def height_detect( + self, + search_limit_mm: float = -500, + max_force_n: float = -15, + thorough: bool = True, + ) -> float: + """Detect the height of a surface below the gripper using motor force sensing. + + The gripper must be positioned at least 10-20mm above the surface before + calling. The robot will descend until it detects contact or reaches the + search limit. + + Args: + search_limit_mm: Maximum downward search distance in mm, must be negative + max_force_n: Maximum contact force in Newtons before stopping, must be negative + thorough: If True uses thorough mode (0.3mm accuracy, ~4s slower), + else quick mode (0.5mm accuracy, faster) + + Returns: + Detected Z height in mm (world coordinates) + """ + mode = 2 if thorough else 1 + response = self.send_command( + f"HeightDetect {mode} {search_limit_mm} {max_force_n}" ) - # Checking yaw angle - if goal_rotation != 0 and cartesian_goal[3] > -10 and cartesian_goal[3] < 10: - goal_location = self.set_plate_rotation(goal_location, goal_rotation) + return float(response.split(" ")[1]) + + # ------------------------------------------------------------------------- + # Motion + # ------------------------------------------------------------------------- + + def check_incorrect_plate_orientation( + self, goal_location: list[float], goal_rotation: float + ) -> list[float]: + """Fix plate rotation on the goal location if recorded with incorrect orientation. + + Args: + goal_location: 6 joint values for the goal location + goal_rotation: Expected rotation angle in degrees (0 or 90) + + Returns: + Corrected joint angles if orientation was wrong, otherwise unchanged. + """ + if goal_rotation == 0: + return goal_location + + cart = self.joint_to_cart(goal_location) + yaw = cart[3] + + # If yaw is close to 0 but rotation is expected, the location was saved + # with the wrong orientation and needs to be corrected + if -10 < yaw < 10: + return self.rotate_yaw(goal_location, goal_rotation) return goal_location @@ -576,39 +626,31 @@ def move_joint( gripper_close: bool = False, gripper_open: bool = False, ) -> str: - """ - Description: Creates the movement commands with the given robot_location, profile, gripper closed and gripper open info - Parameters: - - target: Which location the PF400 will move. - - profile: Motion profile ID. - - gripper_close: If set to TRUE, gripper is closed. If set to FALSE, gripper position will remain same as the previous location. - - gripper_open: If set to TRUE, gripper is opened. If set to FALSE, gripper position will remain same as the previous location. - Return: Returns the created movement command in string format - """ + """Move the robot to a joint angle location. - # Checking unpermitted gripper command - # add check gripper here and remove gripper open/close from state + Args: + target_joint_angles: Target joint angles [j1, j2, j3, j4, j5, rail] + profile: Motion profile ID + gripper_close: If True, gripper is closed before moving + gripper_open: If True, gripper is opened before moving + """ if gripper_close and gripper_open: raise Exception("Gripper cannot be open and closed at the same time!") - - # Setting the gripper location to open or close. If there is no gripper position passed in, target_joint_angles will be used. if gripper_close: target_joint_angles[4] = self.gripper_close elif gripper_open: target_joint_angles[4] = self.gripper_open else: - target_joint_angles[4] = self.get_gripper_position() - + target_joint_angles[4] = self.get_gripper_state() move_command = ( "movej" + " " + str(profile) + " " + " ".join(map(str, target_joint_angles)) ) - - return self.send_robot_command(move_command) + return self.send_command(move_command) def move_cartesian( self, target_cartesian_coordinates: list[float], profile: int = 2 ) -> str: - """Move the arm to a target location in cartesian coordinates.""" + """Move the arm to a target location in Cartesian coordinates.""" move_command = ( "MoveC" + " " @@ -616,29 +658,16 @@ def move_cartesian( + " " + " ".join(map(str, target_cartesian_coordinates)) ) - - return self.send_robot_command(move_command) + return self.send_command(move_command) def move_in_one_axis( self, profile: int = 1, axis_x: int = 0, axis_y: int = 0, axis_z: int = 0 ) -> str: - """ - Description: Moves the end effector on single axis with a goal movement in millimeters. - Parameters: - - axis_x : Goal movement on x axis in mm - - axis_y : Goal movement on y axis in mm - - axis_z : Goal movement on z axis in mm - Returns: A string response from the robot indicating the result of the move command. - """ - - # Find the cartesian coordinates of the target joint states + """Move the end effector on a single axis by a given distance in mm.""" cartesian_coordinates = self.get_cartesian_coordinates() - - # Move end effector on the single axis cartesian_coordinates[0] += axis_x cartesian_coordinates[1] += axis_y cartesian_coordinates[2] += axis_z - move_command = ( "MoveC" + " " @@ -646,15 +675,11 @@ def move_in_one_axis( + " " + " ".join(map(str, cartesian_coordinates)) ) - return self.send_robot_command(move_command) + return self.send_command(move_command) def move_gripper_safe_zone(self) -> None: - """ - Description: Check if end effector is outside the safe boundaries. If it is, move it on the y axis first to prevent collisions with the module frames. - """ - + """Check if end effector is outside safe boundaries and move it in if needed.""" current_cartesian_coordinates = self.get_cartesian_coordinates() - if current_cartesian_coordinates[1] <= self.safe_left_boundary: y_distance = self.safe_left_boundary - current_cartesian_coordinates[1] self.move_in_one_axis(profile=self.slow_motion_profile, axis_y=y_distance) @@ -663,74 +688,67 @@ def move_gripper_safe_zone(self) -> None: self.move_in_one_axis(profile=self.slow_motion_profile, axis_y=y_distance) def move_gripper_neutral(self) -> None: - """ - Description: Move end effector to neutral position - """ - + """Move end effector to neutral position.""" self.move_gripper_safe_zone() gripper_neutral = self.get_joint_states() gripper_neutral[3] = self.neutral_joints[3] - self.move_joint(gripper_neutral, self.slow_motion_profile) def move_arm_neutral(self) -> None: - """ - Description: Move arm to neutral position - """ + """Move arm to neutral position.""" arm_neutral = self.neutral_joints current_location = self.get_joint_states() arm_neutral[0] = current_location[0] arm_neutral[5] = current_location[5] - self.move_joint(arm_neutral, self.slow_motion_profile) def move_rails_neutral( self, v_rail: Optional[float] = None, h_rail: Optional[float] = None ) -> None: - """Setting the target location's linear rail position for pf400_neutral""" - + """Move rails to neutral position.""" current_location = self.get_joint_states() - if not v_rail: - v_rail = current_location[0] # Keep the vertical rail same + v_rail = current_location[0] if not h_rail: - h_rail = current_location[5] # Keep the horizontal rail same - + h_rail = current_location[5] self.neutral_joints[5] = h_rail self.move_joint(self.neutral_joints, self.fast_motion_profile) self.neutral_joints[0] = v_rail + self.default_approach_height self.move_joint(self.neutral_joints, self.slow_motion_profile) def move_all_joints_neutral(self, target: Optional[list[float]] = None) -> None: - """ - Description: Move all joints to neutral position - """ + """Move all joints to neutral position.""" if target is None: target = self.get_joint_states() - # First move end effector to it's nuetral position self.move_gripper_neutral() - # Setting an arm neutral position without moving the horizontal & vertical rails self.move_arm_neutral() - # Setting the target location's linear rail position for pf400_neutral self.move_rails_neutral(target[0], target[5]) def remove_lid( self, source: LocationArgument, target: LocationArgument, - lid_height: float = 7.0, + lid_height: Optional[float] = None, source_approach: LocationArgument = None, target_approach: LocationArgument = None, - source_plate_rotation: str = "", - target_plate_rotation: str = "", + source_plate_rotation: Optional[str] = None, + target_plate_rotation: Optional[str] = None, grab_offset: Optional[float] = None, - approach_height_offset: Optional[float] = None, - ) -> None: + source_approach_height_offset: Optional[float] = None, + target_approach_height_offset: Optional[float] = None, + source_height_limit: Optional[float] = None, + target_height_limit: Optional[float] = None, + source_press_depth: Optional[float] = None, + target_press_depth: Optional[float] = None, + ) -> bool: """Remove the lid from the plate""" + if not lid_height: + lid_height = self.default_lid_height + source.representation = copy.deepcopy(source.representation) source.representation[0] += lid_height - self.transfer( + return self.transfer( source=source, target=target, source_approach=source_approach, @@ -738,26 +756,39 @@ def remove_lid( source_plate_rotation=source_plate_rotation, target_plate_rotation=target_plate_rotation, grab_offset=grab_offset, - approach_height_offset=approach_height_offset, + source_approach_height_offset=source_approach_height_offset, + target_approach_height_offset=target_approach_height_offset, + source_height_limit=source_height_limit, + target_height_limit=target_height_limit, + source_press_depth=source_press_depth, + target_press_depth=target_press_depth, ) def replace_lid( self, source: LocationArgument, target: LocationArgument, - lid_height: float = 7.0, + lid_height: Optional[float] = None, source_approach: LocationArgument = None, target_approach: LocationArgument = None, - source_plate_rotation: str = "", - target_plate_rotation: str = "", + source_plate_rotation: Optional[str] = None, + target_plate_rotation: Optional[str] = None, grab_offset: Optional[float] = None, - approach_height_offset: Optional[float] = None, - ) -> None: + source_approach_height_offset: Optional[float] = None, + target_approach_height_offset: Optional[float] = None, + source_height_limit: Optional[float] = None, + target_height_limit: Optional[float] = None, + source_press_depth: Optional[float] = None, + target_press_depth: Optional[float] = None, + ) -> bool: """Replace the lid on the plate""" + if lid_height is None: + lid_height = self.default_lid_height + target.representation = copy.deepcopy(target.representation) target.representation[0] += lid_height - self.transfer( + return self.transfer( source=source, target=target, source_approach=source_approach, @@ -765,28 +796,33 @@ def replace_lid( source_plate_rotation=source_plate_rotation, target_plate_rotation=target_plate_rotation, grab_offset=grab_offset, - approach_height_offset=approach_height_offset, + source_approach_height_offset=source_approach_height_offset, + target_approach_height_offset=target_approach_height_offset, + source_height_limit=source_height_limit, + target_height_limit=target_height_limit, + source_press_depth=source_press_depth, + target_press_depth=target_press_depth, ) def rotate_plate_on_deck( self, rotation_degree: int, rotation_deck: Optional[LocationArgument] = None ) -> None: - """ - Description: Uses the rotation deck to rotate the plate between two transfers - Parameters: - rotation_degree: Rotation degree. - """ + """Use the rotation deck to rotate the plate between two transfers.""" if not rotation_deck: raise ValueError("Rotation deck location must be provided.") target = rotation_deck.representation - # Fixing the offset on the z axis if rotation_degree == -90: - target = self.set_plate_rotation(target, -rotation_degree) + target = self.rotate_yaw(target, rotation_degree) above_position = list(map(add, target, self.default_approach_vector)) self.move_all_joints_neutral(target) self.move_joint(above_position, self.slow_motion_profile) + target_position_above_compliance = copy.deepcopy(target) + target_position_above_compliance[0] += 1.0 + self.move_joint(target_position_above_compliance, self.slow_motion_profile) + self.enable_compliance() self.move_joint(target, self.slow_motion_profile) self.release_plate() @@ -805,14 +841,15 @@ def rotate_plate_on_deck( self.move_in_one_axis( profile=self.slow_motion_profile, axis_z=self.default_approach_height ) + self.disable_compliance() self.open_gripper(self.gripper_open_wide) - # Rotating gripper to grab the plate from other rotation - target = self.set_plate_rotation(target, rotation_degree) + target = self.rotate_yaw(target, rotation_degree) above_position = list(map(add, target, self.default_approach_vector)) self.move_joint( target_joint_angles=above_position, profile=self.slow_motion_profile ) + self.enable_compliance() self.move_joint( target_joint_angles=target, profile=self.slow_motion_profile, @@ -835,14 +872,12 @@ def rotate_plate_on_deck( self.move_in_one_axis( profile=self.slow_motion_profile, axis_z=self.default_approach_height ) + self.disable_compliance() self.move_all_joints_neutral(target) def _handle_approach_location(self, approach: LocationArgument) -> None: - """ - Handle moving to an approach location, whether single or multiple. - """ + """Handle moving to an approach location, whether single or multiple.""" if isinstance(approach.representation[0], list): - # Multiple approach locations provided self.move_all_joints_neutral(approach.representation[0]) for location in approach.representation: self.move_joint( @@ -850,14 +885,15 @@ def _handle_approach_location(self, approach: LocationArgument) -> None: profile=self.fast_motion_profile, ) else: - # Single approach location provided self.move_all_joints_neutral(approach.representation) self.move_joint( target_joint_angles=approach.representation, profile=self.fast_motion_profile, ) - def _handle_approach_return(self, approach: LocationArgument) -> None: + def _handle_approach_return( + self, approach: LocationArgument, default_motion: Optional[str] = None + ) -> None: """ Handle returning from an approach location, whether single or multiple. Uses straight motion profile for the first approach location (closest to target), @@ -865,11 +901,14 @@ def _handle_approach_return(self, approach: LocationArgument) -> None: """ if isinstance(approach.representation[0], list): for index, location in enumerate(reversed(approach.representation)): - motion_profile = ( - self.straight_motion_profile - if index == 0 - else self.fast_motion_profile - ) + if index == 0: + motion_profile = self.straight_motion_profile + else: + motion_profile = ( + default_motion + if default_motion is not None + else self.fast_motion_profile + ) self.move_joint( target_joint_angles=location, profile=motion_profile, @@ -891,19 +930,18 @@ def _calculate_above_position( """ Calculate the position above a target with optional height offset. """ - above_offset = copy.deepcopy(self.default_approach_vector) + above_offset = ( + [self.default_approach_height, 0, 0, 0, 0, 0] + if approach_height_offset is None + else [approach_height_offset, 0, 0, 0, 0, 0] + ) - if approach_height_offset: - above_offset[0] += approach_height_offset if grab_height_offset: above_offset[0] += grab_height_offset - return list(map(add, position, above_offset)) def _apply_grab_offset(self, position: list, grab_offset: float) -> list: - """ - Apply grab offset to a position. - """ + """Apply grab offset to a position.""" position = copy.deepcopy(position) position[0] += grab_offset return position @@ -914,16 +952,35 @@ def pick_plate( source_approach: LocationArgument = None, grab_offset: Optional[float] = None, approach_height_offset: Optional[float] = None, + height_limit: Optional[float] = None, grip_width: Optional[int] = None, + press_depth: Optional[float] = None, ) -> bool: """ Pick a plate from the source location, optionally using an approach location. Returns True if the plate was successfully grabbed, False otherwise. """ + if press_depth is not None: + source.representation = copy.deepcopy(source.representation) + source.representation[0] -= press_depth + above_position = self._calculate_above_position( source.representation, approach_height_offset, grab_offset ) + if height_limit is not None: + calculated_height = ( + above_position[0] + + self.gripper_clearance_height + - source.representation[0] + ) + if calculated_height >= height_limit: + self.logger.log_error( + f"Height limit validation failed: calculated above position " + f"({calculated_height}) exceeds height limit ({height_limit})" + ) + return False + self.open_gripper() if source_approach: @@ -932,7 +989,6 @@ def pick_plate( else: self.move_all_joints_neutral(source.representation) approach_motion_profile = self.fast_motion_profile - self.move_joint( target_joint_angles=above_position, profile=approach_motion_profile ) @@ -947,6 +1003,7 @@ def pick_plate( profile=approach_motion_profile, gripper_open=True, ) + self.enable_compliance() grab_succeeded = self.grab_plate(width=grip_width, speed=100, force=10) if self.resource_client and grab_succeeded and source.resource_id: @@ -963,9 +1020,12 @@ def pick_plate( if approach_height_offset else self.default_approach_height, ) + self.disable_compliance() if source_approach: - self._handle_approach_return(source_approach) + self._handle_approach_return( + approach=source_approach, default_motion=self.slow_motion_profile + ) else: self.move_all_joints_neutral(source.representation) @@ -977,14 +1037,32 @@ def place_plate( target_approach: LocationArgument = None, grab_offset: Optional[float] = None, approach_height_offset: Optional[float] = None, + height_limit: Optional[float] = None, open_width: Optional[int] = None, + press_depth: Optional[float] = None, ) -> bool: """ Place a plate in the target location """ + if press_depth is not None: + target.representation = copy.deepcopy(target.representation) + target.representation[0] -= press_depth + above_position = self._calculate_above_position( target.representation, approach_height_offset, grab_offset ) + if height_limit is not None: + calculated_height = ( + above_position[0] + + self.gripper_clearance_height + - target.representation[0] + ) + if calculated_height >= height_limit: + self.logger.log_error( + f"Height limit validation failed: calculated above position " + f"({calculated_height}) exceeds height limit ({height_limit})" + ) + return False if target_approach: self._handle_approach_location(target_approach) @@ -1000,6 +1078,10 @@ def place_plate( if grab_offset else target.representation ) + target_position_above_compliance = copy.deepcopy(target_position) + target_position_above_compliance[0] += 2.0 + self.move_joint(target_position_above_compliance) + self.enable_compliance() self.move_joint(target_position, approach_motion_profile) release_succeeded = self.release_plate(width=open_width) @@ -1025,9 +1107,11 @@ def place_plate( if approach_height_offset else self.default_approach_height, ) - + self.disable_compliance() if target_approach: - self._handle_approach_return(target_approach) + self._handle_approach_return( + approach=target_approach, default_motion=self.fast_motion_profile + ) else: self.move_all_joints_neutral(target.representation) @@ -1040,19 +1124,11 @@ def move_to_location( grab_offset: Optional[float] = None, approach_height_offset: Optional[float] = None, ) -> None: - """ - Move to a target location for testing/calibration purposes. - - Follows the same approach and descend sequence as pick_plate, but keeps the - gripper open (unless holding a plate) and does not grip/release or change - resource state. Stays at the target position so the user can inspect and - adjust calibration. Use move_all_joints_neutral() to retract afterward. - """ + """Move to a target location for testing/calibration purposes.""" above_position = self._calculate_above_position( target.representation, approach_height_offset, grab_offset ) - # Check if the gripper is currently holding a plate holding_plate = ( self.resource_client and len( @@ -1061,7 +1137,6 @@ def move_to_location( > 0 ) - # Only open gripper if not holding a plate (prevent dropping labware) if not holding_plate: self.open_gripper() @@ -1088,12 +1163,7 @@ def move_to_location( ) def move_neutral(self, height_offset: Optional[float] = None) -> None: - """ - Retract upward and move to neutral position. - - Retracts the arm upward by height_offset (defaults to default_approach_height) - before moving to neutral, mirroring the retract step in pick_plate/place_plate. - """ + """Retract upward and move to neutral position.""" retract_height = ( height_offset if height_offset is not None else self.default_approach_height ) @@ -1115,6 +1185,10 @@ def transfer( grab_offset: Optional[float] = None, source_approach_height_offset: Optional[float] = None, target_approach_height_offset: Optional[float] = None, + source_height_limit: Optional[float] = None, + target_height_limit: Optional[float] = None, + source_press_depth: Optional[float] = None, + target_press_depth: Optional[float] = None, ) -> bool: """ Description: Plate transfer function that performs series of movements to pick and place the plates @@ -1129,33 +1203,64 @@ def transfer( - grab_offset: Add grab height offset - source_approach_height_offset: Add source approach height offset - target_approach_height_offset: Add target approach height offset + - source_height_limit: Maximum height limit for source pick + - target_height_limit: Maximum height limit for target place + - source_press_depth: Depth to press down when picking from source + - target_press_depth: Depth to press down when placing to target + Returns: + True if transfer was successful, False otherwise. Note: Plate rotation defines the rotation of the plate on the deck, not the grabbing angle. """ source = copy.deepcopy(source) target = copy.deepcopy(target) - # Validate rotation arguments for rotation_arg in [source_plate_rotation, target_plate_rotation]: - if rotation_arg.lower() not in ["wide", "narrow", ""]: + if rotation_arg is not None and rotation_arg.lower() not in [ + "wide", + "narrow", + ]: raise ValueError( f"Invalid plate rotation argument: {rotation_arg}. " - "Expected 'wide', 'narrow', or ''." + "Expected None, 'wide', or 'narrow'." ) # Determine source rotation (0 or 90 degrees) - plate_source_rotation = 90 if source_plate_rotation.lower() == "wide" else 0 - self.grip_wide = source_plate_rotation.lower() == "wide" + plate_source_rotation = ( + 90 + if source_plate_rotation and source_plate_rotation.lower() == "wide" + else 0 + ) + self.grip_wide = ( + source_plate_rotation and source_plate_rotation.lower() == "wide" + ) - source.representation = self.check_incorrect_plate_orientation( - source.representation, plate_source_rotation + # Determine target rotation (0 or 90 degrees) + plate_target_rotation = ( + 90 + if target_plate_rotation and target_plate_rotation.lower() == "wide" + else 0 ) + rotation_needed = plate_target_rotation - plate_source_rotation + if rotation_needed != 0 and rotation_deck is None: + self.logger.log_error( + f"Rotation required ({rotation_needed} degrees) but rotation_deck was not provided." + ) + return False + + """ + Depricating this implementation + source.representation = self.check_incorrect_plate_orientation(source.representation, plate_source_rotation) + """ + pick_result = self.pick_plate( source=source, source_approach=source_approach, grab_offset=grab_offset, approach_height_offset=source_approach_height_offset, + height_limit=source_height_limit, + press_depth=source_press_depth, ) if not pick_result: @@ -1164,15 +1269,17 @@ def transfer( self.logger.error("Transfer failed: no plate detected after picking.") return False - # Determine target rotation (0 or 90 degrees) - plate_target_rotation = 90 if target_plate_rotation.lower() == "wide" else 0 - self.grip_wide = target_plate_rotation.lower() == "wide" + self.grip_wide = ( + target_plate_rotation and target_plate_rotation.lower() == "wide" + ) + """ + Depricating this implementation target.representation = self.check_incorrect_plate_orientation( target.representation, plate_target_rotation ) + """ # Rotate plate if needed - rotation_needed = plate_target_rotation - plate_source_rotation if rotation_needed != 0: self.rotate_plate_on_deck( rotation_degree=rotation_needed, rotation_deck=rotation_deck @@ -1183,6 +1290,8 @@ def transfer( target_approach=target_approach, grab_offset=grab_offset, approach_height_offset=target_approach_height_offset, + height_limit=target_height_limit, + press_depth=target_press_depth, ) if not place_result: self.logger.error("Transfer failed: plate not released properly.") diff --git a/src/pf400_interface/pf400_kinematics.py b/src/pf400_interface/pf400_kinematics.py deleted file mode 100644 index 457918d..0000000 --- a/src/pf400_interface/pf400_kinematics.py +++ /dev/null @@ -1,180 +0,0 @@ -"""Handles the kinematics of the PF400 robot arm""" - -import math - - -class KINEMATICS: - """Class for calculating the forward and inverse kinematics of the PF400 robot arm.""" - - """ - Joint coordinates, list of length 6 for PF400 with horizontal rail, - rail positions units are mm, angles in degrees increasing CCW looking down (-z): - 0 vertical rail position in mm, +z up, range [ 0, 1500 ] mm (for the 1.5m v_rail) - 1 angle of shoulder joint (v_rail to upper_arm), 0 deg perp v_rail, range: [-93, 93] deg - 2 angle of elbow joint (upper_arm to forearm), 0 deg under upper_arm, range: [-168, 168] deg - 3 angle of wrist joint (forearm to end_effector), 0 deg extends forearm, range: [-960, 960] deg - 4 the separation between plate-handling gripper fingers, range: [ ???, ??? ] mm - 5 horizontal rail position in mm, +x perp v_rail, range: [-1000,1000] (for the 2m h_rail) - - Cartesian coordinates, list of length 6: - 0 X increasing away from the face of the v_rail, parallel to the h_rail - 1 Y increasing to the left when facing in the +x direction - 2 Z increasing up along the v_rail - 3 yaw angle of gripper fingers to +x axis - 4 pitch fixed at 90 deg - 5 roll fixed at 180 deg - """ - - def __init__(self) -> None: - """Constructor for the KINEMATICS class.""" - # Robot arm segment lengths (in millimeters) - self.shoulder_length = ( - 302 # mm (perhaps rename to upper_arm, as shoulder is a joint) - ) - self.elbow_length = 289 # mm (perhaps rename to forearm, as elbo is a joint) - self.end_effector_length = ( - 162 # mm (???does this length terminate at the Tool Center Point?) - ) - - def forward_kinematics( - self, joint_states: list[float] - ) -> tuple[list[float], float, float]: - """ - Description: Calculates the forward kinematics for a given array of joint_states. - Parameters: - - joint_states : 6 joint states of the target location - Return: - - cartesian_coordinates: Returns the calculated cartesian coordinates of the given joint states (in millimeters) - - phi: Phi angle in degrees to be used for inverse kinematics - - joint_state[5]: The rail length. Needs to be subtracted from x axis if calculated coordinates will be fed into inverse kinematics - """ - - if joint_states[2] > 180: - adjusted_angle_j3 = ( - joint_states[2] - 360 - ) # Fixing the quadrant on the third joint. Joint 3 range is 10 to 350 instead of -180 to 180 - else: - adjusted_angle_j3 = joint_states[2] - - # Convert angles to radians - shoulder_angle = math.radians(joint_states[1]) # Joint 2 - elbow_angle = math.radians(joint_states[2]) # Joint 3 - gripper_angle = math.radians(joint_states[3]) # Joint 4 - - x = ( - self.shoulder_length * math.cos(shoulder_angle) - + self.elbow_length * math.cos(shoulder_angle + elbow_angle) - + self.end_effector_length - * math.cos(shoulder_angle + elbow_angle + gripper_angle) - ) - y = ( - self.shoulder_length * math.sin(shoulder_angle) - + self.elbow_length * math.sin(shoulder_angle + elbow_angle) - + self.end_effector_length - * math.sin(shoulder_angle + elbow_angle + gripper_angle) - ) - z = joint_states[0] - - phi = ( - math.degrees(shoulder_angle) - + adjusted_angle_j3 - + math.degrees(gripper_angle) - ) - - if phi > 0 and phi < 540: - yaw = phi % 360 - elif phi > 540 and phi < 720: - yaw = phi % 360 - 360 - elif phi > 720 and phi < 900: - yaw = phi % 720 - elif phi > 900 and phi < 1080: - yaw = phi % 720 - 720 - - cartesian_coordinates = self.get_cartesian_coordinates() - - cartesian_coordinates[0] = round(x, 3) + joint_states[5] - cartesian_coordinates[1] = round(y, 3) - cartesian_coordinates[2] = round(z, 3) - cartesian_coordinates[3] = round(yaw, 3) - - return cartesian_coordinates, round(phi, 3), joint_states[5] - - def inverse_kinematics( - self, - cartesian_coordinates: list, - phi: float, - rail: float = 0.0, - get_gripper_length: float = 123.0, - ) -> list[float]: - """ - Description: Calculates the inverse kinematics for a given array of cartesian coordinates. - Parameters: - - cartesian_coordinates: X/Y/Z Yaw/Pitch/Roll cartesian coordinates. - X axis has to be subtracted from the rail length before feeding into this function! - - Phi: Phi angle. Phi = Joint_2_angle + Joint_3_angle + Joint_4_angle - - Rail: Rail length (optional). If provided it will be subtracted from X axis. - Return: - - Joint angles: Calculated 6 new joint angles. - """ - - joint1 = cartesian_coordinates[2] - xe = cartesian_coordinates[0] - rail - ye = cartesian_coordinates[1] - - if phi < 360: - phi = cartesian_coordinates[3] - elif phi > 360 and phi < 540: - phi = cartesian_coordinates[3] + 360 - elif (phi > 540 and phi < 720) or (phi > 720 and phi < 900): - phi = cartesian_coordinates[3] + 720 - elif phi > 900 and phi < 1080: - phi = cartesian_coordinates[3] + 1440 - - phi_e = math.radians(phi) - - x_second_joint = xe - self.end_effector_length * math.cos(phi_e) - y_second_joint = ye - self.end_effector_length * math.sin(phi_e) - - radius = math.sqrt(x_second_joint**2 + y_second_joint**2) - gamma = math.acos( - ( - radius * radius - + self.shoulder_length * self.shoulder_length - - self.elbow_length * self.elbow_length - ) - / (2 * radius * self.shoulder_length) - ) - - theta2 = math.pi - math.acos( - ( - self.shoulder_length * self.shoulder_length - + self.elbow_length * self.elbow_length - - radius * radius - ) - / (2 * self.shoulder_length * self.elbow_length) - ) - theta1 = math.atan2(y_second_joint, x_second_joint) - gamma - theta3 = phi_e - theta1 - theta2 - - if cartesian_coordinates[1] > 0 or ( - cartesian_coordinates[1] < 0 - and math.degrees(theta1) < 0 - and abs(math.degrees(theta1)) < abs(math.degrees(theta1 + 2 * gamma)) - ): - # Robot is in the First Quadrant on the coordinate plane (x:+ , y:+) - joint2 = math.degrees(theta1) - joint3 = math.degrees( - theta2 - ) # Adding 360 degrees to Joint 3 to fix the pose. - joint4 = math.degrees(theta3) - - elif cartesian_coordinates[1] < 0: - # Robot is in the Forth Quadrant on the coordinate plane (x:+ , y:-) - # Use the joint angles for Forth Quadrant - joint2 = math.degrees(theta1 + 2 * gamma) - joint3 = ( - math.degrees(theta2 * -1) + 360 - ) # Adding 360 degrees to Joint 3 to fix the pose. - joint4 = math.degrees(theta3 + 2 * (theta2 - gamma)) - - return [joint1, joint2, joint3, joint4, get_gripper_length, rail] diff --git a/src/pf400_rest_node.py b/src/pf400_rest_node.py index 13e1cb7..8f24682 100644 --- a/src/pf400_rest_node.py +++ b/src/pf400_rest_node.py @@ -161,67 +161,162 @@ def state_handler(self) -> None: "current_joint_angles": current_location, } + def _parse_location_representation( + self, location: LocationArgument + ) -> tuple[ + LocationArgument, + Optional[LocationArgument], + Optional[str], + Optional[float], + Optional[float], + ]: + """ + Parse a LocationArgument that may have a dictionary representation. + + Expected dictionary structure: + { + "location": 6-digit list, + "approach": single or multiple approach locations, + "plate_rotation": "wide" or "narrow" # optional plate rotation + "approach_height_offset": float # optional approach height offset + "height_limit": float # optional height limit for validation + } + + Returns: + tuple: (location_arg_with_list_repr, approach_location_arg or None, plate_rotation or None, approach_height_offset or None, height_limit or None) + """ + if not isinstance(location.representation, dict): + return location, None, None, None, None, None + + repr_dict = location.representation + + if "location" not in repr_dict: + raise ValueError( + "LocationArgument representation dictionary must contain 'location' key" + ) + + location_repr = repr_dict["location"] + approach_repr = repr_dict.get("approach", None) + plate_rotation = repr_dict.get("plate_rotation", None) + approach_height_offset = repr_dict.get("approach_height_offset", None) + height_limit = repr_dict.get("height_limit", None) + press_depth = repr_dict.get("press_depth", None) + + parsed_location = LocationArgument( + representation=location_repr, + resource_id=location.resource_id, + location_name=location.location_name, + reservation=location.reservation, + ) + + parsed_approach = None + if approach_repr is not None: + parsed_approach = LocationArgument( + representation=approach_repr, + resource_id=None, + location_name=None, + ) + + return ( + parsed_location, + parsed_approach, + plate_rotation, + approach_height_offset, + height_limit, + press_depth, + ) + @action( name="transfer", description="Transfer a plate from one location to another" ) - def transfer( + def transfer( # noqa: C901, self, source: Annotated[LocationArgument, "Location to pick a plate from"], target: Annotated[LocationArgument, "Location to place a plate to"], - source_approach: Annotated[ - Optional[LocationArgument], "Location to approach from" - ] = None, - target_approach: Annotated[ - Optional[LocationArgument], "Location to approach from" - ] = None, - source_plate_rotation: Annotated[ - str, "Orientation of the plate at the source, wide or narrow" - ] = "", - target_plate_rotation: Annotated[ - str, "Final orientation of the plate at the target, wide or narrow" - ] = "", rotation_deck: Optional[LocationArgument] = None, - grab_offset: Optional[Annotated[float, "Add grab height offset"]] = None, - source_approach_height_offset: Optional[ - Annotated[float, "Add source approach height offset"] - ] = None, - target_approach_height_offset: Optional[ - Annotated[float, "Add target approach height offset"] - ] = None, ) -> Optional[ActionFailed]: """Transfer a plate from `source` to `target`, optionally using intermediate `approach` positions and target rotations.""" - if source.resource_id: - source_resource = self.resource_client.get_resource(source.resource_id) - if source_resource.quantity == 0: - return ActionFailed( - errors=[ - f"Plate does not exist at source location! Resource_id:{source.resource_id}." - ] - ) - if target.resource_id: - target_resource = self.resource_client.get_resource(target.resource_id) - if ( - target_resource.quantity != 0 - and target_resource.resource_id != source_resource.resource_id - ): - return ActionFailed( - errors=[ - f"Target is occupied by another plate! Resource_id:{target.resource_id}." - ] - ) + grab_height_offset = None + try: + if source.resource_id: + source_resource = self.resource_client.get_resource(source.resource_id) + if source_resource.quantity == 0: + return ActionFailed( + errors=[ + f"Plate does not exist at source location! Resource_id:{source.resource_id}." + ] + ) + if source_resource.children: + plate_resource = source_resource.children[-1] + if plate_resource.attributes: + grab_height_offset = plate_resource.attributes.get( + "grab_height_offset", None + ) + + if target.resource_id: + target_resource = self.resource_client.get_resource(target.resource_id) + if ( + target_resource.quantity != 0 + and target_resource.resource_id != source_resource.resource_id + ): + return ActionFailed( + errors=[ + f"Target is occupied by another plate! Resource_id:{target.resource_id}." + ] + ) + except Exception as e: + return ActionFailed( + errors=[f"Resource manager error during transfer validation: {e}"] + ) + try: + ( + parsed_source, + source_approach, + source_rotation_from_dict, + source_approach_height_offset, + source_height_limit, + source_press_depth, + ) = self._parse_location_representation(source) + ( + parsed_target, + target_approach, + target_rotation_from_dict, + target_approach_height_offset, + target_height_limit, + target_press_depth, + ) = self._parse_location_representation(target) + if rotation_deck is not None: + ( + parsed_rotation, + _, + _, + _, + _, + _, + ) = self._parse_location_representation(rotation_deck) + else: + parsed_rotation = None + except Exception as e: + return ActionFailed( + errors=[f"Failed to parse location representation: {e}"] + ) transfer_result = self.pf400_interface.transfer( - source=source, - target=target, - source_approach=source_approach or None, - target_approach=target_approach or None, - source_plate_rotation=source_plate_rotation, - target_plate_rotation=target_plate_rotation, - rotation_deck=rotation_deck or None, - grab_offset=grab_offset, + source=parsed_source, + target=parsed_target, + source_approach=source_approach, + target_approach=target_approach, + source_plate_rotation=source_rotation_from_dict, + target_plate_rotation=target_rotation_from_dict, + rotation_deck=parsed_rotation, + grab_offset=grab_height_offset, source_approach_height_offset=source_approach_height_offset, target_approach_height_offset=target_approach_height_offset, + source_height_limit=source_height_limit, + target_height_limit=target_height_limit, + source_press_depth=source_press_depth, + target_press_depth=target_press_depth, ) if not transfer_result: return ActionFailed( @@ -234,51 +329,65 @@ def transfer( def pick_plate( self, source: Annotated[LocationArgument, "Location to pick a plate from"], - source_approach: Annotated[ - Optional[LocationArgument], "Location to approach from" - ] = None, - source_plate_rotation: Annotated[ - str, "Orientation of the plate at the source, wide or narrow" - ] = "", - grab_offset: Optional[Annotated[float, "Add grab height offset"]] = None, - approach_height_offset: Optional[ - Annotated[float, "Add approach height offset"] - ] = None, ) -> Optional[ActionFailed]: """Picks a plate from `source`, optionally moving first to `source_approach`.""" - if source.resource_id: - source_resource = self.resource_client.get_resource(source.resource_id) - if source_resource.quantity == 0: - return ActionFailed( - errors=[ - f"Resource manager: Plate does not exist at source! Resource_id:{source.resource_id}." - ] - ) + grab_height_offset = None + try: + if source.resource_id: + source_resource = self.resource_client.get_resource(source.resource_id) + if source_resource.quantity == 0: + return ActionFailed( + errors=[ + f"Resource manager: Plate does not exist at source! Resource_id:{source.resource_id}." + ] + ) + if source_resource.children: + plate_resource = source_resource.children[-1] + if plate_resource.attributes: + grab_height_offset = plate_resource.attributes.get( + "grab_height_offset", None + ) + except Exception as e: + return ActionFailed( + errors=[f"Resource manager error during pick validation: {e}"] + ) - # set plate width for source - if source_plate_rotation.lower() == "wide": - plate_source_rotation = 90 - self.pf400_interface.grip_wide = True - elif source_plate_rotation.lower() == "narrow" or source_plate_rotation == "": - plate_source_rotation = 0 - self.pf400_interface.grip_wide = False - else: + try: + ( + parsed_source, + source_approach, + source_rotation_from_dict, + source_approach_height_offset, + source_height_limit, + press_depth, + ) = self._parse_location_representation(source) + except Exception as e: return ActionFailed( - errors=[ - f"Invalid source plate rotation: {source_plate_rotation}. " - "Expected 'wide', 'narrow', or ''." - ] + errors=[f"Failed to parse location representation: {e}"] ) - source.representation = self.pf400_interface.check_incorrect_plate_orientation( - source.representation, plate_source_rotation + plate_source_rotation = ( + 90 + if source_rotation_from_dict and source_rotation_from_dict.lower() == "wide" + else 0 + ) + self.pf400_interface.grip_wide = ( + source_rotation_from_dict and source_rotation_from_dict.lower() == "wide" + ) + + parsed_source.representation = ( + self.pf400_interface.check_incorrect_plate_orientation( + parsed_source.representation, plate_source_rotation + ) ) pick_result = self.pf400_interface.pick_plate( - source=source, - source_approach=source_approach or None, - grab_offset=grab_offset, - approach_height_offset=approach_height_offset, + source=parsed_source, + source_approach=source_approach, + grab_offset=grab_height_offset, + approach_height_offset=source_approach_height_offset, + height_limit=source_height_limit, + press_depth=press_depth, ) if not pick_result: return ActionFailed( @@ -293,54 +402,75 @@ def pick_plate( def place_plate( self, target: Annotated[LocationArgument, "Location to place a plate to"], - target_approach: Annotated[ - Optional[LocationArgument], "Location to approach from" - ] = None, - target_plate_rotation: Annotated[ - str, "Final orientation of the plate at the target, wide or narrow" - ] = "", - grab_offset: Optional[Annotated[float, "Add grab height offset"]] = None, - approach_height_offset: Optional[ - Annotated[float, "Add approach height offset"] - ] = None, ) -> Optional[ActionFailed]: """Place a plate in the `target` location, optionally moving first to `target_approach`.""" - if target.resource_id: - target_resource = self.resource_client.get_resource(target.resource_id) - if target_resource.quantity != 0: - return ActionFailed( - errors=[ - "Resource manager: Target is occupied by another plate! Resource_id:{target.resource_id}." - ] + grab_height_offset = None + try: + if target.resource_id: + target_resource = self.resource_client.get_resource(target.resource_id) + if target_resource.quantity != 0: + return ActionFailed( + errors=[ + f"Resource manager: Target is occupied by another plate! Resource_id:{target.resource_id}." + ] + ) + if self.gripper_resource.resource_id: + gripper_resource = self.resource_client.get_resource( + self.gripper_resource.resource_id ) + if gripper_resource.quantity > 0 and gripper_resource.children: + plate_in_gripper = gripper_resource.children[-1] + if plate_in_gripper.attributes: + grab_height_offset = plate_in_gripper.attributes.get( + "grab_height_offset", None + ) + except Exception as e: + return ActionFailed( + errors=[f"Resource manager error during pick validation: {e}"] + ) - if target_plate_rotation.lower() == "wide": - plate_target_rotation = 90 - self.pf400_interface.grip_wide = True - elif target_plate_rotation.lower() == "narrow" or target_plate_rotation == "": - plate_target_rotation = 0 - self.pf400_interface.grip_wide = False - else: + try: + ( + parsed_target, + target_approach, + target_rotation_from_dict, + target_approach_height_offset, + target_height_limit, + press_depth, + ) = self._parse_location_representation(target) + except Exception as e: return ActionFailed( - errors=[ - f"Invalid target plate rotation: {target_plate_rotation}. " - "Expected 'wide', 'narrow', or ''." - ] + errors=[f"Failed to parse location representation: {e}"] ) - target.representation = self.pf400_interface.check_incorrect_plate_orientation( - target.representation, plate_target_rotation + plate_target_rotation = ( + 90 + if target_rotation_from_dict and target_rotation_from_dict.lower() == "wide" + else 0 + ) + self.pf400_interface.grip_wide = ( + target_rotation_from_dict and target_rotation_from_dict.lower() == "wide" + ) + + parsed_target.representation = ( + self.pf400_interface.check_incorrect_plate_orientation( + parsed_target.representation, plate_target_rotation + ) ) place_result = self.pf400_interface.place_plate( - target=target, - target_approach=target_approach or None, - grab_offset=grab_offset, - approach_height_offset=approach_height_offset, + target=parsed_target, + target_approach=target_approach, + grab_offset=grab_height_offset, + approach_height_offset=target_approach_height_offset, + height_limit=target_height_limit, + press_depth=press_depth, ) if not place_result: - return ActionFailed("Transfer failed: plate not released properly.") + return ActionFailed( + errors=["Transfer failed: plate not released properly."] + ) return None @@ -384,138 +514,222 @@ def move_neutral( self.pf400_interface.move_neutral(height_offset=height_offset) @action(name="remove_lid", description="Remove a lid from a plate") - def remove_lid( + def remove_lid( # noqa: C901, PLR0911 self, source: Annotated[LocationArgument, "Location to pick a plate from"], target: Annotated[LocationArgument, "Location to place a plate to"], - source_approach: Annotated[ - Optional[LocationArgument], "Location to approach from" - ] = None, - target_approach: Annotated[ - Optional[LocationArgument], "Location to approach from" - ] = None, - source_plate_rotation: Annotated[ - str, "Orientation of the plate at the source, wide or narrow" - ] = "", - target_plate_rotation: Annotated[ - str, "Final orientation of the plate at the target, wide or narrow" - ] = "", - lid_height: Annotated[float, "height of the lid, in steps"] = 7.0, - grab_offset: Optional[Annotated[float, "Add grab height offset"]] = None, - approach_height_offset: Optional[ - Annotated[float, "Add approach height offset"] - ] = None, ) -> Optional[ActionFailed]: - """Remove a lid from a plate located at location .""" + """Remove a lid from a plate located at location.""" - if source.resource_id: - source_resource = self.resource_client.get_resource(source.resource_id) - if source_resource.quantity == 0: - return ActionFailed( - "Resource manager: Plate does not exist at source! Resource_id:{source.resource_id}." - ) - if target.resource_id: - target_resource = self.resource_client.get_resource(target.resource_id) - if target_resource.quantity != 0: - return ActionFailed( - "Resource manager: Target is occupied by another plate! Resource_id:{target.resource_id}." - ) + grab_height_offset = None + resource_lid_height = None + plate_resource = None + try: + if source.resource_id: + source_resource = self.resource_client.get_resource(source.resource_id) + if source_resource.quantity == 0: + return ActionFailed( + errors=[ + f"Resource manager: Plate does not exist at source! Resource_id:{source.resource_id}." + ] + ) + + if source_resource.children: + plate_resource = source_resource.children[-1] + if plate_resource.attributes: + has_lid = plate_resource.attributes.get("has_lid", None) + + if has_lid is None: + self.logger.log_warning( + "Continuing without resource validation for lids - 'has_lid' attribute not found in resource" + ) + elif has_lid is False: + return ActionFailed( + errors=[ + f"Resource manager: Plate at source does not have a lid! Resource_id:{source.resource_id}." + ] + ) + + grab_height_offset = plate_resource.attributes.get( + "grab_height_offset", None + ) + resource_lid_height = plate_resource.attributes.get( + "lid_height", None + ) + + if target.resource_id: + target_resource = self.resource_client.get_resource(target.resource_id) + if target_resource.quantity != 0: + return ActionFailed( + errors=[ + f"Resource manager: Target is occupied by another plate! Resource_id:{target.resource_id}." + ] + ) + + lid_resource = self.resource_client.create_resource_from_template( + template_name="pf400_lid_slot", + resource_name="pf400_lid_slot", + add_to_database=True, + ) - # Extract id of plate resource at source - plate_resource_id = self.resource_client.get_resource( - source.resource_id - ).child.resource_id + lid = self.resource_client.create_resource_from_template( + template_name="plate_lid", + resource_name=f"Lid_from_{plate_resource.resource_id}", + add_to_database=True, + ) - # Create temporary lid slot from template - lid_resource = self.resource_client.create_resource_from_template( - template_name="pf400_lid_slot", - resource_name="pf400_lid_slot", - add_to_database=True, - ) + lid_resource = self.resource_client.push(resource=lid_resource, child=lid) - # Create lid asset from template - lid = self.resource_client.create_resource_from_template( - template_name="plate_lid", - resource_name=f"Lid_from_{plate_resource_id}", - add_to_database=True, - ) + except Exception as e: + return ActionFailed( + errors=[f"Resource manager error during remove lid validation: {e}"] + ) + + try: + ( + parsed_source, + source_approach, + source_rotation_from_dict, + source_approach_height_offset, + source_height_limit, + source_press_depth, + ) = self._parse_location_representation(source) + ( + parsed_target, + target_approach, + target_rotation_from_dict, + target_approach_height_offset, + target_height_limit, + target_press_depth, + ) = self._parse_location_representation(target) + except Exception as e: + return ActionFailed( + errors=[f"Failed to parse location representation: {e}"] + ) - lid_resource = self.resource_client.push(resource=lid_resource, child=lid) - source.resource_id = lid_resource.resource_id + parsed_source.resource_id = lid_resource.resource_id - self.pf400_interface.remove_lid( - source=source, - target=target, - lid_height=lid_height, + remove_lid_result = self.pf400_interface.remove_lid( + source=parsed_source, + target=parsed_target, + lid_height=resource_lid_height, source_approach=source_approach, target_approach=target_approach, - source_plate_rotation=source_plate_rotation, - target_plate_rotation=target_plate_rotation, - grab_offset=grab_offset, - approach_height_offset=approach_height_offset, + source_plate_rotation=source_rotation_from_dict, + target_plate_rotation=target_rotation_from_dict, + grab_offset=grab_height_offset, + source_approach_height_offset=source_approach_height_offset, + target_approach_height_offset=target_approach_height_offset, + source_height_limit=source_height_limit, + target_height_limit=target_height_limit, + source_press_depth=source_press_depth, + target_press_depth=target_press_depth, ) + + if not remove_lid_result: + return ActionFailed(errors=["Failed to remove lid."]) + + if plate_resource and plate_resource.attributes: + plate_resource.attributes["has_lid"] = False + self.resource_client.update_resource(plate_resource) + return None @action(name="replace_lid", description="Replace a lid on a plate") - def replace_lid( + def replace_lid( # noqa: C901 self, source: Annotated[LocationArgument, "Location to pick a plate from"], target: Annotated[LocationArgument, "Location to place a plate to"], - source_approach: Annotated[ - Optional[LocationArgument], "Location to approach from" - ] = None, - target_approach: Annotated[ - Optional[LocationArgument], "Location to approach from" - ] = None, - source_plate_rotation: Annotated[ - str, "Orientation of the plate at the source, wide or narrow" - ] = "", - target_plate_rotation: Annotated[ - str, "Final orientation of the plate at the target, wide or narrow" - ] = "", - lid_height: Annotated[float, "height of the lid, in steps"] = 7.0, - grab_offset: Optional[Annotated[float, "Add grab height offset"]] = None, - approach_height_offset: Optional[ - Annotated[float, "Add approach height offset"] - ] = None, ) -> Optional[ActionFailed]: """A doc string, but not the actual description of the action.""" + grab_height_offset = None + resource_lid_height = None + try: + if source.resource_id: + source_resource = self.resource_client.get_resource(source.resource_id) + if source_resource.quantity == 0: + return ActionFailed( + "Resource manager: Lid does not exist at source! Resource_id:{source.resource_id}." + ) + if source_resource.children: + lid_resource_child = source_resource.children[-1] + if lid_resource_child.attributes: + grab_height_offset = lid_resource_child.attributes.get( + "grab_height_offset", None + ) + resource_lid_height = lid_resource_child.attributes.get( + "lid_height", None + ) + + if target.resource_id: + target_resource = self.resource_client.get_resource(target.resource_id) + if target_resource.quantity == 0: + return ActionFailed( + f"Resource manager: No plate on target! Resource_id:{target.resource_id}." + ) + + # Create temporary lid slot from template + lid_resource = self.resource_client.create_resource_from_template( + template_name="pf400_lid_slot", + resource_name="pf400_lid_slot", + add_to_database=True, + ) + except Exception as e: + return ActionFailed( + errors=[f"Resource manager error during replace lid validation: {e}"] + ) - if source.resource_id: - source_resource = self.resource_client.get_resource(source.resource_id) - if source_resource.quantity == 0: - return ActionFailed( - "Resource manager: Lid does not exist at source! Resource_id:{source.resource_id}." - ) - if target.resource_id: - target_resource = self.resource_client.get_resource(target.resource_id) - if target_resource.quantity == 0: - return ActionFailed( - "Resource manager: No plate on target! Resource_id:{target.resource_id}." - ) + try: + ( + parsed_source, + source_approach, + source_rotation_from_dict, + source_approach_height_offset, + source_height_limit, + source_press_depth, + ) = self._parse_location_representation(source) + ( + parsed_target, + target_approach, + target_rotation_from_dict, + target_approach_height_offset, + target_height_limit, + target_press_depth, + ) = self._parse_location_representation(target) + except Exception as e: + return ActionFailed( + errors=[f"Failed to parse location representation: {e}"] + ) - # Create temporary lid slot from template - lid_resource = self.resource_client.create_resource_from_template( - template_name="pf400_lid_slot", - resource_name="pf400_lid_slot", - add_to_database=True, - ) - target.resource_id = lid_resource.resource_id + parsed_target.resource_id = lid_resource.resource_id - self.pf400_interface.replace_lid( - source=source, - target=target, - lid_height=lid_height, + replace_lid_result = self.pf400_interface.replace_lid( + source=parsed_source, + target=parsed_target, + lid_height=resource_lid_height, source_approach=source_approach, target_approach=target_approach, - source_plate_rotation=source_plate_rotation, - target_plate_rotation=target_plate_rotation, - grab_offset=grab_offset, - approach_height_offset=approach_height_offset, + source_plate_rotation=source_rotation_from_dict, + target_plate_rotation=target_rotation_from_dict, + grab_offset=grab_height_offset, + source_approach_height_offset=source_approach_height_offset, + target_approach_height_offset=target_approach_height_offset, + source_height_limit=source_height_limit, + target_height_limit=target_height_limit, + source_press_depth=source_press_depth, + target_press_depth=target_press_depth, ) + if not replace_lid_result: + return ActionFailed(errors=["Failed to replace lid."]) self.resource_client.remove_resource(lid_resource.resource_id) + if target.resource_id and target_resource.children: + plate_resource = target_resource.children[-1] + if plate_resource.attributes: + plate_resource.attributes["has_lid"] = True + self.resource_client.update_resource(plate_resource) + return None def pause(self) -> None: diff --git a/tests/set_tcp.ipynb b/tests/set_tcp.ipynb new file mode 100644 index 0000000..29c1820 --- /dev/null +++ b/tests/set_tcp.ipynb @@ -0,0 +1,194 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": null, + "id": "0", + "metadata": {}, + "outputs": [], + "source": [ + "from pf400_interface.pf400 import PF400" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "1", + "metadata": {}, + "outputs": [], + "source": [ + "from madsci.common.types.location_types import LocationArgument" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "2", + "metadata": {}, + "outputs": [], + "source": [ + "pf400 = PF400(host=\"rplpf400.cels.anl.gov\")\n", + "pf400.initialize_robot()\n", + "pf400.get_robot_movement_state()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "3", + "metadata": {}, + "outputs": [], + "source": [ + "pf400.open_gripper(130)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "4", + "metadata": {}, + "outputs": [], + "source": [ + "rotation_deck = LocationArgument(\n", + " location_name=\"rotation_deck\",\n", + " location=[63.063, -28.717, 113.020, -82.561, 71.348, 988.948],\n", + ")\n", + "camera_deck = LocationArgument(\n", + " location_name=\"camera_deck\",\n", + " location=[105.024, 22.029, 69.970, -0.551, 73.780, 991.020],\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "5", + "metadata": {}, + "outputs": [], + "source": [ + "camera = pf400.rotate_yaw(camera_deck.representation, -90)\n", + "camera_wide = LocationArgument(\n", + " location_name=\"camera_wide\",\n", + " location=[104.961, 59.382, 36.824, -94.872, 122.180, 990.609],\n", + ")\n", + "print(camera_wide)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "6", + "metadata": {}, + "outputs": [], + "source": [ + "pf400.send_command(\"tool\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "7", + "metadata": {}, + "outputs": [], + "source": [ + "pf400.move_joint(camera_deck.representation)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "8", + "metadata": {}, + "outputs": [], + "source": [ + "pf400.transfer(\n", + " source=camera_wide,\n", + " target=camera_deck,\n", + " rotation_deck=rotation_deck,\n", + " source_plate_rotation=\"wide\",\n", + " target_plate_rotation=\"narrow\",\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "9", + "metadata": {}, + "outputs": [], + "source": [ + "cart = pf400.joint_to_cart(rotation_deck.representation)\n", + "print(cart)\n", + "pf400.move_with_rotation(rotation_deck.representation, -90.0)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "10", + "metadata": {}, + "outputs": [], + "source": [ + "rot = pf400.rotate_yaw(rotation_deck.representation, 90)\n", + "rotation_deck_wide = LocationArgument(location_name=\"rotation_deck_wide\", location=rot)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "11", + "metadata": {}, + "outputs": [], + "source": [ + "pf400.transfer(\n", + " rotation_deck_wide,\n", + " rotation_deck_wide,\n", + " source_plate_rotation=\"wide\",\n", + " target_plate_rotation=\"wide\",\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "12", + "metadata": {}, + "outputs": [], + "source": [ + "pf400.send_command(\"tool 0 1 146.5 0 0 0\")\n", + "pf400.send_command(\"StoreFile\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "13", + "metadata": {}, + "outputs": [], + "source": [ + "pf400.disconnect()" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": ".venv", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.12" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/tests/test_pf400_interface.ipynb b/tests/test_pf400_interface.ipynb index 918f4ab..7968890 100644 --- a/tests/test_pf400_interface.ipynb +++ b/tests/test_pf400_interface.ipynb @@ -69,7 +69,13 @@ "metadata": {}, "outputs": [], "source": [ - "pf400.transfer(source=location, target=location)" + "camera_loc = LocationArgument(\n", + " location_name=\"source\", location=[106.932, 23.387, 69.67, 714.368, 75.058, 994.962]\n", + ")\n", + "rotation_deck = LocationArgument(\n", + " location_name=\"rotation_deck\",\n", + " location=[62.287, -27.703, 113.691, 631.713, 75.088, 994.854],\n", + ")" ] }, { @@ -78,6 +84,224 @@ "id": "6", "metadata": {}, "outputs": [], + "source": [ + "pf400.enablee()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "7", + "metadata": {}, + "outputs": [], + "source": [ + "pf400.disable_compliance()\n", + "pf400.transfer(\n", + " source=camera_loc,\n", + " target=camera_loc,\n", + " rotation_deck=rotation_deck,\n", + " source_plate_rotation=\"narrow\",\n", + " target_plate_rotation=\"wide\",\n", + ")\n", + "# pf400.transfer(source=camera_loc, target=camera_loc, rotation_deck=rotation_deck, source_plate_rotation=\"wide\", target_plate_rotation=\"narrow\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "8", + "metadata": {}, + "outputs": [], + "source": [ + "pf400.disable_compliance()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "9", + "metadata": {}, + "outputs": [], + "source": [ + "pf400.pick_plate(source=rotation_deck)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "10", + "metadata": {}, + "outputs": [], + "source": [ + "pf400.place_plate(target=camera_loc)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "11", + "metadata": {}, + "outputs": [], + "source": [ + "pf400.rotate_plate_on_deck(rotation_degree=90, rotation_deck=rotation_deck)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "12", + "metadata": {}, + "outputs": [], + "source": [ + "rotation_deck_above = LocationArgument(\n", + " location_name=\"rotation_deck_above\",\n", + " location=[75.287, -27.703, 113.691, 631.713, 70.088, 994.854],\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "13", + "metadata": {}, + "outputs": [], + "source": [ + "pf400.move_joint(target_joint_angles=rotation_deck_above.representation)\n", + "pf400.send_command(\"wherec\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "14", + "metadata": {}, + "outputs": [], + "source": [ + "new_loc = pf400.rotate_yaw(\n", + " joint_states=rotation_deck_above.representation, rotation_deg=90.0\n", + ")\n", + "pf400.move_joint(new_loc)\n", + "pf400.send_command(\"wherec\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "15", + "metadata": {}, + "outputs": [], + "source": [ + "cart = pf400.joint_to_cart(rotation_deck_above.representation)\n", + "print(cart)\n", + "pf400.move_with_rotation(rotation_deck_above.representation, 90.0)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "16", + "metadata": {}, + "outputs": [], + "source": [ + "pf400.send_command(\"tool 0 -8 148 0 0 0\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "17", + "metadata": {}, + "outputs": [], + "source": [ + "pf400.open_gripper(pf400.gripper_open_wide)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "18", + "metadata": {}, + "outputs": [], + "source": [ + "pf400.close_gripper(70.0)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "19", + "metadata": {}, + "outputs": [], + "source": [ + "pf400.send_command(\"tool\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "20", + "metadata": {}, + "outputs": [], + "source": [ + "pf400.send_command(\"base\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "21", + "metadata": {}, + "outputs": [], + "source": [ + "pf400.get_cartesian_coordinates()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "22", + "metadata": {}, + "outputs": [], + "source": [ + "pf400.move_cartesian([1427.667, 150.069, 175.292 - 100, -90 + -2.299, 90.0, 180.0])" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "23", + "metadata": {}, + "outputs": [], + "source": [ + "pf400.move_cartesian([1000, 0, 100, 0.0, 90.0, 180.0])" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "24", + "metadata": {}, + "outputs": [], + "source": [ + "pf400.move_neutral()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "25", + "metadata": {}, + "outputs": [], + "source": [ + "pf400.open_gripper()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "26", + "metadata": {}, + "outputs": [], "source": [ "pf400.disconnect()" ] @@ -99,7 +323,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.12.3" + "version": "3.12.12" } }, "nbformat": 4, diff --git a/tests/test_pf400_node.ipynb b/tests/test_pf400_node.ipynb index cf87b71..1f4550b 100644 --- a/tests/test_pf400_node.ipynb +++ b/tests/test_pf400_node.ipynb @@ -1,5 +1,13 @@ { "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# PF400 Node Tests\n", + "## Setup and Basic Tests" + ] + }, { "cell_type": "code", "execution_count": null, @@ -10,7 +18,7 @@ "from madsci.common.types.action_types import ActionRequest\n", "from madsci.common.types.location_types import LocationArgument\n", "\n", - "client = RestNodeClient(url=\"http://localhost:3000\")" + "client = RestNodeClient(url=\"http://parker.cels.anl.gov:2002\")" ] }, { @@ -31,27 +39,150 @@ "client.get_state()" ] }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Test Data Setup" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Test locations\n", + "source_loc = [102.162, 26.226, 67.451, 712.056, 71.524, 995.235] # camera position\n", + "target_loc = [164, 58.489, 86.752, 661.513, 122.221, -994.035]\n", + "rotation_loc = [63, -27.659, 113.485, 631.997, 72.163, 994.458]\n", + "rotation_loc_resource_id = \"01K7T1HRQQMXSB37SA5RGYFKMC\"\n", + "# camera_loc = [90.597, 26.416, 66.422, 714.811, 81.916, 995.074]\n", + "\n", + "# Approach locations (examples)\n", + "source_approach_single = [\n", + " 166.87,\n", + " 0.648,\n", + " 98.948,\n", + " 706.188,\n", + " 70.855,\n", + " 995.16,\n", + "] # camera approach\n", + "source_approach_multiple = [\n", + " [193.278, -4.397, 72.689, 735.146, 70.85, 995.233],\n", + " source_approach_single,\n", + "]" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# ============================================\n", + "# Test Setup: Create Resources\n", + "# ============================================\n", + "\n", + "from madsci.client.resource_client import ResourceClient\n", + "from madsci.common.types.resource_types import Asset, Slot\n", + "\n", + "# Initialize resource client\n", + "resource_client = ResourceClient(resource_server_url=\"http://parker.cels.anl.gov:8004\")\n", + "\n", + "# Create source slot resource\n", + "# source_slot = Slot(\n", + "# resource_name=\"test_source_slot\",\n", + "# resource_class=\"PlateSlot\",\n", + "# capacity=1,\n", + "# attributes={\n", + "# \"description\": \"Test source slot for plate\",\n", + "# },\n", + "# )\n", + "# source_slot = resource_client.add_or_update_resource(source_slot)\n", + "# print(f\"Source slot created: {source_slot.resource_id}\")\n", + "\n", + "# Create target slot resource\n", + "# target_slot = Slot(\n", + "# resource_name=\"test_target_slot\",\n", + "# resource_class=\"PlateSlot\",\n", + "# capacity=1,\n", + "# attributes={\n", + "# \"description\": \"Test target slot for plate\",\n", + "# },\n", + "# )\n", + "# target_slot = resource_client.add_or_update_resource(target_slot)\n", + "# print(f\"Target slot created: {target_slot.resource_id}\")\n", + "\n", + "# Create plate asset resource with attributes\n", + "plate = resource_client.get_resource(\"01K7T1QAXCMSAJ3MAK2GAS24ZK\")\n", + "plate.attributes = {\n", + " \"has_lid\": True,\n", + " \"lid_height\": 2.0,\n", + " \"grab_height_offset\": 0.0,\n", + " \"description\": \"96-well microplate with lid\",\n", + "}\n", + "# plate = Asset(\n", + "# resource_id=\"01K7T1QAXCMSAJ3MAK2GAS24ZK\",\n", + "# resource_name=\"sample_plate\",\n", + "# resource_class=\"Microplate\",\n", + "# attributes={\n", + "# \"has_lid\": True,\n", + "# \"lid_height\": 7.0,\n", + "# \"grab_height_offset\": 20.0,\n", + "# \"description\": \"96-well microplate with lid\",\n", + "# },\n", + "# )\n", + "plate = resource_client.add_or_update_resource(plate)\n", + "print(f\"Plate created with attributes: {plate.resource_id}\")\n", + "\n", + "# Push plate to source slot\n", + "# source_slot = resource_client.push(resource=source_slot.resource_id, child=plate)\n", + "# print(f\"Plate pushed to source slot. Source quantity: {source_slot.quantity}\")\n", + "\n", + "# Store resource IDs for tests\n", + "# source_resource_id = source_slot.resource_id\n", + "# target_resource_id = target_slot.resource_id\n", + "source_resource_id = \"01K7SZNCFGV604BD5C4ZFZQQW9\"\n", + "target_resource_id = \"01K7SZND789S1VN5VX77NKNMRX\"\n", + "\n", + "\n", + "# print(\"\\n=== Resource Setup Complete ===\")\n", + "# print(f\"Source Resource ID: {source_resource_id}\")\n", + "# print(f\"Target Resource ID: {target_resource_id}\")\n", + "# print(f\"Plate Resource ID: {plate.resource_id}\")\n", + "# print(f\"Plate Attributes: {plate.attributes}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Basic Tests " + ] + }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ + "# Test 1: Basic transfer with old style (plain list locations)\n", "request = ActionRequest(\n", " action_name=\"transfer\",\n", " args={\n", " \"source\": LocationArgument(\n", - " location=[223.0, -38.068, 335.876, 325.434, 79.923, 995.062],\n", - " resource_id=\"01JQRWY6SQRZBS8ZXRGZZRMF94\",\n", + " representation=source_loc,\n", + " resource_id=source_resource_id,\n", " ).model_dump(mode=\"json\"),\n", " \"target\": LocationArgument(\n", - " location=[156, 66.112, 83.90, 656.404, 119.405, -946.818],\n", - " resource_id=\"01JQRWY6TGY3G0TYY6ZDT5ZWR5\",\n", + " representation=target_loc,\n", + " resource_id=target_resource_id,\n", " ).model_dump(mode=\"json\"),\n", - " \"source_plate_rotation\": \"narrow\",\n", - " \"target_plate_rotation\": \"wide\",\n", " },\n", - ")" + ")\n", + "print(\"Test 1: Basic transfer\")\n", + "response = client.send_action(action_request=request)" ] }, { @@ -60,15 +191,51 @@ "metadata": {}, "outputs": [], "source": [ + "client.get_action_result(response.action_id)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## New Feature Tests - Dictionary Representation with Rotation" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Test 2: Transfer with rotation using dictionary representation\n", "request = ActionRequest(\n", - " action_name=\"place_plate\",\n", + " action_name=\"transfer\",\n", " args={\n", + " \"source\": LocationArgument(\n", + " representation={\n", + " \"location\": source_loc,\n", + " \"plate_rotation\": \"narrow\",\n", + " },\n", + " resource_id=source_resource_id,\n", + " ).model_dump(mode=\"json\"),\n", " \"target\": LocationArgument(\n", - " location=[156, 66.112, 83.90, 656.404, 119.405, -946.818],\n", - " resource_id=\"01JQRWY6TGY3G0TYY6ZDT5ZWR5\",\n", + " representation={\n", + " \"location\": target_loc,\n", + " \"plate_rotation\": \"wide\",\n", + " \"press_depth\": 5.0,\n", + " },\n", + " resource_id=target_resource_id,\n", + " ).model_dump(mode=\"json\"),\n", + " \"rotation_deck\": LocationArgument(\n", + " representation={\n", + " \"location\": rotation_loc,\n", + " },\n", + " resource_id=rotation_loc_resource_id,\n", " ).model_dump(mode=\"json\"),\n", " },\n", - ")" + ")\n", + "print(\"Test 2: Transfer with rotation (narrow -> wide)\")\n", + "response = client.send_action(action_request=request)" ] }, { @@ -77,7 +244,7 @@ "metadata": {}, "outputs": [], "source": [ - "camera_loc = [90.597, 26.416, 66.422, 714.811, 81.916, 995.074]" + "client.get_action_result(request.action_id)" ] }, { @@ -86,18 +253,135 @@ "metadata": {}, "outputs": [], "source": [ + "# Test 3: Transfer with None rotation (should default to narrow)\n", "request = ActionRequest(\n", - " action_name=\"replace_lid\",\n", + " action_name=\"transfer\",\n", + " args={\n", + " \"source\": LocationArgument(\n", + " representation={\n", + " \"location\": source_loc,\n", + " \"plate_rotation\": None, # Will be treated as narrow\n", + " },\n", + " resource_id=source_resource_id,\n", + " ).model_dump(mode=\"json\"),\n", + " \"target\": LocationArgument(\n", + " representation={\n", + " \"location\": target_loc,\n", + " # No plate_rotation key - will be None\n", + " },\n", + " resource_id=target_resource_id,\n", + " ).model_dump(mode=\"json\"),\n", + " },\n", + ")\n", + "print(\"Test 3: Transfer with None rotation (defaults to narrow)\")\n", + "client.send_action(action_request=request)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "client.get_action_result(request.action_id)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## New Feature Tests - Single Approach Location" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Test 4: Transfer with single approach location\n", + "request = ActionRequest(\n", + " action_name=\"transfer\",\n", " args={\n", + " \"source\": LocationArgument(\n", + " representation={\n", + " \"location\": source_loc,\n", + " \"approach\": source_approach_single,\n", + " \"plate_rotation\": \"narrow\",\n", + " },\n", + " resource_id=source_resource_id,\n", + " ).model_dump(mode=\"json\"),\n", " \"target\": LocationArgument(\n", - " location=[223.0, -38.068, 335.876, 325.434, 79.923, 995.062],\n", - " resource_id=\"01JQRWY6SQRZBS8ZXRGZZRMF94\",\n", + " representation={\n", + " \"location\": target_loc,\n", + " \"plate_rotation\": \"wide\",\n", + " },\n", + " resource_id=target_resource_id,\n", + " ).model_dump(mode=\"json\"),\n", + " \"rotation_deck\": LocationArgument(\n", + " representation={\n", + " \"location\": rotation_loc,\n", + " },\n", + " resource_id=rotation_loc_resource_id,\n", " ).model_dump(mode=\"json\"),\n", + " },\n", + ")\n", + "print(\"Test 4: Transfer with single approach location\")\n", + "client.send_action(action_request=request)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "client.get_action_result(request.action_id)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## New Feature Tests - Multiple Approach Locations" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Test 5: Transfer with multiple approach locations\n", + "request = ActionRequest(\n", + " action_name=\"transfer\",\n", + " args={\n", " \"source\": LocationArgument(\n", - " location=camera_loc, resource_id=\"01JQRWY6T7JQ2QY41Q7QJF0GET\"\n", + " representation={\n", + " \"location\": source_loc,\n", + " \"approach\": source_approach_multiple, # List of lists\n", + " \"plate_rotation\": \"narrow\",\n", + " },\n", + " resource_id=source_resource_id,\n", + " ).model_dump(mode=\"json\"),\n", + " \"target\": LocationArgument(\n", + " representation={\n", + " \"location\": target_loc,\n", + " # \"approach\": source_approach_multiple, # Can have different approach for target\n", + " \"plate_rotation\": \"wide\",\n", + " },\n", + " resource_id=target_resource_id,\n", + " ).model_dump(mode=\"json\"),\n", + " \"rotation_deck\": LocationArgument(\n", + " representation={\n", + " \"location\": rotation_loc,\n", + " },\n", + " resource_id=rotation_loc_resource_id,\n", " ).model_dump(mode=\"json\"),\n", " },\n", - ")" + ")\n", + "print(\"Test 5: Transfer with multiple approach locations\")\n", + "client.send_action(action_request=request)" ] }, { @@ -106,6 +390,117 @@ "metadata": {}, "outputs": [], "source": [ + "client.get_action_result(request.action_id)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## New Feature Tests - Approach Height Offset" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Test 6: Transfer with approach height offset\n", + "request = ActionRequest(\n", + " action_name=\"transfer\",\n", + " args={\n", + " \"source\": LocationArgument(\n", + " representation={\n", + " \"location\": source_loc,\n", + " \"approach_height_offset\": 50.0, # Add 5mm to default approach height\n", + " \"plate_rotation\": \"narrow\",\n", + " },\n", + " resource_id=source_resource_id,\n", + " ).model_dump(mode=\"json\"),\n", + " \"target\": LocationArgument(\n", + " representation={\n", + " \"location\": source_loc,\n", + " \"approach_height_offset\": 10.0, # Different offset for target\n", + " \"plate_rotation\": \"narrow\",\n", + " },\n", + " resource_id=source_resource_id,\n", + " ).model_dump(mode=\"json\"),\n", + " },\n", + ")\n", + "print(\"Test 6: Transfer with approach height offset\")\n", + "client.send_action(action_request=request)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "client.get_action_result(request.action_id)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## New Feature Tests - Height Limit Validation" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Test 7: Pick plate with height limit (should pass)\n", + "request = ActionRequest(\n", + " action_name=\"pick_plate\",\n", + " args={\n", + " \"source\": LocationArgument(\n", + " representation={\n", + " \"location\": source_loc,\n", + " \"height_limit\": 60.0, # High enough limit - should pass\n", + " },\n", + " resource_id=source_resource_id,\n", + " ).model_dump(mode=\"json\"),\n", + " },\n", + ")\n", + "print(\"Test 7: Pick plate with valid height limit\")\n", + "client.send_action(action_request=request)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "client.get_action_result(request.action_id)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Test 8: Pick plate with height limit (should fail)\n", + "request = ActionRequest(\n", + " action_name=\"pick_plate\",\n", + " args={\n", + " \"source\": LocationArgument(\n", + " representation={\n", + " \"location\": source_loc,\n", + " # \"approach_height_offset\": 10.0,\n", + " \"height_limit\": 60.0, # Too low - should fail validation\n", + " },\n", + " resource_id=source_resource_id,\n", + " ).model_dump(mode=\"json\"),\n", + " },\n", + ")\n", + "print(\"Test 8: Pick plate with invalid height limit (should fail)\")\n", "client.send_action(action_request=request)" ] }, @@ -122,6 +517,206 @@ "metadata": {}, "outputs": [], "source": [ + "# This should show ActionFailed with height limit validation error\n", + "client.get_action_result(request.action_id)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## New Feature Tests - Combined Features" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Test 9: Transfer with all features combined\n", + "request = ActionRequest(\n", + " action_name=\"transfer\",\n", + " args={\n", + " \"source\": LocationArgument(\n", + " representation={\n", + " \"location\": source_loc,\n", + " \"approach\": source_approach_multiple,\n", + " \"plate_rotation\": \"narrow\",\n", + " \"approach_height_offset\": 15.0,\n", + " \"height_limit\": 200.0,\n", + " },\n", + " resource_id=source_resource_id,\n", + " ).model_dump(mode=\"json\"),\n", + " \"target\": LocationArgument(\n", + " representation={\n", + " \"location\": target_loc,\n", + " \"plate_rotation\": \"wide\",\n", + " \"approach_height_offset\": 50.0,\n", + " \"height_limit\": 300.0,\n", + " },\n", + " resource_id=target_resource_id,\n", + " ).model_dump(mode=\"json\"),\n", + " \"rotation_deck\": LocationArgument(\n", + " representation={\n", + " \"location\": rotation_loc,\n", + " },\n", + " resource_id=rotation_loc_resource_id,\n", + " ).model_dump(mode=\"json\"),\n", + " },\n", + ")\n", + "print(\"Test 9: Transfer with all features combined\")\n", + "client.send_action(action_request=request)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "client.get_action_result(request.action_id)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Lid Operation Tests" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Test 10: Remove lid with new features\n", + "request = ActionRequest(\n", + " action_name=\"remove_lid\",\n", + " args={\n", + " \"source\": LocationArgument(\n", + " representation={\n", + " \"location\": source_loc,\n", + " \"approach_height_offset\": 15.0,\n", + " },\n", + " resource_id=source_resource_id,\n", + " ).model_dump(mode=\"json\"),\n", + " \"target\": LocationArgument(\n", + " representation={\n", + " \"location\": rotation_loc,\n", + " },\n", + " resource_id=rotation_loc_resource_id,\n", + " ).model_dump(mode=\"json\"),\n", + " },\n", + ")\n", + "print(\"Test 10: Remove lid with approach height offset\")\n", + "client.send_action(action_request=request)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "client.get_action_result(request.action_id)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Test 11: Replace lid with new features\n", + "request = ActionRequest(\n", + " action_name=\"replace_lid\",\n", + " args={\n", + " \"source\": LocationArgument(\n", + " representation={\n", + " \"location\": rotation_loc,\n", + " },\n", + " resource_id=rotation_loc_resource_id,\n", + " ).model_dump(mode=\"json\"),\n", + " \"target\": LocationArgument(\n", + " representation={\n", + " \"location\": source_loc,\n", + " \"approach_height_offset\": 15.0,\n", + " },\n", + " resource_id=source_resource_id,\n", + " ).model_dump(mode=\"json\"),\n", + " },\n", + ")\n", + "print(\"Test 11: Replace lid with approach height offset\")\n", + "client.send_action(action_request=request)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "client.get_action_result(request.action_id)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Place Plate Tests" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Test 12: Place plate with all features\n", + "request = ActionRequest(\n", + " action_name=\"place_plate\",\n", + " args={\n", + " \"target\": LocationArgument(\n", + " representation={\n", + " \"location\": target_loc,\n", + " \"approach\": source_approach_single,\n", + " \"plate_rotation\": \"wide\",\n", + " \"approach_height_offset\": 8.0,\n", + " \"height_limit\": 400.0,\n", + " },\n", + " resource_id=target_resource_id,\n", + " ).model_dump(mode=\"json\"),\n", + " },\n", + ")\n", + "print(\"Test 12: Place plate with all features\")\n", + "client.send_action(action_request=request)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "client.get_action_result(request.action_id)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Utility Commands" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Check last action result\n", "client.get_action_result(request.action_id)" ] }, @@ -131,6 +726,7 @@ "metadata": {}, "outputs": [], "source": [ + "# Reset node\n", "client.send_admin_command(\"reset\")" ] },