From b22fa1d62ccfa5ae68f6fe9d0f90cfcda36e61d0 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Mon, 5 Aug 2024 14:11:07 +0200 Subject: [PATCH] Implement driving to absolute positions Using a linear motion profile should be sufficient for now. --- schunk_egu_egk_gripper_dummy/src/dummy.py | 64 +++++++++++++++++++ .../tests/test_dummy.py | 28 ++++++++ .../tests/test_plc_communication.py | 14 ++++ 3 files changed, 106 insertions(+) diff --git a/schunk_egu_egk_gripper_dummy/src/dummy.py b/schunk_egu_egk_gripper_dummy/src/dummy.py index c8401aa..2d7a045 100644 --- a/schunk_egu_egk_gripper_dummy/src/dummy.py +++ b/schunk_egu_egk_gripper_dummy/src/dummy.py @@ -4,6 +4,36 @@ from pathlib import Path import json import struct +from typing import Tuple + + +class LinearMotion(object): + def __init__( + self, + initial_pos: float, + initial_speed: float, + target_pos: float, + target_speed: float, + ): + self.min_speed = 0.001 + self.initial_pos = initial_pos + self.initial_speed = max(0.0, initial_speed) + self.target_pos = target_pos + self.target_speed = max(self.min_speed, target_speed) + self.time_finish = abs(self.target_pos / self.target_speed) + + def sample(self, t: float) -> Tuple[float, float]: + if t <= 0.0: + return (self.initial_pos, self.initial_speed) + if t >= self.time_finish: + return (self.target_pos, 0.0) + + v = self.target_speed + if self.target_pos < self.initial_pos: + v = -v + current_pos = v * t + self.initial_pos + current_speed = abs(v) + return (current_pos, current_speed) class Dummy(object): @@ -60,6 +90,22 @@ def _run(self) -> None: time.sleep(1) print("Done") + def move(self, target_pos: float, target_speed: float) -> None: + motion = LinearMotion( + initial_pos=self.get_actual_position(), + initial_speed=self.get_actual_speed(), + target_pos=target_pos, + target_speed=target_speed, + ) + start = time.time() + actual_pos, actual_speed = motion.sample(0) + while abs(actual_pos) < abs(target_pos): + t = time.time() - start + actual_pos, actual_speed = motion.sample(t) + self.set_actual_position(actual_pos) + self.set_actual_speed(actual_speed) + time.sleep(0.01) + def post(self, msg: dict) -> dict: if msg["inst"] == self.plc_output: self.plc_output_buffer = bytearray(bytes.fromhex(msg["value"])) @@ -191,6 +237,14 @@ def set_actual_position(self, position: float) -> None: def set_actual_speed(self, speed: float) -> None: self.data[self.actual_speed] = [bytes(struct.pack("f", speed)).hex().upper()] + def get_actual_position(self) -> float: + read_pos = self.data[self.actual_position][0] + return struct.unpack("f", bytes.fromhex(read_pos))[0] + + def get_actual_speed(self) -> float: + read_speed = self.data[self.actual_speed][0] + return struct.unpack("f", bytes.fromhex(read_speed))[0] + def process_control_bits(self) -> None: # Acknowledge @@ -199,3 +253,13 @@ def process_control_bits(self) -> None: self.set_status_bit(bit=7, value=False) self.set_status_error("00") self.set_status_diagnostics("00") + + # Move to absolute position + if self.get_control_bit(13) == 1: + self.toggle_status_bit(5) + self.move( + target_pos=self.get_target_position(), + target_speed=self.get_target_speed(), + ) + self.set_status_bit(bit=13, value=True) + self.set_status_bit(bit=4, value=True) diff --git a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py index ee6da02..b8a610a 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py @@ -1,4 +1,6 @@ from src.dummy import Dummy +import pytest +import struct # [1]: https://stb.cloud.schunk.com/media/IM0046706.PDF @@ -55,3 +57,29 @@ def test_dummy_is_ready_after_acknowledge(): assert dummy.get_status_bit(7) == 0 # no error assert dummy.get_status_error() == "0" assert dummy.get_status_diagnostics() == "0" + + +def test_dummy_moves_to_absolute_position(): + dummy = Dummy() + target_pos = 12.345 + target_speed = 50.3 + + control_double_word = "00200000" + set_position = bytes(struct.pack("f", target_pos)).hex().upper() + set_speed = bytes(struct.pack("f", target_speed)).hex().upper() + gripping_force = "00000000" + command = { + "inst": dummy.plc_output, + "value": control_double_word + set_position + set_speed + gripping_force, + } + before = dummy.get_status_bit(bit=5) # command received toggle + + # Motion + dummy.post(command) + + # Done + assert pytest.approx(dummy.get_actual_position()) == target_pos + after = dummy.get_status_bit(bit=5) + assert after != before + assert dummy.get_status_bit(bit=13) == 1 # position reached + assert dummy.get_status_bit(bit=4) == 1 # command successfully processed diff --git a/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py b/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py index 4a3bcdc..310692f 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py @@ -142,3 +142,17 @@ def test_dummy_supports_writing_actual_speed(): read_speed = dummy.data[dummy.actual_speed][0] read_speed = struct.unpack("f", bytes.fromhex(read_speed))[0] assert pytest.approx(read_speed) == speed + + +def test_dummy_supports_reading_actual_position(): + dummy = Dummy() + pos = 0.123 + dummy.set_actual_position(pos) + assert pytest.approx(dummy.get_actual_position()) == pos + + +def test_dummy_supports_reading_actual_speed(): + dummy = Dummy() + speed = 66.5 + dummy.set_actual_speed(speed) + assert pytest.approx(dummy.get_actual_speed()) == speed