diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index dbabe5c943..8c5e3cb7ed 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -4,9 +4,11 @@ on: push: branches: - main + - LTS-main-1.x pull_request: branches: - main + - LTS-main-1.x jobs: build: @@ -15,7 +17,7 @@ jobs: strategy: matrix: os: [ubuntu-latest, macos-latest, windows-latest] - python: ['3.8', '3.9', '3.10'] + python: ['3.9', '3.10', '3.11', '3.12', '3.13'] steps: - uses: compas-dev/compas-actions.build@v3 diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml index 803a09b602..5a098c527c 100644 --- a/.github/workflows/docs.yml +++ b/.github/workflows/docs.yml @@ -4,6 +4,7 @@ on: push: branches: - main + - LTS-main-1.x tags: - 'v*' pull_request_review: diff --git a/.github/workflows/integration.yml b/.github/workflows/integration.yml index a915724c56..e1c5e005e0 100644 --- a/.github/workflows/integration.yml +++ b/.github/workflows/integration.yml @@ -4,23 +4,25 @@ on: push: branches: - main + - LTS-main-1.x pull_request: branches: - main + - LTS-main-1.x jobs: build: - name: ubuntu-py38-integration + name: ubuntu-py311-integration runs-on: ubuntu-latest steps: - uses: actions/checkout@v2 - - name: Set up Python 3.8 + - name: Set up Python 3.11 uses: actions/setup-python@v2 with: - python-version: 3.8 + python-version: '3.11' - name: Set up docker containers run: | - docker-compose -f "tests/integration_setup/docker-compose.yml" up -d --build + docker compose -f "tests/integration_setup/docker-compose.yml" up -d --build docker ps -a - name: Install dependencies run: | diff --git a/.github/workflows/ironpython.yml b/.github/workflows/ironpython.yml index db6c9602c8..198361c5ae 100644 --- a/.github/workflows/ironpython.yml +++ b/.github/workflows/ironpython.yml @@ -4,9 +4,11 @@ on: push: branches: - main + - LTS-main-1.x pull_request: branches: - main + - LTS-main-1.x jobs: build: @@ -26,6 +28,11 @@ jobs: ipy -X:Frames -m pip install --no-deps compas.tar.gz ipy -X:Frames -m pip install --no-deps roslibpy.tar.gz ipy -X:Frames -m pip install --no-deps compas_robots.tar.gz + + # untar and rename, these cannot be installed using ironpip because they not longer have a setup.py + tar -xf compas.tar.gz && for /d %i in (compas-*) do ren "%i" compas + tar -xf compas_robots.tar.gz && for /d %i in (compas_robots-*) do ren "%i" compas_robots + - uses: NuGet/setup-nuget@v1.0.5 - uses: compas-dev/compas-actions.ghpython_components@v5 with: @@ -35,9 +42,9 @@ jobs: run: | ipy -m compas_fab env: - IRONPYTHONPATH: ./src + IRONPYTHONPATH: ./src;./compas/src;./compas_robots/src - name: Run tests run: | ipy tests/ipy_test_runner.py env: - IRONPYTHONPATH: ./src + IRONPYTHONPATH: ./src;./compas/src;./compas_robots/src diff --git a/.github/workflows/pr-checks.yml b/.github/workflows/pr-checks.yml index a61aab5bdc..23a58cecd1 100644 --- a/.github/workflows/pr-checks.yml +++ b/.github/workflows/pr-checks.yml @@ -4,6 +4,7 @@ on: types: [assigned, opened, synchronize, reopened, labeled, unlabeled] branches: - main + - LTS-main-1.x jobs: build: diff --git a/.github/workflows/publish_yak.yml b/.github/workflows/publish_yak.yml new file mode 100644 index 0000000000..89bc070436 --- /dev/null +++ b/.github/workflows/publish_yak.yml @@ -0,0 +1,83 @@ +name: publish_yak + +on: + workflow_dispatch: + inputs: + environment: + description: "Choose deployment environment" + required: true + type: choice + options: + - test + - prod + +jobs: + + publish_test_yak: + runs-on: windows-latest + + steps: + + - name: Set test flag based on input + shell: pwsh + run: | + if ("${{ github.event.inputs.environment }}" -eq "test") { + echo "TEST_FLAG=--test-server" | Out-File -FilePath $env:GITHUB_ENV -Append + } + else { + echo "TEST_FLAG=" | Out-File -FilePath $env:GITHUB_ENV -Append + } + + - name: Checkout repo + uses: actions/checkout@v4 + + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install -r requiremnets-dev.txt + + - name: Create CPython Grasshopper user objects + run: | + invoke build-cpython-ghuser-components + + - name: Create IronPython Grasshopper user objects + run: | + choco install ironpython --version=2.7.8.1 + invoke clean + invoke build-ghuser-components + + - name: Create Rhino7 Yak package + shell: pwsh + run: | + invoke yakerize -m $Env:YAK_TEMPLATE\manifest.yml -l $Env:YAK_TEMPLATE\icon.png -g $Env:USER_OBJECTS -t rh7 + env: + USER_OBJECTS: src\compas_fab\ghpython\components\ghuser + YAK_TEMPLATE: src\compas_fab\ghpython\yak_template + + - name: Publish to Yak server (Rhino 7) + shell: pwsh + run: | + $test_flag = if ($Env:TEST_FLAG) { $Env:TEST_FLAG } else { "" } + $file = Get-ChildItem -Path dist\yak_package\*rh7*.yak -File | Select-Object -ExpandProperty Name + $command = "invoke publish-yak -y dist\yak_package\$file $test_flag".Trim() + Invoke-Expression $command + env: + YAK_TOKEN: ${{ secrets.YAK_DF_TOKEN }} + + - name: Create Rhino8 Yak package + shell: pwsh + run: | + invoke yakerize -m $Env:YAK_TEMPLATE\manifest.yml -l $Env:YAK_TEMPLATE\icon.png -g $Env:USER_OBJECTS -t rh8 + env: + USER_OBJECTS: src\compas_fab\ghpython\components_cpython\ghuser + YAK_TEMPLATE: src\compas_fab\ghpython\yak_template + + - name: Publish to Yak server (Rhino 8) + shell: pwsh + run: | + $test_flag = if ($Env:TEST_FLAG) { $Env:TEST_FLAG } else { "" } + $file = Get-ChildItem -Path dist\yak_package\*rh8*.yak -File | Select-Object -ExpandProperty Name + $command = "invoke publish-yak -y dist\yak_package\$file $test_flag".Trim() + Invoke-Expression $command + env: + YAK_TOKEN: ${{ secrets.YAK_DF_TOKEN }} diff --git a/.gitignore b/.gitignore index 71c538a6e8..f934e9b26d 100644 --- a/.gitignore +++ b/.gitignore @@ -120,3 +120,4 @@ temp/** # Grasshopper generated objects src/compas_fab/ghpython/components/ghuser/*.ghuser +src/compas_fab/ghpython/components_cpython/ghuser/*.ghuser diff --git a/CHANGELOG.md b/CHANGELOG.md index 2dee4023d9..a182547c35 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,6 +5,27 @@ All notable changes to this project will be documented in this file. The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/), and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html). +## [Unreleased] + +### Added + +* Added helper function `message` to `compas_fab.ghpython.components`. +* Added helper function `error` to `compas_fab.ghpython.components`. +* Added helper function `remark` to `compas_fab.ghpython.components`. +* Added helper function `warning` to `compas_fab.ghpython.components`. +* Added GH component definitions compatible with CPython in Rhino8. + +### Changed + +* Updated dev dependency to `compas_invocations2`. +* Fixed `AttributeError` in `inverse_kinematics_spherical_wrist()`. +* Fixed `AttributeError` in VisualizeRobot GH component. + +### Removed + +* Removed `create_id` from `compas_fab.ghpython.components`, using `compas_ghpython.create_id` instead. + + ## [1.0.2] 2024-02-22 ### Added diff --git a/pyproject.toml b/pyproject.toml index db1ccb638e..8e4889f93f 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -24,3 +24,34 @@ known_first_party = "compas_fab" default_section = "THIRDPARTY" forced_separate = "test_compas_fab" skip = ["__init__.py"] + +[tool.ruff] +line-length = 120 +indent-width = 4 +target-version = "py39" + +[tool.ruff.lint] +select = ["E", "F", "I"] + +[tool.ruff.lint.per-file-ignores] +"__init__.py" = ["I001"] +"tests/*" = ["I001"] +"tasks.py" = ["I001"] +"src/compas_fab/ghpython/components/*/code.py" = ["F821", "F401"] +"src/compas_fab/ghpython/components_cpython/*/code.py" = ["F821", "F401"] + +[tool.ruff.lint.isort] +force-single-line = true +known-first-party = [ + "compas_fab" +] + +[tool.ruff.lint.pydocstyle] +convention = "numpy" + +[tool.ruff.lint.pycodestyle] +max-line-length = 179 + +[tool.ruff.format] +docstring-code-format = true +docstring-code-line-length = "dynamic" diff --git a/requirements-dev.txt b/requirements-dev.txt index b3619e19db..9150706453 100644 --- a/requirements-dev.txt +++ b/requirements-dev.txt @@ -1,19 +1,16 @@ attrs >=19.3.0 -autopep8 black bump2version >=1.0.1 check-manifest >=0.36 -compas_invocations -doc8 -flake8 +compas-invocations2 +ruff importlib_metadata <5.0 invoke>=0.14 -isort -pylint pytest pytest_mock pytest-cov sphinx_compas2_theme -sybil +sybil~=8.0.1 twine +tomlkit -e . diff --git a/src/compas_fab/backends/interfaces/client.py b/src/compas_fab/backends/interfaces/client.py index 057c04e53f..e678da8c34 100644 --- a/src/compas_fab/backends/interfaces/client.py +++ b/src/compas_fab/backends/interfaces/client.py @@ -1,6 +1,7 @@ from __future__ import absolute_import from __future__ import division from __future__ import print_function + from compas_fab.backends.exceptions import BackendFeatureNotSupportedError diff --git a/src/compas_fab/backends/kinematics/solvers/spherical_wrist.py b/src/compas_fab/backends/kinematics/solvers/spherical_wrist.py index 9025cb03d1..a2c45b5f73 100644 --- a/src/compas_fab/backends/kinematics/solvers/spherical_wrist.py +++ b/src/compas_fab/backends/kinematics/solvers/spherical_wrist.py @@ -174,7 +174,10 @@ def inverse_kinematics_spherical_wrist(target_frame, points): elbow_frame = Frame(p1A, elbow_dir, [0, 0, 1]) elbow_plane = (p1A, elbow_frame.normal) - _, (center, radius, normal) = intersection_sphere_sphere(sphere1, sphere2) + sphere1_prim = (sphere1.base, sphere1.radius) + sphere2_prim = (sphere2.base, sphere2.radius) + + _, (center, radius, normal) = intersection_sphere_sphere(sphere1_prim, sphere2_prim) circle = ((center, normal), radius) intersect_pt1, intersect_pt2 = intersection_plane_circle(elbow_plane, circle) diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py index 0a6eaa1153..eb874fedb1 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py @@ -106,7 +106,7 @@ def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=N # I don't know what jointRanges needs to be. Erwin Coumans knows, but he isn't telling. # https://stackoverflow.com/questions/49674179/understanding-inverse-kinematics-pybullet # https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/preview?pru=AAABc7276PI*zazLer2rlZ8tAUI8lF98Kw#heading=h.9i02ojf4k3ve - joint_ranges = [u - l for u, l in zip(upper_limits, lower_limits)] + joint_ranges = [upper - lower for upper, lower in zip(upper_limits, lower_limits)] if options.get("semi-constrained"): ik_options.update( diff --git a/src/compas_fab/backends/pybullet/client.py b/src/compas_fab/backends/pybullet/client.py index e3cedc7d1f..72eb2bc154 100644 --- a/src/compas_fab/backends/pybullet/client.py +++ b/src/compas_fab/backends/pybullet/client.py @@ -139,8 +139,8 @@ class PyBulletClient(PyBulletBase, ClientInterface): Examples -------- >>> from compas_fab.backends import PyBulletClient - >>> with PyBulletClient(connection_type='direct') as client: - ... print('Connected: %s' % client.is_connected) + >>> with PyBulletClient(connection_type="direct") as client: + ... print("Connected: %s" % client.is_connected) Connected: True """ diff --git a/src/compas_fab/backends/pybullet/utils.py b/src/compas_fab/backends/pybullet/utils.py index f818ecc548..b87b3e78a0 100644 --- a/src/compas_fab/backends/pybullet/utils.py +++ b/src/compas_fab/backends/pybullet/utils.py @@ -25,10 +25,10 @@ def redirect_stdout(to=os.devnull, enabled=True): Examples -------- - >>> import os # doctest: +SKIP - >>> with redirect_stdout(to='filename'): # doctest: +SKIP - ... print("from Python") # doctest: +SKIP - ... os.system("echo non-Python applications are also supported") # doctest: +SKIP + >>> import os # doctest: +SKIP + >>> with redirect_stdout(to="filename"): # doctest: +SKIP + ... print("from Python") # doctest: +SKIP + ... os.system("echo non-Python applications are also supported") # doctest: +SKIP """ def _redirect_stdout(to_): diff --git a/src/compas_fab/backends/ros/client.py b/src/compas_fab/backends/ros/client.py index ce9edbd626..e725ba2142 100644 --- a/src/compas_fab/backends/ros/client.py +++ b/src/compas_fab/backends/ros/client.py @@ -82,7 +82,7 @@ def from_local_cache_directory(cls, local_cache_directory): False - >>> local_directory = os.path.join(os.path.expanduser('~'), 'robot_description', 'robocop') + >>> local_directory = os.path.join(os.path.expanduser("~"), "robot_description", "robocop") >>> info = LocalCacheInfo.from_local_cache_directory(local_directory) >>> info.use_local_cache True @@ -126,7 +126,7 @@ class RosClient(Ros, ClientInterface): >>> from compas_fab.backends import RosClient >>> with RosClient() as client: - ... print('Connected: %s' % client.is_connected) + ... print("Connected: %s" % client.is_connected) Connected: True Notes diff --git a/src/compas_fab/backends/ros/messages/geometry_msgs.py b/src/compas_fab/backends/ros/messages/geometry_msgs.py index e12a7bfa03..c5a9173cef 100644 --- a/src/compas_fab/backends/ros/messages/geometry_msgs.py +++ b/src/compas_fab/backends/ros/messages/geometry_msgs.py @@ -201,7 +201,7 @@ class Inertia(ROSmsg): Examples -------- - >>> inertia = compas_fab.robots.Inertia([[0] * 3] * 3, 1., [0.1, 3.1, 4.4]) + >>> inertia = compas_fab.robots.Inertia([[0] * 3] * 3, 1.0, [0.1, 3.1, 4.4]) >>> ros_inertia = Inertia.from_inertia(inertia) >>> ros_inertia.msg {'m': 1.0, 'com': {'x': 0.1, 'y': 3.1, 'z': 4.4}, 'ixx': 0.0, 'ixy': 0.0, 'ixz': 0.0, 'iyy': 0.0, 'iyz': 0.0, 'izz': 0.0} diff --git a/src/compas_fab/ghpython/components/Cf_PlanCartesianMotion/code.py b/src/compas_fab/ghpython/components/Cf_PlanCartesianMotion/code.py index 305f4c3820..36484bc6f6 100644 --- a/src/compas_fab/ghpython/components/Cf_PlanCartesianMotion/code.py +++ b/src/compas_fab/ghpython/components/Cf_PlanCartesianMotion/code.py @@ -4,12 +4,11 @@ COMPAS FAB v1.0.2 """ +from compas_ghpython import create_id from compas_rhino.conversions import plane_to_compas_frame from ghpythonlib.componentbase import executingcomponent as component from scriptcontext import sticky as st -from compas_fab.ghpython.components import create_id - class PlanCartesianMotion(component): def RunScript( diff --git a/src/compas_fab/ghpython/components/Cf_PlanMotion/code.py b/src/compas_fab/ghpython/components/Cf_PlanMotion/code.py index c25f0fd255..cc2033f4c6 100644 --- a/src/compas_fab/ghpython/components/Cf_PlanMotion/code.py +++ b/src/compas_fab/ghpython/components/Cf_PlanMotion/code.py @@ -4,11 +4,10 @@ COMPAS FAB v1.0.2 """ +from compas_ghpython import create_id from ghpythonlib.componentbase import executingcomponent as component from scriptcontext import sticky as st -from compas_fab.ghpython.components import create_id - class PlanMotion(component): def RunScript( diff --git a/src/compas_fab/ghpython/components/Cf_PlanningScene/code.py b/src/compas_fab/ghpython/components/Cf_PlanningScene/code.py index cd37c2ad55..dc5b02be25 100644 --- a/src/compas_fab/ghpython/components/Cf_PlanningScene/code.py +++ b/src/compas_fab/ghpython/components/Cf_PlanningScene/code.py @@ -4,10 +4,10 @@ COMPAS FAB v1.0.2 """ +from compas_ghpython import create_id from ghpythonlib.componentbase import executingcomponent as component from scriptcontext import sticky as st -from compas_fab.ghpython.components import create_id from compas_fab.robots import PlanningScene diff --git a/src/compas_fab/ghpython/components/Cf_RosConnect/code.py b/src/compas_fab/ghpython/components/Cf_RosConnect/code.py index 94bf44ff39..eae8c59fd0 100644 --- a/src/compas_fab/ghpython/components/Cf_RosConnect/code.py +++ b/src/compas_fab/ghpython/components/Cf_RosConnect/code.py @@ -4,11 +4,11 @@ COMPAS FAB v1.0.2 """ +from compas_ghpython import create_id from ghpythonlib.componentbase import executingcomponent as component from scriptcontext import sticky as st from compas_fab.backends import RosClient -from compas_fab.ghpython.components import create_id class ROSConnect(component): diff --git a/src/compas_fab/ghpython/components/Cf_RosRobot/code.py b/src/compas_fab/ghpython/components/Cf_RosRobot/code.py index 5aad8e3136..6472fd0bbb 100644 --- a/src/compas_fab/ghpython/components/Cf_RosRobot/code.py +++ b/src/compas_fab/ghpython/components/Cf_RosRobot/code.py @@ -4,11 +4,11 @@ COMPAS FAB v1.0.2 """ -from compas.scene import SceneObject +from compas_ghpython import create_id from ghpythonlib.componentbase import executingcomponent as component from scriptcontext import sticky as st -from compas_fab.ghpython.components import create_id +from compas.scene import SceneObject class ROSRobot(component): @@ -18,7 +18,7 @@ def RunScript(self, ros_client, load): if ros_client and ros_client.is_connected and load: # Load URDF from ROS st[key] = ros_client.load_robot(load_geometry=True, precision=12) - st[key].scene_object = SceneObject(st[key].model) + st[key].scene_object = SceneObject(item=st[key].model) robot = st.get(key, None) if robot: # client sometimes need to be restarted, without needing to reload geometry diff --git a/src/compas_fab/ghpython/components/Cf_RosTopicPublish/code.py b/src/compas_fab/ghpython/components/Cf_RosTopicPublish/code.py index 2684c9252e..ca8c009e11 100644 --- a/src/compas_fab/ghpython/components/Cf_RosTopicPublish/code.py +++ b/src/compas_fab/ghpython/components/Cf_RosTopicPublish/code.py @@ -6,12 +6,12 @@ import time +from compas_ghpython import create_id from ghpythonlib.componentbase import executingcomponent as component from roslibpy import Topic from scriptcontext import sticky as st from compas_fab.backends.ros.messages import ROSmsg -from compas_fab.ghpython.components import create_id class ROSTopicPublish(component): diff --git a/src/compas_fab/ghpython/components/Cf_RosTopicSubscribe/code.py b/src/compas_fab/ghpython/components/Cf_RosTopicSubscribe/code.py index 7efec6c02d..ba1c2b74cb 100644 --- a/src/compas_fab/ghpython/components/Cf_RosTopicSubscribe/code.py +++ b/src/compas_fab/ghpython/components/Cf_RosTopicSubscribe/code.py @@ -7,12 +7,12 @@ import time import Grasshopper.Kernel +from compas_ghpython import create_id from ghpythonlib.componentbase import executingcomponent as component from roslibpy import Topic from scriptcontext import sticky as st from compas_fab.backends.ros.messages import ROSmsg -from compas_fab.ghpython.components import create_id class ROSTopicSubscribe(component): diff --git a/src/compas_fab/ghpython/components/Cf_VisualizeRobot/code.py b/src/compas_fab/ghpython/components/Cf_VisualizeRobot/code.py index 143c75ff09..100322be1c 100644 --- a/src/compas_fab/ghpython/components/Cf_VisualizeRobot/code.py +++ b/src/compas_fab/ghpython/components/Cf_VisualizeRobot/code.py @@ -6,14 +6,14 @@ import time -from compas.geometry import Frame -from compas.geometry import Transformation -from compas.scene import SceneObject from compas_ghpython import create_id from compas_rhino.conversions import frame_to_rhino_plane from ghpythonlib.componentbase import executingcomponent as component from scriptcontext import sticky as st +from compas.geometry import Frame +from compas.geometry import Transformation +from compas.scene import SceneObject from compas_fab.backends import BackendFeatureNotSupportedError from compas_fab.robots import PlanningScene @@ -115,7 +115,7 @@ def RunScript( else: mesh = cm.mesh - collision_meshes.extend(SceneObject(mesh).draw()) + collision_meshes.extend(SceneObject(item=mesh).draw()) cached_scene["cm"] = collision_meshes @@ -136,7 +136,7 @@ def RunScript( mesh = acm.collision_mesh.mesh.transformed(t) - attached_meshes.extend(SceneObject(mesh).draw()) + attached_meshes.extend(SceneObject(item=mesh).draw()) cached_scene["acm"] = attached_meshes diff --git a/src/compas_fab/ghpython/components/__init__.py b/src/compas_fab/ghpython/components/__init__.py index cecffffbf4..07eb542b82 100644 --- a/src/compas_fab/ghpython/components/__init__.py +++ b/src/compas_fab/ghpython/components/__init__.py @@ -1,5 +1,58 @@ from __future__ import absolute_import +try: + import Grasshopper # type: ignore +except ImportError: + pass -def create_id(component, name): - return "{}_{}".format(name, component.InstanceGuid) + +def warning(component, message): + """Add a warning message to the component. + + Parameters + ---------- + component : Grasshopper.Kernel.IGH_Component + The component instance. Pre-Rhino8 use `self`. Post-Rhino8 use `ghenv.Component`. + message : str + The message to display. + """ + component.AddRuntimeMessage(Grasshopper.Kernel.GH_RuntimeMessageLevel.Warning, message) + + +def error(component, message): + """Add an error message to the component. + + Parameters + ---------- + component : Grasshopper.Kernel.IGH_Component + The component instance. Pre-Rhino8 use `self`. Post-Rhino8 use `ghenv.Component`. + message : str + The message to display. + """ + component.AddRuntimeMessage(Grasshopper.Kernel.GH_RuntimeMessageLevel.Error, message) + + +def remark(component, message): + """Add a remark message to the component. + + Parameters + ---------- + component : Grasshopper.Kernel.IGH_Component + The component instance. Pre-Rhino8 use `self`. Post-Rhino8 use `ghenv.Component`. + message : str + The message to display. + """ + component.AddRuntimeMessage(Grasshopper.Kernel.GH_RuntimeMessageLevel.Remark, message) + + +def message(component, message): + """Add a text that will appear under the component. + + Parameters + ---------- + component : Grasshopper.Kernel.IGH_Component + The component instance. Pre-Rhino8 use `self`. Post-Rhino8 use `ghenv.Component`. + message : str + The message to display. + """ + component.Message = message diff --git a/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/code.py b/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/code.py new file mode 100644 index 0000000000..139bf2d270 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/code.py @@ -0,0 +1,42 @@ +# r: compas_fab>=1.0.2 +""" +Attach a tool to the robot. + +COMPAS FAB v1.0.2 +""" + +import Grasshopper +import Rhino +from compas.geometry import Frame +from compas_rhino.conversions import mesh_to_compas +from compas_rhino.conversions import plane_to_compas_frame + +from compas_fab.robots import Tool + + +class AttachToolComponent(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript( + self, + robot, + visual_mesh: Rhino.Geometry.Mesh, + collision_mesh: Rhino.Geometry.Mesh, + tcf_plane, + group, + connected_to, + ): + if robot and robot.client and robot.client.is_connected and visual_mesh: + if not collision_mesh: + collision_mesh = visual_mesh + + c_visual_mesh = mesh_to_compas(visual_mesh) + c_collision_mesh = mesh_to_compas(collision_mesh) + + if not tcf_plane: + frame = Frame.worldXY() + else: + frame = plane_to_compas_frame(tcf_plane) + tool = Tool(c_visual_mesh, frame, c_collision_mesh, connected_to=connected_to) + + robot.attach_tool(tool, group) + + return robot diff --git a/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/icon.png new file mode 100644 index 0000000000..50e13aa3fe Binary files /dev/null and b/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/icon.png differ diff --git a/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/metadata.json new file mode 100644 index 0000000000..d4aafacbae --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/metadata.json @@ -0,0 +1,48 @@ +{ + "name": "Attached Tool", + "nickname": "Tool", + "category": "COMPAS FAB", + "subcategory": "Scene", + "description": "Attach a tool to the robot.", + "exposure": 4, + + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "robot", + "description": "The robot.", + "wireDisplay": "hidden" + }, + { + "name": "visual_mesh", + "description": "A visual mesh for the tool.", + "typeHintID": "mesh" + }, + { + "name": "collision_mesh", + "description": "A collision mesh for the tool.", + "typeHintID": "mesh" + }, + { + "name": "tcf_plane", + "description": "The frame of the tool in tool0 frame." + }, + { + "name": "group", + "description": "The planning group to which the tool should be attached." + }, + { + "name": "connected_to", + "description": "The link to which the tool should be connected to." + } + ], + "outputParameters": [ + { + "name": "robot", + "description": "The robot." + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/code.py b/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/code.py new file mode 100644 index 0000000000..e196a6692b --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/code.py @@ -0,0 +1,38 @@ +# r: compas_fab>=1.0.2 +""" +Add an attached collision mesh to the robot. + +COMPAS FAB v1.0.2 +""" + +import Grasshopper +import Rhino +import System +from compas_rhino.conversions import mesh_to_compas + +from compas_fab.robots import AttachedCollisionMesh +from compas_fab.robots import CollisionMesh + + +class AttachedCollisionMeshComponent(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript( + self, + scene, + mesh: Rhino.Geometry.Mesh, + identifier: str, + link_name: str, + touch_links: System.Collections.Generic.List[str], + add: bool, + remove: bool, + ): + attached_collision_mesh = None + if scene and mesh and identifier and link_name: + compas_mesh = mesh_to_compas(mesh) + collision_mesh = CollisionMesh(compas_mesh, identifier) + attached_collision_mesh = AttachedCollisionMesh(collision_mesh, link_name, touch_links) + if add: + scene.add_attached_collision_mesh(attached_collision_mesh) + if remove: + scene.remove_attached_collision_mesh(identifier) + scene.remove_collision_mesh(identifier) + return attached_collision_mesh diff --git a/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/icon.png new file mode 100644 index 0000000000..ce20f794b7 Binary files /dev/null and b/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/icon.png differ diff --git a/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/metadata.json new file mode 100644 index 0000000000..408c71dc91 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/metadata.json @@ -0,0 +1,57 @@ +{ + "name": "Attached Collision Mesh", + "nickname": "ACM", + "category": "COMPAS FAB", + "subcategory": "Scene", + "description": "Add or remove an attached collision mesh to the robot.", + "exposure": 4, + + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "scene", + "description": "The planning scene." + }, + { + "name": "mesh", + "description": "A collision mesh.", + "typeHintID": "mesh" + }, + { + "name": "identifier", + "description": "The identifier of the collision mesh.", + "typeHintID": "str" + }, + { + "name": "link_name", + "description": "The robot's link name to attach the mesh to.", + "typeHintID": "str" + }, + { + "name": "touch_links", + "description": "Names of links that the robot is allowed to touch.", + "typeHintID": "str", + "scriptParamAccess": "list" + }, + { + "name": "add", + "description": "If True, adds the collision mesh to the planning scene.", + "typeHintID": "bool" + }, + { + "name": "remove", + "description": "If True, removes the collision mesh from the planning scene.", + "typeHintID": "bool" + } + ], + "outputParameters": [ + { + "name": "attached_collision_mesh", + "description": "A collision mesh that is attached to a robot's link.", + "optional": false + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/code.py b/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/code.py new file mode 100644 index 0000000000..fe6d73e2c7 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/code.py @@ -0,0 +1,45 @@ +# r: compas_fab>=1.0.2 +""" +Add or remove a collision mesh from the planning scene. + +COMPAS FAB v1.0.2 +""" + +import Grasshopper +import Rhino +from compas_rhino.conversions import mesh_to_compas + +from compas_fab.ghpython.components import error +from compas_fab.ghpython.components import message +from compas_fab.robots import CollisionMesh + + +class CollisionMeshComponent(Grasshopper.Kernel.GH_ScriptInstance): + @property + def component(self): + return ghenv.Component + + def RunScript(self, scene, mesh: Rhino.Geometry.Mesh, identifier: str, add: bool, append: bool, remove: bool): + ok = False + message(self.component, "") + + if (add and append) or (append and remove) or (add and remove): + error(self.component, "Use only one operation at a time\n(add, append or remove)") + return + + if scene and mesh and identifier: + compas_mesh = mesh_to_compas(mesh) + collision_mesh = CollisionMesh(compas_mesh, identifier) + if add: + scene.add_collision_mesh(collision_mesh) + message(self.component, "Added") + ok = True + if append: + scene.append_collision_mesh(collision_mesh) + message(self.component, "Appended") + ok = True + if remove: + scene.remove_collision_mesh(identifier) + message(self.component, "Removed") + ok = True + return ok diff --git a/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/icon.png new file mode 100644 index 0000000000..8fe7732766 Binary files /dev/null and b/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/icon.png differ diff --git a/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/metadata.json new file mode 100644 index 0000000000..cee428daae --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/metadata.json @@ -0,0 +1,50 @@ +{ + "name": "Collision Mesh", + "nickname": "CM", + "category": "COMPAS FAB", + "subcategory": "Scene", + "description": "Add or remove a collision mesh from the planning scene.", + "exposure": 4, + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "scene", + "description": "The planning scene." + }, + { + "name": "mesh", + "description": "A collision mesh.", + "typeHintID": "mesh" + }, + { + "name": "identifier", + "description": "The identifier of the collision mesh.", + "typeHintID": "str" + }, + { + "name": "add", + "description": "If True, adds the collision mesh to the planning scene.", + "typeHintID": "bool" + }, + { + "name": "append", + "description": "If True, appends the collision mesh to the planning scene.", + "typeHintID": "bool" + }, + { + "name": "remove", + "description": "If True, removes the collision mesh from the planning scene.", + "typeHintID": "bool" + } + ], + "outputParameters": [ + { + "name": "ok", + "description": "True if the operation was successfully performed.", + "optional": false + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConfigMerge/code.py b/src/compas_fab/ghpython/components_cpython/Cf_ConfigMerge/code.py new file mode 100644 index 0000000000..cdcde7e6ed --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_ConfigMerge/code.py @@ -0,0 +1,16 @@ +# r: compas_fab>=1.0.2 +""" +Merge two configurations. + +COMPAS FAB v1.0.2 +""" + +import Grasshopper + + +class ConfigMerge(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript(self, config_a, config_b): + if config_a and config_b: + return config_a.merged(config_b) + + return None diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConfigMerge/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_ConfigMerge/icon.png new file mode 100644 index 0000000000..0bd8c533a0 Binary files /dev/null and b/src/compas_fab/ghpython/components_cpython/Cf_ConfigMerge/icon.png differ diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConfigMerge/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_ConfigMerge/metadata.json new file mode 100644 index 0000000000..c63dc20c15 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_ConfigMerge/metadata.json @@ -0,0 +1,29 @@ +{ + "name": "Merge config", + "nickname": "Merge config", + "category": "COMPAS FAB", + "subcategory": "Config", + "description": "Merge two configurations.", + "exposure": 4, + + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "config_a", + "description": "The configuration to use as base for the merge." + }, + { + "name": "config_b", + "description": "The configuration used to merge into the base one." + } + ], + "outputParameters": [ + { + "name": "config", + "description": "The merged configuration." + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConfigZero/code.py b/src/compas_fab/ghpython/components_cpython/Cf_ConfigZero/code.py new file mode 100644 index 0000000000..40e9997afe --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_ConfigZero/code.py @@ -0,0 +1,16 @@ +# r: compas_fab>=1.0.2 +""" +Get a zero configuration for a robot. + +COMPAS FAB v1.0.2 +""" + +import Grasshopper + + +class ConfigZero(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript(self, robot, group): + if robot: + return robot.zero_configuration(group) + + return None diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConfigZero/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_ConfigZero/icon.png new file mode 100644 index 0000000000..02520fe07e Binary files /dev/null and b/src/compas_fab/ghpython/components_cpython/Cf_ConfigZero/icon.png differ diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConfigZero/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_ConfigZero/metadata.json new file mode 100644 index 0000000000..32b547efa9 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_ConfigZero/metadata.json @@ -0,0 +1,31 @@ +{ + "name": "Zero config", + "nickname": "Zero config", + "category": "COMPAS FAB", + "subcategory": "Config", + "description": "Get a zero configuration for a robot.", + "exposure": 4, + + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "robot", + "description": "The robot.", + "wireDisplay": "hidden" + }, + { + "name": "group", + "description": "If assigned, it will return the zero configuration for the given group, otherwise a full zero configuration.", + "typeHintID": "str" + } + ], + "outputParameters": [ + { + "name": "config", + "description": "The zero configuration." + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromConfiguration/code.py b/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromConfiguration/code.py new file mode 100644 index 0000000000..4160b59293 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromConfiguration/code.py @@ -0,0 +1,33 @@ +# r: compas_fab>=1.0.2 +""" +Create joint constraints for each of the robot's configurable joints based on a given target configuration. + +COMPAS FAB v1.0.2 +""" + +import math + +import Grasshopper +import System + + +class ConstraintsFromTargetConfiguration(Grasshopper.Kernel.GH_ScriptInstance): + DEFAULT_TOLERANCE_METERS = 0.001 + DEFAULT_TOLERANCE_RADIANS = math.radians(1) + + def RunScript(self, robot, target_configuration, tolerance_above, tolerance_below, group): + if robot and target_configuration: + tolerance_above = tolerance_above or self._generate_default_tolerances(robot.get_configurable_joints(group)) + tolerance_below = tolerance_below or self._generate_default_tolerances(robot.get_configurable_joints(group)) + + constraints = robot.constraints_from_configuration( + configuration=target_configuration, + tolerances_above=tolerance_above, + tolerances_below=tolerance_below, + group=group, + ) + + return constraints + + def _generate_default_tolerances(self, joints): + return [self.DEFAULT_TOLERANCE_METERS if j.is_scalable() else self.DEFAULT_TOLERANCE_RADIANS for j in joints] diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromConfiguration/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromConfiguration/icon.png new file mode 100644 index 0000000000..d721d6bc2e Binary files /dev/null and b/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromConfiguration/icon.png differ diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromConfiguration/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromConfiguration/metadata.json new file mode 100644 index 0000000000..1835341b2d --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromConfiguration/metadata.json @@ -0,0 +1,45 @@ +{ + "name": "Constraints From Target Configuration", + "nickname": "Constraints From Configuration", + "category": "COMPAS FAB", + "subcategory": "Planning", + "description": "Create joint constraints for each of the robot's configurable joints based on a given target configuration.", + "exposure": 8, + + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "robot", + "description": "The robot.", + "wireDisplay": "hidden" + }, + { + "name": "target_configuration", + "description": "The target configuration." + }, + { + "name": "tolerance_above", + "description": "The tolerances above the targeted joint value of each configurable joint, defining the upper bound in radians/meters to be achieved.", + "typeHintID": "float" + }, + { + "name": "tolerance_below", + "description": "The tolerances below the targeted joint value of each configurable joint, defining the lower bound in radians/meters to be achieved.", + "typeHintID": "float" + }, + { + "name": "group", + "description": "The planning group for which we specify the constraint. Defaults to the robot's main planning group.", + "typeHintID": "str" + } + ], + "outputParameters": [ + { + "name": "constraints", + "description": "A list joint constraints in radians/meters." + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromPlane/code.py b/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromPlane/code.py new file mode 100644 index 0000000000..9d14324377 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromPlane/code.py @@ -0,0 +1,32 @@ +# r: compas_fab>=1.0.2 +""" +Create a position and an orientation constraint from a plane calculated for the group's end-effector link. + +COMPAS FAB v1.0.2 +""" + +import math + +import Grasshopper +import System +from compas_rhino.conversions import plane_to_compas_frame + + +class ConstraintsFromPlane(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript(self, robot, plane, group, tolerance_position, tolerance_xaxis, tolerance_yaxis, tolerance_zaxis): + goal_constraints = None + if robot and plane: + tolerance_position = tolerance_position or 0.001 + tolerance_xaxis = tolerance_xaxis or 1.0 + tolerance_yaxis = tolerance_yaxis or 1.0 + tolerance_zaxis = tolerance_zaxis or 1.0 + + frame = plane_to_compas_frame(plane) + tolerances_axes = [ + math.radians(tolerance_xaxis), + math.radians(tolerance_yaxis), + math.radians(tolerance_zaxis), + ] + goal_constraints = robot.constraints_from_frame(frame, tolerance_position, tolerances_axes, group) + + return goal_constraints diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromPlane/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromPlane/icon.png new file mode 100644 index 0000000000..0613d36bb2 Binary files /dev/null and b/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromPlane/icon.png differ diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromPlane/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromPlane/metadata.json new file mode 100644 index 0000000000..2e4473c5da --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromPlane/metadata.json @@ -0,0 +1,55 @@ +{ + "name": "Constraints From Plane", + "nickname": "Constraints From Plane", + "category": "COMPAS FAB", + "subcategory": "Planning", + "description": "Create a position and an orientation constraint from a plane calculated for the group's end-effector link.", + "exposure": 8, + + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "robot", + "description": "The robot.", + "wireDisplay": "hidden" + }, + { + "name": "plane", + "description": "The plane or frame from which we create position and orientation constraints." + }, + { + "name": "group", + "description": "The planning group for which we specify the constraint. Defaults to the robot's main planning group.", + "typeHintID": "str" + }, + { + "name": "tolerance_position", + "description": "The allowed tolerance to the frame's position (defined in the robot's units). Defaults to 0.001", + "typeHintID": "float" + }, + { + "name": "tolerance_xaxis", + "description": "The allowed tolerance of the frame's X-axis in degrees.", + "typeHintID": "float" + }, + { + "name": "tolerance_yaxis", + "description": "The allowed tolerance of the frame's Y-axis in degrees.", + "typeHintID": "float" + }, + { + "name": "tolerance_zaxis", + "description": "The allowed tolerance of the frame's Z-axis in degrees.", + "typeHintID": "float" + } + ], + "outputParameters": [ + { + "name": "constraints", + "description": "A list containing a position and an orientation constraint." + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ForwardKinematics/code.py b/src/compas_fab/ghpython/components_cpython/Cf_ForwardKinematics/code.py new file mode 100644 index 0000000000..7b5a4ef022 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_ForwardKinematics/code.py @@ -0,0 +1,21 @@ +# r: compas_fab>=1.0.2 +""" +Calculate the robot's forward kinematic for a given configuration. + +COMPAS FAB v1.0.2 +""" + +import Grasshopper + + +class ForwardKinematics(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript(self, robot, config, group, link, use_only_model): + frame = None + if robot and config: + options = {} + if link: + options["link"] = link + if use_only_model: + options["solver"] = "model" + frame = robot.forward_kinematics(config, group, options=options) + return frame diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ForwardKinematics/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_ForwardKinematics/icon.png new file mode 100644 index 0000000000..4a6dd5dbe4 Binary files /dev/null and b/src/compas_fab/ghpython/components_cpython/Cf_ForwardKinematics/icon.png differ diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ForwardKinematics/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_ForwardKinematics/metadata.json new file mode 100644 index 0000000000..06fb40b04c --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_ForwardKinematics/metadata.json @@ -0,0 +1,44 @@ +{ + "name": "Forward Kinematics", + "nickname": "FK", + "category": "COMPAS FAB", + "subcategory": "Planning", + "description": "Calculate the robot's forward kinematic for a given configuration.", + "exposure": 2, + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "robot", + "description": "The robot.", + "wireDisplay": "hidden" + }, + { + "name": "config", + "description": "The configuration to calculate the forward kinematics for." + }, + { + "name": "group", + "description": "The planning group used for calculation. Defaults to the robot's main planning group.", + "typeHintID": "str" + }, + { + "name": "link", + "description": "The link name to calculate forward kinematics for. Defaults to the robot's end effector.", + "typeHintID": "str" + }, + { + "name": "use_only_model", + "description": "If True, use only the robot model instead of the connected backend to calculate FK.", + "typeHintID": "bool" + } + ], + "outputParameters": [ + { + "name": "frame", + "description": "The resulting frame." + } + ] + } +} \ No newline at end of file diff --git a/src/compas_fab/ghpython/components_cpython/Cf_InverseKinematics/code.py b/src/compas_fab/ghpython/components_cpython/Cf_InverseKinematics/code.py new file mode 100644 index 0000000000..8a269d5401 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_InverseKinematics/code.py @@ -0,0 +1,18 @@ +# r: compas_fab>=1.0.2 +""" +Calculate the robot's inverse kinematic for a given plane. + +COMPAS FAB v1.0.2 +""" + +import Grasshopper +from compas_rhino.conversions import plane_to_compas_frame + + +class InverseKinematics(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript(self, robot, plane, start_configuration, group): + configuration = None + if robot and robot.client and robot.client.is_connected and plane: + frame = plane_to_compas_frame(plane) + configuration = robot.inverse_kinematics(frame, start_configuration, group) + return configuration diff --git a/src/compas_fab/ghpython/components_cpython/Cf_InverseKinematics/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_InverseKinematics/icon.png new file mode 100644 index 0000000000..8ba864791a Binary files /dev/null and b/src/compas_fab/ghpython/components_cpython/Cf_InverseKinematics/icon.png differ diff --git a/src/compas_fab/ghpython/components_cpython/Cf_InverseKinematics/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_InverseKinematics/metadata.json new file mode 100644 index 0000000000..27c7665a36 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_InverseKinematics/metadata.json @@ -0,0 +1,39 @@ +{ + "name": "Inverse Kinematics", + "nickname": "IK", + "category": "COMPAS FAB", + "subcategory": "Planning", + "description": "Calculate the robot's inverse kinematic for a given plane.", + "exposure": 2, + + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "robot", + "description": "The robot.", + "wireDisplay": "hidden" + }, + { + "name": "plane", + "description": "The plane or frame to calculate the inverse kinematic for." + }, + { + "name": "start_configuration", + "description": "If passed, calculates the inverse such that the joint positions differ the least from the start_configuration. Defaults to the zero configuration." + }, + { + "name": "group", + "description": "The planning group used for calculation. Defaults to the robot's main planning group.", + "typeHintID": "str" + } + ], + "outputParameters": [ + { + "name": "configuration", + "description": "The planning group's configuration." + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/code.py b/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/code.py new file mode 100644 index 0000000000..0d74422591 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/code.py @@ -0,0 +1,47 @@ +# r: compas_fab>=1.0.2 +""" +Calculate a cartesian motion path (linear in tool space). + +COMPAS FAB v1.0.2 +""" + +import Grasshopper +import System +from compas_ghpython import create_id +from compas_rhino.conversions import plane_to_compas_frame +from scriptcontext import sticky as st + + +class PlanCartesianMotion(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript( + self, + robot, + planes: System.Collections.Generic.List[object], + start_configuration, + group: str, + attached_collision_meshes: System.Collections.Generic.List[object], + path_constraints: System.Collections.Generic.List[object], + max_step: float, + compute: bool, + ): + key = create_id(ghenv.Component, "trajectory") # noqa: F821 + + max_step = float(max_step) if max_step else 0.01 + path_constraints = list(path_constraints) if path_constraints else None + attached_collision_meshes = list(attached_collision_meshes) if attached_collision_meshes else None + + if robot and robot.client and robot.client.is_connected and start_configuration and planes and compute: + frames = [plane_to_compas_frame(plane) for plane in planes] + st[key] = robot.plan_cartesian_motion( + frames, + start_configuration=start_configuration, + group=group, + options=dict( + max_step=max_step, + path_constraints=path_constraints, + attached_collision_meshes=attached_collision_meshes, + ), + ) + + trajectory = st.get(key, None) + return trajectory diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/icon.png new file mode 100644 index 0000000000..f401521a5d Binary files /dev/null and b/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/icon.png differ diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/metadata.json new file mode 100644 index 0000000000..bced52a014 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/metadata.json @@ -0,0 +1,60 @@ +{ + "name": "Plan Cartesian Motion", + "nickname": "Plan Cartesian Motion", + "category": "COMPAS FAB", + "subcategory": "Planning", + "description": "Calculate a cartesian motion plan (linear in tool space).", + "exposure": 4, + + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "robot", + "description": "The robot.", + "wireDisplay": "hidden" + }, + { + "name": "planes", + "description": "The planes or frames through which the path is defined.", + "scriptParamAccess": "list" + }, + { + "name": "start_configuration", + "description": "The robot's full configuration, i.e. values for all configurable joints of the entire robot, at the starting position. Defaults to the all-zero configuration." + }, + { + "name": "group", + "description": "The planning group used for calculation. Defaults to the robot's main planning group.", + "typeHintID": "str" + }, + { + "name": "attached_collision_meshes", + "description": "A list of attached collision meshes to be included for planning.", + "scriptParamAccess": "list" + }, + { + "name": "path_constraints", + "description": "Optional constraints that can be imposed along the solution path. Note that path calculation won't work if the start_configuration violates these constraints. Defaults to None.", + "scriptParamAccess": "list" + }, + { + "name": "max_step", + "description": "The approximate distance between the calculated points. (Defined in the robot's units.) Defaults to 0.01.", + "typeHintID": "float" + }, + { + "name": "compute", + "description": "If True, calculates a trajectory.", + "typeHintID": "bool" + } + ], + "outputParameters": [ + { + "name": "trajectory", + "description": "The calculated trajectory." + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/code.py b/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/code.py new file mode 100644 index 0000000000..35fb910586 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/code.py @@ -0,0 +1,52 @@ +# r: compas_fab>=1.0.2 +""" +Calculate a motion path. + +COMPAS FAB v1.0.2 +""" + +import Grasshopper +import System +from compas_ghpython import create_id +from scriptcontext import sticky as st + + +class PlanMotion(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript( + self, + robot, + goal_constraints: System.Collections.Generic.List[object], + start_configuration, + group: str, + attached_collision_meshes: System.Collections.Generic.List[object], + path_constraints: System.Collections.Generic.List[object], + planner_id: str, + compute: bool, + ): + key = create_id(ghenv.Component, "trajectory") # noqa: F821 + + path_constraints = list(path_constraints) if path_constraints else None + attached_collision_meshes = list(attached_collision_meshes) if attached_collision_meshes else None + planner_id = str(planner_id) if planner_id else "RRTConnect" + + if ( + robot + and robot.client + and robot.client.is_connected + and start_configuration + and goal_constraints + and compute + ): + st[key] = robot.plan_motion( + goal_constraints, + start_configuration=start_configuration, + group=group, + options=dict( + attached_collision_meshes=attached_collision_meshes, + path_constraints=path_constraints, + planner_id=planner_id, + ), + ) + + trajectory = st.get(key, None) + return trajectory diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/icon.png new file mode 100644 index 0000000000..ae7dad3dba Binary files /dev/null and b/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/icon.png differ diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/metadata.json new file mode 100644 index 0000000000..2549aa96c2 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/metadata.json @@ -0,0 +1,60 @@ +{ + "name": "Plan Motion", + "nickname": "Plan Motion", + "category": "COMPAS FAB", + "subcategory": "Planning", + "description": "Calculate a motion plan.", + "exposure": 4, + + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "robot", + "description": "The robot.", + "wireDisplay": "hidden" + }, + { + "name": "goal_constraints", + "description": "The goal to be achieved, defined in a set of constraints. Constraints can be very specific, for example defining value domains for each joint, such that the goal configuration is included, or defining a volume in space, to which a specific robot link (e.g. the end-effector) is required to move to.", + "scriptParamAccess": "list" + }, + { + "name": "start_configuration", + "description": "The robot's full configuration, i.e. values for all configurable joints of the entire robot, at the starting position. Defaults to the all-zero configuration." + }, + { + "name": "group", + "description": "The planning group used for calculation. Defaults to the robot's main planning group.", + "typeHintID": "str" + }, + { + "name": "attached_collision_meshes", + "description": "A list of attached collision meshes to be included for planning.", + "scriptParamAccess": "list" + }, + { + "name": "path_constraints", + "description": "Optional constraints that can be imposed along the solution path. Note that path calculation won't work if the start_configuration violates these constraints. Defaults to None.", + "scriptParamAccess": "list" + }, + { + "name": "planner_id", + "description": "The name of the algorithm used for path planning. Defaults to 'RRTConnect'", + "typeHintID": "str" + }, + { + "name": "compute", + "description": "If True, calculates a trajectory.", + "typeHintID": "bool" + } + ], + "outputParameters": [ + { + "name": "trajectory", + "description": "The calculated trajectory." + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/code.py b/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/code.py new file mode 100644 index 0000000000..752d75d987 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/code.py @@ -0,0 +1,20 @@ +# r: compas_fab>=1.0.2 +""" +Create a planning scene. + +COMPAS FAB v1.0.2 +""" + +import Grasshopper +from compas_ghpython import create_id +from scriptcontext import sticky as st + +from compas_fab.robots import PlanningScene + + +class PlanningSceneComponent(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript(self, robot): + key = create_id(ghenv.Component, "planning_scene") # noqa: F821 + if robot: + st[key] = PlanningScene(robot) + return st.get(key, None) diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/icon.png new file mode 100644 index 0000000000..fb9f75869f Binary files /dev/null and b/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/icon.png differ diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/metadata.json new file mode 100644 index 0000000000..b5c6f787ee --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/metadata.json @@ -0,0 +1,26 @@ +{ + "name": "Planning Scene", + "nickname": "Scene", + "category": "COMPAS FAB", + "subcategory": "Scene", + "description": "Create a planning scene.", + "exposure": 2, + + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "robot", + "description": "The robot.", + "wireDisplay": "hidden" + } + ], + "outputParameters": [ + { + "name": "scene", + "description": "A planning scene." + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/code.py b/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/code.py new file mode 100644 index 0000000000..c907aa8879 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/code.py @@ -0,0 +1,33 @@ +# r: compas_fab>=1.0.2 +""" +Connect or disconnect to ROS. + +COMPAS FAB v1.0.2 +""" + +import Grasshopper +from compas_ghpython import create_id +from scriptcontext import sticky as st + +from compas_fab.backends import RosClient + + +class ROSConnect(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript(self, ip, port, connect): + ros_client = None + + ip = ip or "127.0.0.1" + port = port or 9090 + + key = create_id(ghenv.Component, "ros_client") # noqa: F821 + ros_client = st.get(key, None) + + if ros_client: + st[key].close() + if connect: + st[key] = RosClient(ip, port) + st[key].run(5) + + ros_client = st.get(key, None) + is_connected = ros_client.is_connected if ros_client else False + return (ros_client, is_connected) diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/icon.png new file mode 100644 index 0000000000..01236afb81 Binary files /dev/null and b/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/icon.png differ diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/metadata.json new file mode 100644 index 0000000000..1bb60ac4c0 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/metadata.json @@ -0,0 +1,40 @@ +{ + "name": "ROS Connect", + "nickname": "ROS", + "category": "COMPAS FAB", + "subcategory": "ROS", + "description": "Connect or disconnect to ROS.", + "exposure": 2, + + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "ip", + "description": "The ip address ROS. Defaults to 127.0.0.1.", + "typeHintID": "str" + }, + { + "name": "port", + "description": "The port of ROS. Defaults to 9090.", + "typeHintID": "int" + }, + { + "name": "connect", + "description": "If True, connects to ROS. If False, disconnects from ROS. Defaults to False.", + "typeHintID": "bool" + } + ], + "outputParameters": [ + { + "name": "ros_client", + "description": "The ROS client." + }, + { + "name": "is_connected", + "description": "True if connection established." + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py b/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py new file mode 100644 index 0000000000..b90d0f139c --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py @@ -0,0 +1,27 @@ +# r: compas_fab>=1.0.2 +""" +Load robot directly from ROS. + +COMPAS FAB v1.0.2 +""" + +import Grasshopper +from compas_ghpython import create_id +from scriptcontext import sticky as st + +from compas.scene import SceneObject + + +class ROSRobot(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript(self, ros_client, load): + key = create_id(ghenv.Component, "robot") # noqa: F821 + + if ros_client and ros_client.is_connected and load: + # Load URDF from ROS + st[key] = ros_client.load_robot(load_geometry=True, precision=12) + st[key].scene_object = SceneObject(item=st[key].model) + + robot = st.get(key, None) + if robot: # client sometimes need to be restarted, without needing to reload geometry + robot.client = ros_client + return robot diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/icon.png new file mode 100644 index 0000000000..e28d244436 Binary files /dev/null and b/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/icon.png differ diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/metadata.json new file mode 100644 index 0000000000..a5a408a25e --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/metadata.json @@ -0,0 +1,30 @@ +{ + "name": "ROS Robot", + "nickname": "Robot", + "category": "COMPAS FAB", + "subcategory": "ROS", + "description": "Load robot directly from ROS.", + "exposure": 2, + + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "ros_client", + "description": "The ROS client." + }, + { + "name": "load", + "description": "If True, loads the robot from ROS. Defaults to False.", + "typeHintID": "bool" + } + ], + "outputParameters": [ + { + "name": "robot", + "description": "The robot." + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/code.py b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/code.py new file mode 100644 index 0000000000..eb588b31b5 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/code.py @@ -0,0 +1,51 @@ +# r: compas_fab>=1.0.2 +""" +Publishes messages to a ROS topic + +COMPAS FAB v1.0.2 +""" + +import time + +import Grasshopper +from compas_ghpython import create_id +from roslibpy import Topic +from scriptcontext import sticky as st + +from compas_fab.backends.ros.messages import ROSmsg +from compas_fab.ghpython.components import message +from compas_fab.ghpython.components import warning + + +class ROSTopicPublish(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript(self, ros_client, topic_name: str, topic_type: str, msg): + if not topic_name: + warning(ghenv.Component, "Please specify the name of the topic") + return + if not topic_type: + warning(ghenv.Component, "Please specify the type of the topic") + return + + key = create_id(ghenv.Component, "topic") # noqa: F821 + + topic = st.get(key, None) + + if ros_client and ros_client.is_connected: + if not topic or topic.ros != ros_client: + topic = Topic(ros_client, topic_name, topic_type, reconnect_on_close=False) + topic.advertise() + time.sleep(0.2) + + st[key] = topic + + self.is_advertised = topic and topic.is_advertised + + if msg: + msg = ROSmsg.parse(msg, topic_type) + topic.publish(msg.msg) + message(ghenv.Component, "Message published") + else: + if self.is_advertised: + message(ghenv.Component, "Topic advertised") + + return (topic, self.is_advertised) diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/icon.png new file mode 100644 index 0000000000..8ab79bac52 Binary files /dev/null and b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/icon.png differ diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/metadata.json new file mode 100644 index 0000000000..a57edc2076 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/metadata.json @@ -0,0 +1,42 @@ +{ + "name": "Publish to topic", + "nickname": "Publish", + "category": "COMPAS FAB", + "subcategory": "ROS", + "description": "Publishes messages to a ROS topic.", + "exposure": 4, + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "ros_client", + "description": "The ROS client." + }, + { + "name": "topic_name", + "description": "Name of the topic to publish to, eg. /joint_states", + "typeHintID": "str" + }, + { + "name": "topic_type", + "description": "Type of the topic to publish to, eg. sensor_msgs/JointState", + "typeHintID": "str" + }, + { + "name": "msg", + "description": "The message to publish. It can be an instance of ROSMsg, a Python dict, or a JSON string." + } + ], + "outputParameters": [ + { + "name": "topic", + "description": "The topic instance." + }, + { + "name": "is_advertised", + "description": "Indicates if the topic is current advertised or not." + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py new file mode 100644 index 0000000000..d935ccf00d --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py @@ -0,0 +1,85 @@ +# r: compas_fab>=1.0.2 +""" +Subscribe to a ROS topic. + +COMPAS FAB v1.0.2 +""" + +import time + +import Grasshopper +from compas_ghpython import create_id +from roslibpy import Topic +from scriptcontext import sticky as st + +from compas_fab.backends.ros.messages import ROSmsg +from compas_fab.ghpython.components import message +from compas_fab.ghpython.components import warning + + +class ROSTopicSubscribe(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript(self, ros_client, topic_name: str, topic_type: str, interval: int, start: bool, stop: bool): + if not topic_name: + warning(ghenv.Component, "Please specify the name of the topic") # noqa: F821 + if not topic_type: + warning(ghenv.Component, "Please specify the type of the topic") # noqa: F821 + + if not hasattr(self, "message_count"): + self.message_count = 0 + + self.interval = interval or 25 # in milliseconds + self.is_updating = False + self.is_subscribed = False + + self.msg_key = create_id(ghenv.Component, "last_msg") # noqa: F821 + key = create_id(ghenv.Component, "topic") # noqa: F821 + + last_msg = st.get(self.msg_key, None) + topic = st.get(key, None) + + if ros_client and ros_client.is_connected: + if start: + self._unsubscribe(topic) + + topic = Topic(ros_client, topic_name, topic_type) + topic.subscribe(self.topic_callback) + time.sleep(0.2) + + st[key] = topic + + if stop: + self._unsubscribe(topic) + + self.is_subscribed = topic and topic.is_subscribed + + if last_msg: + last_msg = ROSmsg.parse(last_msg, topic_type) + + if self.is_subscribed: + message(ghenv.Component, f"Subscribed, {self.message_count} messages") # noqa: F821 + else: + message(ghenv.Component, "Not subscribed") # noqa: F821 + + return (topic, last_msg, self.is_subscribed) + + def _unsubscribe(self, topic): + if topic and topic.is_subscribed: + topic.unsubscribe() + time.sleep(0.2) + + def topic_callback(self, msg): + self.message_count += 1 + st[self.msg_key] = msg + + if self.is_subscribed: + ghdoc = ghenv.Component.OnPingDocument() # noqa: F821 This is defined by Grasshopper + if not self.is_updating and ghdoc.SolutionState != Grasshopper.Kernel.GH_ProcessStep.Process: + self.is_updating = True + ghdoc.ScheduleSolution( + self.interval, Grasshopper.Kernel.GH_Document.GH_ScheduleDelegate(self.expire_callback) + ) + + def expire_callback(self, ghdoc): + if ghdoc.SolutionState != Grasshopper.Kernel.GH_ProcessStep.Process: + ghenv.Component.ExpireSolution(False) # noqa: F821 This is defined by Grasshopper + self.is_updating = False diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/icon.png new file mode 100644 index 0000000000..02400a6c41 Binary files /dev/null and b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/icon.png differ diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/metadata.json new file mode 100644 index 0000000000..9a66d925e0 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/metadata.json @@ -0,0 +1,57 @@ +{ + "name": "Subscribe to topic", + "nickname": "Subscribe", + "category": "COMPAS FAB", + "subcategory": "ROS", + "description": "Receives messages that have been published to a ROS topic.", + "exposure": 4, + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "ros_client", + "description": "The ROS client." + }, + { + "name": "topic_name", + "description": "Name of the topic to subscribe to, eg. /joint_states", + "typeHintID": "str" + }, + { + "name": "topic_type", + "description": "Type of the topic to subscribe to, eg. sensor_msgs/JointState", + "typeHintID": "str" + }, + { + "name": "interval", + "description": "Interval in milliseconds at which to expire the solution to propagate received messages. Defaults to 25ms. Messages are not queued, so if the interval is greater than the rate at which messages are published, some messages will be dropped.", + "typeHintID": "int" + }, + { + "name": "start", + "description": "Start receiving messages.", + "typeHintID": "bool" + }, + { + "name": "stop", + "description": "Stop receiving messages.", + "typeHintID": "bool" + } + ], + "outputParameters": [ + { + "name": "topic", + "description": "The subscribed topic instance." + }, + { + "name": "last_msg", + "description": "The last received message." + }, + { + "name": "is_subscribed", + "description": "Indicates if the topic is currently subscribed or not." + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py new file mode 100644 index 0000000000..8bb01fbc59 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py @@ -0,0 +1,157 @@ +# r: compas_fab>=1.0.2 +""" +Visualizes the robot. + +COMPAS FAB v1.0.2 +""" + +import time + +import Grasshopper +from compas_ghpython import create_id +from compas_rhino.conversions import frame_to_rhino_plane +from scriptcontext import sticky as st + +from compas.geometry import Frame +from compas.geometry import Transformation +from compas.scene import SceneObject +from compas_fab.backends import BackendFeatureNotSupportedError +from compas_fab.robots import PlanningScene + + +class RobotVisualize(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript( + self, + robot, + group, + configuration, + show_visual, + show_collision, + show_frames, + show_base_frame, + show_end_effector_frame, + show_cm, + show_acm, + ): + visual = None + collision = None + collision_meshes = None + attached_meshes = None + frames = None + base_frame = None + ee_frame = None + + if robot: + show_visual = True if show_visual is None else show_visual + show_cm = True if show_cm is None else show_cm + show_acm = True if show_acm is None else show_acm + configuration = configuration or robot.zero_configuration() + + robot.update(configuration, visual=show_visual, collision=show_collision) + compas_frames = robot.transformed_frames(configuration, group) + + if show_visual: + visual = robot.scene_object.draw_visual() + + if show_collision: + collision = robot.scene_object.draw_collision() + + if show_base_frame: + base_compas_frame = compas_frames[0] + base_frame = frame_to_rhino_plane(base_compas_frame) + + if show_end_effector_frame: + options = dict(solver="model") + if robot.attached_tool: + options["link"] = robot.attached_tool.connected_to + + ee_compas_frame = robot.forward_kinematics(configuration, group, options=options) + ee_frame = frame_to_rhino_plane(ee_compas_frame) + + if show_frames: + frames = [] + for compas_frame in compas_frames[1:]: + frame = frame_to_rhino_plane(compas_frame) + frames.append(frame) + + cached_scene_key = create_id(ghenv.Component, "cached_scene") # noqa: F821 + + if show_cm or show_acm: + cached_scene = st.get(cached_scene_key) + if not cached_scene: + cached_scene = {"time": 0} + + # expire cache if the component has not been executed in the last 2 seconds + # this allows to slide through a list of configurations + # without triggering refreshes of the scene in the middle of it + if time.time() - cached_scene["time"] > 2: + update_scene = True + else: + update_scene = False + + if update_scene: + scene = PlanningScene(robot) + try: + scene = robot.client.get_planning_scene() + except BackendFeatureNotSupportedError: + print( + "The selected backend does not support collision meshes. If you need collision mesh support, use a different backend." + ) + scene = None + show_cm = False + show_acm = False + + if update_scene and show_cm: + collision_meshes = [] + + for co in scene.world.collision_objects: + header = co.header + frame_id = header.frame_id + cms = co.to_collision_meshes() + + for cm in cms: + if cm.frame != Frame.worldXY(): + t = Transformation.from_frame(cm.frame) + mesh = cm.mesh.transformed(t) + else: + mesh = cm.mesh + + collision_meshes.extend(SceneObject(item=mesh).draw()) + + cached_scene["cm"] = collision_meshes + + collision_meshes = cached_scene.get("cm", []) + + if update_scene and show_acm: + attached_meshes = [] + + for aco in scene.robot_state.attached_collision_objects: + for acm in aco.to_attached_collision_meshes(): + frame_id = aco.object["header"]["frame_id"] + frame = robot.forward_kinematics(configuration, options=dict(link=frame_id)) + t = Transformation.from_frame(frame) + + # Local CM frame + if acm.collision_mesh.frame and acm.collision_mesh.frame != Frame.worldXY(): + t = t * Transformation.from_frame(acm.collision_mesh.frame) + + mesh = acm.collision_mesh.mesh.transformed(t) + + attached_meshes.extend(SceneObject(item=mesh).draw()) + + cached_scene["acm"] = attached_meshes + + attached_meshes = cached_scene.get("acm", []) + + cached_scene["time"] = time.time() + st[cached_scene_key] = cached_scene + + return ( + visual, + collision, + collision_meshes, + attached_meshes, + frames, + base_frame, + ee_frame, + ) diff --git a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/icon.png new file mode 100644 index 0000000000..d6178184f5 Binary files /dev/null and b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/icon.png differ diff --git a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/metadata.json new file mode 100644 index 0000000000..dc15f9e870 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/metadata.json @@ -0,0 +1,93 @@ +{ + "name": "Robot Visualize", + "nickname": "Robot Visualize", + "category": "COMPAS FAB", + "subcategory": "Display", + "description": "Visualizes the robot.", + "exposure": 2, + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "robot", + "description": "The robot.", + "wireDisplay": "hidden" + }, + { + "name": "group", + "description": "The planning group used for end-effector and base visualization.", + "typeHintID": "str" + }, + { + "name": "configuration", + "description": "The robot's full configuration." + }, + { + "name": "show_visual", + "description": "Whether or not to show the robot's visual meshes.", + "typeHintID": "bool" + }, + { + "name": "show_collision", + "description": "Whether or not to show the robot's collision meshes.", + "typeHintID": "bool" + }, + { + "name": "show_frames", + "description": "Whether or not to show the robot's joint frames.", + "typeHintID": "bool" + }, + { + "name": "show_base_frame", + "description": "Whether or not to show the robot's base frame.", + "typeHintID": "bool" + }, + { + "name": "show_end_effector_frame", + "description": "Whether or not to show the robot's end-effector frame.", + "typeHintID": "bool" + }, + { + "name": "show_cm", + "description": "Whether or not to show the collision meshes (if any).", + "typeHintID": "bool" + }, + { + "name": "show_acm", + "description": "Whether or not to show the attached collision meshes (if any).", + "typeHintID": "bool" + } + ], + "outputParameters": [ + { + "name": "visual", + "description": "Rhino meshes of the robot's visual geometry (if any)." + }, + { + "name": "collision", + "description": "Rhino meshes of the robot's collision geometry (if any)." + }, + { + "name": "collision_meshes", + "description": "Rhino meshes of the scene's collision meshes (if any)." + }, + { + "name": "attached_meshes", + "description": "Rhino meshes of the scene's attached collision meshes (if any)." + }, + { + "name": "frames", + "description": "The robot's joint frames as Rhino planes (if any)." + }, + { + "name": "base_frame", + "description": "The robot's base frame as a Rhino plane (if any)." + }, + { + "name": "ee_frame", + "description": "The robot's end-effector frame as a Rhino plane (if any)." + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/code.py b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/code.py new file mode 100644 index 0000000000..306a13135a --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/code.py @@ -0,0 +1,47 @@ +# r: compas_fab>=1.0.2 +""" +Visualizes a trajectory. + +COMPAS FAB v1.0.2 +""" + +import Grasshopper +from compas_ghpython.drawing import draw_frame +from compas_ghpython.sets import list_to_ghtree + + +class TrajectoryVisualize(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript(self, robot, group, trajectory): + start_configuration = None + configurations = [] + fraction = 0.0 + time = 0.0 + + planes = [] + positions = [] + velocities = [] + accelerations = [] + + if robot and trajectory: + group = group or robot.main_group_name + + for c in trajectory.points: + configurations.append( + robot.merge_group_with_full_configuration(c, trajectory.start_configuration, group) + ) + frame = robot.forward_kinematics(c, group, options=dict(solver="model")) + planes.append(draw_frame(frame)) + positions.append(c.positions) + velocities.append(c.velocities) + accelerations.append(c.accelerations) + + start_configuration = trajectory.start_configuration + fraction = trajectory.fraction + time = trajectory.time_from_start + + P = list_to_ghtree(list(zip(*positions))) + V = list_to_ghtree(list(zip(*velocities))) + A = list_to_ghtree(list(zip(*accelerations))) + + # return outputs if you have them; here I try it for you: + return (start_configuration, configurations, fraction, time, planes, P, V, A) diff --git a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/icon.png new file mode 100644 index 0000000000..85c6c895a5 Binary files /dev/null and b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/icon.png differ diff --git a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/metadata.json new file mode 100644 index 0000000000..4bd39bd362 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/metadata.json @@ -0,0 +1,63 @@ +{ + "name": "Trajectory Visualize", + "nickname": "Trajectory Visualize", + "category": "COMPAS FAB", + "subcategory": "Display", + "description": "Visualizes the trajectory.", + "exposure": 2, + + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "robot", + "description": "The robot.", + "wireDisplay": "hidden" + }, + { + "name": "group", + "description": "The planning group for which this trajectory was planned.", + "typeHintID": "str" + }, + { + "name": "trajectory", + "description": "The calculated trajectory." + } + ], + "outputParameters": [ + { + "name": "start_configuration", + "description": "The start configuration of the trajectory." + }, + { + "name": "configurations", + "description": "The full configurations along the trajectory." + }, + { + "name": "fraction", + "description": "Indicates the fraction of requested trajectory that was calculated, e.g. 1 means the full trajectory was found." + }, + { + "name": "time", + "description": "The time which is needed to execute that trajectory at full speed." + }, + { + "name": "planes", + "description": "The planes of the robot's end-effector." + }, + { + "name": "P", + "description": "The positions along the trajectory" + }, + { + "name": "V", + "description": "The velocities along the trajectory" + }, + { + "name": "A", + "description": "The accelerations along the trajectory" + } + ] + } +} diff --git a/src/compas_fab/ghpython/reachabilitymapobject.py b/src/compas_fab/ghpython/reachabilitymapobject.py index 58f7dd0f4a..7f3e9320e7 100644 --- a/src/compas_fab/ghpython/reachabilitymapobject.py +++ b/src/compas_fab/ghpython/reachabilitymapobject.py @@ -1,6 +1,7 @@ +from compas_ghpython.scene import GHSceneObject + from compas.colors import ColorMap from compas.scene import SceneObject -from compas_ghpython.scene import GHSceneObject class ReachabilityMapObject(GHSceneObject): @@ -34,12 +35,12 @@ def draw_frames(self, ik_index=None): for frames in self.reachability_map.frames: xframes.append([]) for frame in frames: - xframes[-1].append(SceneObject(frame).draw()) + xframes[-1].append(SceneObject(item=frame).draw()) xframes = list_to_tree(xframes) return xframes else: frames, _ = self.reachability_map.reachable_frames_and_configurations_at_ik_index(ik_index) - return [SceneObject(f).draw() for f in frames] + return [SceneObject(item=f).draw() for f in frames] def draw(self, colormap="viridis"): return self.draw_cloud(colormap) @@ -64,7 +65,7 @@ def draw_cloud(self, colormap="viridis", points=None): xpoints = [] for pt in points: - xpoints.extend(SceneObject(pt).draw()) + xpoints.extend(SceneObject(item=pt).draw()) colors = [] cmap = ColorMap.from_mpl(colormap) diff --git a/src/compas_fab/ghpython/yak_template/icon.png b/src/compas_fab/ghpython/yak_template/icon.png new file mode 100644 index 0000000000..e3ff78cde6 Binary files /dev/null and b/src/compas_fab/ghpython/yak_template/icon.png differ diff --git a/src/compas_fab/ghpython/yak_template/manifest.yml b/src/compas_fab/ghpython/yak_template/manifest.yml new file mode 100644 index 0000000000..737217184f --- /dev/null +++ b/src/compas_fab/ghpython/yak_template/manifest.yml @@ -0,0 +1,22 @@ +name: compas_fab +version: {{ version }} +authors: + - Gramazio Kohler Research, ETH Zurich +description: "Robotic fabrication package for the COMPAS Framework** that facilitates the planning and execution of robotic fabrication processes." +url: "https://github.com/compas-dev/compas_fab" +keywords: [ + "COMPAS", + "compas", + "framework", + "architecture", + "engineering", + "digital", + "fabrication", + "construction", + "robotics", + "robots", + "robotic", + "planning", + "motion", + "kinematics", +] diff --git a/src/compas_fab/rhino/reachabilitymapobject.py b/src/compas_fab/rhino/reachabilitymapobject.py index fdb87994b7..a39d1fe715 100644 --- a/src/compas_fab/rhino/reachabilitymapobject.py +++ b/src/compas_fab/rhino/reachabilitymapobject.py @@ -1,6 +1,7 @@ +from compas_rhino.scene import RhinoSceneObject + from compas.colors import ColorMap from compas.scene import SceneObject -from compas_rhino.scene import RhinoSceneObject class ReachabilityMapObject(RhinoSceneObject): @@ -43,12 +44,12 @@ def draw_frames(self, ik_index=None): for frames in self.reachability_map.frames: xframes.append([]) for frame in frames: - xframes[-1].extend(SceneObject(frame, layer=self.layer, scale=self.scale).draw()) + xframes[-1].extend(SceneObject(item=frame, layer=self.layer, scale=self.scale).draw()) return xframes else: frames, _ = self.reachability_map.reachable_frames_and_configurations_at_ik_index(ik_index) - return [SceneObject(f, layer=self.layer, scale=self.scale).draw() for f in frames] + return [SceneObject(item=f, layer=self.layer, scale=self.scale).draw() for f in frames] def draw(self, colormap="viridis"): return self.draw_cloud(colormap) @@ -82,7 +83,7 @@ def draw_cloud(self, colormap="viridis", points=None): guids = [] for num, pt in zip(score, points): color = cmap(num, minv, maxv) - sceneobject = SceneObject(pt, layer=self.layer) + sceneobject = SceneObject(item=pt, layer=self.layer) guids.extend(sceneobject.draw(color)) return guids diff --git a/src/compas_fab/robots/constraints.py b/src/compas_fab/robots/constraints.py index 88465e8a4e..f19a42fff9 100644 --- a/src/compas_fab/robots/constraints.py +++ b/src/compas_fab/robots/constraints.py @@ -75,7 +75,7 @@ def from_box(cls, box): >>> from compas.geometry import Frame >>> from compas.geometry import Box >>> from compas_fab.robots import BoundingVolume - >>> box = Box(1., 1., 1.) + >>> box = Box(1.0, 1.0, 1.0) >>> bv = BoundingVolume.from_box(box) >>> bv.type 1 @@ -124,7 +124,7 @@ def from_mesh(cls, mesh): >>> import compas >>> from compas.datastructures import Mesh >>> from compas_fab.robots import BoundingVolume - >>> mesh = Mesh.from_obj(compas.get('faces.obj')) + >>> mesh = Mesh.from_obj(compas.get("faces.obj")) >>> bv = BoundingVolume.from_mesh(Mesh) >>> bv.type 3 @@ -437,8 +437,8 @@ class PositionConstraint(Constraint): >>> from compas.geometry import Sphere >>> from compas_fab.robots import PositionConstraint >>> from compas_fab.robots import BoundingVolume - >>> bv = BoundingVolume.from_sphere(Sphere(0.5, point=[3,4,5])) - >>> pc = PositionConstraint('link_0', bv, weight=1.) + >>> bv = BoundingVolume.from_sphere(Sphere(0.5, point=[3, 4, 5])) + >>> pc = PositionConstraint("link_0", bv, weight=1.0) """ def __init__(self, link_name, bounding_volume, weight=1.0): @@ -470,7 +470,7 @@ def from_box(cls, link_name, box, weight=1.0): >>> from compas.geometry import Frame >>> from compas.geometry import Box >>> box = Box(4, 4, 4, Frame.worldXY()) - >>> pc = PositionConstraint.from_box('link_0', box) + >>> pc = PositionConstraint.from_box("link_0", box) """ bounding_volume = BoundingVolume.from_box(box) return cls(link_name, bounding_volume, weight) @@ -497,8 +497,8 @@ def from_sphere(cls, link_name, sphere, weight=1.0): -------- >>> from compas_fab.robots import PositionConstraint >>> from compas.geometry import Sphere - >>> sphere = Sphere(radius=0.5, point=[3,4,5]) - >>> pc = PositionConstraint.from_sphere('link_0', sphere, weight=1.) + >>> sphere = Sphere(radius=0.5, point=[3, 4, 5]) + >>> pc = PositionConstraint.from_sphere("link_0", sphere, weight=1.0) """ bounding_volume = BoundingVolume.from_sphere(sphere) return cls(link_name, bounding_volume, weight) @@ -526,8 +526,8 @@ def from_mesh(cls, link_name, mesh, weight=1.0): -------- >>> from compas.datastructures import Mesh >>> import compas - >>> mesh = Mesh.from_obj(compas.get('faces.obj')) - >>> pc = PositionConstraint.from_mesh('link_0', mesh) + >>> mesh = Mesh.from_obj(compas.get("faces.obj")) + >>> pc = PositionConstraint.from_mesh("link_0", mesh) """ bounding_volume = BoundingVolume.from_mesh(mesh) return cls(link_name, bounding_volume, weight) diff --git a/src/compas_fab/robots/inertia.py b/src/compas_fab/robots/inertia.py index 960a1de372..14e442bf6f 100644 --- a/src/compas_fab/robots/inertia.py +++ b/src/compas_fab/robots/inertia.py @@ -30,7 +30,7 @@ class Inertia: Examples -------- - >>> inertia = Inertia([[0] * 3] * 3, 1., Point(0.1, 3.1, 4.4)) + >>> inertia = Inertia([[0] * 3] * 3, 1.0, Point(0.1, 3.1, 4.4)) >>> inertia Inertia([[0, 0, 0], [0, 0, 0], [0, 0, 0]], 1.0, Point(x=0.1, y=3.1, z=4.4)) >>> inertia.principal_moments diff --git a/src/compas_fab/robots/planning_scene.py b/src/compas_fab/robots/planning_scene.py index 7586e558f9..f846762c5c 100644 --- a/src/compas_fab/robots/planning_scene.py +++ b/src/compas_fab/robots/planning_scene.py @@ -49,8 +49,8 @@ class CollisionMesh(Data): Examples -------- - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/floor.stl')) - >>> cm = CollisionMesh(mesh, 'floor') + >>> mesh = Mesh.from_stl(compas_fab.get("planning_scene/floor.stl")) + >>> cm = CollisionMesh(mesh, "floor") >>> cm.frame Frame(point=Point(x=0.0, y=0.0, z=0.0), xaxis=Vector(x=1.0, y=0.0, z=0.0), yaxis=Vector(x=0.0, y=1.0, z=0.0)) """ @@ -149,10 +149,10 @@ class AttachedCollisionMesh(Data): Examples -------- - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl')) - >>> cm = CollisionMesh(mesh, 'tip') - >>> ee_link_name = 'ee_link' - >>> touch_links = ['wrist_3_link', 'ee_link'] + >>> mesh = Mesh.from_stl(compas_fab.get("planning_scene/cone.stl")) + >>> cm = CollisionMesh(mesh, "tip") + >>> ee_link_name = "ee_link" + >>> touch_links = ["wrist_3_link", "ee_link"] >>> acm = AttachedCollisionMesh(cm, ee_link_name, touch_links) """ @@ -260,9 +260,9 @@ def add_collision_mesh(self, collision_mesh, scale=False): Examples -------- >>> scene = PlanningScene(robot) - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/floor.stl')) - >>> cm = CollisionMesh(mesh, 'floor') - >>> scene.add_collision_mesh(cm) # doctest: +SKIP + >>> mesh = Mesh.from_stl(compas_fab.get("planning_scene/floor.stl")) + >>> cm = CollisionMesh(mesh, "floor") + >>> scene.add_collision_mesh(cm) # doctest: +SKIP """ self.ensure_client() @@ -289,7 +289,7 @@ def remove_collision_mesh(self, id): Examples -------- >>> scene = PlanningScene(robot) - >>> scene.remove_collision_mesh('floor') # doctest: +SKIP + >>> scene.remove_collision_mesh("floor") # doctest: +SKIP """ self.ensure_client() self.robot.client.remove_collision_mesh(id) @@ -321,9 +321,9 @@ def append_collision_mesh(self, collision_mesh, scale=False): Examples -------- >>> scene = PlanningScene(robot) - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/floor.stl')) - >>> cm = CollisionMesh(mesh, 'floor') - >>> scene.append_collision_mesh(cm) # doctest: +SKIP + >>> mesh = Mesh.from_stl(compas_fab.get("planning_scene/floor.stl")) + >>> cm = CollisionMesh(mesh, "floor") + >>> scene.append_collision_mesh(cm) # doctest: +SKIP """ self.ensure_client() @@ -355,12 +355,12 @@ def add_attached_collision_mesh(self, attached_collision_mesh, scale=False): Examples -------- >>> scene = PlanningScene(robot) - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl')) - >>> cm = CollisionMesh(mesh, 'tip') - >>> ee_link_name = 'ee_link' - >>> touch_links = ['wrist_3_link', 'ee_link'] + >>> mesh = Mesh.from_stl(compas_fab.get("planning_scene/cone.stl")) + >>> cm = CollisionMesh(mesh, "tip") + >>> ee_link_name = "ee_link" + >>> touch_links = ["wrist_3_link", "ee_link"] >>> acm = AttachedCollisionMesh(cm, ee_link_name, touch_links) - >>> scene.add_attached_collision_mesh(acm) # doctest: +SKIP + >>> scene.add_attached_collision_mesh(acm) # doctest: +SKIP """ self.ensure_client() @@ -387,7 +387,7 @@ def remove_attached_collision_mesh(self, id): Examples -------- >>> scene = PlanningScene(robot) - >>> scene.remove_attached_collision_mesh('tip') # doctest: +SKIP + >>> scene.remove_attached_collision_mesh("tip") # doctest: +SKIP """ self.ensure_client() self.client.remove_attached_collision_mesh(id) @@ -413,11 +413,11 @@ def attach_collision_mesh_to_robot_end_effector(self, collision_mesh, scale=Fals Examples -------- >>> scene = PlanningScene(robot) - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl')) - >>> cm = CollisionMesh(mesh, 'tip') + >>> mesh = Mesh.from_stl(compas_fab.get("planning_scene/cone.stl")) + >>> cm = CollisionMesh(mesh, "tip") >>> group = robot.main_group_name - >>> scene.attach_collision_mesh_to_robot_end_effector(cm, group=group) # attach to ee - >>> scene.remove_attached_collision_mesh('tip') # now detach + >>> scene.attach_collision_mesh_to_robot_end_effector(cm, group=group) # attach to ee + >>> scene.remove_attached_collision_mesh("tip") # now detach """ self.ensure_client() diff --git a/src/compas_fab/robots/robot.py b/src/compas_fab/robots/robot.py index 68152d951b..720fe5f232 100644 --- a/src/compas_fab/robots/robot.py +++ b/src/compas_fab/robots/robot.py @@ -131,7 +131,7 @@ def basic(cls, name, joints=None, links=None, materials=None, **kwargs): Examples -------- - >>> robot = Robot.basic('A robot') + >>> robot = Robot.basic("A robot") >>> robot.name 'A robot' """ @@ -179,7 +179,7 @@ def group_states(self): Examples -------- - >>> sorted(robot.group_states['manipulator'].keys()) + >>> sorted(robot.group_states["manipulator"].keys()) ['home', 'up'] """ @@ -334,7 +334,7 @@ def get_link_names(self, group=None): Examples -------- - >>> robot.get_link_names('manipulator') + >>> robot.get_link_names("manipulator") ['base_link', 'shoulder_link', 'upper_arm_link', 'forearm_link', 'wrist_1_link', 'wrist_2_link', 'wrist_3_link', 'ee_link'] """ base_link_name = self.get_base_link_name(group) @@ -377,7 +377,7 @@ def get_configurable_joints(self, group=None): Examples -------- - >>> joints = robot.get_configurable_joints('manipulator') + >>> joints = robot.get_configurable_joints("manipulator") >>> [j.name for j in joints] ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] """ @@ -463,7 +463,7 @@ def get_configurable_joint_types(self, group=None): Examples -------- - >>> robot.get_configurable_joint_types('manipulator') + >>> robot.get_configurable_joint_types("manipulator") [0, 0, 0, 0, 0, 0] """ configurable_joints = self.get_configurable_joints(group) @@ -771,7 +771,7 @@ def to_local_coordinates(self, frame_WCF, group=None): -------- >>> frame_WCF = Frame([-0.363, 0.003, -0.147], [0.388, -0.351, -0.852], [0.276, 0.926, -0.256]) >>> frame_RCF = robot.to_local_coordinates(frame_WCF) - >>> frame_RCF # doctest: +SKIP + >>> frame_RCF # doctest: +SKIP Frame(Point(-0.363, 0.003, -0.147), Vector(0.388, -0.351, -0.852), Vector(0.276, 0.926, -0.256)) # doctest: +SKIP """ frame_RCF = frame_WCF.transformed(self.transformation_WCF_RCF(group)) @@ -796,7 +796,7 @@ def to_world_coordinates(self, frame_RCF, group=None): -------- >>> frame_RCF = Frame([-0.363, 0.003, -0.147], [0.388, -0.351, -0.852], [0.276, 0.926, -0.256]) >>> frame_WCF = robot.to_world_coordinates(frame_RCF) - >>> frame_WCF # doctest: +SKIP + >>> frame_WCF # doctest: +SKIP Frame(Point(-0.363, 0.003, -0.147), Vector(0.388, -0.351, -0.852), Vector(0.276, 0.926, -0.256)) # doctest: +SKIP """ frame_WCF = frame_RCF.transformed(self.transformation_RCF_WCF(group)) @@ -835,13 +835,13 @@ def from_tcf_to_t0cf(self, frames_tcf, group=None): Examples -------- - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl')) + >>> mesh = Mesh.from_stl(compas_fab.get("planning_scene/cone.stl")) >>> frame = Frame([0.14, 0, 0], [0, 1, 0], [0, 0, 1]) >>> robot.attach_tool(Tool(mesh, frame)) >>> frames_tcf = [Frame((-0.309, -0.046, -0.266), (0.276, 0.926, -0.256), (0.879, -0.136, 0.456))] - >>> robot.from_tcf_to_t0cf(frames_tcf) # doctest: +SKIP + >>> robot.from_tcf_to_t0cf(frames_tcf) # doctest: +SKIP [Frame(Point(-0.363, 0.003, -0.147), Vector(0.388, -0.351, -0.852), Vector(0.276, 0.926, -0.256))] # doctest: +SKIP - >>> robot.from_tcf_to_t0cf(frames_tcf, group=robot.main_group_name) # doctest: +SKIP + >>> robot.from_tcf_to_t0cf(frames_tcf, group=robot.main_group_name) # doctest: +SKIP [Frame(Point(-0.363, 0.003, -0.147), Vector(0.388, -0.351, -0.852), Vector(0.276, 0.926, -0.256))] # doctest: +SKIP """ tool = self._get_attached_tool_for_group(group_name=group) @@ -871,13 +871,13 @@ def from_t0cf_to_tcf(self, frames_t0cf, group=None): Examples -------- - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl')) + >>> mesh = Mesh.from_stl(compas_fab.get("planning_scene/cone.stl")) >>> frame = Frame([0.14, 0, 0], [0, 1, 0], [0, 0, 1]) >>> robot.attach_tool(Tool(mesh, frame)) >>> frames_t0cf = [Frame((-0.363, 0.003, -0.147), (0.388, -0.351, -0.852), (0.276, 0.926, -0.256))] - >>> robot.from_t0cf_to_tcf(frames_t0cf) # doctest: +SKIP + >>> robot.from_t0cf_to_tcf(frames_t0cf) # doctest: +SKIP [Frame(Point(-0.309, -0.046, -0.266), Vector(0.276, 0.926, -0.256), Vector(0.879, -0.136, 0.456))] # doctest: +SKIP - >>> robot.from_t0cf_to_tcf(frames_t0cf, group=robot.main_group_name) # doctest: +SKIP + >>> robot.from_t0cf_to_tcf(frames_t0cf, group=robot.main_group_name) # doctest: +SKIP [Frame(Point(-0.309, -0.046, -0.266), Vector(0.276, 0.926, -0.256), Vector(0.879, -0.136, 0.456))] # doctest: +SKIP """ tool = self._get_attached_tool_for_group(group_name=group) @@ -907,7 +907,7 @@ def attach_tool(self, tool, group=None, touch_links=None): Examples -------- - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl')) + >>> mesh = Mesh.from_stl(compas_fab.get("planning_scene/cone.stl")) >>> frame = Frame([0.14, 0, 0], [0, 1, 0], [0, 0, 1]) >>> tool = Tool(mesh, frame) >>> robot.attach_tool(tool) @@ -1085,7 +1085,7 @@ def position_constraint_from_frame(self, frame_WCF, tolerance_position, group=No -------- >>> frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) >>> tolerance_position = 0.001 - >>> robot.position_constraint_from_frame(frame, tolerance_position) # doctest: +SKIP + >>> robot.position_constraint_from_frame(frame, tolerance_position) # doctest: +SKIP PositionConstraint('ee_link', BoundingVolume(2, Sphere(Point(0.400, 0.300, 0.400), 0.001)), 1.0) # doctest: +SKIP """ @@ -1146,7 +1146,7 @@ def constraints_from_frame( >>> tolerance_position = 0.001 >>> tolerances_axes = [math.radians(1)] >>> group = robot.main_group_name - >>> robot.constraints_from_frame(frame, tolerance_position, tolerances_axes, group) # doctest: +SKIP + >>> robot.constraints_from_frame(frame, tolerance_position, tolerances_axes, group) # doctest: +SKIP [PositionConstraint('ee_link', BoundingVolume(2, Sphere(Point(0.400, 0.300, 0.400), 0.001)), 1.0), # doctest: +SKIP OrientationConstraint('ee_link', [0.5, 0.5, 0.5, 0.5], [0.017453292519943295, 0.017453292519943295, 0.017453292519943295], 1.0)] # doctest: +SKIP """ @@ -1301,7 +1301,7 @@ def inverse_kinematics( >>> frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]) >>> start_configuration = robot.zero_configuration() >>> group = robot.main_group_name - >>> robot.inverse_kinematics(frame_WCF, start_configuration, group) # doctest: +SKIP + >>> robot.inverse_kinematics(frame_WCF, start_configuration, group) # doctest: +SKIP Configuration((4.045, 5.130, -2.174, -6.098, -5.616, 6.283), (0, 0, 0, 0, 0, 0)) # doctest: +SKIP """ # Pseudo-memoized sequential calls will re-use iterator if not exhaused @@ -1376,7 +1376,7 @@ def iter_inverse_kinematics( >>> frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]) >>> start_configuration = robot.zero_configuration() >>> group = robot.main_group_name - >>> next(robot.iter_inverse_kinematics(frame_WCF, start_configuration, group)) # doctest: +SKIP + >>> next(robot.iter_inverse_kinematics(frame_WCF, start_configuration, group)) # doctest: +SKIP Configuration((4.045, 5.130, -2.174, -6.098, -5.616, 6.283), (0, 0, 0, 0, 0, 0)) # doctest: +SKIP """ options = options or {} @@ -1497,7 +1497,7 @@ def forward_kinematics(self, configuration, group=None, use_attached_tool_frame= >>> configuration = Configuration.from_revolute_values([-2.238, -1.153, -2.174, 0.185, 0.667, 0.000]) >>> group = robot.main_group_name >>> frame_WCF_c = robot.forward_kinematics(configuration, group) - >>> options = {'solver': 'model'} + >>> options = {"solver": "model"} >>> frame_WCF_m = robot.forward_kinematics(configuration, group, options) >>> frame_WCF_c == frame_WCF_m True @@ -1584,23 +1584,23 @@ def plan_cartesian_motion( Examples -------- - >>> ros = RosClient() - >>> ros.run() - >>> robot = ros.load_robot() + >>> ros = RosClient() # doctest: +SKIP + >>> ros.run() # doctest: +SKIP + >>> robot = ros.load_robot() # doctest: +SKIP >>> frames = [Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]),\ - Frame([0.5, 0.1, 0.6], [1, 0, 0], [0, 1, 0])] - >>> start_configuration = Configuration.from_revolute_values([-0.042, 0.033, -2.174, 5.282, -1.528, 0.000]) + Frame([0.5, 0.1, 0.6], [1, 0, 0], [0, 1, 0])] # doctest: +SKIP + >>> start_configuration = Configuration.from_revolute_values([-0.042, 0.033, -2.174, 5.282, -1.528, 0.000]) # doctest: +SKIP >>> group = robot.main_group_name >>> options = {'max_step': 0.01,\ 'jump_threshold': 1.57,\ - 'avoid_collisions': True} + 'avoid_collisions': True} # doctest: +SKIP >>> trajectory = robot.plan_cartesian_motion(frames,\ start_configuration,\ group=group,\ - options=options) - >>> len(trajectory.points) > 1 + options=options) # doctest: +SKIP + >>> len(trajectory.points) > 1 # doctest: +SKIP True - >>> ros.close() + >>> ros.close() # doctest: +SKIP """ options = options or {} max_step = options.get("max_step") @@ -1728,38 +1728,48 @@ def plan_motion(self, goal_constraints, start_configuration=None, group=None, op Using position and orientation constraints: - >>> ros = RosClient() - >>> ros.run() - >>> robot = ros.load_robot() - >>> frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) - >>> tolerance_position = 0.001 - >>> tolerances_axes = [math.radians(1)] * 3 - >>> start_configuration = Configuration.from_revolute_values([-0.042, 4.295, 0, -3.327, 4.755, 0.]) - >>> group = robot.main_group_name - >>> goal_constraints = robot.constraints_from_frame(frame, tolerance_position, tolerances_axes, group) - >>> trajectory = robot.plan_motion(goal_constraints, start_configuration, group, {'planner_id': 'RRTConnect'}) - >>> trajectory.fraction + >>> ros = RosClient() # doctest: +SKIP + >>> ros.run() # doctest: +SKIP + >>> robot = ros.load_robot() # doctest: +SKIP + >>> frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) # doctest: +SKIP + >>> tolerance_position = 0.001 # doctest: +SKIP + >>> tolerances_axes = [math.radians(1)] * 3 # doctest: +SKIP + >>> start_configuration = Configuration.from_revolute_values( + ... [-0.042, 4.295, 0, -3.327, 4.755, 0.0] + ... ) # doctest: +SKIP + >>> group = robot.main_group_name # doctest: +SKIP + >>> goal_constraints = robot.constraints_from_frame( + ... frame, tolerance_position, tolerances_axes, group + ... ) # doctest: +SKIP + >>> trajectory = robot.plan_motion( + ... goal_constraints, start_configuration, group, {"planner_id": "RRTConnect"} + ... ) # doctest: +SKIP + >>> trajectory.fraction # doctest: +SKIP 1.0 - >>> len(trajectory.points) > 1 + >>> len(trajectory.points) > 1 # doctest: +SKIP True - >>> ros.close() + >>> ros.close() # doctest: +SKIP Using joint constraints (to the UP configuration): - >>> ros = RosClient() - >>> ros.run() - >>> robot = ros.load_robot() - >>> configuration = Configuration.from_revolute_values([0.0, -1.5707, 0.0, -1.5707, 0.0, 0.0]) - >>> tolerances_above = [math.radians(5)] * len(configuration.joint_values) - >>> tolerances_below = [math.radians(5)] * len(configuration.joint_values) - >>> group = robot.main_group_name - >>> goal_constraints = robot.constraints_from_configuration(configuration, tolerances_above, tolerances_below, group) - >>> trajectory = robot.plan_motion(goal_constraints, start_configuration, group, {'planner_id': 'RRTConnect'}) - >>> trajectory.fraction + >>> ros = RosClient() # doctest: +SKIP + >>> ros.run() # doctest: +SKIP + >>> robot = ros.load_robot() # doctest: +SKIP + >>> configuration = Configuration.from_revolute_values([0.0, -1.5707, 0.0, -1.5707, 0.0, 0.0]) # doctest: +SKIP + >>> tolerances_above = [math.radians(5)] * len(configuration.joint_values) # doctest: +SKIP + >>> tolerances_below = [math.radians(5)] * len(configuration.joint_values) # doctest: +SKIP + >>> group = robot.main_group_name # doctest: +SKIP + >>> goal_constraints = robot.constraints_from_configuration( + ... configuration, tolerances_above, tolerances_below, group + ... ) # doctest: +SKIP + >>> trajectory = robot.plan_motion( + ... goal_constraints, start_configuration, group, {"planner_id": "RRTConnect"} + ... ) # doctest: +SKIP + >>> trajectory.fraction # doctest: +SKIP 1.0 - >>> len(trajectory.points) > 1 + >>> len(trajectory.points) > 1 # doctest: +SKIP True - >>> ros.close() + >>> ros.close() # doctest: +SKIP """ options = options or {} path_constraints = options.get("path_constraints") diff --git a/src/compas_fab/robots/tool.py b/src/compas_fab/robots/tool.py index f3c8e04c83..25b269552c 100644 --- a/src/compas_fab/robots/tool.py +++ b/src/compas_fab/robots/tool.py @@ -30,7 +30,7 @@ class Tool(Data): Examples -------- - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl')) + >>> mesh = Mesh.from_stl(compas_fab.get("planning_scene/cone.stl")) >>> frame = Frame([0.14, 0, 0], [0, 1, 0], [0, 0, 1]) >>> tool = Tool(mesh, frame) @@ -137,12 +137,12 @@ def from_tcf_to_t0cf(self, frames_tcf): Examples -------- - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl')) + >>> mesh = Mesh.from_stl(compas_fab.get("planning_scene/cone.stl")) >>> frame = Frame([0.14, 0, 0], [0, 1, 0], [0, 0, 1]) >>> tool = Tool(mesh, frame) >>> frames_tcf = [Frame((-0.309, -0.046, -0.266), (0.276, 0.926, -0.256), (0.879, -0.136, 0.456))] >>> frames_t0cf = tool.from_tcf_to_t0cf(frames_tcf) - >>> print(frames_t0cf) # doctest: +SKIP + >>> print(frames_t0cf) # doctest: +SKIP [Frame(Point(-0.363, 0.003, -0.147), Vector(0.388, -0.351, -0.852), Vector(0.276, 0.926, -0.256))] # doctest: +SKIP """ return self.tool_model.from_tcf_to_t0cf(frames_tcf) @@ -162,11 +162,11 @@ def from_t0cf_to_tcf(self, frames_t0cf): Examples -------- - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl')) + >>> mesh = Mesh.from_stl(compas_fab.get("planning_scene/cone.stl")) >>> frame = Frame([0.14, 0, 0], [0, 1, 0], [0, 0, 1]) >>> tool = Tool(mesh, frame) >>> frames_t0cf = [Frame((-0.363, 0.003, -0.147), (0.388, -0.351, -0.852), (0.276, 0.926, -0.256))] - >>> tool.from_t0cf_to_tcf(frames_t0cf) # doctest: +SKIP + >>> tool.from_t0cf_to_tcf(frames_t0cf) # doctest: +SKIP [Frame(Point(-0.309, -0.046, -0.266), Vector(0.276, 0.926, -0.256), Vector(0.879, -0.136, 0.456))] # doctest: +SKIP """ return self.tool_model.from_t0cf_to_tcf(frames_t0cf) diff --git a/src/compas_fab/robots/wrench.py b/src/compas_fab/robots/wrench.py index 5128817d20..bdd8f7c4dc 100644 --- a/src/compas_fab/robots/wrench.py +++ b/src/compas_fab/robots/wrench.py @@ -100,9 +100,9 @@ def by_samples(cls, wrenches, proportion_to_cut=0.1): Examples -------- - >>> w1 = Wrench([1, 1, 1], [.1,.1,.1]) - >>> w2 = Wrench([2, 2, 2], [.2,.2,.2]) - >>> w3 = Wrench([3, 3, 3], [.3,.3,.3]) + >>> w1 = Wrench([1, 1, 1], [0.1, 0.1, 0.1]) + >>> w2 = Wrench([2, 2, 2], [0.2, 0.2, 0.2]) + >>> w3 = Wrench([3, 3, 3], [0.3, 0.3, 0.3]) >>> w = Wrench.by_samples([w1, w2, w3]) >>> print(w.force) Vector(x=2.000, y=2.000, z=2.000) diff --git a/src/compas_fab/sensors/base.py b/src/compas_fab/sensors/base.py index 9897abc622..6e8ed330a1 100644 --- a/src/compas_fab/sensors/base.py +++ b/src/compas_fab/sensors/base.py @@ -7,9 +7,9 @@ class SerialSensor(object): port used to communicate with the sensor. Examples: - >>> from serial import Serial # doctest: +SKIP - >>> with Serial('COM5', 57600, timeout=1) as serial: # doctest: +SKIP - ... sensor = SerialSensor(serial) # doctest: +SKIP + >>> from serial import Serial # doctest: +SKIP + >>> with Serial("COM5", 57600, timeout=1) as serial: # doctest: +SKIP + ... sensor = SerialSensor(serial) # doctest: +SKIP """ def __init__(self, serial): diff --git a/src/compas_fab/sensors/baumer.py b/src/compas_fab/sensors/baumer.py index 99c52e6c4c..db8f476b3d 100644 --- a/src/compas_fab/sensors/baumer.py +++ b/src/compas_fab/sensors/baumer.py @@ -33,7 +33,7 @@ class is a context manager type, so it's best used in combination Examples -------- - >>> from serial import Serial # doctest: +SKIP + >>> from serial import Serial # doctest: +SKIP >>> with Serial('COM5', 57600, timeout=1) as serial: # doctest: +SKIP >>> with PosCon3D(serial, PosCon3D.BROADCAST_ADDRESS) as broadcast_query: # doctest: +SKIP ... addr = broadcast_query.get_address() # doctest: +SKIP @@ -360,7 +360,7 @@ class provides access to the serial interface (RS-485). Examples -------- - >>> from serial import Serial # doctest: +SKIP + >>> from serial import Serial # doctest: +SKIP >>> with Serial('COM5', 57600, parity=PARITY_EVEN, timeout=1) as serial: # doctest: +SKIP >>> with PosConCM(serial, PosConCM.BROADCAST_ADDRESS) as broadcast_query: # doctest: +SKIP ... addr = broadcast_query.get_address() # doctest: +SKIP diff --git a/tasks.py b/tasks.py index c75addbba2..1a16b9a441 100644 --- a/tasks.py +++ b/tasks.py @@ -2,10 +2,11 @@ import os -from compas_invocations import build -from compas_invocations import docs -from compas_invocations import style -from compas_invocations import tests +from compas_invocations2 import build +from compas_invocations2 import docs +from compas_invocations2 import style +from compas_invocations2 import tests +from compas_invocations2 import grasshopper from invoke import Collection ns = Collection( @@ -22,6 +23,9 @@ build.clean, build.release, build.build_ghuser_components, + build.build_cpython_ghuser_components, + grasshopper.yakerize, + grasshopper.publish_yak, ) ns.configure( { @@ -31,5 +35,10 @@ "target_dir": "src/compas_fab/ghpython/components/ghuser", "prefix": "COMPAS FAB: ", }, + "ghuser_cpython": { + "source_dir": "src/compas_fab/ghpython/components_cpython", + "target_dir": "src/compas_fab/ghpython/components_cpython/ghuser", + "prefix": "COMPAS FAB: ", + }, } ) diff --git a/tests/robots/test_robot.py b/tests/robots/test_robot.py index c30cc9f9c0..815407dc9c 100644 --- a/tests/robots/test_robot.py +++ b/tests/robots/test_robot.py @@ -3,6 +3,7 @@ import re import pytest +import compas from compas.data import json_dumps from compas.data import json_loads from compas.datastructures import Mesh @@ -471,11 +472,13 @@ def test_wrong_group_name_raises_exception(ur5_robot_instance, robot_tool1): robot.attach_tool(tool, group=wrong_group_name) -def test_attached_tools_no_assigning(ur5_robot_instance, robot_tool1): - robot = ur5_robot_instance +if not compas.IPY: + # this test causes a stack overflow in IronPython for some reason I couldn't track down + def test_attached_tools_no_assigning(ur5_robot_instance, robot_tool1): + robot = ur5_robot_instance - with pytest.raises(AttributeError): - robot.attached_tools = None + with pytest.raises(AttributeError): + robot.attached_tools = None def test_print_robot_info(ur5_robot_instance):