From 9a2d1fd2b82b0d15ca45b920d5929558e78b6084 Mon Sep 17 00:00:00 2001 From: ZdenekM Date: Fri, 13 Sep 2024 14:57:02 +0200 Subject: [PATCH] feat(arcor2_ur): trajectory speed, payload mass --- compose-files/ur-demo/docker-compose.yml | 11 ++- mypy.ini | 5 +- src/docker/arcor2_ur/BUILD | 2 +- src/docker/arcor2_ur_ot/BUILD | 2 +- src/python/arcor2_ur/CHANGELOG.md | 6 ++ src/python/arcor2_ur/VERSION | 2 +- src/python/arcor2_ur/object_types/ur5e.py | 12 ++- src/python/arcor2_ur/scripts/ur.py | 80 ++++++++++++++++--- .../arcor2_ur/tests/test_interaction.py | 4 +- 9 files changed, 104 insertions(+), 20 deletions(-) diff --git a/compose-files/ur-demo/docker-compose.yml b/compose-files/ur-demo/docker-compose.yml index 95f6d93ff..1cdb985f6 100644 --- a/compose-files/ur-demo/docker-compose.yml +++ b/compose-files/ur-demo/docker-compose.yml @@ -13,13 +13,20 @@ services: - ursim-gui-cache:/ursim/GUI - urcap-build-cache:/ursim/.urcaps ur-demo-robot-api: - image: arcor2/arcor2_ur:1.0.0 + image: arcor2/arcor2_ur:1.1.0 container_name: ur-demo-robot-api ports: - "5012:5012" networks: - ur-demo-network restart: always # the service tends to crash when calling PUT /state/stop + cap_add: + - SYS_NICE + ulimits: + rtprio: 99 + rttime: -1 # corresponds to 'unlimited' + memlock: 8428281856 + # network_mode: host ur-demo-arserver: image: arcor2/arcor2_arserver:1.3.0 container_name: ur-demo-arserver @@ -152,7 +159,7 @@ services: - ur-demo-asset ur-demo-upload-object-types: - image: arcor2/arcor2_ur_ot:1.0.0 + image: arcor2/arcor2_ur_ot:1.1.0 container_name: "ur-demo-upload-object-types" depends_on: ur-demo-project: diff --git a/mypy.ini b/mypy.ini index 06a49bbc2..e7644f022 100644 --- a/mypy.ini +++ b/mypy.ini @@ -135,4 +135,7 @@ ignore_missing_imports = True ignore_missing_imports = True [mypy-ur_dashboard_msgs.*] -ignore_missing_imports = True \ No newline at end of file +ignore_missing_imports = True + +[mypy-ur_msgs.*] +ignore_missing_imports = True diff --git a/src/docker/arcor2_ur/BUILD b/src/docker/arcor2_ur/BUILD index 6114ba0e3..d387e534f 100644 --- a/src/docker/arcor2_ur/BUILD +++ b/src/docker/arcor2_ur/BUILD @@ -1,2 +1,2 @@ shell_source(name="start.sh", source="start.sh") -docker_image(name="arcor2_ur", repository="arcor2/arcor2_ur", dependencies=[":start.sh"], image_tags=["1.0.0"]) +docker_image(name="arcor2_ur", repository="arcor2/arcor2_ur", dependencies=[":start.sh"], image_tags=["1.1.0"]) diff --git a/src/docker/arcor2_ur_ot/BUILD b/src/docker/arcor2_ur_ot/BUILD index 1f0c5849f..445976bd7 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.0.0"]) +docker_image(name="arcor2_ur_ot", repository="arcor2/arcor2_ur_ot", image_tags=["1.1.0"]) diff --git a/src/python/arcor2_ur/CHANGELOG.md b/src/python/arcor2_ur/CHANGELOG.md index 7d625da73..91214a474 100644 --- a/src/python/arcor2_ur/CHANGELOG.md +++ b/src/python/arcor2_ur/CHANGELOG.md @@ -2,6 +2,12 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/), +## [1.1.0] - 2024-09-23 + +### Changed +- Removed waiting after trajectory execution (probably useless). +- Ability to set movement velocity and payload (mass only). + ## [1.0.0] - 2024-09-12 ### Changed diff --git a/src/python/arcor2_ur/VERSION b/src/python/arcor2_ur/VERSION index afaf360d3..1cc5f657e 100644 --- a/src/python/arcor2_ur/VERSION +++ b/src/python/arcor2_ur/VERSION @@ -1 +1 @@ -1.0.0 \ No newline at end of file +1.1.0 \ No newline at end of file diff --git a/src/python/arcor2_ur/object_types/ur5e.py b/src/python/arcor2_ur/object_types/ur5e.py index cb233677e..bfc4bd50e 100644 --- a/src/python/arcor2_ur/object_types/ur5e.py +++ b/src/python/arcor2_ur/object_types/ur5e.py @@ -24,7 +24,10 @@ def _started(self) -> bool: return rest.call(rest.Method.GET, f"{self.settings.url}/state/started", return_type=bool) def _stop(self) -> None: - rest.call(rest.Method.PUT, f"{self.settings.url}/state/stop") + try: # TODO the service crashes on stop (and is auto-restarted after that), so let's tolerate errors here + rest.call(rest.Method.PUT, f"{self.settings.url}/state/stop") + except rest.RestException: + pass def _start(self) -> None: if self._started(): @@ -54,11 +57,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) + self.move(target_pose, speed * 100) def move( self, pose: Pose, + speed: float, + payload: float = 0.0, *, an: None | str = None, ) -> None: @@ -68,11 +73,14 @@ def move( :return: """ + assert 0.0 <= payload <= 5.0 + with self._move_lock: rest.call( rest.Method.PUT, f"{self.settings.url}/eef/pose", body=pose, + params={"velocity": speed, "payload": payload}, ) def robot_joints(self, include_gripper: bool = False) -> list[Joint]: diff --git a/src/python/arcor2_ur/scripts/ur.py b/src/python/arcor2_ur/scripts/ur.py index b734fc1b1..a40e6248a 100644 --- a/src/python/arcor2_ur/scripts/ur.py +++ b/src/python/arcor2_ur/scripts/ur.py @@ -22,6 +22,7 @@ from tf2_ros.buffer import Buffer # pants: no-infer-dep from tf2_ros.transform_listener import TransformListener # pants: no-infer-dep from ur_dashboard_msgs.srv import Load # pants: no-infer-dep +from ur_msgs.srv import SetPayload, SetSpeedSliderFraction # pants: no-infer-dep from arcor2 import env from arcor2 import transformations as tr @@ -50,6 +51,9 @@ LOAD_PROGRAM_SRV = "/dashboard_client/load_program" PLAY_SRV = "/dashboard_client/play" +SET_SPEED_SLIDER_SRV = "/io_and_status_controller/set_speed_slider" +SET_PAYLOAD_SRV = "/io_and_status_controller/set_payload" + def plan_and_execute( robot, @@ -106,6 +110,14 @@ def __init__(self, interact_with_dashboard=True) -> None: while self.interact_with_dashboard and not self._play_client.wait_for_service(timeout_sec=1.0): logger.warning(f"Service {PLAY_SRV} not available, waiting again...") + self._set_speed_slider_client = self.create_client(SetSpeedSliderFraction, SET_SPEED_SLIDER_SRV) + while not self._set_speed_slider_client.wait_for_service(timeout_sec=1.0): + logger.warning(f"Service {SET_SPEED_SLIDER_SRV} not available, waiting again...") + + self._set_payload_client = self.create_client(SetPayload, SET_PAYLOAD_SRV) + while not self._set_payload_client.wait_for_service(timeout_sec=1.0): + logger.warning(f"Service {SET_PAYLOAD_SRV} not available, waiting again...") + def brake_release(self) -> None: if not self.interact_with_dashboard: return @@ -114,11 +126,11 @@ def brake_release(self) -> None: rclpy.spin_until_future_complete(self, future, timeout_sec=2) if future.result() is None: - raise Exception("Service call failed!") + raise UrGeneral("Service call failed!") response = future.result() if not response.success: - raise Exception(f"Service call failed with message: {response.message}") + raise UrGeneral(f"Service call failed with message: {response.message}") def power_off(self) -> None: if not self.interact_with_dashboard: @@ -128,11 +140,11 @@ def power_off(self) -> None: rclpy.spin_until_future_complete(self, future, timeout_sec=2) if future.result() is None: - raise Exception("Service call failed!") + raise UrGeneral("Service call failed!") response = future.result() if not response.success: - raise Exception(f"Service call failed with message: {response.message}") + raise UrGeneral(f"Service call failed with message: {response.message}") def load_program(self) -> None: if not self.interact_with_dashboard: @@ -142,11 +154,11 @@ def load_program(self) -> None: rclpy.spin_until_future_complete(self, future, timeout_sec=2) if future.result() is None: - raise Exception("Service call failed!") + raise UrGeneral("Service call failed!") response = future.result() if not response.success: - raise Exception(f"Service call failed with message: {response.answer}") + raise UrGeneral(f"Service call failed with message: {response.answer}") def play(self) -> None: if not self.interact_with_dashboard: @@ -156,11 +168,39 @@ def play(self) -> None: rclpy.spin_until_future_complete(self, future, timeout_sec=2) if future.result() is None: - raise Exception("Service call failed!") + raise UrGeneral("Service call failed!") + + response = future.result() + if not response.success: + raise UrGeneral(f"Service call failed with message: {response.message}") + + def set_speed_slider(self, value: float): + if value <= 0 or value > 1: + raise UrGeneral("Invalid speed.") + + future = self._set_speed_slider_client.call_async(SetSpeedSliderFraction.Request(speed_slider_fraction=value)) + rclpy.spin_until_future_complete(self, future, timeout_sec=2) + + if future.result() is None: + raise UrGeneral("Service call failed!") response = future.result() if not response.success: - raise Exception(f"Service call failed with message: {response.message}") + raise UrGeneral(f"Service call failed (speed: {value}).") + + def set_payload(self, value: float): + if value < 0: + raise UrGeneral("Invalid payload.") + + future = self._set_payload_client.call_async(SetPayload.Request(mass=value)) + rclpy.spin_until_future_complete(self, future, timeout_sec=2) + + if future.result() is None: + raise UrGeneral("Service call failed!") + + response = future.result() + if not response.success: + raise UrGeneral("Service call failed.") def listener_callback(self, msg: JointState) -> None: joints = [] @@ -523,6 +563,22 @@ def put_eef_pose() -> RespT: description: Set the EEF pose. tags: - Robot + parameters: + - name: velocity + in: query + schema: + type: number + format: float + minimum: 0 + maximum: 100 + parameters: + - name: payload + in: query + schema: + type: number + format: float + minimum: 0 + maximum: 5 requestBody: content: application/json: @@ -545,6 +601,9 @@ def put_eef_pose() -> RespT: raise UrGeneral("Body should be a JSON dict containing Pose.") pose = Pose.from_dict(request.json) + velocity = float(request.args.get("velocity", default=50.0)) / 100.0 + payload = float(request.args.get("payload", default=0.0)) + pose = tr.make_pose_rel(globs.state.pose, pose) pose_goal = PoseStamped() @@ -566,7 +625,10 @@ def put_eef_pose() -> RespT: globs.state.ur_manipulator.set_start_state_to_current_state() globs.state.ur_manipulator.set_goal_state(pose_stamped_msg=pose_goal, pose_link=TOOL_LINK) - plan_and_execute(globs.state.moveitpy, globs.state.ur_manipulator, logger, sleep_time=3.0) + globs.state.node.set_speed_slider(velocity) + globs.state.node.set_payload(payload) + + plan_and_execute(globs.state.moveitpy, globs.state.ur_manipulator, logger) return Response(status=204) diff --git a/src/python/arcor2_ur/tests/test_interaction.py b/src/python/arcor2_ur/tests/test_interaction.py index f6e90f985..767b55129 100644 --- a/src/python/arcor2_ur/tests/test_interaction.py +++ b/src/python/arcor2_ur/tests/test_interaction.py @@ -1,7 +1,6 @@ import pytest from arcor2.data.common import Pose -from arcor2.rest import RestException from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -15,5 +14,4 @@ def test_basics(start_processes: Urls) -> None: pos.position.z -= 0.01 ot.move_to_pose("", pos, 0.5) - with pytest.raises(RestException): # TODO stopping the service is broken ATM (Segmentation Fault) - ot.cleanup() + ot.cleanup()