diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index f0062a95..a06fc0d4 100644 --- a/.github/workflows/lint.yml +++ b/.github/workflows/lint.yml @@ -5,16 +5,32 @@ on: [push] jobs: black: - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 steps: - uses: actions/checkout@v3 - uses: psf/black@stable with: - options: "--check --verbose --line-length 128" + options: "--check --verbose" version: "23.3.0" + isort: + runs-on: ubuntu-22.04 + steps: + - uses: actions/checkout@v3 + - name: Set up Python 3.10 + uses: actions/setup-python@v4 + with: + python-version: "3.10" + cache: 'pip' # caching pip dependencies + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install .[dev] + - name : Check import order + run : isort . -c + flake8: - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 steps: - uses: actions/checkout@v3 - name: Set up Python 3.10 @@ -31,10 +47,10 @@ jobs: # stop the build if there are Python syntax errors or undefined names flake8 . --count --select=E9,F63,F7,F82 --show-source --statistics # option --exit-zero can be added to this line to set these errors as warnings - flake8 . --count --max-complexity=10 --max-line-length=128 --statistics + flake8 . --count --statistics --config setup.cfg mypy: - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 steps: - uses: actions/checkout@v3 - name: Set up Python 3.10 diff --git a/README.md b/README.md index 192f22f1..056ada23 100644 --- a/README.md +++ b/README.md @@ -1 +1,10 @@ -# Python SDK for Reachy v2 \ No newline at end of file +# Python SDK for Reachy v2 + +## Install + +```console +$ pip install -e .[pollen,dev] +``` + +*[dev]* contains the tools for developers. +*[pollen]* has the custom pollen robotics repos. It is mandatory but not in the default install because github actions cannot fetch private repo directly. \ No newline at end of file diff --git a/docs/convention_maths.md b/docs/convention_maths.md deleted file mode 100644 index d3ccb37f..00000000 --- a/docs/convention_maths.md +++ /dev/null @@ -1,57 +0,0 @@ -# Coding convention for Maths - - -## Conventions and Scope - -We use the Modern Robotics' convention, but they don't cover everything we need when coding. So here's a proposal. We use the mnemotechnic trick of "frames of the same name are close", so we prefix the frame of reference when naming a point. - -T_a_c = T_a_b * T_b_c --> Transform composition. The 2 "b" are close, we feel safe :) - -P_a_x = T_a_b * P_b_x --> Application to a point. The 2 "b" are close, we feel safe :) - - -Names should be ONE word long to avoid confusion. Otherwise this is ambiguous: T_shoulder_camera_wrist, is it a transformation from the wrist to the shoulder_camera, or a transformation from the camera_wrist to the shoulder? - -We could use capital letters for 3D and lower case letters for 2D: - - "3D homogeneous transformation matrix" --> A pose or transformation (letter T) is a 4x4 matrix with a 3x3 rotation matrix (letter R) + 3x1 column vector (for translation) + 1x4 column vector (for padding). Here a point is a 3D vector (letter P). - "2D homogeneous transformation matrix" --> A pose or transformation (letter t) is a 3x3 matrix with a 2x2 rotation matrix (letter r) + 3x1 column vector (for translation) + 1x3 row vector (for padding). Here a point is a 2D vector (letter p). - -There are other cases but we wouldn't create a convention for them (like sometimes it's useful to pad the 3x3 rotation matrix into a 4x4 rotation matrix...). - -What matters is that someone reading your code doesn't struggle to understand it, so if you encounter a situation that's not covered by these conventions just add a clear comment explaining your stuff and be locally coherent with it. - -## Code example - -Using Python and numpy arrays. if: -``` -T_camera_object == The 4x4 pose of the object in the camera frame -T_torso_camera == The 4x4 pose of the camera in the torso frame -T_torso_object == The 4x4 pose of the object in the torso frame -``` -then this code is correct: -``` -T_torso_object = T_torso_camera @ T_camera_object -``` - -We can also define the rotation matrix of the transform: -``` -R_torso_object = T_torso_object[:3, :3] -``` - -The 3D point object in the torso frame: -``` -P_torso_object = T_torso_object[:3, 3] -``` - -Which is the same thing as the coordinates x, y and z of the object in the torso frame: -``` -x = T_torso_object[0][3] -y = T_torso_object[1][3] -z = T_torso_object[2][3] -``` - -## References - -This post uses Modern Robotics' conventions -https://www.mecharithm.com/homogenous-transformation-matrices-configurations-in-robotics/ \ No newline at end of file diff --git a/pyproject.toml b/pyproject.toml index 7fd26b97..3fa7277f 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,3 +1,9 @@ [build-system] requires = ["setuptools"] -build-backend = "setuptools.build_meta" \ No newline at end of file +build-backend = "setuptools.build_meta" + +[tool.isort] +profile = "black" + +[tool.black] +line-length = 128 \ No newline at end of file diff --git a/scripts/git_hooks/pre-commit b/scripts/git_hooks/pre-commit index a4ccaa35..a1cace09 100755 --- a/scripts/git_hooks/pre-commit +++ b/scripts/git_hooks/pre-commit @@ -9,9 +9,13 @@ set -eo pipefail # Run black against all code in the `source_code` directory -black . --line-length 128 --check +black . --check echo "-------> Black passed!" +# Run isort against all code in the `source_code` directory +isort . -c +echo "-------> Isort passed!" + # Run flake8 against all code in the `source_code` directory flake8 . echo "-------> Flake8 passed!" @@ -19,4 +23,3 @@ echo "-------> Flake8 passed!" # Run mypy against all code in the `source_code` directory mypy . echo "-------> Mypy passed!" - diff --git a/setup.cfg b/setup.cfg index 733b7766..6b22fd2f 100644 --- a/setup.cfg +++ b/setup.cfg @@ -1,37 +1,35 @@ [metadata] name = reachy-v2-sdk -version = 0.1.0 +version = 2.0 author = Pollen Robotics author_email = contact@pollen-robotics.com -url = https://github.com/pollen-robotics/python-template -description = Python template project +url = https://github.com/pollen-robotics/reachy-v2-sdk +description = Reachy SDK V2 long_description = file: README.md long_description_content_type = text/markdown - [options] -packages = find: +packages = reachy_v2_sdk zip_safe = True include_package_data = True package_dir= =src install_requires = - numpy + numpy==1.26.1 + protobuf==4.25.0 + grpcio==1.59.2 + pyquaternion==0.9.9 -[options.packages.find] -where=reachy_sdk [options.extras_require] -dev = black==23.3.0 - flake8==6.0.0 - pytest==7.3.1 - coverage==7.2.5 - mypy==1.0.0 +dev = black==23.10.1 + flake8==6.1.0 + pytest==7.4.3 + coverage==7.3.2 + mypy==1.6.1 + isort==5.12.0 types-protobuf==4.24.0.1 - -[options.entry_points] -console_scripts = - example_entry_point = example.celcius:main +pollen = reachy-sdk-api-v2@git+ssh://git@github.com/pollen-robotics/reachy-sdk-api-v2.git#subdirectory=python [flake8] exclude = tests diff --git a/src/example/__init__.py b/src/example/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/src/reachy_v2_sdk/arm.py b/src/reachy_v2_sdk/arm.py index b102faa2..5ae3bd41 100644 --- a/src/reachy_v2_sdk/arm.py +++ b/src/reachy_v2_sdk/arm.py @@ -136,7 +136,10 @@ def forward_kinematics( return np.array(resp.end_effector.pose.data).reshape((4, 4)) def inverse_kinematics( - self, target: npt.NDArray[np.float64], q0: Optional[List[float]] = None, degrees: bool = True + self, + target: npt.NDArray[np.float64], + q0: Optional[List[float]] = None, + degrees: bool = True, ) -> List[float]: """Compute the inverse kinematics of the arm. @@ -182,8 +185,14 @@ def _list_to_arm_position(self, positions: List[float], degrees: bool = True) -> if degrees: positions = self._convert_to_radians(positions) arm_pos = ArmPosition( - shoulder_position=Pose2D(axis_1=FloatValue(value=positions[0]), axis_2=FloatValue(value=positions[1])), - elbow_position=Pose2D(axis_1=FloatValue(value=positions[2]), axis_2=FloatValue(value=positions[3])), + shoulder_position=Pose2D( + axis_1=FloatValue(value=positions[0]), + axis_2=FloatValue(value=positions[1]), + ), + elbow_position=Pose2D( + axis_1=FloatValue(value=positions[2]), + axis_2=FloatValue(value=positions[3]), + ), wrist_position=Rotation3D( rpy=ExtEulerAngles( roll=positions[4], @@ -277,7 +286,9 @@ def goto( ) if orientation_tol is not None: target.orientation_tolerance = ExtEulerAnglesTolerances( - x_tol=orientation_tol[0], y_tol=orientation_tol[1], z_tol=orientation_tol[2] + x_tol=orientation_tol[0], + y_tol=orientation_tol[1], + z_tol=orientation_tol[2], ) self._arm_stub.GoToCartesianPosition(target) diff --git a/src/reachy_v2_sdk/audio.py b/src/reachy_v2_sdk/audio.py new file mode 100644 index 00000000..f9f956c9 --- /dev/null +++ b/src/reachy_v2_sdk/audio.py @@ -0,0 +1,70 @@ +"""Reachy Audio module. + +Handles all specific method related to audio especially: +- playing sounds +- recording sounds +""" +from typing import List + +import grpc +from google.protobuf.empty_pb2 import Empty +from reachy_sdk_api_v2.sound_pb2 import ( + RecordingRequest, + SoundId, + SoundRequest, + VolumeRequest, +) +from reachy_sdk_api_v2.sound_pb2_grpc import SoundServiceStub + + +class Audio: + """Audio class used for microphone and speakers. + + It exposes functions to: + - play / stop sounds with defined speakers, + - test the speakers, + - record tracks with a defined microphone, + """ + + def __init__(self, host: str, port: int) -> None: + """Set up audio module, along with microphone and speakers.""" + self._grpc_audio_channel = grpc.insecure_channel(f"{host}:{port}") + + self._audio_stub = SoundServiceStub(self._grpc_audio_channel) + self._setup_microphones() + self._setup_speakers() + + def _setup_microphones(self) -> None: + micro_info = self._audio_stub.GetAllMicrophone(Empty()) + self._microphone_id = micro_info.microphone_info[0].id + + def _setup_speakers(self) -> None: + speaker_info = self._audio_stub.GetAllSpeaker(Empty()) + self._speaker_id = speaker_info.speaker_info[0].id + + def testing(self) -> None: + self._audio_stub.TestSpeaker(self._speaker_id) + + def get_sounds_list(self) -> List[str]: + sounds = self._audio_stub.GetSoundsList(Empty()) + return [soundId.id for soundId in sounds.sounds] + + def play(self, sound_name: str, volume: float = 0.5) -> None: + available_sounds = self.get_sounds_list() + if sound_name not in available_sounds: + raise ValueError(f"Sound to play not available! Sounds available are {available_sounds}") + if not 0 <= volume <= 1: + raise ValueError(f"Volume should be between 0 and 1, got {volume}") + self._audio_stub.PlaySound(SoundRequest(speaker=self._speaker_id, sound=SoundId(id=sound_name, volume=volume))) + + def stop(self) -> None: + self._audio_stub.StopSound(ComponentId=self._speaker_id) + + def start_recording(self, sound_name: str) -> None: + self._audio_stub.StartRecording(RecordingRequest(micro=self._microphone_id, recording_id=SoundId(id=sound_name))) + + def stop_recording(self) -> None: + self._audio_stub.StopRecording(self._microphone_id) + + def set_audio_volume(self, volume: int) -> None: + self._audio_stub.ChangeVolume(VolumeRequest(id=self._speaker_id, volume=volume)) diff --git a/src/reachy_v2_sdk/dynamixel_motor.py b/src/reachy_v2_sdk/dynamixel_motor.py index 7b5d9611..3b52780d 100644 --- a/src/reachy_v2_sdk/dynamixel_motor.py +++ b/src/reachy_v2_sdk/dynamixel_motor.py @@ -36,7 +36,13 @@ class DynamixelMotor: speed_limit = Register(readonly=False, type=FloatValue, label="speed_limit") torque_limit = Register(readonly=False, type=FloatValue, label="torque_limit") - def __init__(self, uid: int, name: str, initial_state: DynamixelMotorState, grpc_channel: Channel): + def __init__( + self, + uid: int, + name: str, + initial_state: DynamixelMotorState, + grpc_channel: Channel, + ): self.id = uid self.name = name self._stub = DynamixelMotorServiceStub(grpc_channel) diff --git a/src/reachy_v2_sdk/head.py b/src/reachy_v2_sdk/head.py index 1224759c..a27fa6cc 100644 --- a/src/reachy_v2_sdk/head.py +++ b/src/reachy_v2_sdk/head.py @@ -110,7 +110,9 @@ def forward_kinematics(self, rpy_position: Optional[Tuple[float, float, float]] return pyQuat(w=quat.w, x=quat.x, y=quat.y, z=quat.z) def inverse_kinematics( - self, orientation: Optional[pyQuat] = None, rpy_q0: Optional[Tuple[float, float, float]] = None + self, + orientation: Optional[pyQuat] = None, + rpy_q0: Optional[Tuple[float, float, float]] = None, ) -> Tuple[float, float, float]: """Compute the inverse kinematics of the arm. @@ -137,7 +139,11 @@ def inverse_kinematics( req_params["q0"] = Rotation3D(rpy=ExtEulerAngles(roll=rpy_q0[0], pitch=rpy_q0[1], yaw=rpy_q0[2])) req = NeckIKRequest(**req_params) rpy_pos = self._head_stub.ComputeNeckIK(req) - return (rpy_pos.position.rpy.roll, rpy_pos.position.rpy.pitch, rpy_pos.position.rpy.yaw) + return ( + rpy_pos.position.rpy.roll, + rpy_pos.position.rpy.pitch, + rpy_pos.position.rpy.yaw, + ) def look_at(self, x: float, y: float, z: float, duration: float) -> None: """Compute and send neck rpy position to look at the (x, y, z) point in Reachy cartesian space (torso frame). diff --git a/src/reachy_v2_sdk/orbita2d.py b/src/reachy_v2_sdk/orbita2d.py index 726569c7..abbecb3e 100644 --- a/src/reachy_v2_sdk/orbita2d.py +++ b/src/reachy_v2_sdk/orbita2d.py @@ -92,7 +92,10 @@ def __init__( # noqa: C901 axis2_name, OrbitaJoint2D(initial_state=init_state["axis_2"], axis_type=axis2_name, actuator=self), ) - self._joints = {"axis_1": getattr(self, axis1_name), "axis_2": getattr(self, axis2_name)} + self._joints = { + "axis_1": getattr(self, axis1_name), + "axis_2": getattr(self, axis2_name), + } self.__motor_1 = OrbitaMotor(initial_state=init_state["motor_1"], actuator=self) self.__motor_2 = OrbitaMotor(initial_state=init_state["motor_2"], actuator=self) diff --git a/src/reachy_v2_sdk/orbita3d.py b/src/reachy_v2_sdk/orbita3d.py index 616e7a1b..8cd2e30d 100644 --- a/src/reachy_v2_sdk/orbita3d.py +++ b/src/reachy_v2_sdk/orbita3d.py @@ -41,17 +41,39 @@ class Orbita3d: compliant = Register(readonly=False, type=BoolValue, label="compliant") - def __init__(self, uid: int, name: str, initial_state: Orbita3DState, grpc_channel: Channel): # noqa: C901 + def __init__(self, uid: int, name: str, initial_state: Orbita3DState, grpc_channel: Channel): """Initialize the Orbita2d with its joints, motors and axis.""" self.name = name self.id = uid self._stub = Orbita3DServiceStub(grpc_channel) self._state: Dict[str, bool] = {} - init_state: Dict[str, Dict[str, float]] = {} + init_state: Dict[str, Dict[str, float]] = self._create_init_state(initial_state) self._register_needing_sync: List[str] = [] + self.roll = OrbitaJoint3D(initial_state=init_state["roll"], axis_type="roll", actuator=self) + self.pitch = OrbitaJoint3D(initial_state=init_state["pitch"], axis_type="pitch", actuator=self) + self.yaw = OrbitaJoint3D(initial_state=init_state["yaw"], axis_type="yaw", actuator=self) + self._joints = {"roll": self.roll, "pitch": self.pitch, "yaw": self.yaw} + + self.__motor_1 = OrbitaMotor(initial_state=init_state["motor_1"], actuator=self) + self.__motor_2 = OrbitaMotor(initial_state=init_state["motor_2"], actuator=self) + self.__motor_3 = OrbitaMotor(initial_state=init_state["motor_3"], actuator=self) + self._motors = { + "motor_1": self.__motor_1, + "motor_2": self.__motor_2, + "motor_3": self.__motor_3, + } + + self.__x = OrbitaAxis(initial_state=init_state["x"]) + self.__y = OrbitaAxis(initial_state=init_state["y"]) + self.__z = OrbitaAxis(initial_state=init_state["z"]) + self._axis = {"x": self.__x, "y": self.__y, "z": self.__z} + + def _create_init_state(self, initial_state: Orbita3DState) -> Dict[str, Dict[str, float]]: # noqa: C901 + init_state: Dict[str, Dict[str, float]] = {} + for field, value in initial_state.ListFields(): if field.name == "compliant": self._state[field.name] = value @@ -74,21 +96,7 @@ def __init__(self, uid: int, name: str, initial_state: Orbita3DState, grpc_chann if axis.name not in init_state: init_state[axis.name] = {} init_state[axis.name][field.name] = val - - self.roll = OrbitaJoint3D(initial_state=init_state["roll"], axis_type="roll", actuator=self) - self.pitch = OrbitaJoint3D(initial_state=init_state["pitch"], axis_type="pitch", actuator=self) - self.yaw = OrbitaJoint3D(initial_state=init_state["yaw"], axis_type="yaw", actuator=self) - self._joints = {"roll": self.roll, "pitch": self.pitch, "yaw": self.yaw} - - self.__motor_1 = OrbitaMotor(initial_state=init_state["motor_1"], actuator=self) - self.__motor_2 = OrbitaMotor(initial_state=init_state["motor_2"], actuator=self) - self.__motor_3 = OrbitaMotor(initial_state=init_state["motor_3"], actuator=self) - self._motors = {"motor_1": self.__motor_1, "motor_2": self.__motor_2, "motor_3": self.__motor_3} - - self.__x = OrbitaAxis(initial_state=init_state["x"]) - self.__y = OrbitaAxis(initial_state=init_state["y"]) - self.__z = OrbitaAxis(initial_state=init_state["z"]) - self._axis = {"x": self.__x, "y": self.__y, "z": self.__z} + return init_state def __repr__(self) -> str: """Clean representation of an Orbita3D.""" diff --git a/src/reachy_v2_sdk/orbita_utils.py b/src/reachy_v2_sdk/orbita_utils.py index c4deced2..0baccc24 100644 --- a/src/reachy_v2_sdk/orbita_utils.py +++ b/src/reachy_v2_sdk/orbita_utils.py @@ -34,10 +34,16 @@ class OrbitaJoint2D: """ present_position = Register( - readonly=True, type=FloatValue, label="present_position", conversion=(_to_internal_position, _to_position) + readonly=True, + type=FloatValue, + label="present_position", + conversion=(_to_internal_position, _to_position), ) goal_position = Register( - readonly=False, type=FloatValue, label="goal_position", conversion=(_to_internal_position, _to_position) + readonly=False, + type=FloatValue, + label="goal_position", + conversion=(_to_internal_position, _to_position), ) def __init__(self, initial_state: Dict[str, float], axis_type: str, actuator: Any) -> None: @@ -65,10 +71,16 @@ class OrbitaJoint3D: """ present_position = Register( - readonly=True, type=float, label="present_position", conversion=(_to_internal_position, _to_position) + readonly=True, + type=float, + label="present_position", + conversion=(_to_internal_position, _to_position), ) goal_position = Register( - readonly=False, type=float, label="goal_position", conversion=(_to_internal_position, _to_position) + readonly=False, + type=float, + label="goal_position", + conversion=(_to_internal_position, _to_position), ) def __init__(self, initial_state: Dict[str, float], axis_type: str, actuator: Any) -> None: @@ -102,7 +114,10 @@ class OrbitaMotor: temperature = Register(readonly=True, type=FloatValue, label="temperature") speed_limit = Register( - readonly=False, type=FloatValue, label="speed_limit", conversion=(_to_internal_position, _to_position) + readonly=False, + type=FloatValue, + label="speed_limit", + conversion=(_to_internal_position, _to_position), ) torque_limit = Register(readonly=False, type=FloatValue, label="torque_limit") compliant = Register(readonly=True, type=BoolValue, label="compliant") @@ -137,7 +152,10 @@ class OrbitaAxis: """ present_speed = Register( - readonly=True, type=FloatValue, label="present_speed", conversion=(_to_internal_position, _to_position) + readonly=True, + type=FloatValue, + label="present_speed", + conversion=(_to_internal_position, _to_position), ) present_load = Register(readonly=True, type=FloatValue, label="present_load") diff --git a/src/reachy_v2_sdk/reachy.py b/src/reachy_v2_sdk/reachy.py index ab9cf7ab..23cc73a5 100644 --- a/src/reachy_v2_sdk/reachy.py +++ b/src/reachy_v2_sdk/reachy.py @@ -23,7 +23,15 @@ def __init__(self, host: str, info_msg: ReachyInfo_proto) -> None: def get_config(msg: Reachy) -> str: - if msg.HasField("l_arm"): - return "full_kit" + mobile_base_presence = "" + if msg.HasField("mobile_base"): + mobile_base_presence = " with mobile_base" + if msg.HasField("head"): + if msg.HasField("l_arm") and msg.HasField("r_arm"): + return "full_kit" + mobile_base_presence + elif msg.HasField("l_arm"): + return "starter_kit (left arm)" + mobile_base_presence + else: + return "starter_kit (right arm)" + mobile_base_presence else: - return "none" + return "custom_config" diff --git a/src/reachy_v2_sdk/reachy_sdk.py b/src/reachy_v2_sdk/reachy_sdk.py index 55f61d95..94d4db7e 100644 --- a/src/reachy_v2_sdk/reachy_sdk.py +++ b/src/reachy_v2_sdk/reachy_sdk.py @@ -25,16 +25,13 @@ from reachy_sdk_api_v2.orbita3d_pb2_grpc import Orbita3DServiceStub from .arm import Arm +from .audio import Audio from .head import Head from .orbita2d import Orbita2d from .orbita3d import Orbita3d from .reachy import ReachyInfo, get_config # from reachy_sdk_api_v2.dynamixel_motor_pb2_grpc import DynamixelMotorServiceStub - - - - # from .dynamixel_motor import DynamixelMotor @@ -73,16 +70,28 @@ def __init__( self, host: str, sdk_port: int = 50051, + audio_port: int = 50063, ) -> None: """Set up the connection with the robot.""" self._logger = getLogger() self._host = host self._sdk_port = sdk_port + self._audio_port = audio_port + + self._grpc_connected = False + self.connect() + + def connect(self) -> None: + if self._grpc_status == "connected": + print("Already connected to Reachy.") + return + self._grpc_channel = grpc.insecure_channel(f"{self._host}:{self._sdk_port}") self._enabled_parts: Dict[str, Any] = {} self._disabled_parts: List[str] = [] + self._stop_flag = threading.Event() self._ready = threading.Event() self._pushed_2dcommand = threading.Event() self._pushed_3dcommand = threading.Event() @@ -99,12 +108,47 @@ def __init__( return self._setup_parts() + self._setup_audio() self._sync_thread = threading.Thread(target=self._start_sync_in_bg) self._sync_thread.daemon = True self._sync_thread.start() + self._grpc_status = "connected" _open_connection.append(self) + print("Connected to Reachy.") + + def disconnect(self) -> None: + if self._grpc_status == "disconnected": + print("Already disconnected from Reachy.") + return + + for part in self._enabled_parts.values(): + for actuator in part._actuators.values(): + actuator._need_sync.clear() + + self._stop_flag.set() + time.sleep(0.1) + self._grpc_status = "disconnected" + + self._grpc_channel.close() + attributs = [attr for attr in dir(self) if not attr.startswith("_")] + for attr in attributs: + if attr not in [ + "grpc_status", + "connect", + "disconnect", + "turn_on", + "turn_off", + "enabled_parts", + "disabled_parts", + ]: + delattr(self, attr) + + for task in asyncio.all_tasks(loop=self._loop): + task.cancel() + + print("Disconnected from Reachy.") def __repr__(self) -> str: """Clean representation of a Reachy.""" @@ -152,11 +196,6 @@ def _grpc_status(self, status: str) -> None: self._grpc_connected = True elif status == "disconnected": self._grpc_connected = False - self._grpc_channel.close() - attributs = [attr for attr in dir(self) if not attr.startswith("_")] - for attr in attributs: - if attr not in ["grpc_status", "turn_on", "turn_off", "enabled_parts", "disabled_parts"]: - delattr(self, attr) else: raise ValueError("_grpc_status can only be set to 'connected' or 'disconnected'") @@ -178,6 +217,12 @@ def _get_info(self) -> None: self.config = get_config(self._robot) self._grpc_status = "connected" + def _setup_audio(self) -> None: + try: + self.audio = Audio(self._host, self._audio_port) + except Exception: + print("Failed to connect to audio server. ReachySDK.audio will not be available.") + def _setup_parts(self) -> None: """Setup all parts of the robot. @@ -217,6 +262,11 @@ def _setup_parts(self) -> None: # if self._robot.HasField("mobile_base"): # pass + async def _wait_for_stop(self) -> None: + while not self._stop_flag.is_set(): + await asyncio.sleep(0.1) + raise ConnectionError("Connection with Reachy lost, check the sdk server status.") + async def _poll_waiting_2dcommands(self) -> Orbita2DsCommand: """Poll registers to update for Orbita2d actuators of the robot.""" tasks = [] @@ -224,14 +274,14 @@ async def _poll_waiting_2dcommands(self) -> Orbita2DsCommand: for part in self._enabled_parts.values(): for actuator in part._actuators.values(): if isinstance(actuator, Orbita2d): - tasks.append(asyncio.create_task(actuator._need_sync.wait(), name=f"Task for {actuator.name}")) + # tasks.append(asyncio.create_task(actuator._need_sync.wait(), name=f"Task for {actuator.name}")) + tasks.append(asyncio.create_task(actuator._need_sync.wait())) if len(tasks) > 0: await asyncio.wait( tasks, return_when=asyncio.FIRST_COMPLETED, ) - commands = [] for part in self._enabled_parts.values(): @@ -251,7 +301,8 @@ async def _poll_waiting_3dcommands(self) -> Orbita3DsCommand: for part in self._enabled_parts.values(): for actuator in part._actuators.values(): if isinstance(actuator, Orbita3d): - tasks.append(asyncio.create_task(actuator._need_sync.wait(), name=f"Task for {actuator.name}")) + tasks.append(asyncio.create_task(actuator._need_sync.wait())) + # tasks.append(asyncio.create_task(actuator._need_sync.wait(), name=f"Task for {actuator.name}")) if len(tasks) > 0: await asyncio.wait( @@ -301,7 +352,12 @@ def _start_sync_in_bg(self) -> None: """Start the synchronization asyncio tasks with the robot in background.""" self._loop = asyncio.new_event_loop() asyncio.set_event_loop(self._loop) - self._loop.run_until_complete(self._sync_loop()) + + try: + self._loop.run_until_complete(self._sync_loop()) + self.disconnect() + except asyncio.CancelledError: + print("Sync loop cancelled.") async def _sync_loop(self) -> None: """Define the synchronization loop. @@ -328,12 +384,18 @@ async def _sync_loop(self) -> None: orbita3d_stub = Orbita3DServiceStub(async_channel) # dynamixel_motor_stub = DynamixelMotorServiceStub(async_channel) - await asyncio.gather( - self._stream_orbita2d_commands_loop(orbita2d_stub, freq=100), - self._stream_orbita3d_commands_loop(orbita3d_stub, freq=100), - # self._stream_dynamixel_motor_commands_loop(dynamixel_motor_stub, freq=100), - self._get_stream_update_loop(reachy_stub, freq=100), - ) + try: + await asyncio.gather( + self._stream_orbita2d_commands_loop(orbita2d_stub, freq=100), + self._stream_orbita3d_commands_loop(orbita3d_stub, freq=100), + # self._stream_dynamixel_motor_commands_loop(dynamixel_motor_stub, freq=100), + self._get_stream_update_loop(reachy_stub, freq=1), + self._wait_for_stop(), + ) + except ConnectionError: + print("Connection with Reachy lost, check the sdk server status.") + except asyncio.CancelledError: + print("Stopped streaming commands.") async def _get_stream_update_loop(self, reachy_stub: reachy_pb2_grpc.ReachyServiceStub, freq: float) -> None: """Update the state of the robot at a given frequency.""" @@ -353,8 +415,7 @@ async def _get_stream_update_loop(self, reachy_stub: reachy_pb2_grpc.ReachyServi if hasattr(self, "mobile_base"): self.mobile_base._update_with(state_update.mobile_base_state) except grpc.aio._call.AioRpcError: - print("Connection with Reachy lost, check the sdk server status.") - self._grpc_status = "disconnected" + raise ConnectionError("") async def _stream_orbita2d_commands_loop(self, orbita2d_stub: Orbita2DServiceStub, freq: float) -> None: """Stream commands for the 2d actuators of the robot at a given frequency. @@ -381,7 +442,7 @@ async def command_poll_2d() -> Orbita2DsCommand: try: await orbita2d_stub.StreamCommand(command_poll_2d()) except grpc.aio._call.AioRpcError: - self._grpc_status = "disconnected" + pass async def _stream_orbita3d_commands_loop(self, orbita3d_stub: Orbita3DServiceStub, freq: float) -> None: """Stream commands for the 3d actuators of the robot at a given frequency. @@ -408,7 +469,7 @@ async def command_poll_3d() -> Orbita3DsCommand: try: await orbita3d_stub.StreamCommand(command_poll_3d()) except grpc.aio._call.AioRpcError: - self._grpc_status = "disconnected" + pass # async def _stream_dynamixel_motor_commands_loop(self, dynamixel_motor_stub: DynamixelMotorServiceStub, freq: float) -> None: # noqa: E501 # async def command_poll_dm() -> DynamixelMotorsCommand: diff --git a/src/reachy_v2_sdk/register.py b/src/reachy_v2_sdk/register.py index 5dbb22c3..cf195827 100644 --- a/src/reachy_v2_sdk/register.py +++ b/src/reachy_v2_sdk/register.py @@ -82,6 +82,10 @@ def wrapped_value(self, value: Any) -> Any: if self.internal_class in (BoolValue, FloatValue, UInt32Value): return self.internal_class(value=value) elif self.internal_class.__name__ == "PIDGains": - return self.internal_class(p=FloatValue(value=value[0]), i=FloatValue(value=value[1]), d=FloatValue(value=value[2])) + return self.internal_class( + p=FloatValue(value=value[0]), + i=FloatValue(value=value[1]), + d=FloatValue(value=value[2]), + ) return value