feat(arcor2_dobot): setting eef parameters

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
 

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
 

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:

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

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

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.

src/python/arcor2_dobot/VERSION

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

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

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,

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