Skip to content

Commit

Permalink
feat(arcor2_ur): handling of collision objects
Browse files Browse the repository at this point in the history
  • Loading branch information
ZdenekM committed Dec 12, 2024
1 parent 2b2f7bd commit 0cf849b
Showing 13 changed files with 486 additions and 55 deletions.
17 changes: 3 additions & 14 deletions compose-files/ur-demo/docker-compose.yml
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
services:
ur-demo-robot-api:
image: arcor2/arcor2_ur:1.4.1
image: arcor2/arcor2_ur:1.5.0
container_name: ur-demo-robot-api
ports:
- "5012:5012"
@@ -26,8 +26,6 @@ services:
condition: service_healthy
ur-demo-execution:
condition: service_started
ur-demo-scene:
condition: service_healthy
ur-demo-calibration:
condition: service_healthy
ports:
@@ -38,7 +36,7 @@ services:
environment:
- ARCOR2_PROJECT_SERVICE_URL=http://ur-demo-project:10000
- ARCOR2_ASSET_SERVICE_URL=http://ur-demo-asset:10040
- ARCOR2_SCENE_SERVICE_URL=http://ur-demo-scene:5013
- ARCOR2_SCENE_SERVICE_URL=http://ur-demo-robot-api:5012
- ARCOR2_EXECUTION_URL=ws://ur-demo-execution:6790
- ARCOR2_BUILD_URL=http://ur-demo-build:5008
- ARCOR2_CALIBRATION_URL=http://ur-demo-calibration:5014
@@ -63,7 +61,7 @@ services:
networks:
- ur-demo-network
environment:
- ARCOR2_SCENE_SERVICE_URL=http://ur-demo-scene:5013
- ARCOR2_SCENE_SERVICE_URL=http://ur-demo-robot-api:5012
- ARCOR2_PROJECT_PATH=/root/project
volumes:
- ur-demo-execution:/root/project
@@ -97,15 +95,6 @@ services:
- ARCOR2_CALIBRATION_MOCK=false
volumes:
- ./calibration.yaml:/root/calibration.yaml

ur-demo-scene:
image: arcor2/arcor2_scene:1.1.0
container_name: ur-demo-scene
networks:
- ur-demo-network
ports:
- "5013:5013"

ur-demo-asset:
image: registry.gitlab.com/kinalisoft/test-it-off/asset:2.0.2
container_name: "ur-demo-asset"
6 changes: 6 additions & 0 deletions mypy.ini
Original file line number Diff line number Diff line change
@@ -141,4 +141,10 @@ ignore_missing_imports = True
ignore_missing_imports = True

[mypy-std_msgs.*]
ignore_missing_imports = True

[mypy-moveit_msgs.*]
ignore_missing_imports = True

[mypy-shape_msgs.*]
ignore_missing_imports = True
2 changes: 1 addition & 1 deletion src/docker/arcor2_ur/BUILD
Original file line number Diff line number Diff line change
@@ -1,2 +1,2 @@
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.4.1"])
docker_image(name="arcor2_ur", repository="arcor2/arcor2_ur", dependencies=[":start.sh", "build-support:install_ur_dependencies.sh"], image_tags=["1.5.0"])
2 changes: 1 addition & 1 deletion src/docker/arcor2_ur_ot/BUILD
Original file line number Diff line number Diff line change
@@ -1 +1 @@
docker_image(name="arcor2_ur_ot", repository="arcor2/arcor2_ur_ot", image_tags=["1.4.1"])
docker_image(name="arcor2_ur_ot", repository="arcor2/arcor2_ur_ot", image_tags=["1.5.0"])
8 changes: 6 additions & 2 deletions src/python/arcor2_arserver/tests/testutils.py
Original file line number Diff line number Diff line change
@@ -32,8 +32,12 @@ def log_proc_output(out: tuple[bytes, bytes]) -> None:

def finish_processes(processes) -> None:
for proc in processes:
proc.terminate()
proc.wait()
if proc.poll() is None:
proc.terminate()
try:
proc.wait(timeout=5)
except sp.TimeoutExpired:
proc.kill()
log_proc_output(proc.communicate())


7 changes: 7 additions & 0 deletions src/python/arcor2_ur/CHANGELOG.md
Original file line number Diff line number Diff line change
@@ -2,6 +2,13 @@

The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/),

## [1.5.0] - 2024-12-12

### Changed

- Checking for robot's state disabled as it was not working reliable.
- Added Scene functions - ability to manage collision objects. Please note that so far, only boxes are considered. Other types can be added, but will be ignored.

## [1.4.1] - 2024-11-21

### Fixed
4 changes: 3 additions & 1 deletion src/python/arcor2_ur/README.md
Original file line number Diff line number Diff line change
@@ -2,7 +2,9 @@

The service communicates over ROS 2 and MoveItPy with Universal Robots and provides simple REST API to control the robot and allow integration into ARCOR2.

The service is tested with Ubuntu 24.04, ROS 2 Jazzy and the UR5e robot - however, it should be possible to use it with any robot supported by the [ROS 2 driver](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/tree/main). It expects that the `ur_control.launch.py` is already running (which is handled in the [docker image](https://github.com/robofit/arcor2/blob/master/src/docker/arcor2_ur/start.sh)).
It is tested with Ubuntu 24.04, ROS 2 Jazzy and the UR5e robot - however, it should be possible to use it with any robot supported by the [ROS 2 driver](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/tree/main). It expects that the `ur_control.launch.py` is already running (which is handled in the [docker image](https://github.com/robofit/arcor2/blob/master/src/docker/arcor2_ur/start.sh)).

This service also offers API for managing collision objects. The API is compatible with `arcor2_scene`, meaning that the standard client `arcor2.clients.scene_service` can be used, and it is also fully compatible with ARServer. Typically, in ARCOR2 architecture, this would be done by a separate service. This solution was chosen for simplicity. In the future, there might be something like `arcor2_scene_ros2`, which would enable managing collision objects for multiple robots (which could be just a proxy for forwarding collision object requests to individual ROS-based robots, if they will use separate ROS domains).

## Setup

2 changes: 1 addition & 1 deletion src/python/arcor2_ur/VERSION
Original file line number Diff line number Diff line change
@@ -1 +1 @@
1.4.1
1.5.0
4 changes: 4 additions & 0 deletions src/python/arcor2_ur/exceptions.py
Original file line number Diff line number Diff line change
@@ -10,6 +10,10 @@ class UrGeneral(UrException):
description = General.description


class UrCollisions(UrException):
description = "Something regarding collision objects went wrong"


class NotFound(UrException):
description = "Occurs when something is not found"

6 changes: 4 additions & 2 deletions src/python/arcor2_ur/object_types/ur5e.py
Original file line number Diff line number Diff line change
@@ -75,12 +75,13 @@ def get_end_effector_pose(self, end_effector_id: str) -> Pose:
def move_to_pose(
self, end_effector_id: str, target_pose: Pose, speed: float, safe: bool = True, linear: bool = True
) -> None:
self.move(target_pose, speed * 100)
self.move(target_pose, speed * 100, safe)

def move(
self,
pose: Pose,
speed: float = 50.0,
safe: bool = True,
payload: float = 0.0,
*,
an: None | str = None,
@@ -89,6 +90,7 @@ def move(
:param pose: Target pose.
:param speed: Relative speed.
:param safe: Avoid collisions.
:param payload: Object weight.
:return:
@@ -102,7 +104,7 @@ def move(
rest.Method.PUT,
f"{self.settings.url}/eef/pose",
body=pose,
params={"velocity": speed, "payload": payload},
params={"velocity": speed, "payload": payload, "safe": safe},
)

def suck(
450 changes: 419 additions & 31 deletions src/python/arcor2_ur/scripts/ur.py

Large diffs are not rendered by default.

3 changes: 2 additions & 1 deletion src/python/arcor2_ur/tests/conftest.py
Original file line number Diff line number Diff line change
@@ -34,7 +34,7 @@ def start_processes(request) -> Iterator[Urls]:
pypath = ":".join(sys.path)
my_env["PYTHONPATH"] = pypath

kwargs = {"env": my_env, "stdout": sp.PIPE, "stderr": sp.STDOUT}
kwargs = {"env": my_env, "stdout": sp.PIPE, "stderr": sp.STDOUT, "start_new_session": True}

processes.append(
sp.Popen( # type: ignore
@@ -61,6 +61,7 @@ def start_processes(request) -> Iterator[Urls]:
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

30 changes: 29 additions & 1 deletion src/python/arcor2_ur/tests/test_interaction.py
Original file line number Diff line number Diff line change
@@ -1,12 +1,21 @@
import pytest

from arcor2.data.common import Pose
from arcor2.clients import scene_service
from arcor2.data.common import Orientation, Pose, Position
from arcor2.data.object_type import Box
from arcor2.exceptions import Arcor2Exception
from arcor2_ur.object_types.ur5e import Ur5e, UrSettings
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)
scene_service.upsert_collision(box, Pose(Position(1, 0, 0), Orientation(0, 0, 0, 1)))
scene_service.start()
assert scene_service.started()

ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url))

assert len(ot.robot_joints()) == 6
@@ -17,4 +26,23 @@ def test_basics(start_processes: Urls) -> None:
ot.suck()
ot.release()

assert scene_service.collision_ids() == {box.id}

scene_service.upsert_collision(box, pos)
pos.position.z += 0.01
with pytest.raises(Arcor2Exception): # attempt to move into a collision object
ot.move_to_pose("", pos, 0.5)

# now without collision checking
ot.move_to_pose("", pos, 0.5, safe=False)

pos.position.z -= 0.01
with pytest.raises(Arcor2Exception): # start state in collision
ot.move_to_pose("", pos, 0.5)

scene_service.delete_all_collisions()
assert not scene_service.collision_ids()

ot.move_to_pose("", pos, 0.5)

ot.cleanup()

0 comments on commit 0cf849b

Please sign in to comment.