From f44081af594ed903212c98885c3bd2380404220b Mon Sep 17 00:00:00 2001 From: ZdenekM Date: Thu, 8 Feb 2024 10:51:09 +0100 Subject: [PATCH] feat(arcor2_dobot): setting eef parameters --- README.md | 2 +- compose-files/fit-demo/docker-compose.lab.yml | 4 ++++ compose-files/fit-demo/docker-compose.yml | 6 +++--- src/docker/arcor2_dobot/BUILD | 2 +- src/python/arcor2_dobot/CHANGELOG.md | 6 ++++++ src/python/arcor2_dobot/README.md | 1 + src/python/arcor2_dobot/VERSION | 2 +- src/python/arcor2_dobot/dobot.py | 8 +++++++ src/python/arcor2_dobot/dobot_api.py | 21 +++++++++++++++++++ src/python/arcor2_dobot/magician.py | 18 +++------------- 10 files changed, 49 insertions(+), 21 deletions(-) diff --git a/README.md b/README.md index 181bdfd73..0ebf7b319 100644 --- a/README.md +++ b/README.md @@ -78,7 +78,7 @@ The following video by [Kinali](https://www.kinali.cz/en/) shows the use case (o [README](src/python/arcor2_dobot/README.md) | [CHANGELOG](src/python/arcor2_dobot/CHANGELOG.md) - - 2023-02-14: [1.0.0](https://github.com/robofit/arcor2/releases/tag/arcor2_dobot%2F1.0.0) ([docker](https://hub.docker.com/r/arcor2/arcor2_dobot/tags?page=1&ordering=last_updated&name=1.0.0), [pypi](https://pypi.org/project/arcor2-dobot/1.0.0/)). + - 2024-02-08: [1.1.0](https://github.com/robofit/arcor2/releases/tag/arcor2_dobot%2F1.1.0) ([docker](https://hub.docker.com/r/arcor2/arcor2_dobot/tags?page=1&ordering=last_updated&name=1.1.0), [pypi](https://pypi.org/project/arcor2-dobot/1.1.0/)). ### arcor2_execution diff --git a/compose-files/fit-demo/docker-compose.lab.yml b/compose-files/fit-demo/docker-compose.lab.yml index 3ed5d2954..141d9d887 100644 --- a/compose-files/fit-demo/docker-compose.lab.yml +++ b/compose-files/fit-demo/docker-compose.lab.yml @@ -4,11 +4,15 @@ services: fit-demo-dobot-magician: environment: - ARCOR2_DOBOT_MOCK=false + - ARCOR2_DOBOT_BIAS_X=0.06 + - ARCOR2_DOBOT_BIAS_Z=-0.06 devices: - /dev/dobotMagician:/dev/dobot fit-demo-dobot-magician2: environment: - ARCOR2_DOBOT_MOCK=false + - ARCOR2_DOBOT_BIAS_X=0.06 + - ARCOR2_DOBOT_BIAS_Z=-0.06 devices: - /dev/dobotMagician2:/dev/dobot diff --git a/compose-files/fit-demo/docker-compose.yml b/compose-files/fit-demo/docker-compose.yml index 304d792e3..a4202c441 100644 --- a/compose-files/fit-demo/docker-compose.yml +++ b/compose-files/fit-demo/docker-compose.yml @@ -98,7 +98,7 @@ services: - ./calibration.yaml:/root/calibration.yaml fit-demo-dobot-magician: - image: arcor2/arcor2_dobot:1.0.0 + image: arcor2/arcor2_dobot:1.1.0 container_name: fit-demo-dobot-magician depends_on: fit-demo-scene: @@ -114,7 +114,7 @@ services: - ARCOR2_DOBOT_MODEL=magician fit-demo-dobot-magician2: - image: arcor2/arcor2_dobot:1.0.0 + image: arcor2/arcor2_dobot:1.1.0 container_name: fit-demo-dobot-magician2 depends_on: fit-demo-scene: @@ -130,7 +130,7 @@ services: - ARCOR2_DOBOT_MODEL=magician fit-demo-dobot-m1: - image: arcor2/arcor2_dobot:1.0.0 + image: arcor2/arcor2_dobot:1.1.0 container_name: fit-demo-dobot-m1 depends_on: fit-demo-scene: diff --git a/src/docker/arcor2_dobot/BUILD b/src/docker/arcor2_dobot/BUILD index 83a42a29e..3cd55d41a 100644 --- a/src/docker/arcor2_dobot/BUILD +++ b/src/docker/arcor2_dobot/BUILD @@ -1 +1 @@ -docker_image(name="arcor2_dobot", repository="arcor2/arcor2_dobot", image_tags=["1.0.0"]) +docker_image(name="arcor2_dobot", repository="arcor2/arcor2_dobot", image_tags=["1.1.0"]) diff --git a/src/python/arcor2_dobot/CHANGELOG.md b/src/python/arcor2_dobot/CHANGELOG.md index 3b63e3714..adfc7ad88 100644 --- a/src/python/arcor2_dobot/CHANGELOG.md +++ b/src/python/arcor2_dobot/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-02-08 + +### Changed + +- Added `ARCOR2_DOBOT_BIAS_X`, `ARCOR2_DOBOT_BIAS_Y`, `ARCOR2_DOBOT_BIAS_Z` env. variables to set EEF parameters - offset, which is different for different tools. + ## [1.0.0] - 2023-02-14 ### Changed diff --git a/src/python/arcor2_dobot/README.md b/src/python/arcor2_dobot/README.md index 44ac270db..0ba94b5e2 100644 --- a/src/python/arcor2_dobot/README.md +++ b/src/python/arcor2_dobot/README.md @@ -4,6 +4,7 @@ - `ARCOR2_DOBOT_URL=http://0.0.0.0:5018` - by default, the service listens on port 5018. - `ARCOR2_DOBOT_MODEL=magician` - can be set to `magician` or `m1`. +- `ARCOR2_DOBOT_BIAS_X` (as well as `_Y` and `_Z`) - sets EEF parameters (offset), default value is 0. - `ARCOR2_DOBOT_MOCK=1` - the service will start in a mock (simulator) mode. - Please note that the mock mode is fully functional only for `magician`. The `m1` does not have IK/FK implemented yet. `ARCOR2_DOBOT_DEBUG=1` - turns on debug logging. \ No newline at end of file diff --git a/src/python/arcor2_dobot/VERSION b/src/python/arcor2_dobot/VERSION index afaf360d3..1cc5f657e 100644 --- a/src/python/arcor2_dobot/VERSION +++ b/src/python/arcor2_dobot/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_dobot/dobot.py b/src/python/arcor2_dobot/dobot.py index 99f03c783..908c5423e 100644 --- a/src/python/arcor2_dobot/dobot.py +++ b/src/python/arcor2_dobot/dobot.py @@ -6,6 +6,7 @@ import arcor2.transformations as tr from arcor2.data.common import Joint, Orientation, Pose, StrEnum +from arcor2.env import get_float from arcor2.exceptions import Arcor2NotImplemented from arcor2.helpers import NonBlockingLock from arcor2.object_types.abstract import RobotException @@ -14,6 +15,11 @@ # TODO jogging +BIAS_X = get_float("ARCOR2_DOBOT_BIAS_X", 0.0) +BIAS_Y = get_float("ARCOR2_DOBOT_BIAS_Y", 0.0) +BIAS_Z = get_float("ARCOR2_DOBOT_BIAS_Z", 0.0) + + class DobotException(RobotException): pass @@ -46,6 +52,8 @@ def __init__(self, pose: Pose, port: str = "/dev/dobot", simulator: bool = False except DobotApiException as e: raise DobotApiException("Could not connect to the robot.") from e + self._dobot.set_end_effector_params(BIAS_X, BIAS_Y, BIAS_Z) + else: self._joint_values: list[Joint] = [] # has to be set to some initial value in derived classes self._hand_teaching = False diff --git a/src/python/arcor2_dobot/dobot_api.py b/src/python/arcor2_dobot/dobot_api.py index fbdae9a52..2226d0fea 100644 --- a/src/python/arcor2_dobot/dobot_api.py +++ b/src/python/arcor2_dobot/dobot_api.py @@ -762,6 +762,27 @@ def _set_cple_cmd(self, x: float, y: float, z: float, power: float, absolute: bo msg.params.extend(bytearray(struct.pack("f", power))) return self._send_command(msg) + def get_end_effector_params(self) -> tuple[float, float, float]: + msg = Message() + msg.id = 60 + response = self._send_command(msg) + + bias_x = struct.unpack_from("f", response.params, 0)[0] + bias_y = struct.unpack_from("f", response.params, 4)[0] + bias_z = struct.unpack_from("f", response.params, 8)[0] + + return bias_x, bias_y, bias_z + + def set_end_effector_params(self, bias_x: float, bias_y: float, bias_z: float) -> None: + msg = Message() + msg.id = 60 + msg.ctrl = 0x3 + msg.params = bytearray([]) + msg.params.extend(bytearray(struct.pack("f", bias_x))) + msg.params.extend(bytearray(struct.pack("f", bias_y))) + msg.params.extend(bytearray(struct.pack("f", bias_z))) + self._send_command(msg) + def engrave( self, image: np.ndarray, diff --git a/src/python/arcor2_dobot/magician.py b/src/python/arcor2_dobot/magician.py index 026487300..58f1599bb 100644 --- a/src/python/arcor2_dobot/magician.py +++ b/src/python/arcor2_dobot/magician.py @@ -4,7 +4,7 @@ import arcor2.transformations as tr from arcor2.data.common import Joint, Orientation, Pose, Position, StrEnum -from arcor2_dobot.dobot import Dobot, DobotException +from arcor2_dobot.dobot import BIAS_X, BIAS_Z, Dobot, DobotException from arcor2_dobot.dobot_api import DobotApiException @@ -20,8 +20,8 @@ class DobotMagician(Dobot): # Dimensions in meters (according to URDF) link_2_length = 0.135 link_3_length = 0.147 - link_4_length = 0.06 - end_effector_length = 0.06 + link_4_length = BIAS_X + end_effector_length = -BIAS_Z def __init__(self, pose: Pose, port: str = "/dev/dobot", simulator: bool = False) -> None: super(DobotMagician, self).__init__(pose, port, simulator) @@ -35,18 +35,6 @@ def __init__(self, pose: Pose, port: str = "/dev/dobot", simulator: bool = False Joint(Joints.J5, -0.568), ] - def _handle_pose_in(self, pose: Pose) -> None: - base_angle = math.atan2(pose.position.y, pose.position.x) - pose.position.x -= self.link_4_length * math.cos(base_angle) - pose.position.y -= self.link_4_length * math.sin(base_angle) - pose.position.z += self.end_effector_length - - def _handle_pose_out(self, pose: Pose) -> None: - base_angle = math.atan2(pose.position.y, pose.position.x) - pose.position.x += self.link_4_length * math.cos(base_angle) - pose.position.y += self.link_4_length * math.sin(base_angle) - pose.position.z -= self.end_effector_length - # TODO joint4/5 valid_ranges: dict[Joints, tuple[float, float]] = { Joints.J1: (-2, 2),