diff --git a/abb_egm_client/egm_client.py b/abb_egm_client/egm_client.py index 1dc36ff2f2901e8eaddf0530d041c43d5f5f8e55..e8a08786f2fb9a6095187437a8849c719965d0da 100644 --- a/abb_egm_client/egm_client.py +++ b/abb_egm_client/egm_client.py @@ -1,11 +1,13 @@ import socket import logging -from typing import Any, Sequence, Tuple +from typing import Any, Tuple from abb_egm_client.atomic_counter import AtomicCounter from abb_egm_client.egm_pb2 import EgmRobot, EgmSensor, EgmHeader +import numpy as np +import numpy.typing as npt log = logging.getLogger(__name__) @@ -109,7 +111,7 @@ class EGMClient: return msg - def send_planned_configuration(self, configuration: Sequence[float]) -> None: + def send_planned_configuration(self, configuration: npt.ArrayLike[float]) -> None: """Send target configuration to robot controller. configuration @@ -118,7 +120,8 @@ class EGMClient: """ msg = self._create_sensor_msg() - msg.planned.joints.joints.extend(configuration) + conf_as_list = np.asarray(configuration).tolist() + msg.planned.joints.joints.extend(conf_as_list) self.send_msg(msg) diff --git a/environment.yml b/environment.yml index b776b14b280d709ea9a0ad8ce7e3347d0d88ad6d..0d84b9528317e7016bbeeb2d99420dca02fe2800 100644 --- a/environment.yml +++ b/environment.yml @@ -4,3 +4,4 @@ channels: dependencies: - python >=3.7, <3.10 - protobuf + - numpy diff --git a/examples/send_configuration.py b/examples/send_configuration.py index 1feddc0f6b392c6d8e1dde713402a0a22745ea07..73a405f29c361bf062ef595cde09f79d461b5165 100644 --- a/examples/send_configuration.py +++ b/examples/send_configuration.py @@ -3,6 +3,8 @@ import sys from typing import Sequence import time +import numpy as np +import numpy.typing as npt try: from abb_egm_client import EGMClient @@ -28,13 +30,14 @@ def send_configuration(target_conf: Sequence[float], joint_vel=1.0) -> None: egm_client = EGMClient(port=UDP_PORT) pb_robot_msg = egm_client.receive_msg() - start_conf = pb_robot_msg.feedBack.joints.joints + start_conf: Sequence[float] = pb_robot_msg.feedBack.joints.joints - deltas = [ - target_pos - start_pos for start_pos, target_pos in zip(start_conf, target_conf) - ] + start_conf_arr = np.array(start_conf) + target_conf_arr = np.array(target_conf) - max_delta = max([abs(d) for d in deltas]) + deltas: npt.ArrayLike[float] = target_conf_arr - start_conf_arr + + max_delta: float = np.fabs(deltas).max() num_msgs = round(max_delta / joint_vel * rate) @@ -43,6 +46,8 @@ def send_configuration(target_conf: Sequence[float], joint_vel=1.0) -> None: cur_configuration: Sequence[float] = pb_robot_msg.feedBack.joints.joints print(f"Current configuration {cur_configuration}") + + new_conf = start_conf_arr conf = [] for start_pos, delta in zip(start_conf, deltas): diff --git a/pyproject.toml b/pyproject.toml index a8df0f8cc4d4a1403f17ee7f4a19d6723bec6aa1..4d7a504912c911d160496b53129b5fb75bda679f 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,9 +1,9 @@ [build-system] -requires = [ - "setuptools >= 40.9.0", - "wheel", -] +requires = ["setuptools >= 40.9.0", "wheel"] build-backend = "setuptools.build_meta" [tool.isort] profile = "black" + +[tool.mypy] +plugins = "numpy.typing.mypy_plugin"