diff --git a/.github/workflows/pants.yaml b/.github/workflows/pants.yaml
index 847e2d4e..1a6980ae 100644
--- a/.github/workflows/pants.yaml
+++ b/.github/workflows/pants.yaml
@@ -80,7 +80,12 @@ jobs:
- name: Test
run: |
source /opt/ros/jazzy/setup.bash
- pants --changed-since=origin/master --changed-dependents=transitive test
+ pants --changed-since=origin/master --changed-dependents=transitive --tag='-unstable_ros' test
+
+ - name: Test changed unstable ROS tests serially with retries
+ run: |
+ source /opt/ros/jazzy/setup.bash
+ pants --process-execution-local-parallelism=1 --changed-since=origin/master --changed-dependents=transitive --tag='unstable_ros' test --test-attempts-default=1
- name: Build Python packages
run: |
pants --filter-target-type=python_distribution package ::
diff --git a/build-support/install_ur_dependencies.sh b/build-support/install_ur_dependencies.sh
index c1a82933..ff28ca6d 100755
--- a/build-support/install_ur_dependencies.sh
+++ b/build-support/install_ur_dependencies.sh
@@ -1,4 +1,10 @@
#!/usr/bin/env bash
+set -euxo pipefail
+
+apt-get update
+
+# debug check
+apt-cache search ros-jazzy | head || true
# Keep ROS packages pinned so CI and the arcor2_ur image exercise the same UR stack over time.
apt-get install -y -q --no-install-recommends \
diff --git a/compose-files/ur-demo/docker-compose.yml b/compose-files/ur-demo/docker-compose.yml
index 33f38923..30e0b9f9 100644
--- a/compose-files/ur-demo/docker-compose.yml
+++ b/compose-files/ur-demo/docker-compose.yml
@@ -1,11 +1,18 @@
services:
ur-demo-robot-api:
- image: arcor2/arcor2_ur:1.7.0
+ image: arcor2/arcor2_ur:1.8.0
container_name: ur-demo-robot-api
ports:
- "5012:5012"
-# environment:
-# - ARCOR2_UR_DEBUG=1
+ environment:
+ - DISPLAY=${DISPLAY}
+ - WAYLAND_DISPLAY=${WAYLAND_DISPLAY}
+ - XDG_RUNTIME_DIR=${XDG_RUNTIME_DIR}
+ - PULSE_SERVER=${PULSE_SERVER}
+ # - ARCOR2_UR_DEBUG=1
+ volumes:
+ - /tmp/.X11-unix:/tmp/.X11-unix
+ - /mnt/wslg:/mnt/wslg
restart: always # the service tends to crash when calling PUT /state/stop
cap_add:
- SYS_NICE
@@ -15,7 +22,7 @@ services:
memlock: 8428281856
# network_mode: host
ur-demo-arserver:
- image: arcor2/arcor2_arserver:1.4.0
+ image: arcor2/arcor2_arserver:1.8.0
container_name: ur-demo-arserver
depends_on:
ur-demo-storage:
@@ -53,7 +60,7 @@ services:
- ur-demo-network
ur-demo-execution:
- image: arcor2/arcor2_execution:1.7.0
+ image: arcor2/arcor2_execution:1.8.0
container_name: ur-demo-execution
networks:
- ur-demo-network
@@ -117,7 +124,7 @@ services:
condition: service_healthy
ur-demo-upload-object-types:
- image: arcor2/arcor2_ur_ot:1.7.0
+ image: arcor2/arcor2_ur_ot:1.8.0
container_name: "ur-demo-upload-object-types"
depends_on:
ur-demo-storage:
@@ -128,7 +135,7 @@ services:
- ARCOR2_STORAGE_SERVICE_URL=http://ur-demo-storage:10000
ur-demo-upload-builtin-objects:
- image: arcor2/arcor2_upload_object_types:2.0.0
+ image: arcor2/arcor2_upload_object_types:1.8.0
container_name: "ur-demo-upload-builtin-objects"
depends_on:
ur-demo-storage:
diff --git a/src/docker/arcor2_arserver/BUILD b/src/docker/arcor2_arserver/BUILD
index dc650f02..5940ea54 100644
--- a/src/docker/arcor2_arserver/BUILD
+++ b/src/docker/arcor2_arserver/BUILD
@@ -1,5 +1,5 @@
shell_source(name="start.sh", source="start.sh")
docker_image(
- name="arcor2_arserver", repository="arcor2/arcor2_arserver", dependencies=[":start.sh"], image_tags=["1.4.0"]
+ name="arcor2_arserver", repository="arcor2/arcor2_arserver", dependencies=[":start.sh"], image_tags=["1.8.0"]
)
diff --git a/src/docker/arcor2_execution/BUILD b/src/docker/arcor2_execution/BUILD
index ab18237f..10dcdf3a 100644
--- a/src/docker/arcor2_execution/BUILD
+++ b/src/docker/arcor2_execution/BUILD
@@ -1 +1 @@
-docker_image(name="arcor2_execution", repository="arcor2/arcor2_execution", image_tags=["1.7.0"])
+docker_image(name="arcor2_execution", repository="arcor2/arcor2_execution", image_tags=["1.8.0"])
diff --git a/src/docker/arcor2_scene/BUILD b/src/docker/arcor2_scene/BUILD
index 05625c89..8839d116 100644
--- a/src/docker/arcor2_scene/BUILD
+++ b/src/docker/arcor2_scene/BUILD
@@ -1 +1 @@
-docker_image(name="arcor2_scene", repository="arcor2/arcor2_scene", image_tags=["1.1.0"])
+docker_image(name="arcor2_scene", repository="arcor2/arcor2_scene", image_tags=["1.8.0"])
diff --git a/src/docker/arcor2_upload_builtin_objects/BUILD b/src/docker/arcor2_upload_builtin_objects/BUILD
index 79472ed2..3e0adf93 100644
--- a/src/docker/arcor2_upload_builtin_objects/BUILD
+++ b/src/docker/arcor2_upload_builtin_objects/BUILD
@@ -1,3 +1 @@
-docker_image(
- name="arcor2_upload_object_types", repository="arcor2/arcor2_upload_object_types", image_tags=["2.0.0"]
-)
+docker_image(name="arcor2_upload_object_types", repository="arcor2/arcor2_upload_object_types", image_tags=["1.8.0"])
diff --git a/src/docker/arcor2_ur/BUILD b/src/docker/arcor2_ur/BUILD
index 153e5de9..c0f9537e 100644
--- a/src/docker/arcor2_ur/BUILD
+++ b/src/docker/arcor2_ur/BUILD
@@ -1,2 +1,13 @@
shell_source(name="start.sh", source="start.sh")
-docker_image(name="arcor2_ur", repository="arcor2/arcor2_ur", dependencies=[":start.sh", "build-support:install_ur_dependencies.sh"], image_tags=["1.7.0"])
+
+docker_image(
+ name="arcor2_ur",
+ repository="arcor2/arcor2_ur",
+ dependencies=[
+ ":start.sh",
+ "build-support:install_ur_dependencies.sh",
+ "src/python/arcor2_ur/scripts:ur",
+ "src/python/arcor2_ur/scripts:robot_publisher",
+ ],
+ image_tags=["1.8.0"],
+)
diff --git a/src/docker/arcor2_ur/Dockerfile b/src/docker/arcor2_ur/Dockerfile
index c7146f90..e72fdbed 100644
--- a/src/docker/arcor2_ur/Dockerfile
+++ b/src/docker/arcor2_ur/Dockerfile
@@ -42,6 +42,7 @@ RUN set -euo pipefail \
&& apt-get update \
&& /root/install_ur_dependencies.sh \
&& apt-get install -y -q --no-install-recommends \
+ ros-jazzy-rviz2 \
libglib2.0-0t64 \
libgomp1 \
libusb-1.0-0 \
@@ -54,6 +55,7 @@ ENV ARCOR2_UR_TYPE=${ARCOR2_UR_TYPE}
ENV ARCOR2_UR_ROBOT_IP=${ARCOR2_UR_ROBOT_IP}
ENV ARCOR2_UR_STARTUP_SLEEP=${ARCOR2_UR_STARTUP_SLEEP}
+COPY src.python.arcor2_ur.scripts/robot_publisher.pex /bin/robot_publisher.pex
COPY src/docker/arcor2_ur/start.sh /root/start.sh
RUN chmod +x /root/start.sh
ENTRYPOINT ["/root/start.sh"]
diff --git a/src/docker/arcor2_ur/start.sh b/src/docker/arcor2_ur/start.sh
index e4ec1ced..8ef1b0ee 100755
--- a/src/docker/arcor2_ur/start.sh
+++ b/src/docker/arcor2_ur/start.sh
@@ -6,6 +6,7 @@ set -e
source /opt/ros/jazzy/setup.bash
: "${ARCOR2_UR_TYPE:=ur5e}"
+: "${ARCOR2_UR_START_ROBOT_PUBLISHER:=true}"
# simulator needs some time to get running...
if [[ -n "$ARCOR2_UR_STARTUP_SLEEP" && "$ARCOR2_UR_STARTUP_SLEEP" =~ ^[0-9]+$ ]]; then
@@ -17,4 +18,9 @@ cp --update=none "$(ros2 pkg prefix --share ur_description)/config/$ARCOR2_UR_TY
ros2 launch ur_robot_driver ur_control.launch.py ur_type:="$ARCOR2_UR_TYPE" robot_ip:="$ARCOR2_UR_ROBOT_IP" launch_rviz:=false kinematics_params_file:="/root/robot_calibration.yaml" &
+if [[ "$ARCOR2_UR_START_ROBOT_PUBLISHER" == "true" ]]; then
+ echo "Starting ARCOR2 UR robot publisher for suction TCP static transforms..."
+ PEX_EXTRA_SYS_PATH=/opt/ros/jazzy/lib/python3.12/site-packages PYTHONOPTIMIZE=1 python3 /bin/robot_publisher.pex &
+fi
+
PEX_EXTRA_SYS_PATH=/opt/ros/jazzy/lib/python3.12/site-packages PYTHONOPTIMIZE=1 /bin/app/pex
diff --git a/src/docker/arcor2_ur_ot/BUILD b/src/docker/arcor2_ur_ot/BUILD
index 2c9556dd..cc1ed7bf 100644
--- a/src/docker/arcor2_ur_ot/BUILD
+++ b/src/docker/arcor2_ur_ot/BUILD
@@ -1 +1 @@
-docker_image(name="arcor2_ur_ot", repository="arcor2/arcor2_ur_ot", image_tags=["1.7.0"])
+docker_image(name="arcor2_ur_ot", repository="arcor2/arcor2_ur_ot", image_tags=["1.8.0"])
diff --git a/src/python/arcor2/BUILD b/src/python/arcor2/BUILD
index 8d927e09..a8e3ace1 100644
--- a/src/python/arcor2/BUILD
+++ b/src/python/arcor2/BUILD
@@ -6,5 +6,5 @@ arcor2_python_distribution(
"src/python/arcor2/data/rpc",
"src/python/arcor2/exceptions",
"src/python/arcor2/source",
- ]
+ ],
)
diff --git a/src/python/arcor2/VERSION b/src/python/arcor2/VERSION
index 359a5b95..27f9cd32 100644
--- a/src/python/arcor2/VERSION
+++ b/src/python/arcor2/VERSION
@@ -1 +1 @@
-2.0.0
\ No newline at end of file
+1.8.0
diff --git a/src/python/arcor2/data/common.py b/src/python/arcor2/data/common.py
index 0d12f093..7708d5d4 100644
--- a/src/python/arcor2/data/common.py
+++ b/src/python/arcor2/data/common.py
@@ -2,6 +2,7 @@
import abc
import copy
+import math
from dataclasses import dataclass, field
from datetime import datetime
from enum import Enum
@@ -139,6 +140,11 @@ def __imul__(self, other: object) -> Position:
return self
+ def distance(self, other: Position) -> float:
+ """Compute Euclidean distance between two positions."""
+ diff = self - other
+ return math.sqrt(diff.x**2 + diff.y**2 + diff.z**2)
+
def to_dict(
self,
omit_none: bool = True,
diff --git a/src/python/arcor2_build/scripts/BUILD b/src/python/arcor2_build/scripts/BUILD
index 586235f6..a65b567a 100644
--- a/src/python/arcor2_build/scripts/BUILD
+++ b/src/python/arcor2_build/scripts/BUILD
@@ -2,5 +2,8 @@ python_sources()
arcor2_pex_binary(
name="build",
- dependencies=["src/python/arcor2_object_types","src/python/arcor2_runtime",]
+ dependencies=[
+ "src/python/arcor2_object_types",
+ "src/python/arcor2_runtime",
+ ],
)
diff --git a/src/python/arcor2_calibration/BUILD b/src/python/arcor2_calibration/BUILD
index c0ce6603..de15e019 100644
--- a/src/python/arcor2_calibration/BUILD
+++ b/src/python/arcor2_calibration/BUILD
@@ -1,5 +1,5 @@
arcor2_python_distribution(
name="arcor2_calibration",
description="ARCOR2 Calibration",
- binaries={"arcor2_calibration": "src/python/arcor2_calibration/scripts:calibration"}
+ binaries={"arcor2_calibration": "src/python/arcor2_calibration/scripts:calibration"},
)
diff --git a/src/python/arcor2_object_types/VERSION b/src/python/arcor2_object_types/VERSION
index 227cea21..27f9cd32 100644
--- a/src/python/arcor2_object_types/VERSION
+++ b/src/python/arcor2_object_types/VERSION
@@ -1 +1 @@
-2.0.0
+1.8.0
diff --git a/src/python/arcor2_object_types/abstract.py b/src/python/arcor2_object_types/abstract.py
index 01a64c2c..bab81db7 100644
--- a/src/python/arcor2_object_types/abstract.py
+++ b/src/python/arcor2_object_types/abstract.py
@@ -2,6 +2,7 @@
import copy
import inspect
from dataclasses import dataclass
+from datetime import datetime, timezone
from dataclasses_jsonschema import JsonSchemaMixin
from PIL import Image
@@ -14,6 +15,7 @@
from arcor2.docstring import parse_docstring
from arcor2.exceptions import Arcor2Exception, Arcor2NotImplemented
from arcor2.helpers import NonBlockingLock
+from arcor2_ur.common import GraspableSource, GraspableState
class GenericException(Arcor2Exception):
@@ -183,6 +185,125 @@ def set_enabled(self, state: bool, *, an: None | str = None) -> None:
set_enabled.__action__ = ActionMetadata() # type: ignore
+def _utc_now_iso() -> str:
+ """Return the current UTC timestamp in ISO 8601 format."""
+ return datetime.now(timezone.utc).isoformat()
+
+
+class GraspableObject(GenericWithPose):
+ """Object in the scene that can potentially be grasped."""
+
+ mesh_filename: None | str = None
+
+ def __init__(
+ self,
+ obj_id: str,
+ name: str,
+ pose: Pose,
+ collision_model: Models,
+ state: GraspableState = GraspableState.WORLD,
+ source: GraspableSource = GraspableSource.OTHER,
+ stamp: str | None = None,
+ settings: None | Settings = None,
+ ) -> None:
+ super().__init__(obj_id, name, pose, settings)
+
+ self.collision_model = copy.deepcopy(collision_model)
+ self._enabled = True
+ self._state = state
+ self._source = source
+ self._stamp = stamp or _utc_now_iso()
+
+ # originally, each model has id == object type (e.g. BigBox) but here we need to set it to something unique
+ self.collision_model.id = self.id
+ self._upsert_graspable()
+
+ def _upsert_graspable(self) -> None:
+ _scene_service().upsert_graspable(
+ self.collision_model,
+ self.pose,
+ state=self._state,
+ source=self._source,
+ stamp=self._stamp,
+ )
+
+ def set_pose(self, pose: Pose) -> None:
+ super().set_pose(pose)
+ if self._enabled:
+ self._upsert_graspable()
+
+ @property
+ def enabled(self) -> bool:
+ """If the object has a collision model, this indicates whether the
+ model is enabled (set on the Scene service).
+
+ When set, it updates the state of the model on the Scene service.
+ :return:
+ """
+
+ svc = _scene_service()
+ assert (self.id in svc.collision_ids()) == self._enabled
+ return self._enabled
+
+ @enabled.setter
+ def enabled(self, enabled: bool) -> None:
+ if not self._enabled and enabled:
+ svc = _scene_service()
+ assert self.id not in svc.collision_ids()
+ self._upsert_graspable()
+ if self._enabled and not enabled:
+ _scene_service().delete_collision_id(self.id)
+ self._enabled = enabled
+
+ def set_enabled(self, state: bool, *, an: None | str = None) -> None:
+ """Enables control of the object's collision model.
+
+ :param state: State of a collision model.
+ :return:
+ """
+ self.enabled = state
+
+ set_enabled.__action__ = ActionMetadata() # type: ignore
+
+ @property
+ def state(self) -> GraspableState:
+ return self._state
+
+ @state.setter
+ def state(self, state: GraspableState) -> None:
+ self._state = state
+ self._stamp = _utc_now_iso()
+
+ if self._enabled:
+ self._upsert_graspable()
+
+ @property
+ def source(self) -> GraspableSource:
+ return self._source
+
+ @source.setter
+ def source(self, source: GraspableSource) -> None:
+ self._source = source
+ self._stamp = _utc_now_iso()
+
+ if self._enabled:
+ self._upsert_graspable()
+
+ @property
+ def stamp(self) -> str:
+ return self._stamp
+
+ def set_state(self, state: GraspableState, *, an: None | str = None) -> None:
+ self.state = state
+
+ set_state.__action__ = ActionMetadata() # type: ignore
+
+ def set_source(self, source: GraspableSource, *, an: None | str = None) -> None:
+ self.source = source
+
+ set_source.__action__ = ActionMetadata() # type: ignore
+
+
class VirtualCollisionObject(CollisionObject):
"""Should be used to represent obstacles or another 'dumb' objects at the
workplace."""
diff --git a/src/python/arcor2_scene/BUILD b/src/python/arcor2_scene/BUILD
index 2c872bc4..5810d059 100644
--- a/src/python/arcor2_scene/BUILD
+++ b/src/python/arcor2_scene/BUILD
@@ -3,5 +3,5 @@ arcor2_python_distribution(
description="ARCOR2 Scene Service",
binaries={
"arcor2_scene": "src/python/arcor2_scene/scripts:scene",
- }
+ },
)
diff --git a/src/python/arcor2_scene/VERSION b/src/python/arcor2_scene/VERSION
index 1cc5f657..27f9cd32 100644
--- a/src/python/arcor2_scene/VERSION
+++ b/src/python/arcor2_scene/VERSION
@@ -1 +1 @@
-1.1.0
\ No newline at end of file
+1.8.0
diff --git a/src/python/arcor2_scene/scripts/scene.py b/src/python/arcor2_scene/scripts/scene.py
index 08ac5977..a9b4fef4 100644
--- a/src/python/arcor2_scene/scripts/scene.py
+++ b/src/python/arcor2_scene/scripts/scene.py
@@ -18,6 +18,7 @@
from arcor2.logging import get_logger
from arcor2_scene import SCENE_PORT, SCENE_SERVICE_NAME, version
from arcor2_scene.exceptions import NotFound, SceneGeneral, WebApiError
+from arcor2_ur.common import parse_collision_body
from arcor2_web.flask import Response, RespT, create_app, run_app
app = create_app(__name__)
@@ -97,12 +98,11 @@ def put_box() -> RespT:
$ref: WebApiError
"""
- if not isinstance(request.json, dict):
- raise SceneGeneral("Body should be a JSON dict containing Pose.")
+ pose, _ = parse_collision_body()
args = request.args.to_dict()
box = object_type.Box(args["boxId"], float(args["sizeX"]), float(args["sizeY"]), float(args["sizeZ"]))
- collision_objects[box.id] = CollisionObject(box, common.Pose.from_dict(humps.decamelize(request.json)))
+ collision_objects[box.id] = CollisionObject(box, pose)
return jsonify("ok"), 200
@@ -147,12 +147,11 @@ def put_sphere() -> RespT:
$ref: WebApiError
"""
- if not isinstance(request.json, dict):
- raise SceneGeneral("Body should be a JSON dict containing Pose.")
+ pose, _ = parse_collision_body()
args = humps.decamelize(request.args.to_dict())
sphere = object_type.Sphere(args["sphere_id"], float(args["radius"]))
- collision_objects[sphere.id] = CollisionObject(sphere, common.Pose.from_dict(humps.decamelize(request.json)))
+ collision_objects[sphere.id] = CollisionObject(sphere, pose)
return jsonify("ok"), 200
@@ -201,12 +200,11 @@ def put_cylinder() -> RespT:
$ref: WebApiError
"""
- if not isinstance(request.json, dict):
- raise SceneGeneral("Body should be a JSON dict containing Pose.")
+ pose, _ = parse_collision_body()
args = humps.decamelize(request.args.to_dict())
cylinder = object_type.Cylinder(args["cylinder_id"], float(args["radius"]), float(args["height"]))
- collision_objects[cylinder.id] = CollisionObject(cylinder, common.Pose.from_dict(humps.decamelize(request.json)))
+ collision_objects[cylinder.id] = CollisionObject(cylinder, pose)
return jsonify("ok"), 200
@@ -267,12 +265,11 @@ def put_mesh() -> RespT:
$ref: WebApiError
"""
- if not isinstance(request.json, dict):
- raise SceneGeneral("Body should be a JSON dict containing Pose.")
+ pose, _ = parse_collision_body()
args = humps.decamelize(request.args.to_dict())
mesh = object_type.Mesh(args["mesh_id"], args["mesh_file_id"])
- collision_objects[mesh.id] = CollisionObject(mesh, common.Pose.from_dict(humps.decamelize(request.json)))
+ collision_objects[mesh.id] = CollisionObject(mesh, pose)
return jsonify("ok"), 200
diff --git a/src/python/arcor2_scene_data/VERSION b/src/python/arcor2_scene_data/VERSION
index 3eefcb9d..27f9cd32 100644
--- a/src/python/arcor2_scene_data/VERSION
+++ b/src/python/arcor2_scene_data/VERSION
@@ -1 +1 @@
-1.0.0
+1.8.0
diff --git a/src/python/arcor2_scene_data/scene_service.py b/src/python/arcor2_scene_data/scene_service.py
index 18f0aa35..f5928285 100644
--- a/src/python/arcor2_scene_data/scene_service.py
+++ b/src/python/arcor2_scene_data/scene_service.py
@@ -1,6 +1,8 @@
import os
import time
-from dataclasses import dataclass
+from dataclasses import dataclass, field
+from datetime import datetime, timezone
+from typing import Any
from dataclasses_jsonschema import JsonSchemaMixin
@@ -10,6 +12,7 @@
from arcor2.exceptions import Arcor2Exception
from arcor2.exceptions.helpers import handle
from arcor2.logging import get_logger
+from arcor2_object_types.abstract import GraspableSource, GraspableState
from arcor2_web import rest
URL = os.getenv("ARCOR2_SCENE_SERVICE_URL", "http://0.0.0.0:5013")
@@ -22,6 +25,12 @@ class SceneServiceException(Arcor2Exception):
pass
+@dataclass
+class CollisionBody(JsonSchemaMixin):
+ pose: Pose
+ metadata: dict[str, Any] = field(default_factory=dict)
+
+
@dataclass
class MeshParameters(JsonSchemaMixin):
mesh_scale_x: float = 1.0
@@ -30,6 +39,23 @@ class MeshParameters(JsonSchemaMixin):
transform_id: str = "world"
+def _utc_now_iso() -> str:
+ return datetime.now(timezone.utc).isoformat()
+
+
+def _graspable_metadata(
+ state: GraspableState = GraspableState.WORLD,
+ source: GraspableSource = GraspableSource.OTHER,
+ stamp: str | None = None,
+) -> dict[str, Any]:
+ return {
+ "object_type": "graspable",
+ "state": state.value,
+ "source": source.value,
+ "stamp": stamp or _utc_now_iso(),
+ }
+
+
def wait_for(timeout: float = 10.0) -> None:
start_time = time.monotonic()
while time.monotonic() < start_time + timeout:
@@ -42,8 +68,28 @@ def wait_for(timeout: float = 10.0) -> None:
raise SceneServiceException("Failed to contact Scene service.")
+def _collision_params(model: Models, mesh_parameters: None | MeshParameters = None) -> tuple[dict[str, Any], str]:
+ params = model.to_dict()
+ model_type = model.type().value.lower()
+
+ params[f"{model_type}Id"] = model.id
+ del params["id"]
+
+ if model.type() == Model3dType.MESH:
+ params["meshFileId"] = params.pop("asset_id")
+
+ if mesh_parameters:
+ params.update(mesh_parameters.to_dict())
+
+ return params, model_type
+
+
@handle(SceneServiceException, logger, message="Failed to add or update the collision model.")
-def upsert_collision(model: Models, pose: Pose, mesh_parameters: None | MeshParameters = None) -> None:
+def upsert_collision(
+ model: Models,
+ pose: Pose,
+ mesh_parameters: None | MeshParameters = None,
+) -> None:
"""Adds arbitrary collision model to the collision scene.
:param model: Box, Sphere, Cylinder, Mesh
@@ -60,19 +106,50 @@ def upsert_collision(model: Models, pose: Pose, mesh_parameters: None | MeshPara
>>> scene_service.upsert_collision(box, Pose(Position(1, 0, 0), Orientation(0, 0, 0, 1)))
"""
- params = model.to_dict()
- model_type = model.type().value.lower()
+ params, model_type = _collision_params(model, mesh_parameters)
+ body = CollisionBody(pose, {})
+ rest.call(rest.Method.PUT, f"{URL}/collisions/{model_type}", body=body, params=params)
- params[f"{model_type}Id"] = model.id
- del params["id"]
- if model.type() == Model3dType.MESH:
- params["meshFileId"] = params.pop("asset_id")
+@handle(SceneServiceException, logger, message="Failed to add or update the graspable object.")
+def upsert_graspable(
+ model: Models,
+ pose: Pose,
+ state: GraspableState,
+ source: GraspableSource = GraspableSource.OTHER,
+ stamp: str | None = None,
+ mesh_parameters: None | MeshParameters = None,
+) -> None:
+ """Adds arbitrary graspable object to the collision scene.
- if mesh_parameters:
- params.update(mesh_parameters.to_dict())
+ Internally, graspable objects are transferred using collision API enriched
+ by graspable metadata.
+
+ :param model: Box, Sphere, Cylinder, Mesh
+ :param pose: Pose of the graspable object.
+ :param state: Logical state of the graspable object.
+ :param source: Source of the graspable pose information.
+ :param stamp: Optional timestamp for graspable metadata.
+ :param mesh_parameters: Some additional parameters might be specified for mesh collision model.
+ :return:
+
+ Example usage:
+
+ >>> from arcor2_scene_data import scene_service
+ >>> from arcor2.data.object_type import Box
+ >>> from arcor2.data.common import Pose, Position, Orientation
+ >>> from arcor2_object_types.abstract import GraspableState
+ >>> box = Box("UniqueBoxId", 0.1, 0.1, 0.1)
+ >>> scene_service.upsert_graspable(
+ ... box,
+ ... Pose(Position(1, 0, 0), Orientation(0, 0, 0, 1)),
+ ... state=GraspableState.WORLD,
+ ... )
+ """
- rest.call(rest.Method.PUT, f"{URL}/collisions/{model_type}", body=pose, params=params)
+ params, model_type = _collision_params(model, mesh_parameters)
+ body = CollisionBody(pose, _graspable_metadata(state, source, stamp))
+ rest.call(rest.Method.PUT, f"{URL}/collisions/{model_type}", body=body, params=params)
@handle(SceneServiceException, logger, message="Failed to delete the collision.")
@@ -157,6 +234,7 @@ def world_pose(transform_id: str) -> Pose:
__all__ = [
SceneServiceException.__name__,
upsert_collision.__name__,
+ upsert_graspable.__name__,
delete_collision_id.__name__,
collision_ids.__name__,
focus.__name__,
diff --git a/src/python/arcor2_ur/VERSION b/src/python/arcor2_ur/VERSION
index 9dbb0c00..27f9cd32 100644
--- a/src/python/arcor2_ur/VERSION
+++ b/src/python/arcor2_ur/VERSION
@@ -1 +1 @@
-1.7.0
\ No newline at end of file
+1.8.0
diff --git a/src/python/arcor2_ur/common.py b/src/python/arcor2_ur/common.py
new file mode 100644
index 00000000..5a30d68a
--- /dev/null
+++ b/src/python/arcor2_ur/common.py
@@ -0,0 +1,85 @@
+from dataclasses import dataclass, field
+from typing import Any
+
+import humps
+from flask import request
+
+from arcor2.data import common, object_type
+from arcor2.data.common import StrEnum
+from arcor2_ur.exceptions import UrGeneral
+
+
+class GraspPosition(StrEnum):
+ TOP = "TOP"
+ RIGHT = "RIGHT"
+ LEFT = "LEFT"
+ FRONT = "FRONT"
+ BACK = "BACK"
+ BOTTOM = "BOTTOM"
+ ALL = "ALL"
+
+
+class EffectorType(StrEnum):
+ SUCK = "SUCK"
+
+
+class GraspableState(StrEnum):
+ """Logical state of a graspable object in the scene.
+
+ WORLD
+ Free object in the environment. Acts as a collision obstacle.
+
+ RESERVED
+ Reserved by the robot. Waiting for pickup.
+
+ HIDDEN
+ Object is hidden so the robot can attach it.
+ This state is used only in ros_worker file.
+
+ ATTACHED
+ Attached to the robot end-effector.
+ """
+
+ WORLD = "WORLD"
+ RESERVED = "RESERVED"
+ HIDDEN = "HIDDEN"
+ ATTACHED = "ATTACHED"
+
+
+class GraspableSource(StrEnum):
+ """Source of the object pose information.
+
+ CAMERA
+ Detected by a vision system.
+
+ FIXED
+ Predefined static object.
+
+ OTHER
+ Arbitrary or unspecified source (e.g., tests, debugging, simulations, or other scenarios).
+ """
+
+ CAMERA = "CAMERA"
+ FIXED = "FIXED"
+ OTHER = "OTHER"
+
+
+@dataclass
+class CollisionSceneObject:
+ model: object_type.Models
+ pose: common.Pose
+ metadata: dict[str, Any] = field(default_factory=dict)
+
+
+def parse_collision_body() -> tuple[common.Pose, dict[str, Any]]:
+ if not isinstance(request.json, dict):
+ raise UrGeneral("Body should be a JSON dict containing Pose or pose and metadata.")
+
+ body = humps.decamelize(request.json)
+
+ if "pose" in body:
+ pose = common.Pose.from_dict(body["pose"])
+ metadata = body.get("metadata", {}) or {}
+ return pose, metadata
+
+ return common.Pose.from_dict(body), {}
diff --git a/src/python/arcor2_ur/data/BUILD b/src/python/arcor2_ur/data/BUILD
index 1edd5c1b..0d71e86a 100644
--- a/src/python/arcor2_ur/data/BUILD
+++ b/src/python/arcor2_ur/data/BUILD
@@ -1 +1,14 @@
-resources(name="moveit.yaml", sources=["moveit.yaml"])
\ No newline at end of file
+resources(
+ name="moveit_yaml",
+ sources=["moveit.yaml"],
+)
+
+resources(
+ name="urdf_assets",
+ sources=[
+ "urdf/ur5e.urdf",
+ "urdf/ur5e.srdf",
+ "urdf/meshes/ur5e/collision/*.stl",
+ "urdf/meshes/ur5e/visual/*.dae",
+ ],
+)
\ No newline at end of file
diff --git a/src/python/arcor2_ur/data/moveit.yaml b/src/python/arcor2_ur/data/moveit.yaml
index f7ac8d26..a31eabf8 100644
--- a/src/python/arcor2_ur/data/moveit.yaml
+++ b/src/python/arcor2_ur/data/moveit.yaml
@@ -11,12 +11,12 @@ planning_pipelines:
pipeline_names: ["ompl"]
plan_request_params:
- planning_attempts: 2
+ planning_attempts: 3
planning_pipeline: ompl
planner_id: RRTConnectkConfigDefault
max_velocity_scaling_factor: 1.0
max_acceleration_scaling_factor: 1.0
- planning_time: 5.0
+ planning_time: 10.0
goal_joint_tolerance: 0.001
goal_position_tolerance: 0.005
goal_orientation_tolerance: 0.01
@@ -39,4 +39,13 @@ ompl:
response_adapters:
- default_planning_response_adapters/AddTimeOptimalParameterization
- default_planning_response_adapters/ValidateSolution
- - default_planning_response_adapters/DisplayMotionPath
\ No newline at end of file
+ - default_planning_response_adapters/DisplayMotionPath
+
+ planner_configs:
+ RRTConnectkConfigDefault:
+ type: geometric::RRTConnect
+ range: 0.0
+
+ ur_manipulator:
+ planner_configs:
+ - RRTConnectkConfigDefault
\ No newline at end of file
diff --git a/src/python/arcor2_ur/data/urdf/BUILD b/src/python/arcor2_ur/data/urdf/BUILD
index 73b4efc3..ea7c4948 100644
--- a/src/python/arcor2_ur/data/urdf/BUILD
+++ b/src/python/arcor2_ur/data/urdf/BUILD
@@ -1,8 +1,7 @@
-resources(
- name="ur5e",
+files(
+ name="urdf",
sources=[
"ur5e.urdf",
- "meshes/ur5e/collision/*.stl",
- "meshes/ur5e/visual/*.dae",
+ "meshes/**",
],
-)
+)
\ No newline at end of file
diff --git a/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/base_link_1cup.stl b/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/base_link_1cup.stl
new file mode 100644
index 00000000..0325ac47
--- /dev/null
+++ b/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/base_link_1cup.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ad35d228922a4e59b18359b84740f77dabb1176050960050be2cb939ee815c7c
+size 250084
diff --git a/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/stary_suction_cup_1cup.stl b/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/stary_suction_cup_1cup.stl
new file mode 100644
index 00000000..67b71ffa
--- /dev/null
+++ b/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/stary_suction_cup_1cup.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:10686a45842b55d7ce5dc6d3dbf1e51b235a1722e4697e4cec2ccf22e65bbfc4
+size 250034
diff --git a/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/suction_cup_1cup.stl b/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/suction_cup_1cup.stl
new file mode 100644
index 00000000..67b71ffa
--- /dev/null
+++ b/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/suction_cup_1cup.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:10686a45842b55d7ce5dc6d3dbf1e51b235a1722e4697e4cec2ccf22e65bbfc4
+size 250034
diff --git a/src/python/arcor2_ur/data/urdf/meshes/ur5e/visual/base_link_1cup.stl b/src/python/arcor2_ur/data/urdf/meshes/ur5e/visual/base_link_1cup.stl
new file mode 100644
index 00000000..17bc267e
--- /dev/null
+++ b/src/python/arcor2_ur/data/urdf/meshes/ur5e/visual/base_link_1cup.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:521d044495d657ab0f843fec69d3b9025477c3f139fb6b0060b0c9fca682865a
+size 750084
diff --git a/src/python/arcor2_ur/data/urdf/meshes/ur5e/visual/suction_cup_1cup.stl b/src/python/arcor2_ur/data/urdf/meshes/ur5e/visual/suction_cup_1cup.stl
new file mode 100644
index 00000000..547804c8
--- /dev/null
+++ b/src/python/arcor2_ur/data/urdf/meshes/ur5e/visual/suction_cup_1cup.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:160de600a401f908bec47d7f8553adbc926e4e09a0d3cc16b508d16e2353a3e4
+size 250084
diff --git a/src/python/arcor2_ur/data/urdf/ur5e.srdf b/src/python/arcor2_ur/data/urdf/ur5e.srdf
new file mode 100644
index 00000000..31df2964
--- /dev/null
+++ b/src/python/arcor2_ur/data/urdf/ur5e.srdf
@@ -0,0 +1,117 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/python/arcor2_ur/data/urdf/ur5e.urdf b/src/python/arcor2_ur/data/urdf/ur5e.urdf
index f0e7f573..f2754ef5 100644
--- a/src/python/arcor2_ur/data/urdf/ur5e.urdf
+++ b/src/python/arcor2_ur/data/urdf/ur5e.urdf
@@ -288,4 +288,50 @@
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/python/arcor2_ur/debug/BUILD b/src/python/arcor2_ur/debug/BUILD
new file mode 100644
index 00000000..3cc4b908
--- /dev/null
+++ b/src/python/arcor2_ur/debug/BUILD
@@ -0,0 +1,26 @@
+python_sources(
+ name="lib",
+ dependencies=[
+ "src/python/arcor2_ur/data:urdf_assets",
+ ],
+)
+
+pex_binary(
+ name="debug",
+ entry_point="arcor2_ur.debug.debug",
+ dependencies=[
+ ":lib",
+ "src/python/arcor2_ur/scripts:ur",
+ "src/python/arcor2_ur/scripts:robot_publisher",
+ ],
+)
+
+pex_binary(
+ name="box_in_path_debug",
+ entry_point="arcor2_ur.debug.box_in_path_debug",
+ dependencies=[
+ ":lib",
+ "src/python/arcor2_ur/scripts:ur",
+ "src/python/arcor2_ur/scripts:robot_publisher",
+ ],
+)
\ No newline at end of file
diff --git a/src/python/arcor2_ur/debug/custom.rviz b/src/python/arcor2_ur/debug/custom.rviz
new file mode 100644
index 00000000..60938c92
--- /dev/null
+++ b/src/python/arcor2_ur/debug/custom.rviz
@@ -0,0 +1,511 @@
+Panels:
+ - Class: rviz_common/Displays
+ Help Height: 70
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /MotionPlanning1
+ - /MotionPlanning1/Planned Path1
+ Splitter Ratio: 0.5
+ Tree Height: 373
+ - Class: rviz_common/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 1
+ Cell Size: 1
+ Class: rviz_default_plugins/Grid
+ Color: 160; 160; 160
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 20
+ Reference Frame:
+ Value: true
+ - Alpha: 1
+ Class: rviz_default_plugins/RobotModel
+ Collision Enabled: false
+ Description File: ""
+ Description Source: Topic
+ Description Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /robot_description
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ base:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ base_link_inertia:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ flange:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ forearm_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ ft_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ shoulder_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ suction_base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ suction_cup_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ suction_tcp:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ tool0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ upper_arm_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ world:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ wrist_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ wrist_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ wrist_3_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Mass Properties:
+ Inertia: false
+ Mass: false
+ Name: RobotModel
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ - Class: rviz_default_plugins/TF
+ Enabled: true
+ Filter (blacklist): ""
+ Filter (whitelist): ""
+ Frame Timeout: 15
+ Frames:
+ All Enabled: true
+ base:
+ Value: true
+ base_link:
+ Value: true
+ base_link_inertia:
+ Value: true
+ flange:
+ Value: true
+ forearm_link:
+ Value: true
+ ft_frame:
+ Value: true
+ shoulder_link:
+ Value: true
+ suction_base_link:
+ Value: true
+ suction_cup_link:
+ Value: true
+ suction_tcp:
+ Value: true
+ tool0:
+ Value: true
+ upper_arm_link:
+ Value: true
+ world:
+ Value: true
+ wrist_1_link:
+ Value: true
+ wrist_2_link:
+ Value: true
+ wrist_3_link:
+ Value: true
+ Marker Scale: 0.20000000298023224
+ Name: TF
+ Show Arrows: false
+ Show Axes: false
+ Show Names: false
+ Tree:
+ world:
+ base_link:
+ base:
+ {}
+ base_link_inertia:
+ shoulder_link:
+ upper_arm_link:
+ forearm_link:
+ wrist_1_link:
+ wrist_2_link:
+ wrist_3_link:
+ flange:
+ tool0:
+ suction_base_link:
+ suction_cup_link:
+ suction_tcp:
+ {}
+ ft_frame:
+ {}
+ Update Interval: 0
+ Value: true
+ - Acceleration_Scaling_Factor: 0.1
+ Class: moveit_rviz_plugin/MotionPlanning
+ Enabled: true
+ Move Group Namespace: ""
+ MoveIt_Allow_Approximate_IK: false
+ MoveIt_Allow_External_Program: false
+ MoveIt_Allow_Replanning: false
+ MoveIt_Allow_Sensor_Positioning: false
+ MoveIt_Planning_Attempts: 10
+ MoveIt_Planning_Time: 5
+ MoveIt_Use_Cartesian_Path: false
+ MoveIt_Use_Constraint_Aware_IK: false
+ MoveIt_Workspace:
+ Center:
+ X: 0
+ Y: 0
+ Z: 0
+ Size:
+ X: 2
+ Y: 2
+ Z: 2
+ Name: MotionPlanning
+ Planned Path:
+ Color Enabled: false
+ Interrupt Display: false
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ base:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ base_link_inertia:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ flange:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ forearm_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ ft_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ shoulder_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ suction_base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ suction_cup_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ suction_tcp:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ tool0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ upper_arm_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ world:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ wrist_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ wrist_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ wrist_3_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Loop Animation: false
+ Robot Alpha: 0.5
+ Robot Color: 150; 50; 150
+ Show Robot Collision: false
+ Show Robot Visual: true
+ Show Trail: false
+ State Display Time: 3x
+ Trail Step Size: 1
+ Trajectory Topic: /display_planned_path
+ Use Sim Time: false
+ Planning Metrics:
+ Payload: 1
+ Show Joint Torques: false
+ Show Manipulability: false
+ Show Manipulability Index: false
+ Show Weight Limit: false
+ TextHeight: 0.07999999821186066
+ Planning Request:
+ Colliding Link Color: 255; 0; 0
+ Goal State Alpha: 1
+ Goal State Color: 250; 128; 0
+ Interactive Marker Size: 0
+ Joint Violation Color: 255; 0; 255
+ Planning Group: ur_manipulator
+ Query Goal State: false
+ Query Start State: false
+ Show Workspace: false
+ Start State Alpha: 1
+ Start State Color: 0; 255; 0
+ Planning Scene Topic: /monitored_planning_scene
+ Robot Description: robot_description
+ Scene Geometry:
+ Scene Alpha: 0.8999999761581421
+ Scene Color: 50; 230; 50
+ Scene Display Time: 0.009999999776482582
+ Show Scene Geometry: true
+ Voxel Coloring: Z-Axis
+ Voxel Rendering: Occupied Voxels
+ Scene Robot:
+ Attached Body Color: 150; 50; 150
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ base:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ base_link_inertia:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ flange:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ forearm_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ ft_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ shoulder_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ suction_base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ suction_cup_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ suction_tcp:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ tool0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ upper_arm_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ world:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ wrist_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ wrist_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ wrist_3_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Robot Alpha: 1
+ Show Robot Collision: false
+ Show Robot Visual: true
+ Value: true
+ Velocity_Scaling_Factor: 0.1
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Fixed Frame: base_link
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz_default_plugins/Interact
+ Hide Inactive Objects: true
+ - Class: rviz_default_plugins/MoveCamera
+ - Class: rviz_default_plugins/Select
+ - Class: rviz_default_plugins/FocusCamera
+ - Class: rviz_default_plugins/Measure
+ Line color: 128; 128; 0
+ - Class: rviz_default_plugins/SetInitialPose
+ Covariance x: 0.25
+ Covariance y: 0.25
+ Covariance yaw: 0.06853891909122467
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: initialpose
+ - Class: rviz_default_plugins/SetGoal
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: goal_pose
+ - Class: rviz_default_plugins/PublishPoint
+ Single click: true
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /clicked_point
+ Transformation:
+ Current:
+ Class: rviz_default_plugins/TF
+ Value: true
+ Views:
+ Current:
+ Class: rviz_default_plugins/Orbit
+ Distance: 2.4266345500946045
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 0
+ Y: 0
+ Z: 0.4000000059604645
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.6399993896484375
+ Target Frame:
+ Value: Orbit (rviz_default_plugins)
+ Yaw: 2.0200021266937256
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: true
+ Height: 652
+ Hide Left Dock: true
+ Hide Right Dock: false
+ MotionPlanning:
+ collapsed: false
+ MotionPlanning - Trajectory Slider:
+ collapsed: false
+ QMainWindow State: 000000ff00000000fd0000000100000000000001c9000003aafc0200000005fb000000100044006900730070006c006100790073000000003b000003aa000000c700fffffffb0000000a00560069006500770073000000015a000000a0000000a000fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067000000015f0000020a0000016900ffffff000003170000023600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Views:
+ collapsed: false
+ Width: 791
+ X: 98
+ Y: 24
diff --git a/src/python/arcor2_ur/debug/custom_rsp.launch.py b/src/python/arcor2_ur/debug/custom_rsp.launch.py
new file mode 100644
index 00000000..91c6fa25
--- /dev/null
+++ b/src/python/arcor2_ur/debug/custom_rsp.launch.py
@@ -0,0 +1,36 @@
+# noqa: TAE001
+# mypy: ignore-errors
+from pathlib import Path
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+ package_root = Path(__file__).resolve().parents[1]
+ urdf_path = package_root / "data" / "urdf" / "ur5e.urdf"
+ meshes_dir = package_root / "data" / "urdf" / "meshes"
+
+ text = urdf_path.read_text(encoding="utf-8")
+ text = text.replace(
+ "package://meshes/",
+ f"file://{meshes_dir.as_posix()}/",
+ )
+
+ return LaunchDescription(
+ [
+ DeclareLaunchArgument("ur_type", default_value="ur5e"),
+ DeclareLaunchArgument("robot_ip", default_value="xyz"),
+ DeclareLaunchArgument("use_mock_hardware", default_value="true"),
+ DeclareLaunchArgument("mock_sensor_commands", default_value="false"),
+ DeclareLaunchArgument("headless_mode", default_value="false"),
+ DeclareLaunchArgument("kinematics_parameters_file", default_value=""),
+ Node(
+ package="robot_state_publisher",
+ executable="robot_state_publisher",
+ output="screen",
+ parameters=[{"robot_description": text}],
+ ),
+ ]
+ )
diff --git a/src/python/arcor2_ur/debug/ur5e_custom.launch.py b/src/python/arcor2_ur/debug/ur5e_custom.launch.py
new file mode 100644
index 00000000..a05f9383
--- /dev/null
+++ b/src/python/arcor2_ur/debug/ur5e_custom.launch.py
@@ -0,0 +1,45 @@
+# noqa: TAE001
+# mypy: ignore-errors
+from pathlib import Path
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description() -> LaunchDescription:
+ declared_arguments = [
+ DeclareLaunchArgument("robot_ip"),
+ DeclareLaunchArgument("use_mock_hardware", default_value="false"),
+ DeclareLaunchArgument("mock_sensor_commands", default_value="false"),
+ DeclareLaunchArgument(
+ "initial_joint_controller",
+ default_value="scaled_joint_trajectory_controller",
+ ),
+ DeclareLaunchArgument("activate_joint_controller", default_value="true"),
+ ]
+
+ robot_ip = LaunchConfiguration("robot_ip")
+ use_mock_hardware = LaunchConfiguration("use_mock_hardware")
+ mock_sensor_commands = LaunchConfiguration("mock_sensor_commands")
+ initial_joint_controller = LaunchConfiguration("initial_joint_controller")
+ activate_joint_controller = LaunchConfiguration("activate_joint_controller")
+
+ ur_control = "/opt/ros/jazzy/share/ur_robot_driver/launch/ur_control.launch.py"
+ custom_rsp = str(Path(__file__).resolve().parent / "custom_rsp.launch.py")
+
+ base_launch = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(ur_control),
+ launch_arguments={
+ "ur_type": "ur5e",
+ "robot_ip": robot_ip,
+ "use_mock_hardware": use_mock_hardware,
+ "mock_sensor_commands": mock_sensor_commands,
+ "initial_joint_controller": initial_joint_controller,
+ "activate_joint_controller": activate_joint_controller,
+ "description_launchfile": custom_rsp,
+ }.items(),
+ )
+
+ return LaunchDescription(declared_arguments + [base_launch])
diff --git a/src/python/arcor2_ur/object_types/ur5e.py b/src/python/arcor2_ur/object_types/ur5e.py
index 5dab40a4..4c3ecb21 100644
--- a/src/python/arcor2_ur/object_types/ur5e.py
+++ b/src/python/arcor2_ur/object_types/ur5e.py
@@ -4,9 +4,11 @@
from dataclasses_jsonschema import JsonSchemaMixin
-from arcor2.data.common import ActionMetadata, Joint, Pose, StrEnum
+from arcor2.data import object_type
+from arcor2.data.common import ActionMetadata, Joint, Pose, Position, StrEnum
from arcor2.data.robot import InverseKinematicsRequest
from arcor2_object_types.abstract import Robot, Settings
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
from arcor2_web import rest
@@ -24,6 +26,36 @@ def avg(self) -> float:
return (self.a + self.b) / 2
+@dataclass
+class PickUpObjectByPosition(JsonSchemaMixin):
+ position: Position
+ radius: float
+ effector_type: EffectorType = EffectorType.SUCK
+ grasp_position: GraspPosition = GraspPosition.ALL
+ object_type_name: object_type.Model3dType | None = None
+ velocity: float = 50.0
+ payload: float = 0.0
+ safe: bool = True
+
+
+@dataclass
+class PickUpObjectById(JsonSchemaMixin):
+ object_id: str
+ effector_type: EffectorType = EffectorType.SUCK
+ grasp_position: GraspPosition = GraspPosition.ALL
+ velocity: float = 50.0
+ payload: float = 0.0
+ safe: bool = True
+
+
+@dataclass
+class DetachObject(JsonSchemaMixin):
+ pose: Pose
+ velocity: float = 50.0
+ payload: float = 0.0
+ safe: bool = True
+
+
class VacuumChannel(StrEnum):
A = "a"
B = "b"
@@ -73,15 +105,24 @@ def grippers(self) -> set[str]:
return set()
def suctions(self) -> set[str]:
- return set("default")
+ return {"default"}
def get_end_effector_pose(self, end_effector_id: str) -> Pose:
return rest.call(rest.Method.GET, f"{self.settings.url}/eef/pose", return_type=Pose)
def move_to_pose(
- self, end_effector_id: str, target_pose: Pose, speed: float, safe: bool = True, linear: bool = True
+ self,
+ end_effector_id: str,
+ target_pose: Pose,
+ speed: float,
+ safe: bool = True,
+ linear: bool = True,
) -> None:
- self.move(target_pose, speed * 100, safe)
+ self.move(
+ target_pose,
+ speed * 100,
+ safe=safe,
+ )
def move(
self,
@@ -110,9 +151,155 @@ def move(
rest.Method.PUT,
f"{self.settings.url}/eef/pose",
body=pose,
- params={"velocity": speed, "payload": payload, "safe": safe},
+ timeout=rest.Timeout(read=120),
+ params={
+ "velocity": speed,
+ "payload": payload,
+ "safe": safe,
+ },
+ )
+
+ def pick_up_object_by_position(
+ self,
+ position: Position,
+ radius: float,
+ effector_type: EffectorType = EffectorType.SUCK,
+ grasp_position: GraspPosition = GraspPosition.ALL,
+ object_type_name: object_type.Model3dType | None = None,
+ speed: float = 50.0,
+ safe: bool = True,
+ payload: float = 0.0,
+ *,
+ an: None | str = None,
+ ) -> None:
+ """Picks up a graspable object from the selected area.
+
+ :param position: Center of the area where the object should be found.
+ :param radius: Search radius.
+ :param effector_type: Type of end effector used for grasping.
+ :param grasp_position: Preferred grasp position.
+ :param object_type_name: Optional object model type filter.
+ :param speed: Relative speed.
+ :param safe: Avoid collisions.
+ :param payload: Object weight.
+ :return:
+ """
+
+ assert radius >= 0.0
+ assert 0.0 <= speed <= 100.0
+ assert 0.0 <= payload <= 5.0
+
+ with self._move_lock:
+ rest.call(
+ rest.Method.PUT,
+ f"{self.settings.url}/graspable/pick_up_object_by_position",
+ body=PickUpObjectByPosition(
+ position=position,
+ radius=radius,
+ effector_type=effector_type,
+ grasp_position=grasp_position,
+ object_type_name=object_type_name,
+ velocity=speed,
+ payload=payload,
+ safe=safe,
+ ),
+ timeout=rest.Timeout(read=120),
+ )
+
+ def pick_up_object_by_id(
+ self,
+ object_id: str,
+ effector_type: EffectorType = EffectorType.SUCK,
+ grasp_position: GraspPosition = GraspPosition.ALL,
+ speed: float = 50.0,
+ safe: bool = True,
+ payload: float = 0.0,
+ *,
+ an: None | str = None,
+ ) -> None:
+ """Picks up a graspable object by its ID.
+
+ :param object_id: Collision/graspable object ID.
+ :param effector_type: Type of end effector used for grasping.
+ :param grasp_position: Preferred grasp position.
+ :param speed: Relative speed.
+ :param safe: Avoid collisions.
+ :param payload: Object weight.
+ :return:
+ """
+
+ assert 0.0 <= speed <= 100.0
+ assert 0.0 <= payload <= 5.0
+
+ with self._move_lock:
+ rest.call(
+ rest.Method.PUT,
+ f"{self.settings.url}/graspable/pick_up_object_by_id",
+ body=PickUpObjectById(
+ object_id=object_id,
+ effector_type=effector_type,
+ grasp_position=grasp_position,
+ velocity=speed,
+ payload=payload,
+ safe=safe,
+ ),
+ timeout=rest.Timeout(read=120),
)
+ def place_object(
+ self,
+ pose: Pose,
+ speed: float = 50.0,
+ safe: bool = True,
+ payload: float = 0.0,
+ *,
+ an: None | str = None,
+ ) -> None:
+ """Places the currently attached object to the target pose.
+
+ :param pose: Target pose of the placed object.
+ :param speed: Relative speed.
+ :param safe: Avoid collisions.
+ :param payload: Object weight.
+ :return:
+ """
+
+ assert 0.0 <= speed <= 100.0
+ assert 0.0 <= payload <= 5.0
+
+ with self._move_lock:
+ rest.call(
+ rest.Method.PUT,
+ f"{self.settings.url}/graspable/place_object",
+ body=DetachObject(
+ pose=pose,
+ velocity=speed,
+ payload=payload,
+ safe=safe,
+ ),
+ timeout=rest.Timeout(read=120),
+ )
+
+ def graspable_state(self, object_id: str) -> GraspableState:
+ """Returns graspable object state from the UR service."""
+
+ return GraspableState(
+ rest.call(
+ rest.Method.GET,
+ f"{self.settings.url}/graspable/{object_id}/state",
+ return_type=str,
+ )
+ )
+
+ def graspable_position(self, object_id: str) -> Position:
+ """Returns graspable object position from the UR service."""
+
+ return rest.call(
+ rest.Method.GET,
+ f"{self.settings.url}/graspable/{object_id}/position",
+ return_type=Position,
+ )
+
def suck(
self,
vacuum: int = 60,
@@ -190,3 +377,6 @@ def inverse_kinematics(
)
move.__action__ = ActionMetadata() # type: ignore
+ pick_up_object_by_position.__action__ = ActionMetadata() # type: ignore
+ pick_up_object_by_id.__action__ = ActionMetadata() # type: ignore
+ place_object.__action__ = ActionMetadata() # type: ignore
diff --git a/src/python/arcor2_ur/scripts/BUILD b/src/python/arcor2_ur/scripts/BUILD
index f1f8f16f..28b15d68 100644
--- a/src/python/arcor2_ur/scripts/BUILD
+++ b/src/python/arcor2_ur/scripts/BUILD
@@ -1,14 +1,37 @@
-python_sources()
+python_sources(
+ sources=[
+ "*.py",
+ ],
+ dependencies=[
+ "3rdparty#PyYAML",
+ "3rdparty#numpy",
+ "3rdparty#open3d",
+ "src/python/arcor2_ur/data:moveit_yaml",
+ "src/python/arcor2_ur/data:urdf_assets",
+ ],
+)
arcor2_pex_binary(
name="ur",
- dependencies=["3rdparty#lark", "src/python/arcor2_ur/data:moveit.yaml"],
+ dependencies=[
+ "3rdparty#lark",
+ "src/python/arcor2_ur/data:moveit_yaml",
+ "src/python/arcor2_ur/data:urdf_assets",
+ ],
)
+
arcor2_pex_binary(
- name="upload_objects", dependencies=["src/python/arcor2_ur/data/urdf:ur5e"]
+ name="upload_objects",
+ dependencies=[
+ "src/python/arcor2_ur/data:urdf_assets",
+ ],
)
arcor2_pex_binary(
name="robot_publisher",
- dependencies=["3rdparty#PyYAML", "3rdparty#numpy"],
-)
+ dependencies=[
+ "3rdparty#PyYAML",
+ "3rdparty#numpy",
+ "src/python/arcor2_ur/data:urdf_assets",
+ ],
+)
\ No newline at end of file
diff --git a/src/python/arcor2_ur/scripts/grasp_planner.py b/src/python/arcor2_ur/scripts/grasp_planner.py
new file mode 100644
index 00000000..0e7ebde1
--- /dev/null
+++ b/src/python/arcor2_ur/scripts/grasp_planner.py
@@ -0,0 +1,247 @@
+import math
+
+import numpy as np
+import open3d as o3d
+from shape_msgs.msg import Mesh as RosMesh # pants: no-infer-dep
+
+from arcor2.data import object_type
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2_ur.common import CollisionSceneObject, EffectorType, GraspPosition
+
+PRE_GRASP_OFFSET = 0.12
+GRASP_OFFSET = 0.06
+
+
+def generate_grasp_poses(
+ obj: CollisionSceneObject,
+ effector_type: EffectorType,
+ grasp_position: GraspPosition = GraspPosition.ALL,
+ ros_mesh: RosMesh | None = None,
+) -> list[tuple[Pose, Pose]]:
+
+ mesh = object_to_mesh(obj, ros_mesh)
+
+ geometry = filter_geometry_by_grasp_position(mesh, grasp_position)
+
+ candidates = create_grasp_candidates_from_surfaces(geometry, effector_type)
+
+ candidates.sort(key=candidate_sort_key)
+
+ return candidates[:20]
+
+
+def object_to_mesh(obj: CollisionSceneObject, ros_mesh: RosMesh | None) -> o3d.geometry.TriangleMesh:
+ """Box/Cylinder/Sphere/Mesh -> Open3D world mesh."""
+
+ model = obj.model
+
+ if isinstance(model, object_type.Box):
+ mesh = o3d.geometry.TriangleMesh.create_box(width=model.size_x, height=model.size_y, depth=model.size_z)
+ mesh.translate((-model.size_x / 2, -model.size_y / 2, -model.size_z / 2))
+
+ elif isinstance(model, object_type.Cylinder):
+ mesh = o3d.geometry.TriangleMesh.create_cylinder(radius=model.radius, height=model.height)
+
+ elif isinstance(model, object_type.Sphere):
+ mesh = o3d.geometry.TriangleMesh.create_sphere(radius=model.radius)
+
+ elif isinstance(model, object_type.Mesh):
+ if ros_mesh is None:
+ raise ValueError("Object is Mesh, but ros_mesh is None.")
+ mesh = o3d.geometry.TriangleMesh()
+ mesh.vertices = o3d.utility.Vector3dVector([(vertex.x, vertex.y, vertex.z) for vertex in ros_mesh.vertices])
+ mesh.triangles = o3d.utility.Vector3iVector([tuple(triangle.vertex_indices) for triangle in ros_mesh.triangles])
+
+ else:
+ raise ValueError(f"Unsupported object model: {type(model).__name__}")
+
+ apply_pose_to_mesh(mesh, obj.pose)
+ mesh.compute_triangle_normals()
+ mesh.compute_vertex_normals()
+ return mesh
+
+
+def apply_pose_to_mesh(mesh: o3d.geometry.TriangleMesh, pose: Pose) -> None:
+ q = pose.orientation
+ rotation = quaternion_to_rotation_matrix(q)
+
+ mesh.rotate(rotation, center=(0.0, 0.0, 0.0))
+ mesh.translate((pose.position.x, pose.position.y, pose.position.z))
+
+
+def quaternion_to_rotation_matrix(q: Orientation) -> np.ndarray:
+ x, y, z, w = q.x, q.y, q.z, q.w
+
+ norm = math.sqrt(x * x + y * y + z * z + w * w)
+ if norm == 0.0:
+ raise ValueError("Invalid quaternion.")
+
+ x, y, z, w = x / norm, y / norm, z / norm, w / norm
+
+ return np.array(
+ [
+ [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
+ [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
+ [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
+ ]
+ )
+
+
+def filter_geometry_by_grasp_position(
+ mesh: o3d.geometry.TriangleMesh,
+ grasp_position: GraspPosition = GraspPosition.ALL,
+) -> o3d.geometry.TriangleMesh:
+ """TOP/BOTTOM/LEFT/RIGHT/FRONT/BACK filter."""
+
+ if grasp_position == GraspPosition.ALL:
+ return mesh
+
+ mesh.compute_triangle_normals()
+
+ triangles = np.asarray(mesh.triangles)
+ normals = np.asarray(mesh.triangle_normals)
+
+ wanted_dirs = {
+ GraspPosition.BOTTOM: (0.0, 0.0, -1.0),
+ GraspPosition.BACK: (0.0, 1.0, 0.0),
+ GraspPosition.FRONT: (0.0, -1.0, 0.0),
+ GraspPosition.LEFT: (-1.0, 0.0, 0.0),
+ GraspPosition.RIGHT: (1.0, 0.0, 0.0),
+ GraspPosition.TOP: (0.0, 0.0, 1.0),
+ }
+
+ kept = []
+ for triangle, normal in zip(triangles, normals):
+ direction = wanted_dirs[grasp_position]
+ if dot(normal, direction) >= 0.707: # maximum allowed rotation from the original normal: 45°
+ kept.append(triangle)
+
+ filtered = o3d.geometry.TriangleMesh()
+ filtered.vertices = mesh.vertices
+ filtered.triangles = o3d.utility.Vector3iVector(kept)
+ filtered.compute_triangle_normals()
+ filtered.compute_vertex_normals()
+
+ return filtered
+
+
+def dot(a, b) -> float:
+ return float(a[0] * b[0] + a[1] * b[1] + a[2] * b[2])
+
+
+def create_grasp_candidates_from_surfaces(
+ mesh: o3d.geometry.TriangleMesh, effector_type: EffectorType
+) -> list[tuple[Pose, Pose]]:
+ """Mesh triangles -> list[(pre_grasp_pose, grasp_pose)]."""
+
+ mesh.compute_triangle_normals()
+
+ vertices = np.asarray(mesh.vertices)
+ triangles = np.asarray(mesh.triangles)
+ normals = np.asarray(mesh.triangle_normals)
+
+ poses = []
+
+ for triangle, normal in zip(triangles, normals):
+ normal = normalize_vector((float(normal[0]), float(normal[1]), float(normal[2])))
+ center = vertices[triangle].mean(axis=0)
+ orientation = orientation_from_normal(normal)
+
+ grasp_pose = Pose(
+ Position(
+ float(center[0] + normal[0] * GRASP_OFFSET),
+ float(center[1] + normal[1] * GRASP_OFFSET),
+ float(center[2] + normal[2] * GRASP_OFFSET),
+ ),
+ orientation,
+ )
+ pre_grasp_pose = Pose(
+ Position(
+ float(center[0] + normal[0] * PRE_GRASP_OFFSET),
+ float(center[1] + normal[1] * PRE_GRASP_OFFSET),
+ float(center[2] + normal[2] * PRE_GRASP_OFFSET),
+ ),
+ orientation,
+ )
+
+ poses.append((pre_grasp_pose, grasp_pose))
+
+ return poses
+
+
+def orientation_from_normal(normal: tuple[float, float, float]) -> Orientation:
+ """Surface normal -> TCP orientation."""
+
+ nx, ny, nz = normalize_vector(normal)
+ z_axis = (-nx, -ny, -nz)
+
+ helper = (0.0, 0.0, 1.0)
+ if abs(dot(z_axis, helper)) > 0.95:
+ helper = (0.0, 1.0, 0.0)
+
+ x_axis = normalize_vector(cross(helper, z_axis))
+ y_axis = cross(z_axis, x_axis)
+
+ return quaternion_from_axes(x_axis, y_axis, z_axis)
+
+
+def quaternion_from_axes(
+ x_axis: tuple[float, float, float], y_axis: tuple[float, float, float], z_axis: tuple[float, float, float]
+) -> Orientation:
+ """TCP axes -> quaternion."""
+
+ m00, m01, m02 = x_axis[0], y_axis[0], z_axis[0]
+ m10, m11, m12 = x_axis[1], y_axis[1], z_axis[1]
+ m20, m21, m22 = x_axis[2], y_axis[2], z_axis[2]
+
+ trace = m00 + m11 + m22
+
+ if trace > 0.0:
+ s = math.sqrt(trace + 1.0) * 2.0
+ x, y, z, w = (m21 - m12) / s, (m02 - m20) / s, (m10 - m01) / s, 0.25 * s
+ elif m00 > m11 and m00 > m22:
+ s = math.sqrt(1.0 + m00 - m11 - m22) * 2.0
+ x, y, z, w = 0.25 * s, (m01 + m10) / s, (m02 + m20) / s, (m21 - m12) / s
+ elif m11 > m22:
+ s = math.sqrt(1.0 + m11 - m00 - m22) * 2.0
+ x, y, z, w = (m01 + m10) / s, 0.25 * s, (m12 + m21) / s, (m02 - m20) / s
+ else:
+ s = math.sqrt(1.0 + m22 - m00 - m11) * 2.0
+ x, y, z, w = (m02 + m20) / s, (m12 + m21) / s, 0.25 * s, (m10 - m01) / s
+
+ return normalize_orientation(Orientation(x, y, z, w))
+
+
+def normalize_vector(v: tuple[float, float, float]) -> tuple[float, float, float]:
+ norm = math.sqrt(dot(v, v))
+ if norm == 0.0:
+ raise ValueError("Invalid zero vector.")
+ return (v[0] / norm, v[1] / norm, v[2] / norm)
+
+
+def normalize_orientation(q: Orientation) -> Orientation:
+ norm = math.sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w)
+ if norm == 0.0:
+ raise ValueError("Invalid quaternion.")
+ return Orientation(q.x / norm, q.y / norm, q.z / norm, q.w / norm)
+
+
+def cross(a: tuple[float, float, float], b: tuple[float, float, float]) -> tuple[float, float, float]:
+ return (a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0])
+
+
+def candidate_sort_key(candidate: tuple[Pose, Pose]) -> tuple[float, float, float, float]:
+ pre_grasp_pose, grasp_pose = candidate
+
+ return (
+ -grasp_pose.position.z,
+ abs(grasp_pose.position.y),
+ abs(grasp_pose.position.x),
+ distance_from_origin(pre_grasp_pose),
+ )
+
+
+def distance_from_origin(pose: Pose) -> float:
+ return math.sqrt(
+ pose.position.x * pose.position.x + pose.position.y * pose.position.y + pose.position.z * pose.position.z
+ )
diff --git a/src/python/arcor2_ur/scripts/robot_publisher.py b/src/python/arcor2_ur/scripts/robot_publisher.py
index 564c61d5..cd88e220 100644
--- a/src/python/arcor2_ur/scripts/robot_publisher.py
+++ b/src/python/arcor2_ur/scripts/robot_publisher.py
@@ -1,34 +1,91 @@
+from importlib import resources
+
import rclpy # pants: no-infer-dep
+from geometry_msgs.msg import TransformStamped # pants: no-infer-dep
from rclpy.executors import ExternalShutdownException # pants: no-infer-dep
from rclpy.node import Node # pants: no-infer-dep
from rclpy.qos import QoSDurabilityPolicy, QoSProfile, QoSReliabilityPolicy # pants: no-infer-dep
from std_msgs.msg import Bool # pants: no-infer-dep
+from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster # pants: no-infer-dep
from ur_dashboard_msgs.msg import RobotMode # pants: no-infer-dep
from arcor2_ur.topics import ROBOT_MODE_TOPIC, ROBOT_PROGRAM_RUNNING_TOPIC
+def load_robot_description() -> str:
+ urdf_res = resources.files("arcor2_ur").joinpath("data/urdf/ur5e.urdf")
+ meshes_res = resources.files("arcor2_ur").joinpath("data/urdf/meshes")
+
+ text = urdf_res.read_text(encoding="utf-8")
+
+ with resources.as_file(meshes_res) as meshes_dir:
+ text = text.replace(
+ "package://meshes/",
+ f"file://{meshes_dir.as_posix()}/",
+ )
+
+ return text
+
+
+def load_robot_description_semantic() -> str:
+ srdf_res = resources.files("arcor2_ur").joinpath("data/urdf/ur5e.srdf")
+ return srdf_res.read_text(encoding="utf-8")
+
+
class PublisherNode(Node):
def __init__(self) -> None:
- super().__init__("minimal_publisher")
+ super().__init__("arcor2_ur_test_robot_publisher")
qos = QoSProfile(
- depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, reliability=QoSReliabilityPolicy.RELIABLE
+ depth=1,
+ durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
+ reliability=QoSReliabilityPolicy.RELIABLE,
)
self.robot_program_running_pub = self.create_publisher(Bool, ROBOT_PROGRAM_RUNNING_TOPIC, qos)
self.robot_mode_pub = self.create_publisher(RobotMode, ROBOT_MODE_TOPIC, qos)
+ self.static_tf_broadcaster = StaticTransformBroadcaster(self)
+
+ self._publish_static_transforms()
+ self.timer = self.create_timer(1.0, self._republish)
+
+ def _publish_static_transforms(self) -> None:
+ transforms = [
+ self._transform("tool0", "suction_base_link", 0.0, 0.0, 0.0),
+ self._transform("suction_base_link", "suction_cup_link", 0.0, 0.0, 0.1025),
+ self._transform("suction_cup_link", "suction_tcp", 0.0, 0.0, 0.0),
+ ]
+
+ self.static_tf_broadcaster.sendTransform(transforms)
+
+ def _transform(self, parent_frame: str, child_frame: str, x: float, y: float, z: float) -> TransformStamped:
+ transform = TransformStamped()
+ transform.header.stamp = self.get_clock().now().to_msg()
+ transform.header.frame_id = parent_frame
+ transform.child_frame_id = child_frame
+ transform.transform.translation.x = x
+ transform.transform.translation.y = y
+ transform.transform.translation.z = z
+ transform.transform.rotation.x = 0.0
+ transform.transform.rotation.y = 0.0
+ transform.transform.rotation.z = 0.0
+ transform.transform.rotation.w = 1.0
+ return transform
+
+ def _republish(self) -> None:
+ self.robot_program_running_pub.publish(Bool(data=True))
+ self.robot_mode_pub.publish(RobotMode(mode=RobotMode.RUNNING))
def main() -> None:
rclpy.init()
publisher_node = PublisherNode()
- publisher_node.robot_program_running_pub.publish(Bool(data=True))
- publisher_node.robot_mode_pub.publish(RobotMode(mode=RobotMode.RUNNING))
+
+ publisher_node._republish()
+
try:
rclpy.spin(publisher_node)
except ExternalShutdownException:
- # Happens when the process is terminated from the outside; exit cleanly.
pass
finally:
publisher_node.destroy_node()
diff --git a/src/python/arcor2_ur/scripts/ros_worker.py b/src/python/arcor2_ur/scripts/ros_worker.py
index 3e80e981..5f6e53d9 100644
--- a/src/python/arcor2_ur/scripts/ros_worker.py
+++ b/src/python/arcor2_ur/scripts/ros_worker.py
@@ -1,20 +1,29 @@
+import copy
import importlib
import multiprocessing
+import tempfile
import threading
import time
from multiprocessing.connection import Connection
-from typing import Any, Callable, NamedTuple, cast
+from pathlib import Path
+from typing import Any, Callable, cast
+import open3d as o3d
import rclpy # pants: no-infer-dep
+from geometry_msgs.msg import Point # pants: no-infer-dep
from geometry_msgs.msg import Pose as RosPose # pants: no-infer-dep
-from geometry_msgs.msg import PoseStamped # pants: no-infer-dep
from moveit.planning import MoveItPy, PlanningComponent # pants: no-infer-dep
+from moveit_msgs.msg import AttachedCollisionObject # pants: no-infer-dep
from moveit_msgs.msg import CollisionObject # pants: no-infer-dep
+from moveit_msgs.msg import Constraints # pants: no-infer-dep
+from moveit_msgs.msg import OrientationConstraint # pants: no-infer-dep
+from moveit_msgs.msg import PositionConstraint # pants: no-infer-dep
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup # pants: no-infer-dep
from rclpy.node import Node # pants: no-infer-dep
from rclpy.qos import QoSDurabilityPolicy, QoSProfile, QoSReliabilityPolicy # pants: no-infer-dep
from sensor_msgs.msg import JointState # pants: no-infer-dep
-from shape_msgs.msg import SolidPrimitive # pants: no-infer-dep
+from shape_msgs.msg import Mesh as RosMesh # pants: no-infer-dep
+from shape_msgs.msg import MeshTriangle, SolidPrimitive # pants: no-infer-dep
from std_msgs.msg import Bool, String # pants: no-infer-dep
from std_srvs.srv import Trigger # pants: no-infer-dep
from tf2_ros.buffer import Buffer # pants: no-infer-dep
@@ -24,13 +33,16 @@
from ur_msgs.srv import SetPayload, SetSpeedSliderFraction # pants: no-infer-dep
from arcor2 import transformations as tr
-from arcor2.data import common, object_type
-from arcor2.data.common import Joint, Pose
+from arcor2.data import object_type
+from arcor2.data.common import Joint, Orientation, Pose, Position
from arcor2.data.robot import InverseKinematicsRequest
from arcor2.logging import get_logger
+from arcor2_storage import client as storage_client
from arcor2_ur import topics
+from arcor2_ur.common import CollisionSceneObject, EffectorType, GraspableState, GraspPosition
from arcor2_ur.exceptions import UrGeneral
from arcor2_ur.object_types.ur5e import Vacuum
+from arcor2_ur.scripts.grasp_planner import generate_grasp_poses
from arcor2_ur.vgc10 import VGC10
logger = get_logger(__name__)
@@ -40,11 +52,6 @@
WORKSPACE_MAX = (1.0, 1.0, 1.0)
-class CollisionObjectTuple(NamedTuple):
- model: object_type.Models
- pose: common.Pose
-
-
def pose_to_ros_pose(ps: Pose) -> RosPose:
rp = RosPose()
rp.position.x = ps.position.x
@@ -57,6 +64,93 @@ def pose_to_ros_pose(ps: Pose) -> RosPose:
return rp
+def mesh_file_to_ros_mesh(
+ mesh_data: bytes,
+ asset_id: str,
+ scale: tuple[float, float, float] = (1.0, 1.0, 1.0),
+) -> RosMesh:
+ if not mesh_data:
+ raise UrGeneral(f"Mesh asset '{asset_id}' is empty.")
+
+ suffix = Path(asset_id).suffix.lower()
+ if not suffix:
+ raise UrGeneral(f"Mesh asset '{asset_id}' has no file extension.")
+
+ with tempfile.NamedTemporaryFile(suffix=suffix, delete=True) as tmp:
+ tmp.write(mesh_data)
+ tmp.flush()
+
+ loaded = o3d.io.read_triangle_mesh(tmp.name)
+
+ if loaded.is_empty():
+ raise UrGeneral(f"Mesh asset '{asset_id}' has no geometry.")
+
+ vertices = tuple(
+ (
+ float(vertex[0]) * scale[0],
+ float(vertex[1]) * scale[1],
+ float(vertex[2]) * scale[2],
+ )
+ for vertex in loaded.vertices
+ )
+ triangles = tuple((int(triangle[0]), int(triangle[1]), int(triangle[2])) for triangle in loaded.triangles)
+
+ if not vertices or not triangles:
+ raise UrGeneral(f"Mesh asset '{asset_id}' has no usable mesh data.")
+
+ ros_mesh = RosMesh()
+
+ for x, y, z in vertices:
+ point = Point()
+ point.x = x
+ point.y = y
+ point.z = z
+ ros_mesh.vertices.append(point)
+
+ for a, b, c in triangles:
+ triangle = MeshTriangle()
+ triangle.vertex_indices = [a, b, c]
+ ros_mesh.triangles.append(triangle)
+
+ return ros_mesh
+
+
+def make_pose_constraints(
+ pose: Pose,
+ frame_id: str,
+ link_name: str,
+ position_tolerance: float = 0.01,
+ orientation_tolerance: float = 0.01,
+) -> Constraints:
+ constraints = Constraints()
+
+ position_constraint = PositionConstraint()
+ position_constraint.header.frame_id = frame_id
+ position_constraint.link_name = link_name
+ position_constraint.weight = 1.0
+
+ sphere = SolidPrimitive()
+ sphere.type = SolidPrimitive.SPHERE
+ sphere.dimensions = [position_tolerance]
+
+ position_constraint.constraint_region.primitives.append(sphere)
+ position_constraint.constraint_region.primitive_poses.append(pose_to_ros_pose(pose))
+
+ orientation_constraint = OrientationConstraint()
+ orientation_constraint.header.frame_id = frame_id
+ orientation_constraint.link_name = link_name
+ orientation_constraint.orientation = pose_to_ros_pose(pose).orientation
+ orientation_constraint.absolute_x_axis_tolerance = orientation_tolerance
+ orientation_constraint.absolute_y_axis_tolerance = orientation_tolerance
+ orientation_constraint.absolute_z_axis_tolerance = orientation_tolerance
+ orientation_constraint.weight = 1.0
+
+ constraints.position_constraints.append(position_constraint)
+ constraints.orientation_constraints.append(orientation_constraint)
+
+ return constraints
+
+
def plan_and_execute(
robot,
planning_component,
@@ -95,6 +189,47 @@ def wait_for_future(future, *, timeout_sec: float = 2.0):
return future.result()
+def rotate_vector(q: Orientation, v: tuple[float, float, float]) -> tuple[float, float, float]:
+ x, y, z, w = q.x, q.y, q.z, q.w
+ vx, vy, vz = v
+
+ # quaternion * vector * quaternion_conjugate
+ tx = 2 * (y * vz - z * vy)
+ ty = 2 * (z * vx - x * vz)
+ tz = 2 * (x * vy - y * vx)
+
+ rx = vx + w * tx + (y * tz - z * ty)
+ ry = vy + w * ty + (z * tx - x * tz)
+ rz = vz + w * tz + (x * ty - y * tx)
+
+ return rx, ry, rz
+
+
+def normalize_orientation(q: Orientation) -> Orientation:
+ norm = (q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w) ** 0.5
+ if norm == 0.0:
+ raise UrGeneral("Invalid quaternion.")
+ return Orientation(q.x / norm, q.y / norm, q.z / norm, q.w / norm)
+
+
+def compute_offset_pose_in_tool_axis(pose: Pose, distance: float) -> Pose:
+ q = normalize_orientation(pose.orientation)
+ dx, dy, dz = rotate_vector(q, (0.0, 0.0, -distance))
+
+ return Pose(
+ Position(
+ pose.position.x + dx,
+ pose.position.y + dy,
+ pose.position.z + dz,
+ ),
+ q,
+ )
+
+
+def compute_target_eef_pose(target_object_pose: Pose, attached_pose: Pose) -> Pose:
+ return tr.make_pose_abs(target_object_pose, attached_pose.inversed())
+
+
class MyNode(Node):
def __init__(
self,
@@ -297,41 +432,13 @@ def joint_states_cb(self, msg: JointState) -> None:
self._joint_state_handler(list(msg.name), list(msg.position))
-def apply_collision_objects(
- scene, collision_objects: dict[str, CollisionObjectTuple], base_pose: Pose, base_link: str
-) -> None:
- scene.remove_all_collision_objects()
-
- for obj_id, obj in collision_objects.items():
- if not isinstance(obj.model, object_type.Box):
- continue
-
- collision_object = CollisionObject()
- collision_object.header.frame_id = base_link
- collision_object.id = obj_id
-
- box_pose = pose_to_ros_pose(tr.make_pose_rel(base_pose, obj.pose))
-
- box = SolidPrimitive()
- box.type = SolidPrimitive.BOX
- box.dimensions = (obj.model.size_x, obj.model.size_y, obj.model.size_z)
-
- collision_object.primitives.append(box)
- collision_object.primitive_poses.append(box_pose)
- collision_object.operation = CollisionObject.ADD
-
- scene.apply_collision_object(collision_object)
-
- scene.current_state.update()
-
-
class RosWorkerRuntime:
def __init__(
self,
base_pose: Pose,
base_link: str,
tool_link: str,
- collision_objects: dict[str, CollisionObjectTuple],
+ collision_objects: dict[str, CollisionSceneObject],
interact_with_dashboard: bool,
robot_ip: str,
vgc10_port: int,
@@ -343,6 +450,7 @@ def __init__(
self.base_link = base_link
self.tool_link = tool_link
self.collision_objects = collision_objects.copy()
+ self.mesh_models: dict[tuple[str, tuple[float, float, float]], RosMesh] = {}
self.joints: list[Joint] = []
self.tool: VGC10 | None = None
self.freedrive_mode = False
@@ -404,7 +512,7 @@ def __init__(
moveitpy, _ = self._get_moveit()
with moveitpy.get_planning_scene_monitor().read_write() as scene:
- apply_collision_objects(scene, self.collision_objects, self.base_pose, self.base_link)
+ self.apply_collision_objects(scene)
def _update_joints(self, names: list[str], positions: list[float]) -> None:
self.joints = [Joint(name, position) for name, position in zip(names, positions)]
@@ -461,7 +569,7 @@ def _active_controllers(self) -> set[str]:
return set()
list_controllers_cls: Any = self._list_controllers_client.srv_type
req = list_controllers_cls.Request()
- resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=5.0)
+ resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0)
if resp is None:
return set()
return {c.name for c in resp.controller if c.state == "active"}
@@ -494,22 +602,33 @@ def _switch_freedrive_controller(self, enable: bool) -> None:
req.deactivate_controllers = to_deactivate
req.strictness = 2 # STRICT
req.activate_asap = False
- req.timeout.sec = 5
+ req.timeout.sec = 15
req.timeout.nanosec = 0
- resp = wait_for_future(self._switch_controller_client.call_async(req), timeout_sec=10.0)
+ resp = wait_for_future(self._switch_controller_client.call_async(req), timeout_sec=20.0)
if resp is None or not resp.ok:
msg = getattr(resp, "message", "")
raise UrGeneral(f"Controller switch failed. {msg}")
def _current_pose(self) -> Pose:
ros_time = rclpy.time.Time()
+ deadline = time.monotonic() + 30.0
+ last_error: Exception | None = None
- if not self.node.buffer.can_transform(
- self.base_link, self.tool_link, ros_time, timeout=rclpy.time.Duration(seconds=5.0)
- ):
- raise UrGeneral("Can't get transform")
- transform = self.node.buffer.lookup_transform(self.base_link, self.tool_link, ros_time)
+ while time.monotonic() < deadline:
+ try:
+ transform = self.node.buffer.lookup_transform(
+ self.base_link,
+ self.tool_link,
+ ros_time,
+ timeout=rclpy.time.Duration(seconds=1.0),
+ )
+ break
+ except Exception as exc:
+ last_error = exc
+ time.sleep(0.05)
+ else:
+ raise UrGeneral(f"Can't get transform {self.base_link}->{self.tool_link}: {last_error}")
t = transform.transform
@@ -575,7 +694,13 @@ def _wait_for_pose_reached(self, target: Pose, timeout: float = 15.0, tolerance:
time.sleep(0.02)
raise UrGeneral("Target pose was not reached.")
- def move_to_pose(self, pose: Pose, velocity: float, payload: float, safe: bool) -> dict:
+ def move_to_pose(
+ self,
+ pose: Pose,
+ velocity: float,
+ payload: float,
+ safe: bool,
+ ) -> dict:
if self.freedrive_mode:
raise UrGeneral("Freedrive mode is active. Disable hand teaching before commanding motion.")
@@ -584,21 +709,10 @@ def move_to_pose(self, pose: Pose, velocity: float, payload: float, safe: bool)
previous_joints = list(self.joints)
pose = tr.make_pose_rel(self.base_pose, pose)
- pose_goal = PoseStamped()
- pose_goal.header.frame_id = self.base_link
-
- pose_goal.pose.orientation.x = pose.orientation.x
- pose_goal.pose.orientation.y = pose.orientation.y
- pose_goal.pose.orientation.z = pose.orientation.z
- pose_goal.pose.orientation.w = pose.orientation.w
- pose_goal.pose.position.x = pose.position.x
- pose_goal.pose.position.y = pose.position.y
- pose_goal.pose.position.z = pose.position.z
-
moveitpy, ur_manipulator = self._get_moveit()
with moveitpy.get_planning_scene_monitor().read_write() as scene:
if safe:
- apply_collision_objects(scene, self.collision_objects, self.base_pose, self.base_link)
+ self.apply_collision_objects(scene)
else:
scene.remove_all_collision_objects()
@@ -607,7 +721,14 @@ def move_to_pose(self, pose: Pose, velocity: float, payload: float, safe: bool)
scene.current_state.update()
ur_manipulator.set_start_state_to_current_state()
- ur_manipulator.set_goal_state(pose_stamped_msg=pose_goal, pose_link=self.tool_link)
+ goal_constraints = make_pose_constraints(
+ pose,
+ self.base_link,
+ self.tool_link,
+ position_tolerance=0.01,
+ orientation_tolerance=0.01,
+ )
+ ur_manipulator.set_goal_state(motion_plan_constraints=[goal_constraints])
ur_manipulator.set_workspace(*WORKSPACE_MIN, *WORKSPACE_MAX) # not documented anywhere :-D
self.node.set_speed_slider(velocity)
@@ -617,10 +738,109 @@ def move_to_pose(self, pose: Pose, velocity: float, payload: float, safe: bool)
# TODO why is this needed (for tests to pass)?
self._wait_for_joint_update(previous_joints)
- self._wait_for_transform_update(previous_transform_time)
- self._wait_for_pose_reached(target_abs)
+ try:
+ self._wait_for_transform_update(previous_transform_time, timeout=15.0)
+ except UrGeneral:
+ logger.warning("Transform did not update after motion, checking pose anyway.")
+ self._wait_for_pose_reached(target_abs, tolerance=0.01)
return {}
+ def apply_collision_objects(self, scene) -> None:
+ scene.remove_all_collision_objects()
+
+ scene.current_state.update(force=True)
+ attached_objects = scene.planning_scene_message.robot_state.attached_collision_objects
+ attached_id = attached_objects[0].object.id if attached_objects else None
+
+ for obj_id, obj in self.collision_objects.items():
+ state = obj.metadata.get("state")
+
+ if state == GraspableState.HIDDEN:
+ continue
+
+ # object is already attached
+ elif state == GraspableState.ATTACHED and attached_id is not None:
+ continue
+
+ elif state == GraspableState.ATTACHED:
+ attached_pose_dict = obj.metadata.get("attached_pose")
+ if attached_pose_dict is None:
+ raise UrGeneral(f"Attached object {obj_id} has no attached_pose metadata.")
+
+ pose_in_frame = Pose.from_dict(cast(dict[str, Any], attached_pose_dict))
+
+ attached = self.create_collision_object(obj, pose_in_frame, attach=True)
+ scene.process_attached_collision_object(attached)
+ logger.info(f"Attached object id: {obj_id}")
+ continue
+
+ else: # normal collision object
+ pose_in_frame = tr.make_pose_rel(self.base_pose, obj.pose)
+ collision = self.create_collision_object(obj, pose_in_frame)
+ scene.apply_collision_object(collision)
+
+ scene.current_state.update(force=True)
+ scene.current_state.update()
+
+ def create_collision_object(
+ self, obj: CollisionSceneObject, pose_in_frame: Pose, attach: bool = False
+ ) -> CollisionObject | AttachedCollisionObject:
+
+ collision_object = CollisionObject()
+ if attach:
+ collision_object.header.frame_id = self.tool_link
+ else:
+ collision_object.header.frame_id = self.base_link
+ collision_object.id = obj.model.id
+ collision_object.operation = CollisionObject.ADD
+
+ if isinstance(obj.model, object_type.Box):
+ prim = SolidPrimitive()
+ prim.type = SolidPrimitive.BOX
+ prim.dimensions = [obj.model.size_x, obj.model.size_y, obj.model.size_z]
+
+ collision_object.primitives.append(prim)
+ collision_object.primitive_poses.append(pose_to_ros_pose(pose_in_frame))
+
+ elif isinstance(obj.model, object_type.Sphere):
+ prim = SolidPrimitive()
+ prim.type = SolidPrimitive.SPHERE
+ prim.dimensions = [obj.model.radius]
+
+ collision_object.primitives.append(prim)
+ collision_object.primitive_poses.append(pose_to_ros_pose(pose_in_frame))
+
+ elif isinstance(obj.model, object_type.Cylinder):
+ prim = SolidPrimitive()
+ prim.type = SolidPrimitive.CYLINDER
+ prim.dimensions = [obj.model.height, obj.model.radius]
+
+ collision_object.primitives.append(prim)
+ collision_object.primitive_poses.append(pose_to_ros_pose(pose_in_frame))
+
+ elif isinstance(obj.model, object_type.Mesh):
+ asset_id = obj.model.asset_id
+ scale = obj.metadata.get("mesh_scale", (1.0, 1.0, 1.0))
+ cache_key = (asset_id, scale)
+ cached_mesh = self.mesh_models.get(cache_key)
+
+ if cached_mesh is None:
+ mesh_data = storage_client.get_asset_data(asset_id)
+ cached_mesh = mesh_file_to_ros_mesh(mesh_data, asset_id, scale)
+ self.mesh_models[cache_key] = cached_mesh
+
+ collision_object.meshes.append(copy.deepcopy(cached_mesh))
+ collision_object.mesh_poses.append(pose_to_ros_pose(pose_in_frame))
+
+ if attach:
+ attached = AttachedCollisionObject()
+ attached.link_name = self.tool_link
+ attached.object = collision_object
+ attached.touch_links = [self.tool_link, "suction_base_link", "suction_cup_link"] # robot parts
+ return attached
+
+ return collision_object
+
def get_joints(self) -> list[dict]:
return [joint.to_dict() for joint in self.joints]
@@ -635,7 +855,7 @@ def inverse_kinematics(self, ikr: InverseKinematicsRequest) -> list[dict]:
moveitpy, _ = self._get_moveit()
with moveitpy.get_planning_scene_monitor().read_write() as scene:
if ikr.avoid_collisions:
- apply_collision_objects(scene, self.collision_objects, self.base_pose, self.base_link)
+ self.apply_collision_objects(scene)
else:
scene.remove_all_collision_objects()
@@ -658,7 +878,7 @@ def inverse_kinematics(self, ikr: InverseKinematicsRequest) -> list[dict]:
raise UrGeneral("State is in collision.")
except UrGeneral:
if not ikr.avoid_collisions:
- apply_collision_objects(scene, self.collision_objects, self.base_pose, self.base_link)
+ self.apply_collision_objects(scene)
raise
if len(scene.current_state.joint_positions) != len(self.joints):
@@ -675,11 +895,11 @@ def set_freedrive_mode(self, enabled: bool) -> dict:
def get_freedrive_mode(self) -> bool:
return self.freedrive_mode
- def update_collisions(self, collision_objects: dict[str, CollisionObjectTuple]) -> dict:
+ def update_collisions(self, collision_objects: dict[str, CollisionSceneObject]) -> dict:
self.collision_objects = collision_objects.copy()
moveitpy, _ = self._get_moveit()
with moveitpy.get_planning_scene_monitor().read_write() as scene:
- apply_collision_objects(scene, self.collision_objects, self.base_pose, self.base_link)
+ self.apply_collision_objects(scene)
return {}
def suck(self, vacuum: int) -> dict:
@@ -707,13 +927,172 @@ def _get_moveit(self) -> tuple[MoveItPy, PlanningComponent]:
raise UrGeneral("MoveIt is not initialized.")
return self.moveitpy, self.ur_manipulator
+ def attach_object(
+ self,
+ object_id: str,
+ effector_type: EffectorType,
+ grasp_position: GraspPosition,
+ velocity: float,
+ payload: float,
+ safe: bool,
+ ) -> dict:
+ object = self.collision_objects[object_id]
+ object.metadata.pop("attached_pose", None)
+
+ ros_mesh = None
+ if isinstance(object.model, object_type.Mesh):
+ asset_id = object.model.asset_id
+ scale = object.metadata.get("mesh_scale", (1.0, 1.0, 1.0))
+ cache_key = (asset_id, scale)
+ ros_mesh = self.mesh_models.get(cache_key)
+
+ if ros_mesh is None:
+ mesh_data = storage_client.get_asset_data(asset_id)
+ ros_mesh = mesh_file_to_ros_mesh(mesh_data, asset_id, scale)
+ self.mesh_models[cache_key] = ros_mesh
+
+ grasp_options = generate_grasp_poses(object, effector_type, grasp_position, ros_mesh)
+ logger.info(f"Generated {len(grasp_options)} grasp options for object {object_id}.")
+
+ trail = 0
+ for pre_grasp_pose, grasp_pose in grasp_options:
+ trail += 1
+ try:
+ self.inverse_kinematics(
+ InverseKinematicsRequest(pose=pre_grasp_pose, start_joints=self.joints, avoid_collisions=False)
+ )
+ self.inverse_kinematics(
+ InverseKinematicsRequest(pose=grasp_pose, start_joints=self.joints, avoid_collisions=False)
+ )
+ logger.info(f"IK accepted: {trail} grasp options.")
+ except Exception as exc:
+ logger.exception(
+ f"IK rejected: {trail}/{len(grasp_options)} grasp candidate. "
+ f"pre_grasp_pose={pre_grasp_pose.to_dict()}, "
+ f"grasp_pose={grasp_pose.to_dict()}: {exc}"
+ )
+ continue
+
+ try:
+ self.move_to_pose(pre_grasp_pose, velocity, payload, safe)
+
+ if self.tool and effector_type == EffectorType.SUCK:
+ self.suck(60)
+ time.sleep(1.0)
+
+ object.metadata["state"] = GraspableState.HIDDEN
+ self.update_collisions(self.collision_objects)
+
+ self.move_to_pose(grasp_pose, velocity, payload, safe)
+
+ if self.tool and effector_type == EffectorType.SUCK:
+ vac = Vacuum.from_dict(self.vacuum())
+ if vac.avg() < 20:
+ self.release()
+ raise UrGeneral(f"Failed to attach object {object_id}: vacuum not reached.")
+
+ object.metadata["state"] = GraspableState.ATTACHED
+ attached_pose = tr.make_pose_rel(grasp_pose, object.pose)
+ object.metadata["attached_pose"] = attached_pose.to_dict()
+
+ self.update_collisions(self.collision_objects)
+
+ except Exception as exc:
+ logger.exception(
+ f"Failed grasp candidate {trail}/{len(grasp_options)} "
+ f"for object {object_id}. "
+ f"pre_grasp_pose={pre_grasp_pose.to_dict()}, "
+ f"grasp_pose={grasp_pose.to_dict()}: {exc}"
+ )
+
+ if self.tool:
+ self.release()
+
+ object.metadata.pop("attached_pose", None)
+ object.metadata["state"] = GraspableState.WORLD
+ self.update_collisions(self.collision_objects)
+
+ continue
+
+ return object.metadata.copy()
+
+ raise UrGeneral(f"Unable to attach object {object_id}.")
+
+ def detach_object(
+ self,
+ target_pose: Pose,
+ velocity: float,
+ payload: float,
+ safe: bool,
+ ) -> dict[str, Any]:
+ object_id = self.get_attached_object_id()
+ if object_id is None:
+ raise UrGeneral("No object to detach.")
+
+ logger.info(f"Detaching object id: {object_id}")
+ object = self.collision_objects[object_id]
+
+ attached_pose_dict = object.metadata.get("attached_pose")
+ if attached_pose_dict is None:
+ raise UrGeneral(f"Object {object_id} is not attached.")
+
+ attached_pose = Pose.from_dict(attached_pose_dict)
+ target_eef_pose = compute_target_eef_pose(target_pose, attached_pose)
+ post_detach_pose = compute_offset_pose_in_tool_axis(target_eef_pose, 0.20)
+
+ self.move_to_pose(target_eef_pose, velocity, payload, safe)
+
+ if self.tool:
+ self.release()
+ time.sleep(1.0)
+
+ self.move_to_pose(post_detach_pose, velocity, payload, safe)
+
+ self.remove_attached_object(object_id)
+ object.metadata.pop("attached_pose", None)
+ object.pose = target_pose
+ object.metadata["state"] = GraspableState.WORLD
+ self.update_collisions(self.collision_objects)
+
+ logger.info(f"Detached object id: {object_id}")
+
+ return {"object_id": object_id, "state": object.metadata["state"]}
+
+ def get_attached_object_id(self) -> str | None:
+
+ moveitpy, _ = self._get_moveit()
+
+ planning_scene_msg = None
+ with moveitpy.get_planning_scene_monitor().read_write() as scene:
+ scene.current_state.update(force=True)
+ planning_scene_msg = scene.planning_scene_message
+
+ attached_objects = planning_scene_msg.robot_state.attached_collision_objects
+
+ if not attached_objects:
+ return None
+
+ return attached_objects[0].object.id
+
+ def remove_attached_object(self, object_id: str) -> None:
+ moveitpy, _ = self._get_moveit()
+
+ attached = AttachedCollisionObject()
+ attached.link_name = self.tool_link
+ attached.object.id = object_id
+ attached.object.operation = CollisionObject.REMOVE
+
+ with moveitpy.get_planning_scene_monitor().read_write() as scene:
+ scene.process_attached_collision_object(attached)
+ scene.current_state.update(force=True)
+
def ros_worker_main(
conn: Connection,
base_pose: Pose,
base_link: str,
tool_link: str,
- collision_objects: dict[str, CollisionObjectTuple],
+ collision_objects: dict[str, CollisionSceneObject],
interact_with_dashboard: bool,
robot_ip: str,
vgc10_port: int,
@@ -771,7 +1150,29 @@ def ros_worker_main(
result = runtime.get_eef_pose()
elif op == "move_to_pose":
pose = Pose.from_dict(kwargs["pose"])
- result = runtime.move_to_pose(pose, kwargs["velocity"], kwargs["payload"], kwargs["safe"])
+ result = runtime.move_to_pose(
+ pose,
+ kwargs["velocity"],
+ kwargs["payload"],
+ kwargs["safe"],
+ )
+ elif op == "pick_up_object":
+ result = runtime.attach_object(
+ kwargs["object_id"],
+ EffectorType(kwargs["effector_type"]),
+ GraspPosition(kwargs["grasp_position"]),
+ kwargs["velocity"],
+ kwargs["payload"],
+ kwargs["safe"],
+ )
+ elif op == "place_object":
+ pose = Pose.from_dict(kwargs["pose"])
+ result = runtime.detach_object(
+ pose,
+ kwargs["velocity"],
+ kwargs["payload"],
+ kwargs["safe"],
+ )
elif op == "get_joints":
result = runtime.get_joints()
elif op == "ik":
@@ -811,7 +1212,7 @@ class RosWorkerClient:
def __init__(
self,
pose: Pose,
- collision_objects: dict[str, CollisionObjectTuple],
+ collision_objects: dict[str, CollisionSceneObject],
base_link: str,
tool_link: str,
planning_group_name: str,
diff --git a/src/python/arcor2_ur/scripts/ur.py b/src/python/arcor2_ur/scripts/ur.py
index 24fe4913..4af9ab26 100644
--- a/src/python/arcor2_ur/scripts/ur.py
+++ b/src/python/arcor2_ur/scripts/ur.py
@@ -3,8 +3,12 @@
import argparse
import logging
import os
+import shutil
+import tempfile
from dataclasses import dataclass, field
from functools import wraps
+from importlib import resources
+from pathlib import Path
import humps
from ament_index_python.packages import get_package_share_directory # pants: no-infer-dep
@@ -17,17 +21,18 @@
from arcor2.data.robot import InverseKinematicsRequest
from arcor2.helpers import port_from_url
from arcor2.logging import get_logger
-from arcor2_ur import get_data, version
+from arcor2_ur import version
+from arcor2_ur.common import CollisionSceneObject, EffectorType, GraspableState, GraspPosition, parse_collision_body
from arcor2_ur.exceptions import NotFound, StartError, UrGeneral, WebApiError
from arcor2_ur.object_types.ur5e import Vacuum
-from arcor2_ur.scripts.ros_worker import CollisionObjectTuple, RosWorkerClient
+from arcor2_ur.scripts.ros_worker import RosWorkerClient
from arcor2_web.flask import RespT, create_app, run_app
logger = get_logger(__name__)
URL = os.getenv("ARCOR2_UR_URL", "http://localhost:5012")
BASE_LINK = os.getenv("ARCOR2_UR_BASE_LINK", "base_link")
-TOOL_LINK = os.getenv("ARCOR2_UR_TOOL_LINK", "tool0")
+TOOL_LINK = os.getenv("ARCOR2_UR_TOOL_LINK", "suction_tcp")
UR_TYPE = os.getenv("ARCOR2_UR_TYPE", "ur5e")
PLANNING_GROUP_NAME = os.getenv("ARCOR2_UR_PLANNING_GROUP_NAME", "ur_manipulator")
ROBOT_IP = os.getenv("ARCOR2_UR_ROBOT_IP", "")
@@ -47,32 +52,59 @@ class ServiceState:
class Globs:
debug = False
state: ServiceState | None = None
- collision_objects: dict[str, CollisionObjectTuple] = field(default_factory=dict)
+ collision_objects: dict[str, CollisionSceneObject] = field(default_factory=dict)
scene_started = False # flag for "Scene service"
globs: Globs = Globs()
app = create_app(__name__)
+
# this is normally specified in a launch file
-moveit_config = (
- MoveItConfigsBuilder(robot_name="ur", package_name="ur_moveit_config")
- .robot_description(
- os.path.join(get_package_share_directory("ur_description"), "urdf", "ur.urdf.xacro"),
- {"name": "ur", "ur_type": UR_TYPE},
- )
- .robot_description_semantic(
- os.path.join(get_package_share_directory("ur_moveit_config"), "srdf", "ur.srdf.xacro"), {"name": UR_TYPE}
- )
- .trajectory_execution(
- os.path.join(get_package_share_directory("ur_moveit_config"), "config", "moveit_controllers.yaml")
- )
- .robot_description_kinematics(
- os.path.join(get_package_share_directory("ur_moveit_config"), "config", "kinematics.yaml")
+def load_custom_urdf() -> str:
+ urdf_res = resources.files("arcor2_ur").joinpath("data/urdf/ur5e.urdf")
+ meshes_res = resources.files("arcor2_ur").joinpath("data/urdf/meshes")
+
+ text = urdf_res.read_text(encoding="utf-8")
+
+ meshes_cache_dir = Path(tempfile.gettempdir()) / f"arcor2_ur_meshes_{version()}"
+
+ if not meshes_cache_dir.exists():
+ with resources.as_file(meshes_res) as meshes_dir:
+ shutil.copytree(meshes_dir, meshes_cache_dir, dirs_exist_ok=True)
+
+ text = text.replace(
+ "package://meshes/",
+ f"file://{meshes_cache_dir.as_posix()}/",
)
- .moveit_cpp(file_path=get_data("moveit.yaml"))
- .to_moveit_configs()
-).to_dict()
+
+ return text
+
+
+def load_custom_srdf() -> str:
+ srdf_res = resources.files("arcor2_ur").joinpath("data/urdf/ur5e.srdf")
+ return srdf_res.read_text(encoding="utf-8")
+
+
+moveit_yaml_res = resources.files("arcor2_ur").joinpath("data/moveit.yaml")
+custom_srdf_res = resources.files("arcor2_ur").joinpath("data/urdf/ur5e.srdf")
+
+with resources.as_file(moveit_yaml_res) as moveit_yaml_path, resources.as_file(custom_srdf_res) as custom_srdf_path:
+ moveit_config = (
+ MoveItConfigsBuilder(robot_name="ur", package_name="ur_moveit_config")
+ .robot_description_semantic(str(custom_srdf_path))
+ .trajectory_execution(
+ os.path.join(get_package_share_directory("ur_moveit_config"), "config", "moveit_controllers.yaml")
+ )
+ .robot_description_kinematics(
+ os.path.join(get_package_share_directory("ur_moveit_config"), "config", "kinematics.yaml")
+ )
+ .moveit_cpp(file_path=str(moveit_yaml_path))
+ .to_moveit_configs()
+ ).to_dict()
+
+moveit_config["robot_description"] = load_custom_urdf()
+moveit_config["robot_description_semantic"] = load_custom_srdf()
def started() -> bool:
@@ -192,6 +224,349 @@ def get_started() -> RespT:
return jsonify(started())
+@app.route("/graspable//state", methods=["GET"])
+def get_graspable_state(object_id: str) -> RespT:
+ """Return graspable object state."""
+ try:
+ obj = globs.collision_objects[object_id]
+ except KeyError:
+ raise NotFound("Graspable object not found")
+
+ return jsonify(obj.metadata["state"])
+
+
+@app.route("/graspable//position", methods=["GET"])
+def get_graspable_position(object_id: str) -> RespT:
+ """Return graspable object position."""
+ try:
+ obj = globs.collision_objects[object_id]
+ except KeyError:
+ raise NotFound("Graspable object not found")
+
+ return jsonify(obj.pose.position.to_dict())
+
+
+@app.route("/graspable/pick_up_object_by_position", methods=["PUT"])
+@requires_started
+def pick_up_object_by_position() -> RespT:
+ """Find nearest graspable object and attach it.
+ ---
+ put:
+ tags:
+ - Graspable
+ summary: Find nearest graspable object in radius and attach it.
+ requestBody:
+ required: true
+ content:
+ application/json:
+ schema:
+ type: object
+ required:
+ - position
+ - radius
+ properties:
+ position:
+ $ref: '#/components/schemas/Position'
+ radius:
+ type: number
+ format: float
+ effector_type:
+ type: string
+ enum:
+ - SUCK
+ default: SUCK
+ grasp_position:
+ type: string
+ enum:
+ - TOP
+ - RIGHT
+ - LEFT
+ - FRONT
+ - BACK
+ - BOTTOM
+ - ALL
+ default: ALL
+ object_type_name:
+ type: string
+ description: Optional model type filter.
+ velocity:
+ type: number
+ format: float
+ default: 50.0
+ payload:
+ type: number
+ format: float
+ default: 0.0
+ safe:
+ type: boolean
+ default: true
+ responses:
+ 204:
+ description: Ok
+ 500:
+ description: "Error types: **General**, **UrGeneral**."
+ content:
+ application/json:
+ schema:
+ $ref: WebApiError
+ """
+ if not isinstance(request.json, dict):
+ raise UrGeneral("Body should be a JSON dict.")
+
+ body = humps.decamelize(request.json)
+
+ try:
+ position = common.Position.from_dict(body["position"])
+ radius = float(body["radius"])
+ except KeyError as exc:
+ raise UrGeneral(f"Missing key: {exc.args[0]}")
+
+ if radius < 0.0:
+ raise UrGeneral("Radius has to be >= 0.")
+
+ effector_type = EffectorType(body.get("effector_type", EffectorType.SUCK))
+ grasp_position = GraspPosition(body.get("grasp_position", GraspPosition.ALL))
+
+ object_type_name = (
+ object_type.MODEL_MAPPING[object_type.Model3dType(body["object_type_name"])]
+ if body.get("object_type_name")
+ else None
+ )
+ velocity = float(body.get("velocity", 50.0)) / 100.0
+ payload = float(body.get("payload", 0.0))
+ safe = bool(body.get("safe", True))
+
+ nearest_id: str | None = None
+ nearest_distance: float | None = None
+
+ for obj_id, obj in globs.collision_objects.items():
+ if obj.metadata.get("object_type") != "graspable":
+ continue
+
+ if obj.metadata.get("state") != GraspableState.WORLD:
+ continue
+
+ if object_type_name is not None and not isinstance(obj.model, object_type_name):
+ continue
+
+ current_distance = position.distance(obj.pose.position)
+ if current_distance > radius:
+ continue
+
+ if nearest_distance is None or current_distance < nearest_distance:
+ nearest_id = obj_id
+ nearest_distance = current_distance
+
+ if nearest_id is None:
+ raise UrGeneral("No graspable object found for given position, radius and type.")
+
+ selected = globs.collision_objects[nearest_id]
+ previous_state = selected.metadata.get("state")
+ selected.metadata["state"] = GraspableState.RESERVED
+
+ assert globs.state
+
+ try:
+ globs.state.worker.request("update_collisions", collision_objects=globs.collision_objects)
+ metadata = globs.state.worker.request(
+ "pick_up_object",
+ object_id=nearest_id,
+ effector_type=effector_type,
+ grasp_position=grasp_position,
+ velocity=velocity,
+ payload=payload,
+ safe=safe,
+ )
+
+ selected.metadata.clear()
+ selected.metadata.update(metadata)
+ except Exception:
+ selected.metadata["state"] = previous_state
+ globs.state.worker.request("update_collisions", collision_objects=globs.collision_objects)
+ raise
+
+ return Response(status=204)
+
+
+@app.route("/graspable/pick_up_object_by_id", methods=["PUT"])
+@requires_started
+def pick_up_object_by_id() -> RespT:
+ """Attach graspable object by ID.
+ ---
+ put:
+ tags:
+ - Graspable
+ summary: Attach graspable object by ID.
+ requestBody:
+ required: true
+ content:
+ application/json:
+ schema:
+ type: object
+ required:
+ - object_id
+ properties:
+ object_id:
+ type: string
+ effector_type:
+ type: string
+ enum:
+ - SUCK
+ default: SUCK
+ grasp_position:
+ type: string
+ enum:
+ - TOP
+ - RIGHT
+ - LEFT
+ - FRONT
+ - BACK
+ - BOTTOM
+ - ALL
+ default: ALL
+ velocity:
+ type: number
+ format: float
+ default: 50.0
+ payload:
+ type: number
+ format: float
+ default: 0.0
+ safe:
+ type: boolean
+ default: true
+ responses:
+ 204:
+ description: Ok
+ 500:
+ description: "Error types: **General**, **UrGeneral**."
+ content:
+ application/json:
+ schema:
+ $ref: WebApiError
+ """
+ body = humps.decamelize(request.json)
+
+ object_id = body["object_id"]
+ effector_type = EffectorType(body.get("effector_type", EffectorType.SUCK))
+ grasp_position = GraspPosition(body.get("grasp_position", GraspPosition.ALL))
+ velocity = float(body.get("velocity", 50.0)) / 100.0
+ payload = float(body.get("payload", 0.0))
+ safe = bool(body.get("safe", True))
+
+ try:
+ selected = globs.collision_objects[object_id]
+ except KeyError:
+ raise NotFound("Graspable object not found")
+
+ if selected.metadata.get("object_type") != "graspable":
+ raise UrGeneral(f"Object {object_id} is not graspable.")
+
+ if selected.metadata.get("state") != GraspableState.WORLD:
+ raise UrGeneral(f"Object {object_id} is not in WORLD state.")
+
+ previous_state = selected.metadata.get("state")
+ selected.metadata["state"] = GraspableState.RESERVED
+
+ assert globs.state
+
+ try:
+ globs.state.worker.request("update_collisions", collision_objects=globs.collision_objects)
+ metadata = globs.state.worker.request(
+ "pick_up_object",
+ object_id=object_id,
+ effector_type=effector_type,
+ grasp_position=grasp_position,
+ velocity=velocity,
+ payload=payload,
+ safe=safe,
+ )
+
+ selected.metadata.clear()
+ selected.metadata.update(metadata)
+ except Exception:
+ selected.metadata["state"] = previous_state
+ globs.state.worker.request("update_collisions", collision_objects=globs.collision_objects)
+ raise
+
+ return Response(status=204)
+
+
+@app.route("/graspable/place_object", methods=["PUT"])
+@requires_started
+def place_object() -> RespT:
+ """Moves graspable object using robot.
+ ---
+ put:
+ tags:
+ - Graspable
+ summary: Move graspable object to target pose.
+ requestBody:
+ required: true
+ content:
+ application/json:
+ schema:
+ type: object
+ required:
+ - effector_type
+ - pose
+ properties:
+ object_id:
+ type: string
+ description: Optional object ID.
+ effector_type:
+ type: string
+ enum:
+ - SUCK
+ default: SUCK
+ pose:
+ $ref: Pose
+ velocity:
+ type: number
+ format: float
+ default: 50.0
+ payload:
+ type: number
+ format: float
+ default: 0.0
+ safe:
+ type: boolean
+ default: true
+ responses:
+ 204:
+ description: Ok
+ 500:
+ description: "Error types: **General**."
+ content:
+ application/json:
+ schema:
+ $ref: WebApiError
+ """
+
+ body = humps.decamelize(request.json)
+
+ pose = Pose.from_dict(body["pose"])
+ velocity = float(body.get("velocity", 50.0)) / 100.0
+ payload = float(body.get("payload", 0.0))
+ safe = bool(body.get("safe", True))
+
+ assert globs.state
+ update = globs.state.worker.request(
+ "place_object",
+ pose=pose.to_dict(),
+ velocity=velocity,
+ payload=payload,
+ safe=safe,
+ )
+
+ obj = globs.collision_objects[update["object_id"]]
+ obj.metadata["state"] = update["state"]
+ obj.metadata.pop("attached_pose", None)
+ obj.pose = pose
+
+ return Response(status=204)
+
+
@app.route("/collisions/box", methods=["PUT"])
def put_box() -> RespT:
"""Add or update collision box.
@@ -199,7 +574,7 @@ def put_box() -> RespT:
put:
tags:
- Collisions
- description: Add or update collision box.
+ description: Add or update collision box. Use metadata.object_type=graspable for graspable objects.
parameters:
- name: boxId
in: query
@@ -223,10 +598,41 @@ def put_box() -> RespT:
type: number
format: float
requestBody:
- content:
+ required: true
+ content:
application/json:
- schema:
- $ref: Pose
+ schema:
+ type: object
+ required:
+ - pose
+ properties:
+ pose:
+ $ref: Pose
+ metadata:
+ type: object
+ description: Optional metadata for graspable object support.
+ properties:
+ object_type:
+ type: string
+ enum:
+ - graspable
+ state:
+ type: string
+ enum:
+ - WORLD
+ - RESERVED
+ - HIDDEN
+ - ATTACHED
+ source:
+ type: string
+ enum:
+ - CAMERA
+ - FIXED
+ - OTHER
+ stamp:
+ type: string
+ format: date-time
+ additionalProperties: true
responses:
204:
description: Ok
@@ -237,12 +643,12 @@ def put_box() -> RespT:
schema:
$ref: WebApiError
"""
- if not isinstance(request.json, dict):
- raise UrGeneral("Body should be a JSON dict containing Pose.")
+ pose, metadata = parse_collision_body()
args = request.args.to_dict()
box = object_type.Box(args["boxId"], float(args["sizeX"]), float(args["sizeY"]), float(args["sizeZ"]))
- globs.collision_objects[box.id] = CollisionObjectTuple(box, common.Pose.from_dict(humps.decamelize(request.json)))
+
+ globs.collision_objects[box.id] = CollisionSceneObject(box, pose, metadata)
if started():
assert globs.state
@@ -258,7 +664,7 @@ def put_sphere() -> RespT:
put:
tags:
- Collisions
- description: Add or update collision sphere.
+ description: Add or update collision sphere. Use metadata.object_type=graspable for graspable objects.
parameters:
- name: sphereId
in: query
@@ -272,10 +678,41 @@ def put_sphere() -> RespT:
type: number
format: float
requestBody:
- content:
+ required: true
+ content:
application/json:
- schema:
- $ref: Pose
+ schema:
+ type: object
+ required:
+ - pose
+ properties:
+ pose:
+ $ref: Pose
+ metadata:
+ type: object
+ description: Optional metadata for graspable object support.
+ properties:
+ object_type:
+ type: string
+ enum:
+ - graspable
+ state:
+ type: string
+ enum:
+ - WORLD
+ - RESERVED
+ - HIDDEN
+ - ATTACHED
+ source:
+ type: string
+ enum:
+ - CAMERA
+ - FIXED
+ - OTHER
+ stamp:
+ type: string
+ format: date-time
+ additionalProperties: true
responses:
204:
description: Ok
@@ -286,16 +723,12 @@ def put_sphere() -> RespT:
schema:
$ref: WebApiError
"""
- if not isinstance(request.json, dict):
- raise UrGeneral("Body should be a JSON dict containing Pose.")
+ pose, metadata = parse_collision_body()
args = humps.decamelize(request.args.to_dict())
sphere = object_type.Sphere(args["sphere_id"], float(args["radius"]))
- globs.collision_objects[sphere.id] = CollisionObjectTuple(
- sphere, common.Pose.from_dict(humps.decamelize(request.json))
- )
- logger.warning("Sphere collision object added but will be ignored as only boxes are supported at the moment.")
+ globs.collision_objects[sphere.id] = CollisionSceneObject(sphere, pose, metadata)
if started():
assert globs.state
@@ -311,7 +744,7 @@ def put_cylinder() -> RespT:
put:
tags:
- Collisions
- description: Add or update collision cylinder.
+ description: Add or update collision cylinder. Use metadata.object_type=graspable for graspable objects.
parameters:
- name: cylinderId
in: query
@@ -330,17 +763,44 @@ def put_cylinder() -> RespT:
type: number
format: float
requestBody:
- content:
+ required: true
+ content:
application/json:
- schema:
- $ref: Pose
+ schema:
+ type: object
+ required:
+ - pose
+ properties:
+ pose:
+ $ref: Pose
+ metadata:
+ type: object
+ description: Optional metadata for graspable object support.
+ properties:
+ object_type:
+ type: string
+ enum:
+ - graspable
+ state:
+ type: string
+ enum:
+ - WORLD
+ - RESERVED
+ - HIDDEN
+ - ATTACHED
+ source:
+ type: string
+ enum:
+ - CAMERA
+ - FIXED
+ - OTHER
+ stamp:
+ type: string
+ format: date-time
+ additionalProperties: true
responses:
- 200:
+ 204:
description: Ok
- content:
- application/json:
- schema:
- type: string
500:
description: "Error types: **General**, **SceneGeneral**."
content:
@@ -348,16 +808,12 @@ def put_cylinder() -> RespT:
schema:
$ref: WebApiError
"""
- if not isinstance(request.json, dict):
- raise UrGeneral("Body should be a JSON dict containing Pose.")
+ pose, metadata = parse_collision_body()
args = humps.decamelize(request.args.to_dict())
cylinder = object_type.Cylinder(args["cylinder_id"], float(args["radius"]), float(args["height"]))
- globs.collision_objects[cylinder.id] = CollisionObjectTuple(
- cylinder, common.Pose.from_dict(humps.decamelize(request.json))
- )
- logger.warning("Cylinder collision object added but will be ignored as only boxes are supported at the moment.")
+ globs.collision_objects[cylinder.id] = CollisionSceneObject(cylinder, pose, metadata)
if started():
assert globs.state
@@ -373,7 +829,7 @@ def put_mesh() -> RespT:
put:
tags:
- Collisions
- description: Add or update collision mesh.
+ description: Add or update collision mesh. Use metadata.object_type=graspable for graspable objects.
parameters:
- name: meshId
in: query
@@ -404,10 +860,41 @@ def put_mesh() -> RespT:
format: float
default: 1.0
requestBody:
- content:
+ required: true
+ content:
application/json:
- schema:
- $ref: Pose
+ schema:
+ type: object
+ required:
+ - pose
+ properties:
+ pose:
+ $ref: Pose
+ metadata:
+ type: object
+ description: Optional metadata for graspable object support.
+ properties:
+ object_type:
+ type: string
+ enum:
+ - graspable
+ state:
+ type: string
+ enum:
+ - WORLD
+ - RESERVED
+ - HIDDEN
+ - ATTACHED
+ source:
+ type: string
+ enum:
+ - CAMERA
+ - FIXED
+ - OTHER
+ stamp:
+ type: string
+ format: date-time
+ additionalProperties: true
responses:
204:
description: Ok
@@ -418,14 +905,22 @@ def put_mesh() -> RespT:
schema:
$ref: WebApiError
"""
- if not isinstance(request.json, dict):
- raise UrGeneral("Body should be a JSON dict containing Pose.")
+ pose, metadata = parse_collision_body()
args = humps.decamelize(request.args.to_dict())
+
+ mesh_scale = (
+ float(args.get("mesh_scale_x", 1.0)),
+ float(args.get("mesh_scale_y", 1.0)),
+ float(args.get("mesh_scale_z", 1.0)),
+ )
+
+ metadata = metadata.copy()
+ metadata["mesh_scale"] = mesh_scale
+
mesh = object_type.Mesh(args["mesh_id"], args["mesh_file_id"])
- globs.collision_objects[mesh.id] = CollisionObjectTuple(mesh, common.Pose.from_dict(humps.decamelize(request.json)))
- logger.warning("Mesh collision object added but will be ignored as only boxes are supported at the moment.")
+ globs.collision_objects[mesh.id] = CollisionSceneObject(mesh, pose, metadata)
if started():
assert globs.state
@@ -711,7 +1206,13 @@ def put_eef_pose() -> RespT:
payload = float(request.args.get("payload", default=0.0))
safe = request.args.get("safe", default="true") == "true"
- globs.state.worker.request("move_to_pose", pose=pose.to_dict(), velocity=velocity, payload=payload, safe=safe)
+ globs.state.worker.request(
+ "move_to_pose",
+ pose=pose.to_dict(),
+ velocity=velocity,
+ payload=payload,
+ safe=safe,
+ )
return Response(status=204)
diff --git a/src/python/arcor2_ur/tests/BUILD b/src/python/arcor2_ur/tests/BUILD
index 83ad122a..4541c50d 100644
--- a/src/python/arcor2_ur/tests/BUILD
+++ b/src/python/arcor2_ur/tests/BUILD
@@ -1,8 +1,34 @@
-python_tests(
- dependencies=["3rdparty#PyYAML"],
- runtime_package_dependencies=[
- "src/python/arcor2_ur/scripts:ur", "src/python/arcor2_ur/scripts:robot_publisher",
+files(
+ name="mesh_assets",
+ sources=[
+ "test_mesh_object/triangle_block.stl",
],
)
-python_test_utils(name="test_utils")
+python_test_utils(
+ name="test_utils",
+ sources=["conftest.py"],
+)
+
+python_tests(
+ name="tests",
+ sources=[
+ "test_*.py",
+ "test_collision/test_*.py",
+ "test_pick_up/test_*.py",
+ "test_pick_and_place/test_*.py",
+ "test_pick_and_move_whit_collisions/test_*.py",
+ ],
+ dependencies=[
+ ":test_utils",
+ ":mesh_assets",
+ "src/python/arcor2_ur/data/urdf:urdf",
+ "src/python/arcor2_ur/data:moveit_yaml",
+ ],
+ runtime_package_dependencies=[
+ "src/python/arcor2_ur/scripts:ur",
+ "src/python/arcor2_ur/scripts:robot_publisher",
+ "src/python/arcor2_storage/scripts:storage",
+ ],
+ tags=["unstable_ros"],
+)
\ No newline at end of file
diff --git a/src/python/arcor2_ur/tests/conftest.py b/src/python/arcor2_ur/tests/conftest.py
index 18c45c02..93158619 100644
--- a/src/python/arcor2_ur/tests/conftest.py
+++ b/src/python/arcor2_ur/tests/conftest.py
@@ -4,12 +4,13 @@
import signal
import subprocess as sp
import time
-from typing import Iterator, NamedTuple
+from typing import Any, Iterator, NamedTuple
import pytest
from arcor2.helpers import find_free_port
from arcor2_arserver.tests.testutils import CheckHealthException, check_health, log_proc_output
+from arcor2_storage import client as storage_client
LOGGER = logging.getLogger(__name__)
@@ -17,15 +18,17 @@
class Urls(NamedTuple):
ros_domain_id: str
robot_url: str
+ storage_url: str
-def _finish_processes(processes) -> None:
+def _finish_processes(processes: list[sp.Popen]) -> None:
for proc in processes:
if proc.poll() is None:
try:
os.killpg(proc.pid, signal.SIGTERM)
except ProcessLookupError:
pass
+
try:
proc.wait(timeout=5)
except sp.TimeoutExpired:
@@ -34,86 +37,135 @@ def _finish_processes(processes) -> None:
except ProcessLookupError:
pass
proc.wait(timeout=1)
+
log_proc_output(proc.communicate())
def _load_ros_env() -> dict[str, str]:
- """Load environment variables from ROS setup script."""
try:
output = sp.check_output(
["bash", "-lc", "source /opt/ros/jazzy/setup.bash >/dev/null && env -0"],
text=False,
env={},
)
- except sp.CalledProcessError as exc: # pragma: no cover - best effort helper
+ except sp.CalledProcessError as exc:
raise RuntimeError("Failed to source /opt/ros/jazzy/setup.bash") from exc
env: dict[str, str] = {}
+
for chunk in output.split(b"\0"):
if not chunk:
continue
+
key, _, value = chunk.partition(b"=")
env[key.decode()] = value.decode()
+
return env
+def _ros_launch_env(env: dict[str, str]) -> dict[str, str]:
+ launch_env = {key: value for key, value in env.items() if not key.startswith("PEX_")}
+
+ for key in ("PYTEST_DISABLE_PLUGIN_AUTOLOAD", "PYTEST_PLUGINS", "PYTHONDEVMODE", "PYTHONWARNINGS"):
+ launch_env.pop(key, None)
+
+ return launch_env
+
+
+def _assert_process_alive(proc: sp.Popen, error_message: str) -> None:
+ if proc.poll() is not None:
+ log_proc_output(proc.communicate())
+ raise RuntimeError(error_message)
+
+
@pytest.fixture(scope="module", params=["ur5e"])
def start_processes(request) -> Iterator[Urls]:
- """Starts Dobot dependencies."""
-
ros_domain_id = str(random.sample(range(0, 232 + 1), 1)[0])
ur_type: str = request.param
- processes = []
+ processes: list[sp.Popen] = []
+
ros_env = _load_ros_env()
+
my_env = os.environ.copy()
my_env.update(ros_env)
my_env["ROS_DOMAIN_ID"] = ros_domain_id
- # Keep ROS view of the world dominant; strip test-only/debug env for the ROS launcher
- ros_launch_env = {k: v for k, v in my_env.items() if not k.startswith("PEX_")}
- for key in ("PYTEST_DISABLE_PLUGIN_AUTOLOAD", "PYTEST_PLUGINS", "PYTHONDEVMODE", "PYTHONWARNINGS"):
- ros_launch_env.pop(key, None)
-
- kwargs = {"env": my_env, "stdout": sp.PIPE, "stderr": sp.STDOUT, "start_new_session": True}
-
- processes.append(
- sp.Popen(
- [
- "ros2",
- "launch",
- "ur_robot_driver",
- "ur_control.launch.py",
- "launch_rviz:=false",
- f"ur_type:={ur_type}",
- "use_mock_hardware:=true",
- "robot_ip:=xyz",
- ],
- env=ros_launch_env,
- stdout=sp.PIPE,
- stderr=sp.STDOUT,
- start_new_session=True,
- )
+ ros_launch_env = _ros_launch_env(my_env)
+
+ kwargs: dict[str, Any] = {
+ "env": my_env,
+ "stdout": sp.PIPE,
+ "stderr": sp.STDOUT,
+ "start_new_session": True,
+ }
+
+ ur_control_proc = sp.Popen(
+ [
+ "ros2",
+ "launch",
+ "ur_robot_driver",
+ "ur_control.launch.py",
+ "launch_rviz:=false",
+ f"ur_type:={ur_type}",
+ "use_mock_hardware:=true",
+ "robot_ip:=xyz",
+ ],
+ env=ros_launch_env,
+ stdout=sp.PIPE,
+ stderr=sp.STDOUT,
+ start_new_session=True,
)
- time.sleep(3) # TODO find another way how to make sure that everything is running
- if processes[-1].poll():
- log_proc_output(processes[-1].communicate())
- raise Exception("Launch died...")
+
+ processes.append(ur_control_proc)
+
+ time.sleep(3)
+ _assert_process_alive(ur_control_proc, "UR control launch died.")
+
+ storage_port = find_free_port()
+ storage_url = f"http://127.0.0.1:{storage_port}"
+
+ my_env["ARCOR2_STORAGE_SERVICE_PORT"] = str(storage_port)
+ my_env["ARCOR2_STORAGE_SERVICE_URL"] = storage_url
+ my_env["ARCOR2_STORAGE_DB_PATH"] = f"/tmp/arcor2_storage_{ros_domain_id}.sqlite"
+
+ storage_client.URL = storage_url
+
+ storage_proc = sp.Popen(
+ ["python", "src.python.arcor2_storage.scripts/storage.pex"],
+ **kwargs,
+ )
+
+ processes.append(storage_proc)
+
+ if storage_proc.poll() is not None:
+ _finish_processes(processes)
+ raise RuntimeError("Storage service died.")
+
+ try:
+ check_health("Storage", storage_url, timeout=20)
+ except CheckHealthException:
+ _finish_processes(processes)
+ raise
robot_url = f"http://0.0.0.0:{find_free_port()}"
+
my_env["ARCOR2_UR_URL"] = robot_url
my_env["ARCOR2_UR_INTERACT_WITH_DASHBOARD"] = "false"
my_env["ARCOR2_UR_TYPE"] = ur_type
my_env["PEX_EXTRA_SYS_PATH"] = "/opt/ros/jazzy/lib/python3.12/site-packages"
my_env["ARCOR2_REST_API_DEBUG"] = "true"
- robot_proc = sp.Popen(["python", "src.python.arcor2_ur.scripts/ur.pex"], **kwargs) # type: ignore
+ robot_proc = sp.Popen(
+ ["python", "src.python.arcor2_ur.scripts/ur.pex"],
+ **kwargs,
+ )
processes.append(robot_proc)
- if robot_proc.poll():
+ if robot_proc.poll() is not None:
_finish_processes(processes)
- raise Exception("Robot service died.")
+ raise RuntimeError("Robot service died.")
try:
check_health("UR", robot_url, timeout=20)
@@ -121,15 +173,17 @@ def start_processes(request) -> Iterator[Urls]:
_finish_processes(processes)
raise
- # robot_mode etc. is not published with mock_hw -> there is this helper node to do that
- # it can't be published from here as it depends on ROS (Python 3.12)
- robot_pub_proc = sp.Popen(["python", "src.python.arcor2_ur.scripts/robot_publisher.pex"], **kwargs) # type: ignore
+ robot_pub_proc = sp.Popen(
+ ["python", "src.python.arcor2_ur.scripts/robot_publisher.pex"],
+ **kwargs,
+ )
+
processes.append(robot_pub_proc)
- if robot_pub_proc.poll():
+ if robot_pub_proc.poll() is not None:
_finish_processes(processes)
- raise Exception("Robot publisher node died.")
+ raise RuntimeError("Robot publisher node died.")
- yield Urls(ros_domain_id, robot_url)
+ yield Urls(ros_domain_id, robot_url, storage_url)
_finish_processes(processes)
diff --git a/src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py b/src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py
new file mode 100644
index 00000000..8c4b74a8
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py
@@ -0,0 +1,54 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Box, Cylinder, Sphere
+from arcor2_scene_data import scene_service
+from arcor2_ur.common import GraspableState
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_graspable(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.5
+ Y = 0.0
+ Z = 0.3
+
+ start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0))
+ goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0))
+ ot.move_to_pose("", start_pose, 0.3, safe=False)
+
+ box = Box("Box", 0.2, 0.2, 0.95)
+ scene_service.upsert_graspable(box, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ assert box.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ cyl = Cylinder("Cyl", 0.1, 0.95)
+ scene_service.upsert_graspable(
+ cyl, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1)), GraspableState.RESERVED
+ )
+ assert cyl.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ sphere = Sphere("Sphere1", 0.09)
+ scene_service.upsert_graspable(
+ sphere, Pose(Position(0.3, 0.0, 1.0), Orientation(0, 0, 0, 1)), GraspableState.HIDDEN
+ )
+ assert sphere.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ ot.move_to_pose("", goal_pose, 0.3)
+
+ reached = ot.get_end_effector_pose("")
+ assert goal_pose.position.distance(reached.position) < 0.03
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
+
+ box = Box("Box1", 0.1, 0.1, 0.2)
diff --git a/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py b/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py
new file mode 100644
index 00000000..7dc5a6a6
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py
@@ -0,0 +1,52 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Box
+from arcor2_scene_data import scene_service
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_overhead_box(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.5
+ Y = 0.0
+ Z = 0.3
+
+ start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0))
+ goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0))
+ ot.move_to_pose("", start_pose, 0.3, safe=False)
+
+ box = Box("Box1", 0.2, 0.2, 0.2)
+ scene_service.upsert_collision(box, Pose(Position(0.3, 0.0, 1.0), Orientation(0, 0, 0, 1)))
+ assert box.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ box = Box("Box2", 0.2, 0.2, 0.2)
+ scene_service.upsert_collision(box, Pose(Position(-0.3, 0.0, 1.0), Orientation(0, 0, 0, 1)))
+ assert box.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ box = Box("Box3", 0.2, 0.2, 0.2)
+ scene_service.upsert_collision(box, Pose(Position(0.0, 0.3, 1.0), Orientation(0, 0, 0, 1)))
+ assert box.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ box = Box("Box4", 0.2, 0.2, 0.2)
+ scene_service.upsert_collision(box, Pose(Position(0.0, -0.3, 1.0), Orientation(0, 0, 0, 1)))
+ assert box.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ ot.move_to_pose("", goal_pose, 0.3)
+
+ reached = ot.get_end_effector_pose("")
+ assert goal_pose.position.distance(reached.position) < 0.03
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py b/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py
new file mode 100644
index 00000000..1875adf3
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py
@@ -0,0 +1,52 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Cylinder
+from arcor2_scene_data import scene_service
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_overhead_cylinder(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.5
+ Y = 0.0
+ Z = 0.3
+
+ start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0))
+ goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0))
+ ot.move_to_pose("", start_pose, 0.3, safe=False)
+
+ cyl = Cylinder("Cyl1", 0.2, 0.2)
+ scene_service.upsert_collision(cyl, Pose(Position(0.3, 0.0, 1.0), Orientation(0, 0, 0, 1)))
+ assert cyl.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ cyl = Cylinder("Cyl2", 0.2, 0.2)
+ scene_service.upsert_collision(cyl, Pose(Position(-0.3, 0.0, 1.0), Orientation(0, 0, 0, 1)))
+ assert cyl.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ cyl = Cylinder("Cyl3", 0.2, 0.2)
+ scene_service.upsert_collision(cyl, Pose(Position(0.0, 0.3, 1.0), Orientation(0, 0, 0, 1)))
+ assert cyl.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ cyl = Cylinder("Cyl4", 0.2, 0.2)
+ scene_service.upsert_collision(cyl, Pose(Position(0.0, -0.3, 1.0), Orientation(0, 0, 0, 1)))
+ assert cyl.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ ot.move_to_pose("", goal_pose, 0.3)
+
+ reached = ot.get_end_effector_pose("")
+ assert goal_pose.position.distance(reached.position) < 0.03
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_mesh.py b/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_mesh.py
new file mode 100644
index 00000000..ef2d2204
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_mesh.py
@@ -0,0 +1,67 @@
+import time
+from pathlib import Path
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Mesh
+from arcor2_scene_data import scene_service
+from arcor2_storage import client as storage_client
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+MESH_PATH = Path(__file__).parents[1] / "test_mesh_object" / "triangle_block.stl"
+MESH_ASSET_ID = "triangle_block.stl"
+
+
+def test_overhead_mesh(start_processes: Urls) -> None:
+ assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}"
+
+ storage_client.URL = start_processes.storage_url
+ storage_client.create_asset(
+ MESH_ASSET_ID,
+ MESH_PATH.read_bytes(),
+ description="Test mesh for UR5e collision avoidance.",
+ )
+
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.6
+ Y = 0.0
+ Z = 0.3
+
+ start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0))
+ goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0))
+
+ ot.move_to_pose("", start_pose, 0.3, safe=False)
+
+ mesh = Mesh("Mesh1", MESH_ASSET_ID)
+ scene_service.upsert_collision(mesh, Pose(Position(0.3, 0.0, 1.0), Orientation(0, 0, 0, 1)))
+ assert mesh.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ mesh = Mesh("Mesh2", MESH_ASSET_ID)
+ scene_service.upsert_collision(mesh, Pose(Position(-0.3, 0.0, 1.0), Orientation(0, 0, 0, 1)))
+ assert mesh.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ mesh = Mesh("Mesh3", MESH_ASSET_ID)
+ scene_service.upsert_collision(mesh, Pose(Position(0.0, 0.3, 1.0), Orientation(0, 0, 0, 1)))
+ assert mesh.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ mesh = Mesh("Mesh4", MESH_ASSET_ID)
+ scene_service.upsert_collision(mesh, Pose(Position(0.0, -0.3, 1.0), Orientation(0, 0, 0, 1)))
+ assert mesh.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ ot.move_to_pose("", goal_pose, 0.3)
+
+ reached = ot.get_end_effector_pose("")
+ assert goal_pose.position.distance(reached.position) < 0.03
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_sphere.py b/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_sphere.py
new file mode 100644
index 00000000..d9bde171
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_sphere.py
@@ -0,0 +1,52 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Sphere
+from arcor2_scene_data import scene_service
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_overhead_sphere(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.6
+ Y = 0.0
+ Z = 0.3
+
+ start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0))
+ goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0))
+ ot.move_to_pose("", start_pose, 0.3, safe=False)
+
+ sphere = Sphere("Sphere1", 0.09)
+ scene_service.upsert_collision(sphere, Pose(Position(0.3, 0.0, 1.0), Orientation(0, 0, 0, 1)))
+ assert sphere.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ sphere = Sphere("Sphere2", 0.09)
+ scene_service.upsert_collision(sphere, Pose(Position(-0.3, 0.0, 1.0), Orientation(0, 0, 0, 1)))
+ assert sphere.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ sphere = Sphere("Sphere3", 0.09)
+ scene_service.upsert_collision(sphere, Pose(Position(0.0, 0.3, 1.0), Orientation(0, 0, 0, 1)))
+ assert sphere.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ sphere = Sphere("Sphere4", 0.09)
+ scene_service.upsert_collision(sphere, Pose(Position(0.0, -0.3, 1.0), Orientation(0, 0, 0, 1)))
+ assert sphere.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ ot.move_to_pose("", goal_pose, 0.3)
+
+ reached = ot.get_end_effector_pose("")
+ assert goal_pose.position.distance(reached.position) < 0.03
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py b/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py
new file mode 100644
index 00000000..b3755629
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py
@@ -0,0 +1,42 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Box
+from arcor2_scene_data import scene_service
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_side_box(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.5
+ Y = 0.0
+ Z = 0.3
+
+ start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0))
+ goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0))
+ ot.move_to_pose("", start_pose, 0.3, safe=False)
+
+ box = Box("Box1", 0.2, 0.2, 0.95)
+ scene_service.upsert_collision(box, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1)))
+ assert box.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ box = Box("Box2", 0.2, 0.2, 0.95)
+ scene_service.upsert_collision(box, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1)))
+ assert box.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ ot.move_to_pose("", goal_pose, 0.3)
+
+ reached = ot.get_end_effector_pose("")
+ assert goal_pose.position.distance(reached.position) < 0.03
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py b/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py
new file mode 100644
index 00000000..a5aaa71c
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py
@@ -0,0 +1,42 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Cylinder
+from arcor2_scene_data import scene_service
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_side_cylinder(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.5
+ Y = 0.0
+ Z = 0.3
+
+ start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0))
+ goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0))
+ ot.move_to_pose("", start_pose, 0.3, safe=False)
+
+ cyl = Cylinder("Cyl1", 0.1, 0.95)
+ scene_service.upsert_collision(cyl, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1)))
+ assert cyl.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ cyl = Cylinder("Cyl2", 0.1, 0.95)
+ scene_service.upsert_collision(cyl, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1)))
+ assert cyl.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ ot.move_to_pose("", goal_pose, 0.3)
+
+ reached = ot.get_end_effector_pose("")
+ assert goal_pose.position.distance(reached.position) < 0.03
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py b/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py
new file mode 100644
index 00000000..452a5858
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py
@@ -0,0 +1,69 @@
+import time
+from pathlib import Path
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Mesh
+from arcor2_scene_data import scene_service
+from arcor2_storage import client as storage_client
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+MESH_PATH = Path(__file__).parents[1] / "test_mesh_object" / "triangle_block.stl"
+MESH_ASSET_ID = "triangle_block.stl"
+
+
+def test_side_mesh(start_processes: Urls) -> None:
+ assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}"
+
+ storage_client.URL = start_processes.storage_url
+
+ storage_client.create_asset(
+ MESH_ASSET_ID,
+ MESH_PATH.read_bytes(),
+ description="Test mesh for UR5e collision avoidance.",
+ )
+
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.5
+ Y = 0.0
+ Z = 0.3
+
+ start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0))
+ goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0))
+
+ ot.move_to_pose("", start_pose, 0.3, safe=False)
+ time.sleep(1)
+
+ mesh_positions = [
+ # rgith
+ Position(0.5, 0.5, 0.25),
+ Position(0.5, 0.5, 0.55),
+ # left
+ Position(-0.5, 0.5, 0.25),
+ Position(-0.5, 0.5, 0.55),
+ ]
+
+ for idx, position in enumerate(mesh_positions, start=1):
+ mesh = Mesh(f"Mesh{idx}", MESH_ASSET_ID)
+
+ scene_service.upsert_collision(
+ mesh,
+ Pose(position, Orientation(0, 0, 0, 1)),
+ )
+
+ assert mesh.id in scene_service.collision_ids()
+ time.sleep(3)
+
+ ot.move_to_pose("", goal_pose, 0.3)
+
+ reached = ot.get_end_effector_pose("")
+ assert goal_pose.position.distance(reached.position) < 0.03
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py b/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py
new file mode 100644
index 00000000..d57c26c8
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py
@@ -0,0 +1,62 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Sphere
+from arcor2_scene_data import scene_service
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_side_sphere(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.5
+ Y = 0.0
+ Z = 0.3
+
+ start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0))
+ goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0))
+ ot.move_to_pose("", start_pose, 0.3, safe=False)
+
+ sphere = Sphere("Sphere1", 0.09)
+ scene_service.upsert_collision(sphere, Pose(Position(0.5, 0.5, 0.1), Orientation(0, 0, 0, 1)))
+ assert sphere.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ sphere = Sphere("Sphere2", 0.1)
+ scene_service.upsert_collision(sphere, Pose(Position(0.5, 0.5, 0.3), Orientation(0, 0, 0, 1)))
+ assert sphere.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ sphere = Sphere("Sphere3", 0.09)
+ scene_service.upsert_collision(sphere, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1)))
+ assert sphere.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ sphere = Sphere("Sphere4", 0.09)
+ scene_service.upsert_collision(sphere, Pose(Position(-0.5, 0.5, 0.1), Orientation(0, 0, 0, 1)))
+ assert sphere.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ sphere = Sphere("Sphere5", 0.1)
+ scene_service.upsert_collision(sphere, Pose(Position(-0.5, 0.5, 0.3), Orientation(0, 0, 0, 1)))
+ assert sphere.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ sphere = Sphere("Sphere6", 0.09)
+ scene_service.upsert_collision(sphere, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1)))
+ assert sphere.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ ot.move_to_pose("", goal_pose, 0.3)
+
+ reached = ot.get_end_effector_pose("")
+ assert goal_pose.position.distance(reached.position) < 0.03
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_interaction.py b/src/python/arcor2_ur/tests/test_interaction.py
index c9023493..9aa20d63 100644
--- a/src/python/arcor2_ur/tests/test_interaction.py
+++ b/src/python/arcor2_ur/tests/test_interaction.py
@@ -10,7 +10,6 @@
from arcor2_ur.tests.conftest import Urls
-@pytest.mark.timeout(60)
def test_basics(start_processes: Urls) -> None:
scene_service.URL = start_processes.robot_url
box = Box("UniqueBoxId", 0.1, 0.1, 0.1)
@@ -26,7 +25,7 @@ def test_basics(start_processes: Urls) -> None:
pos.position.z -= 0.05
ot.move_to_pose("", pos, 0.5)
pos_after = ot.get_end_effector_pose("")
- assert orig_z - pos_after.position.z > 0.045
+ assert orig_z - pos_after.position.z > 0.035
ot.suck()
ot.release()
diff --git a/src/python/arcor2_ur/tests/test_mesh_object/triangle_block.stl b/src/python/arcor2_ur/tests/test_mesh_object/triangle_block.stl
new file mode 100644
index 00000000..4447107a
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_mesh_object/triangle_block.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f716942e9d3e23090ab22378eae5f238cbe19329172af17c32a25bbe52221e47
+size 484
diff --git a/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_overhead_obstacle.py b/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_overhead_obstacle.py
new file mode 100644
index 00000000..12358dc9
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_overhead_obstacle.py
@@ -0,0 +1,53 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Box
+from arcor2_scene_data import scene_service
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_pick_and_place_side_obstacle(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ object = Box("Box1", 0.2, 0.2, 0.2)
+ scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.TOP)
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ box = Box("Box2", 0.2, 0.2, 0.2)
+ scene_service.upsert_collision(box, Pose(Position(0.3, 0.0, 1.0), Orientation(0, 0, 0, 1)))
+ assert box.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ box = Box("Box3", 0.2, 0.2, 0.2)
+ scene_service.upsert_collision(box, Pose(Position(-0.3, 0.0, 1.0), Orientation(0, 0, 0, 1)))
+ assert box.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ box = Box("Box4", 0.2, 0.2, 0.2)
+ scene_service.upsert_collision(box, Pose(Position(0.0, 0.3, 1.0), Orientation(0, 0, 0, 1)))
+ assert box.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ box = Box("Box5", 0.2, 0.2, 0.2)
+ scene_service.upsert_collision(box, Pose(Position(0.0, -0.3, 1.0), Orientation(0, 0, 0, 1)))
+ assert box.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ ot.move_to_pose("", Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)), 0.3)
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_side_obstacle.py b/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_side_obstacle.py
new file mode 100644
index 00000000..eb2ce96e
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_side_obstacle.py
@@ -0,0 +1,38 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Box
+from arcor2_scene_data import scene_service
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_pick_and_place_side_obstacle(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ object = Box("Box1", 0.2, 0.2, 0.2)
+ scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.TOP)
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ box = Box("Box2", 0.2, 0.2, 0.95)
+ scene_service.upsert_collision(box, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1)))
+ assert box.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ ot.move_to_pose("", Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)), 0.3)
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_wall.py b/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_wall.py
new file mode 100644
index 00000000..f34a574a
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_wall.py
@@ -0,0 +1,42 @@
+import time
+
+import pytest
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Box
+from arcor2.exceptions import Arcor2Exception
+from arcor2_scene_data import scene_service
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_pick_and_move_wall(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ object = Box("Box1", 0.2, 0.2, 0.2)
+ scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.TOP)
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ box = Box("Wall1", 0.1, 2.0, 1.0)
+ scene_service.upsert_collision(box, Pose(Position(0.4, 0.0, 0.5), Orientation(0, 0, 0, 1)))
+ assert box.id in scene_service.collision_ids()
+ time.sleep(1)
+
+ with pytest.raises(Arcor2Exception):
+ ot.move_to_pose("", Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)), 0.3)
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_id.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_id.py
new file mode 100644
index 00000000..607a6980
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_id.py
@@ -0,0 +1,37 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Box
+from arcor2_scene_data import scene_service
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_pick_and_place_box_by_id(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ object = Box("Box1", 0.2, 0.2, 0.2)
+ scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.TOP)
+
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)))
+
+ assert Position(Y, X, Z).distance(ot.graspable_position(object.id)) < 0.03
+ assert ot.graspable_state(object.id) == GraspableState.WORLD
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_position.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_position.py
new file mode 100644
index 00000000..0a2aeb36
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_position.py
@@ -0,0 +1,37 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Box
+from arcor2_scene_data import scene_service
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_pick_and_place_box_by_position(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ object = Box("Box1", 0.2, 0.2, 0.2)
+ scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, GraspPosition.TOP)
+
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)))
+
+ assert Position(Y, X, Z).distance(ot.graspable_position(object.id)) < 0.03
+ assert ot.graspable_state(object.id) == GraspableState.WORLD
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_left.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_left.py
new file mode 100644
index 00000000..04f75d1e
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_left.py
@@ -0,0 +1,37 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Box
+from arcor2_scene_data import scene_service
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_pick_and_place_box_by_id(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ object = Box("Box1", 0.2, 0.2, 0.2)
+ scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.LEFT)
+
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)))
+
+ assert Position(Y, X, Z).distance(ot.graspable_position(object.id)) < 0.03
+ assert ot.graspable_state(object.id) == GraspableState.WORLD
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_id.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_id.py
new file mode 100644
index 00000000..30fb18c0
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_id.py
@@ -0,0 +1,37 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Cylinder
+from arcor2_scene_data import scene_service
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_pick_and_place_cylinder_by_id(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ object = Cylinder("Cyl1", 0.1, 0.2)
+ scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.TOP)
+
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)))
+
+ assert Position(Y, X, Z).distance(ot.graspable_position(object.id)) < 0.03
+ assert ot.graspable_state(object.id) == GraspableState.WORLD
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_position.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_position.py
new file mode 100644
index 00000000..5f8d9b58
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_position.py
@@ -0,0 +1,37 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Cylinder
+from arcor2_scene_data import scene_service
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_pick_and_place_cylinder_by_position(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ object = Cylinder("Cyl1", 0.1, 0.2)
+ scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, GraspPosition.TOP)
+
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)))
+
+ assert Position(Y, X, Z).distance(ot.graspable_position(object.id)) < 0.03
+ assert ot.graspable_state(object.id) == GraspableState.WORLD
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_id.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_id.py
new file mode 100644
index 00000000..b740af2b
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_id.py
@@ -0,0 +1,52 @@
+import time
+from pathlib import Path
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Mesh
+from arcor2_scene_data import scene_service
+from arcor2_storage import client as storage_client
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+MESH_PATH = Path(__file__).parents[1] / "test_mesh_object" / "triangle_block.stl"
+MESH_ASSET_ID = "triangle_block.stl"
+
+
+def test_pick_and_place_mesh_by_id(start_processes: Urls) -> None:
+ assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}"
+
+ storage_client.URL = start_processes.storage_url
+
+ storage_client.create_asset(
+ MESH_ASSET_ID,
+ MESH_PATH.read_bytes(),
+ description="Test mesh for UR5e collision avoidance.",
+ )
+
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ object = Mesh("Mesh1", MESH_ASSET_ID)
+ scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.TOP)
+
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)))
+
+ assert Position(Y, X, Z).distance(ot.graspable_position(object.id)) < 0.03
+ assert ot.graspable_state(object.id) == GraspableState.WORLD
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_position.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_position.py
new file mode 100644
index 00000000..3ae304f0
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_position.py
@@ -0,0 +1,51 @@
+import time
+from pathlib import Path
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Mesh
+from arcor2_scene_data import scene_service
+from arcor2_storage import client as storage_client
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+MESH_PATH = Path(__file__).parents[1] / "test_mesh_object" / "triangle_block.stl"
+MESH_ASSET_ID = "triangle_block.stl"
+
+
+def test_pick_and_place_mesh_by_position(start_processes: Urls) -> None:
+ assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}"
+
+ storage_client.URL = start_processes.storage_url
+
+ storage_client.create_asset(
+ MESH_ASSET_ID,
+ MESH_PATH.read_bytes(),
+ description="Test mesh for UR5e collision avoidance.",
+ )
+
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ object = Mesh("Mesh1", MESH_ASSET_ID)
+ scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, GraspPosition.TOP)
+
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)))
+
+ assert Position(Y, X, Z).distance(ot.graspable_position(object.id)) < 0.03
+ assert ot.graspable_state(object.id) == GraspableState.WORLD
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_rotated_box.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_rotated_box.py
new file mode 100644
index 00000000..52540ff8
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_rotated_box.py
@@ -0,0 +1,37 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Box
+from arcor2_scene_data import scene_service
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_pick_and_place_box_by_id(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ object = Box("Box1", 0.2, 0.2, 0.2)
+ scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0.5, 0.5, 0.5, 0)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.TOP)
+
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)))
+
+ assert Position(Y, X, Z).distance(ot.graspable_position(object.id)) < 0.03
+ assert ot.graspable_state(object.id) == GraspableState.WORLD
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_id.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_id.py
new file mode 100644
index 00000000..258c8ced
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_id.py
@@ -0,0 +1,37 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Sphere
+from arcor2_scene_data import scene_service
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_pick_and_place_sphere_by_id(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ object = Sphere("Sphere1", 0.1)
+ scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.TOP)
+
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)))
+
+ assert Position(Y, X, Z).distance(ot.graspable_position(object.id)) < 0.03
+ assert ot.graspable_state(object.id) == GraspableState.WORLD
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_position.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_position.py
new file mode 100644
index 00000000..94bdad54
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_position.py
@@ -0,0 +1,37 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Sphere
+from arcor2_scene_data import scene_service
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_pick_and_place_sphere_by_position(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ object = Sphere("Sphere1", 0.1)
+ scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, GraspPosition.TOP)
+
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)))
+
+ assert Position(Y, X, Z).distance(ot.graspable_position(object.id)) < 0.03
+ assert ot.graspable_state(object.id) == GraspableState.WORLD
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_all.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_all.py
new file mode 100644
index 00000000..467d461f
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_all.py
@@ -0,0 +1,32 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Box
+from arcor2_scene_data import scene_service
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_pick_up_box_by_id(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ object = Box("Box1", 0.2, 0.2, 0.2)
+ scene_service.upsert_graspable(object, Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.ALL)
+
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_back.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_back.py
new file mode 100644
index 00000000..77027705
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_back.py
@@ -0,0 +1,32 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Box
+from arcor2_scene_data import scene_service
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_pick_up_box_by_id(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ object = Box("Box1", 0.2, 0.2, 0.2)
+ scene_service.upsert_graspable(object, Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.BACK)
+
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_bottom.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_bottom.py
new file mode 100644
index 00000000..ebc94b4e
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_bottom.py
@@ -0,0 +1,32 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Box
+from arcor2_scene_data import scene_service
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_pick_up_box_by_id(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.0
+ Y = 0.7
+ Z = 0.7
+
+ object = Box("Box1", 0.2, 0.2, 0.2)
+ scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.BOTTOM)
+
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py
new file mode 100644
index 00000000..c4be2b90
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py
@@ -0,0 +1,32 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Box
+from arcor2_scene_data import scene_service
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_pick_up_box_by_id(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ object = Box("Box1", 0.2, 0.2, 0.2)
+ scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.TOP)
+
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py
new file mode 100644
index 00000000..66e210f4
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py
@@ -0,0 +1,32 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Box
+from arcor2_scene_data import scene_service
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_pick_up_box_by_position(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ object = Box("Box1", 0.2, 0.2, 0.2)
+ scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, GraspPosition.TOP)
+
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_front.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_front.py
new file mode 100644
index 00000000..c6d87dab
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_front.py
@@ -0,0 +1,32 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Box
+from arcor2_scene_data import scene_service
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_pick_up_box_by_id(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ object = Box("Box1", 0.2, 0.2, 0.2)
+ scene_service.upsert_graspable(object, Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.FRONT)
+
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_left.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_left.py
new file mode 100644
index 00000000..91f75875
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_left.py
@@ -0,0 +1,32 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Box
+from arcor2_scene_data import scene_service
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_pick_up_box_by_id(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ object = Box("Box1", 0.2, 0.2, 0.2)
+ scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.LEFT)
+
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_right.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_right.py
new file mode 100644
index 00000000..d7697854
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_right.py
@@ -0,0 +1,32 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Box
+from arcor2_scene_data import scene_service
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_pick_up_box_by_id(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ object = Box("Box1", 0.2, 0.2, 0.2)
+ scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.RIGHT)
+
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py
new file mode 100644
index 00000000..2a552ec4
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py
@@ -0,0 +1,32 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Cylinder
+from arcor2_scene_data import scene_service
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_pick_up_cylinder_by_id(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ object = Cylinder("Cyl1", 0.1, 0.2)
+ scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.TOP)
+
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py
new file mode 100644
index 00000000..06f6f2d9
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py
@@ -0,0 +1,32 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Cylinder
+from arcor2_scene_data import scene_service
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_pick_up_cylinder_by_position(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ object = Cylinder("Cyl1", 0.1, 0.2)
+ scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, GraspPosition.TOP)
+
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py
new file mode 100644
index 00000000..4e7ae0ec
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py
@@ -0,0 +1,47 @@
+import time
+from pathlib import Path
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Mesh
+from arcor2_scene_data import scene_service
+from arcor2_storage import client as storage_client
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+MESH_PATH = Path(__file__).parents[1] / "test_mesh_object" / "triangle_block.stl"
+MESH_ASSET_ID = "triangle_block.stl"
+
+
+def test_pick_up_mesh_by_id(start_processes: Urls) -> None:
+ assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}"
+
+ storage_client.URL = start_processes.storage_url
+
+ storage_client.create_asset(
+ MESH_ASSET_ID,
+ MESH_PATH.read_bytes(),
+ description="Test mesh for UR5e collision avoidance.",
+ )
+
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ object = Mesh("Mesh1", MESH_ASSET_ID)
+ scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.TOP)
+
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py
new file mode 100644
index 00000000..1bddebe7
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py
@@ -0,0 +1,47 @@
+import time
+from pathlib import Path
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Mesh
+from arcor2_scene_data import scene_service
+from arcor2_storage import client as storage_client
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+MESH_PATH = Path(__file__).parents[1] / "test_mesh_object" / "triangle_block.stl"
+MESH_ASSET_ID = "triangle_block.stl"
+
+
+def test_pick_up_mesh_by_position(start_processes: Urls) -> None:
+ assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}"
+
+ storage_client.URL = start_processes.storage_url
+
+ storage_client.create_asset(
+ MESH_ASSET_ID,
+ MESH_PATH.read_bytes(),
+ description="Test mesh for UR5e collision avoidance.",
+ )
+
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ object = Mesh("Mesh1", MESH_ASSET_ID)
+ scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, GraspPosition.TOP)
+
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py
new file mode 100644
index 00000000..31c550a5
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py
@@ -0,0 +1,32 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Sphere
+from arcor2_scene_data import scene_service
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_pick_up_sphere_by_id(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ object = Sphere("Sphere1", 0.1)
+ scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.TOP)
+
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py
new file mode 100644
index 00000000..9219042d
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py
@@ -0,0 +1,32 @@
+import time
+
+from arcor2.data.common import Orientation, Pose, Position
+from arcor2.data.object_type import Sphere
+from arcor2_scene_data import scene_service
+from arcor2_ur.common import EffectorType, GraspableState, GraspPosition
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_pick_up_sphere_by_position(start_processes: Urls) -> None:
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+ assert len(ot.robot_joints()) == 6
+
+ X = 0.0
+ Y = 0.5
+ Z = 0.1
+
+ object = Sphere("Sphere1", 0.1)
+ scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD)
+ time.sleep(1)
+
+ ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, GraspPosition.TOP)
+
+ assert ot.graspable_state(object.id) == GraspableState.ATTACHED
+
+ scene_service.delete_all_collisions()
+ ot.cleanup()
diff --git a/src/python/arcor2_ur/tests/test_suck_effector.py b/src/python/arcor2_ur/tests/test_suck_effector.py
new file mode 100644
index 00000000..d96fdc52
--- /dev/null
+++ b/src/python/arcor2_ur/tests/test_suck_effector.py
@@ -0,0 +1,33 @@
+from arcor2.data.common import Pose
+from arcor2_scene_data import scene_service
+from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
+from arcor2_ur.scripts.robot_publisher import load_robot_description
+from arcor2_ur.tests.conftest import Urls
+
+
+def test_suck_effector(start_processes: Urls) -> None:
+
+ robot_description = load_robot_description()
+ assert "suction_base_link" in robot_description
+ assert "suction_cup_link" in robot_description
+ assert "suction_tcp" in robot_description
+ assert "tool0_to_suction_base" in robot_description
+ assert "suction_base_to_cup" in robot_description
+ assert "suction_cup_to_tcp" in robot_description
+
+ scene_service.URL = start_processes.robot_url
+ scene_service.start()
+ assert scene_service.started()
+
+ ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))
+
+ assert len(ot.robot_joints()) == 6
+
+ pos = ot.get_end_effector_pose("")
+ orig_z = pos.position.z
+ pos.position.z -= 0.05
+ ot.move_to_pose("", pos, 0.5)
+ pos_after = ot.get_end_effector_pose("")
+ assert orig_z - pos_after.position.z > 0.035
+
+ ot.cleanup()