Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

399 robot is stuck in infinite loop using translate by with wait=true #417

Merged
merged 7 commits into from
Oct 9, 2024
Merged

399 robot is stuck in infinite loop using translate by with wait=true #417

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
a13589d
bug #399: do not wait for a movement not sent
FabienDanieau Oct 7, 2024
351fa72
bug #399: wait only when reponse !=-1 for all goto
FabienDanieau Oct 8, 2024
83b3234
bug #399: adding timeout to wait for goto and factorise function
FabienDanieau Oct 8, 2024
edd0743
Merge branch 'develop' into 399-robot-is-stuck-in-infinite-loop-using…
FabienDanieau Oct 8, 2024
5645c58
bug #399: clearer comment
FabienDanieau Oct 8, 2024
4187ee6
bug #399: _wait_go waits for previous goto. global timeout is hard coded
FabienDanieau Oct 8, 2024
db01002
bug #399: compute the timeout from the list of goto on the part provi…
FabienDanieau Oct 9, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
24 changes: 9 additions & 15 deletions src/reachy2_sdk/parts/arm.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -368,11 +368,8 @@ 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:
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.")
elif wait:
self._wait_goto(response, duration + 1)
return response

def send_cartesian_interpolation(
Expand Down Expand Up @@ -591,11 +588,8 @@ 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:
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.")
elif wait:
self._wait_goto(response, duration + 1)
return response

def get_translation_by(
Expand Down Expand Up @@ -763,11 +757,11 @@ 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):
time.sleep(0.1)
self._logger.info(f"Movement with {response} finished.")

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)
return response

def get_joints_positions(self, degrees: bool = True, round: Optional[int] = None) -> List[float]:
Expand Down
14 changes: 14 additions & 0 deletions src/reachy2_sdk/parts/goto_based_part.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,3 +1,5 @@
import logging
import time
from abc import ABC
from typing import List, Optional

Expand Down Expand Up @@ -27,6 +29,7 @@ def __init__(
"""Initialize the common attributes."""
self.part = part
self._goto_stub = goto_stub
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"""
Expand Down Expand Up @@ -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:
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think timeout for the gotos should be checked once the movement has begun only.

If we do:

pose2 = get_pose_matrix([0.4, -0.2, -0.1], [0, -90, 0])
reachy.goto_posture("default")
reachy.r_arm.goto_from_matrix(pose2, wait=True)

Then the timeout is triggered, whereas the goto was just queued

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The timeout is now the sum of the duration of all pending movements on the part

self._logger_goto.warning(f"Waiting time for movement with {id} is timeout.")
return
self._logger_goto.info(f"Movement with {id} finished.")
37 changes: 16 additions & 21 deletions src/reachy2_sdk/parts/head.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,6 @@
- look_at function
"""

import time
from typing import List

import grpc
Expand Down Expand Up @@ -131,11 +130,10 @@ def look_at(
interpolation_mode=get_grpc_interpolation_mode(interpolation_mode),
)
response = self._goto_stub.GoToCartesian(request)
if 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.")
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)
return response

def goto_joints(
Expand Down Expand Up @@ -177,11 +175,10 @@ def goto_joints(
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):
time.sleep(0.1)
self._logger.info(f"Movement with {response} finished.")
if response.id == -1:
self._logger.error(f"Position {positions} was not reachable. No command sent.")
elif wait:
self._wait_goto(response, duration + 1)
return response

def _goto_single_joint(
Expand All @@ -207,11 +204,10 @@ 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):
time.sleep(0.1)
self._logger.info(f"Movement with {response} finished.")
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)
return response

def goto_quat(
Expand All @@ -235,11 +231,10 @@ def goto_quat(
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):
time.sleep(0.1)
self._logger.info(f"Movement with {response} finished.")
if response.id == -1:
self._logger.error(f"Orientation {q} was not reachable. No command sent.")
elif wait:
self._wait_goto(response, duration + 1)
return response

def send_goal_positions(self) -> None:
Expand Down
Loading