feat(arcor2_ur): trajectory speed, payload mass

.github/workflows/pants.yaml

@@ -62,7 +62,7 @@ jobs:
         sudo apt install jq cargo
     - name: install ROS 2 dependencies
       run: |  # copied from arcor2_ur Dockerfile TODO move it into some bash script to have it in one place
-        sudo apt install ros-jazzy-ros-base=0.11.0-1noble.20240731.172749 ros-jazzy-ur=2.4.9-1noble.20240820.020145 ros-jazzy-moveit-py=2.10.0-1noble.20240731.183757
+        sudo apt install ros-jazzy-ros-base=0.11.0-1noble.20240916.013254 ros-jazzy-ur=2.4.10-1noble.20240916.021917 ros-jazzy-moveit-py=2.10.0-1noble.20240916.020150
     - name: Lint
       run: |
         pants --changed-since=origin/master lint

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:

mypy.ini

@@ -135,4 +135,7 @@ ignore_missing_imports = True
 ignore_missing_imports = True
 
 [mypy-ur_dashboard_msgs.*]
-ignore_missing_imports = True
+ignore_missing_imports = True
+
+[mypy-ur_msgs.*]
+ignore_missing_imports = True

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"])

src/docker/arcor2_ur/Dockerfile

@@ -38,9 +38,9 @@ RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/r
 
 
 RUN apt-get update && apt-get install -y -q --no-install-recommends \
-    ros-jazzy-ros-base=0.11.0-1noble.20240731.172749 \
-    ros-jazzy-ur=2.4.9-1noble.20240820.020145 \
-    ros-jazzy-moveit-py=2.10.0-1noble.20240731.183757 \
+    ros-jazzy-ros-base=0.11.0-1noble.20240916.013254 \
+    ros-jazzy-ur=2.4.10-1noble.20240916.021917 \
+    ros-jazzy-moveit-py=2.10.0-1noble.20240916.020150 \
     && apt-get clean \
     && rm -rf /var/lib/apt/lists/*
 RUN echo "source /opt/ros/jazzy/setup.bash" >> /etc/bash.bashrc

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"])

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

src/python/arcor2_ur/VERSION

@@ -1 +1 @@
-1.0.0
+1.1.0

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]:

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)
 

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()