From a13589de7fefeaa0b6757f01f6940b777fde7400 Mon Sep 17 00:00:00 2001 From: Fabien Danieau Date: Mon, 7 Oct 2024 17:12:22 +0200 Subject: [PATCH 1/6] bug #399: do not wait for a movement not sent --- src/reachy2_sdk/parts/arm.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/reachy2_sdk/parts/arm.py b/src/reachy2_sdk/parts/arm.py index fbfe7da7..49e667cd 100644 --- a/src/reachy2_sdk/parts/arm.py +++ b/src/reachy2_sdk/parts/arm.py @@ -368,7 +368,7 @@ def goto_from_matrix( response = self._goto_stub.GoToCartesian(request) if response.id == -1: self._logger.error(f"Target pose:\n {target} \nwas not reachable. No command sent.") - if wait: + elif wait: self._logger.info(f"Waiting for movement with {response}.") while not self._is_goto_finished(response): time.sleep(0.1) @@ -591,7 +591,7 @@ def goto_joints( response = self._goto_stub.GoToJoints(request) if response.id == -1: self._logger.error(f"Position {positions} was not reachable. No command sent.") - if wait: + elif wait: self._logger.info(f"Waiting for movement with {response}.") while not self._is_goto_finished(response): time.sleep(0.1) @@ -763,6 +763,7 @@ def _goto_single_joint( interpolation_mode=get_grpc_interpolation_mode(interpolation_mode), ) response = self._goto_stub.GoToJoints(request) + if wait: self._logger.info(f"Waiting for movement with {response}.") while not self._is_goto_finished(response): From 351fa722a8c67a237e01c918c56a7a538c2509c7 Mon Sep 17 00:00:00 2001 From: Fabien Danieau Date: Tue, 8 Oct 2024 09:17:18 +0200 Subject: [PATCH 2/6] bug #399: wait only when reponse !=-1 for all goto --- src/reachy2_sdk/parts/arm.py | 4 +++- src/reachy2_sdk/parts/head.py | 16 ++++++++++++---- 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/src/reachy2_sdk/parts/arm.py b/src/reachy2_sdk/parts/arm.py index 49e667cd..3059df60 100644 --- a/src/reachy2_sdk/parts/arm.py +++ b/src/reachy2_sdk/parts/arm.py @@ -764,7 +764,9 @@ def _goto_single_joint( ) response = self._goto_stub.GoToJoints(request) - if wait: + if response.id == -1: + self._logger.error(f"Position {goal_position} was not reachable. No command sent.") + elif wait: self._logger.info(f"Waiting for movement with {response}.") while not self._is_goto_finished(response): time.sleep(0.1) diff --git a/src/reachy2_sdk/parts/head.py b/src/reachy2_sdk/parts/head.py index 2bace818..21dda146 100644 --- a/src/reachy2_sdk/parts/head.py +++ b/src/reachy2_sdk/parts/head.py @@ -131,7 +131,9 @@ def look_at( interpolation_mode=get_grpc_interpolation_mode(interpolation_mode), ) response = self._goto_stub.GoToCartesian(request) - if wait: + if response.id == -1: + self._logger.error(f"Position {x}, {y}, {z} was not reachable. No command sent.") + elif wait: self._logger.info(f"Waiting for movement with {response}.") while not self._is_goto_finished(response): time.sleep(0.1) @@ -177,7 +179,9 @@ def goto_joints( interpolation_mode=get_grpc_interpolation_mode(interpolation_mode), ) response = self._goto_stub.GoToJoints(request) - if wait: + if response.id == -1: + self._logger.error(f"Position {positions} was not reachable. No command sent.") + elif wait: self._logger.info(f"Waiting for movement with {response}.") while not self._is_goto_finished(response): time.sleep(0.1) @@ -207,7 +211,9 @@ def _goto_single_joint( interpolation_mode=get_grpc_interpolation_mode(interpolation_mode), ) response = self._goto_stub.GoToJoints(request) - if wait: + if response.id == -1: + self._logger.error(f"Position {goal_position} was not reachable. No command sent.") + elif wait: self._logger.info(f"Waiting for movement with {response}.") while not self._is_goto_finished(response): time.sleep(0.1) @@ -235,7 +241,9 @@ def goto_quat( interpolation_mode=get_grpc_interpolation_mode(interpolation_mode), ) response = self._goto_stub.GoToJoints(request) - if wait: + if response.id == -1: + self._logger.error(f"Orientation {q} was not reachable. No command sent.") + elif wait: self._logger.info(f"Waiting for movement with {response}.") while not self._is_goto_finished(response): time.sleep(0.1) From 83b3234a537ac076b4f18dfb0d02254815571f68 Mon Sep 17 00:00:00 2001 From: Fabien Danieau Date: Tue, 8 Oct 2024 09:52:34 +0200 Subject: [PATCH 3/6] bug #399: adding timeout to wait for goto and factorise function --- src/reachy2_sdk/parts/arm.py | 15 +++------------ src/reachy2_sdk/parts/goto_based_part.py | 14 ++++++++++++++ src/reachy2_sdk/parts/head.py | 21 ++++----------------- 3 files changed, 21 insertions(+), 29 deletions(-) diff --git a/src/reachy2_sdk/parts/arm.py b/src/reachy2_sdk/parts/arm.py index 3059df60..e4427d1c 100644 --- a/src/reachy2_sdk/parts/arm.py +++ b/src/reachy2_sdk/parts/arm.py @@ -369,10 +369,7 @@ def goto_from_matrix( if response.id == -1: self._logger.error(f"Target pose:\n {target} \nwas not reachable. No command sent.") elif wait: - self._logger.info(f"Waiting for movement with {response}.") - while not self._is_goto_finished(response): - time.sleep(0.1) - self._logger.info(f"Movement with {response} finished.") + self._wait_goto(response, duration + 1) return response def send_cartesian_interpolation( @@ -592,10 +589,7 @@ def goto_joints( if response.id == -1: self._logger.error(f"Position {positions} was not reachable. No command sent.") elif wait: - self._logger.info(f"Waiting for movement with {response}.") - while not self._is_goto_finished(response): - time.sleep(0.1) - self._logger.info(f"Movement with {response} finished.") + self._wait_goto(response, duration + 1) return response def get_translation_by( @@ -767,10 +761,7 @@ def _goto_single_joint( if response.id == -1: self._logger.error(f"Position {goal_position} was not reachable. No command sent.") elif wait: - self._logger.info(f"Waiting for movement with {response}.") - while not self._is_goto_finished(response): - time.sleep(0.1) - self._logger.info(f"Movement with {response} finished.") + self._wait_goto(response, duration + 1) return response def get_joints_positions(self, degrees: bool = True, round: Optional[int] = None) -> List[float]: diff --git a/src/reachy2_sdk/parts/goto_based_part.py b/src/reachy2_sdk/parts/goto_based_part.py index 3aea170d..dcdcdb3a 100644 --- a/src/reachy2_sdk/parts/goto_based_part.py +++ b/src/reachy2_sdk/parts/goto_based_part.py @@ -1,3 +1,5 @@ +import logging +import time from abc import ABC from typing import List, Optional @@ -27,6 +29,7 @@ def __init__( """Initialize the common attributes.""" self.part = part self._goto_stub = goto_stub + self._logger_goto = logging.getLogger(__name__) # avoid name conflict with class logger def get_goto_playing(self) -> GoToId: """Return the id of the goto currently playing on the part""" @@ -80,3 +83,14 @@ def _is_goto_finished(self, id: GoToId) -> bool: or state.goal_status == GoalStatus.STATUS_SUCCEEDED ) return result + + def _wait_goto(self, id: GoToId, timeout: float = 10) -> None: + """Wait for a goto to finish. timeout is in seconds.""" + self._logger_goto.info(f"Waiting for movement with {id}.") + t1 = time.time() + while not self._is_goto_finished(id): + time.sleep(0.1) + if time.time() - t1 > timeout: + self._logger_goto.warning(f"Waiting time for movement with {id} is timeout.") + return + self._logger_goto.info(f"Movement with {id} finished.") diff --git a/src/reachy2_sdk/parts/head.py b/src/reachy2_sdk/parts/head.py index 21dda146..3ee9a074 100644 --- a/src/reachy2_sdk/parts/head.py +++ b/src/reachy2_sdk/parts/head.py @@ -5,7 +5,6 @@ - look_at function """ -import time from typing import List import grpc @@ -134,10 +133,7 @@ def look_at( if response.id == -1: self._logger.error(f"Position {x}, {y}, {z} was not reachable. No command sent.") elif wait: - self._logger.info(f"Waiting for movement with {response}.") - while not self._is_goto_finished(response): - time.sleep(0.1) - self._logger.info(f"Movement with {response} finished.") + self._wait_goto(response, duration + 1) return response def goto_joints( @@ -182,10 +178,7 @@ def goto_joints( if response.id == -1: self._logger.error(f"Position {positions} was not reachable. No command sent.") elif wait: - self._logger.info(f"Waiting for movement with {response}.") - while not self._is_goto_finished(response): - time.sleep(0.1) - self._logger.info(f"Movement with {response} finished.") + self._wait_goto(response, duration + 1) return response def _goto_single_joint( @@ -214,10 +207,7 @@ def _goto_single_joint( if response.id == -1: self._logger.error(f"Position {goal_position} was not reachable. No command sent.") elif wait: - self._logger.info(f"Waiting for movement with {response}.") - while not self._is_goto_finished(response): - time.sleep(0.1) - self._logger.info(f"Movement with {response} finished.") + self._wait_goto(response, duration + 1) return response def goto_quat( @@ -244,10 +234,7 @@ def goto_quat( if response.id == -1: self._logger.error(f"Orientation {q} was not reachable. No command sent.") elif wait: - self._logger.info(f"Waiting for movement with {response}.") - while not self._is_goto_finished(response): - time.sleep(0.1) - self._logger.info(f"Movement with {response} finished.") + self._wait_goto(response, duration + 1) return response def send_goal_positions(self) -> None: From 5645c580876a8d18c199f9231f5c2feb53847679 Mon Sep 17 00:00:00 2001 From: Fabien Danieau Date: Tue, 8 Oct 2024 11:55:45 +0200 Subject: [PATCH 4/6] bug #399: clearer comment --- src/reachy2_sdk/parts/goto_based_part.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/reachy2_sdk/parts/goto_based_part.py b/src/reachy2_sdk/parts/goto_based_part.py index dcdcdb3a..abe4ecd6 100644 --- a/src/reachy2_sdk/parts/goto_based_part.py +++ b/src/reachy2_sdk/parts/goto_based_part.py @@ -29,7 +29,7 @@ def __init__( """Initialize the common attributes.""" self.part = part self._goto_stub = goto_stub - self._logger_goto = logging.getLogger(__name__) # avoid name conflict with class logger + self._logger_goto = logging.getLogger(__name__) # not using self._logger to avoid name conflict in multiple inheritance def get_goto_playing(self) -> GoToId: """Return the id of the goto currently playing on the part""" From 4187ee65f8c9c5a19a69e7fa93d4fa9343d8bf22 Mon Sep 17 00:00:00 2001 From: Fabien Danieau Date: Tue, 8 Oct 2024 17:25:57 +0200 Subject: [PATCH 5/6] bug #399: _wait_go waits for previous goto. global timeout is hard coded --- src/reachy2_sdk/parts/goto_based_part.py | 16 ++++++++++++++-- .../units/online/test_advanced_goto_functions.py | 6 ++++++ 2 files changed, 20 insertions(+), 2 deletions(-) diff --git a/src/reachy2_sdk/parts/goto_based_part.py b/src/reachy2_sdk/parts/goto_based_part.py index abe4ecd6..494d5a38 100644 --- a/src/reachy2_sdk/parts/goto_based_part.py +++ b/src/reachy2_sdk/parts/goto_based_part.py @@ -87,10 +87,22 @@ def _is_goto_finished(self, id: GoToId) -> bool: def _wait_goto(self, id: GoToId, timeout: float = 10) -> None: """Wait for a goto to finish. timeout is in seconds.""" self._logger_goto.info(f"Waiting for movement with {id}.") - t1 = time.time() + + t_goto = None # timeout for this goto + t_all = time.time() # timeout for others while not self._is_goto_finished(id): time.sleep(0.1) - if time.time() - t1 > timeout: + + if t_goto is None: + if self.get_goto_playing() == id: + t_goto = time.time() + elif time.time() - t_all > 60: # ToDo: we need to know how long to wait for + self._logger_goto.warning( + f"Waiting time for movement with {id} is timeout. Previous movements are not finished." + ) + return + + elif time.time() - t_goto > timeout: self._logger_goto.warning(f"Waiting time for movement with {id} is timeout.") return self._logger_goto.info(f"Movement with {id} finished.") diff --git a/tests/units/online/test_advanced_goto_functions.py b/tests/units/online/test_advanced_goto_functions.py index 5a6d41e1..732f9076 100644 --- a/tests/units/online/test_advanced_goto_functions.py +++ b/tests/units/online/test_advanced_goto_functions.py @@ -483,6 +483,12 @@ def test_wait_move(reachy_sdk_zeroed: ReachySDK) -> None: elapsed_time = time.time() - tic assert elapsed_time < 0.1 + tic = time.time() + reachy_sdk_zeroed.goto_posture("default", duration=1.0) + reachy_sdk_zeroed.r_arm.goto_from_matrix(A, wait=True, duration=1.0) + elapsed_time = time.time() - tic + assert elapsed_time >= 2.0 + @pytest.mark.online def test_is_goto_finished(reachy_sdk_zeroed: ReachySDK) -> None: From db0100265462fa0c4d359ef8d20d76fa02db85eb Mon Sep 17 00:00:00 2001 From: Fabien Danieau Date: Wed, 9 Oct 2024 11:56:05 +0200 Subject: [PATCH 6/6] bug #399: compute the timeout from the list of goto on the part provided by the server --- src/reachy2_sdk/parts/arm.py | 6 ++--- src/reachy2_sdk/parts/goto_based_part.py | 30 ++++++++++++++---------- src/reachy2_sdk/parts/head.py | 8 +++---- 3 files changed, 24 insertions(+), 20 deletions(-) diff --git a/src/reachy2_sdk/parts/arm.py b/src/reachy2_sdk/parts/arm.py index e4427d1c..3f7f0cf4 100644 --- a/src/reachy2_sdk/parts/arm.py +++ b/src/reachy2_sdk/parts/arm.py @@ -369,7 +369,7 @@ def goto_from_matrix( if response.id == -1: self._logger.error(f"Target pose:\n {target} \nwas not reachable. No command sent.") elif wait: - self._wait_goto(response, duration + 1) + self._wait_goto(response) return response def send_cartesian_interpolation( @@ -589,7 +589,7 @@ def goto_joints( if response.id == -1: self._logger.error(f"Position {positions} was not reachable. No command sent.") elif wait: - self._wait_goto(response, duration + 1) + self._wait_goto(response) return response def get_translation_by( @@ -761,7 +761,7 @@ def _goto_single_joint( if response.id == -1: self._logger.error(f"Position {goal_position} was not reachable. No command sent.") elif wait: - self._wait_goto(response, duration + 1) + self._wait_goto(response) return response def get_joints_positions(self, degrees: bool = True, round: Optional[int] = None) -> List[float]: diff --git a/src/reachy2_sdk/parts/goto_based_part.py b/src/reachy2_sdk/parts/goto_based_part.py index 494d5a38..026a78dc 100644 --- a/src/reachy2_sdk/parts/goto_based_part.py +++ b/src/reachy2_sdk/parts/goto_based_part.py @@ -84,25 +84,29 @@ def _is_goto_finished(self, id: GoToId) -> bool: ) return result - def _wait_goto(self, id: GoToId, timeout: float = 10) -> None: + def _wait_goto(self, id: GoToId) -> None: """Wait for a goto to finish. timeout is in seconds.""" self._logger_goto.info(f"Waiting for movement with {id}.") - t_goto = None # timeout for this goto - t_all = time.time() # timeout for others + id_playing = self.get_goto_playing() + info_gotos = [self._get_goto_joints_request(id_playing)] + ids_queue = self.get_goto_queue() + for id in ids_queue: + info_gotos.append(self._get_goto_joints_request(id)) + + timeout = 1 # adding one more sec + for igoto in info_gotos: + if igoto is not None: + timeout += igoto.duration + + self._logger_goto.debug(f"timeout is set to {timeout}") + + t_start = time.time() # timeout for others while not self._is_goto_finished(id): time.sleep(0.1) - if t_goto is None: - if self.get_goto_playing() == id: - t_goto = time.time() - elif time.time() - t_all > 60: # ToDo: we need to know how long to wait for - self._logger_goto.warning( - f"Waiting time for movement with {id} is timeout. Previous movements are not finished." - ) - return - - elif time.time() - t_goto > timeout: + if time.time() - t_start > timeout: self._logger_goto.warning(f"Waiting time for movement with {id} is timeout.") return + self._logger_goto.info(f"Movement with {id} finished.") diff --git a/src/reachy2_sdk/parts/head.py b/src/reachy2_sdk/parts/head.py index 3ee9a074..02be6c01 100644 --- a/src/reachy2_sdk/parts/head.py +++ b/src/reachy2_sdk/parts/head.py @@ -133,7 +133,7 @@ def look_at( if response.id == -1: self._logger.error(f"Position {x}, {y}, {z} was not reachable. No command sent.") elif wait: - self._wait_goto(response, duration + 1) + self._wait_goto(response) return response def goto_joints( @@ -178,7 +178,7 @@ def goto_joints( if response.id == -1: self._logger.error(f"Position {positions} was not reachable. No command sent.") elif wait: - self._wait_goto(response, duration + 1) + self._wait_goto(response) return response def _goto_single_joint( @@ -207,7 +207,7 @@ def _goto_single_joint( if response.id == -1: self._logger.error(f"Position {goal_position} was not reachable. No command sent.") elif wait: - self._wait_goto(response, duration + 1) + self._wait_goto(response) return response def goto_quat( @@ -234,7 +234,7 @@ def goto_quat( if response.id == -1: self._logger.error(f"Orientation {q} was not reachable. No command sent.") elif wait: - self._wait_goto(response, duration + 1) + self._wait_goto(response) return response def send_goal_positions(self) -> None: