diff --git a/src/reachy2_sdk/orbita/orbita.py b/src/reachy2_sdk/orbita/orbita.py index 81dada06..b543d0d9 100644 --- a/src/reachy2_sdk/orbita/orbita.py +++ b/src/reachy2_sdk/orbita/orbita.py @@ -31,10 +31,11 @@ class Orbita(ABC): """The Orbita class is an abstract class to represent any Orbita actuator. The Orbita class is used to store the up-to-date state of the actuator, especially: - - its compliancy - - its joints state - - its motors state - - its axis state + - its compliancy + - its joints state + - its motors state + - its axis state + And apply speed, torque, pid and compliancy to all motors of the actuator. This class is meant to be derived by Orbita2d and Orbita3d @@ -106,11 +107,11 @@ def set_speed_limits(self, speed_limit: float | int) -> None: speed_limit: The desired speed limit as a percentage (0-100). Raises: - ValueError: If the provided speed_limit is not a float or int. + TypeError: If the provided speed_limit is not a float or int. ValueError: If the provided speed_limit is outside the range [0, 100]. """ if not isinstance(speed_limit, float | int): - raise ValueError(f"Expected one of: float, int for speed_limit, got {type(speed_limit).__name__}") + raise TypeError(f"Expected one of: float, int for speed_limit, got {type(speed_limit).__name__}") if not (0 <= speed_limit <= 100): raise ValueError(f"speed_limit must be in [0, 100], got {speed_limit}.") @@ -125,11 +126,11 @@ def set_torque_limits(self, torque_limit: float | int) -> None: torque_limit: The desired torque limit as a percentage (0-100). Raises: - ValueError: If the provided torque_limit is not a float or int. + TypeError: If the provided torque_limit is not a float or int. ValueError: If the provided torque_limit is outside the range [0, 100]. """ if not isinstance(torque_limit, float | int): - raise ValueError(f"Expected one of: float, int for torque_limit, got {type(torque_limit).__name__}") + raise TypeError(f"Expected one of: float, int for torque_limit, got {type(torque_limit).__name__}") if not (0 <= torque_limit <= 100): raise ValueError(f"torque_limit must be in [0, 100], got {torque_limit}.") diff --git a/src/reachy2_sdk/orbita/orbita2d.py b/src/reachy2_sdk/orbita/orbita2d.py index 2952dad0..60d40ef7 100644 --- a/src/reachy2_sdk/orbita/orbita2d.py +++ b/src/reachy2_sdk/orbita/orbita2d.py @@ -31,16 +31,17 @@ class Orbita2d(Orbita): """The Orbita2d class represents any Orbita2d actuator and its joints, motors and axis. The Orbita2d class is used to store the up-to-date state of the actuator, especially: - - its compliancy - - its joints state - - its motors state - - its axis state + - its compliancy + - its joints state + - its motors state + - its axis state You can access properties of the motors from the actuators with function that act on all the actuator's motors: - speed limit (in percentage, for all motors of the actuator) - torque limit (in percentage, for all motors of the actuator) - pid (for all motors of the actuator) - compliancy (for all motors of the actuator) + Lower properties that are read-only but acessible at actuator level: - temperatures (temperatures of all motors of the actuator) """ diff --git a/src/reachy2_sdk/orbita/orbita3d.py b/src/reachy2_sdk/orbita/orbita3d.py index fab55f5f..12ebd1ba 100644 --- a/src/reachy2_sdk/orbita/orbita3d.py +++ b/src/reachy2_sdk/orbita/orbita3d.py @@ -30,16 +30,17 @@ class Orbita3d(Orbita): """The Orbita3d class represents any Orbita3d actuator and its joints, motors and axis. The Orbita3d class is used to store the up-to-date state of the actuator, especially: - - its compliancy - - its joints state - - its motors state - - its axis state + - its compliancy + - its joints state + - its motors state + - its axis state You can access properties of the motors from the actuators with function that act on all the actuator's motors: - speed limit (in percentage, for all motors of the actuator) - torque limit (in percentage, for all motors of the actuator) - pid (for all motors of the actuator) - compliancy (for all motors of the actuator) + Lower properties that are read-only but acessible at actuator level: - temperatures (temperatures of all motors of the actuator) """ diff --git a/src/reachy2_sdk/orbita/orbita_axis.py b/src/reachy2_sdk/orbita/orbita_axis.py index 4e7b9364..5e862d53 100644 --- a/src/reachy2_sdk/orbita/orbita_axis.py +++ b/src/reachy2_sdk/orbita/orbita_axis.py @@ -14,8 +14,8 @@ class OrbitaAxis: """The OrbitaAxis class represents any Orbita3d or Orbita2d axis. The OrbitaAxis class is used to store the up-to-date state of the axis, especially: - - its present speed (RO) - - its present load (RO) + - its present speed (RO) + - its present load (RO) """ def __init__(self, initial_state: Dict[str, FloatValue]) -> None: diff --git a/src/reachy2_sdk/orbita/orbita_joint.py b/src/reachy2_sdk/orbita/orbita_joint.py index 3abece9d..8d3ee8b8 100644 --- a/src/reachy2_sdk/orbita/orbita_joint.py +++ b/src/reachy2_sdk/orbita/orbita_joint.py @@ -15,8 +15,8 @@ class OrbitaJoint: """The OrbitaJoint class represents any Orbita2d or Orbita3d joint. The OrbitaJoint class is used to store the up-to-date state of the joint, especially: - - its present_position (RO) - - its goal_position (RW) + - its present_position (RO) + - its goal_position (RW) """ def __init__( diff --git a/src/reachy2_sdk/orbita/orbita_motor.py b/src/reachy2_sdk/orbita/orbita_motor.py index e8294001..cd440344 100644 --- a/src/reachy2_sdk/orbita/orbita_motor.py +++ b/src/reachy2_sdk/orbita/orbita_motor.py @@ -16,11 +16,11 @@ class OrbitaMotor: """The OrbitaMotor class represents any Orbita3d or Orbita2d motor. The OrbitaMotor class is used to store the up-to-date state of the motor, especially: - - its temperature (RO) - - its compliancy (RO) - - its speed limit (RW) - - its torque limit (RW) - - its pid (RW) + - its temperature (RO) + - its compliancy (RO) + - its speed limit (RW) + - its torque limit (RW) + - its pid (RW) """ def __init__(self, initial_state: Dict[str, Any], actuator: Any) -> None: diff --git a/src/reachy2_sdk/parts/arm.py b/src/reachy2_sdk/parts/arm.py index 30bef0a7..e27366c2 100644 --- a/src/reachy2_sdk/parts/arm.py +++ b/src/reachy2_sdk/parts/arm.py @@ -326,7 +326,7 @@ def inverse_kinematics( Returns: A list of joint angles representing the solution to reach the target pose, in the following order: - [shoulder.pitch, shoulder.roll, elbow.pitch, elbow.yaw, wrist.roll, wrist.pitch, wrist.yaw]. + [shoulder_pitch, shoulder_roll, elbo_yaw, elbow_pitch, wrist.roll, wrist.pitch, wrist.yaw]. Raises: ValueError: If the target shape is not (4, 4). @@ -427,18 +427,14 @@ def goto( GoToId: The unique GoToId identifier for the movement command. Raises: - ValueError: If the `target` is neither a list of 7 joint values nor a 4x4 pose matrix. + TypeError: If the `target` is neither a list nor a pose matrix. + TypeError: If the `q0` is not a list. + ValueError: If the `target` list has a length other than 7, or the pose matrix is not + of shape (4, 4). ValueError: If the `q0` list has a length other than 7. ValueError: If the `duration` is set to 0. """ - if not ((isinstance(target, list) and len(target) == 7) or (isinstance(target, np.ndarray) and target.shape == (4, 4))): - raise ValueError("Invalid target: must be either a list of 7 joint positions or a 4x4 matrix.") - - if q0 is not None and len(q0) != 7: - raise ValueError(f"q0 should be of length 7 (got {len(q0)} instead)!") - - if duration == 0: - raise ValueError("duration cannot be set to 0.") + self._check_goto_parameters(duration, target, q0) if self.is_off(): self._logger.warning(f"{self._part_id.name} is off. Goto not sent.") @@ -504,9 +500,6 @@ def _goto_from_matrix( Raises: ValueError: If the length of `q0` is not 7. """ - if q0 is not None and len(q0) != 7: - raise ValueError(f"q0 should be of length 7 (got {len(q0)} instead)!") - goal_pose = Matrix4x4(data=target.flatten().tolist()) request = GoToRequest( cartesian_goal=CartesianGoal( @@ -521,6 +514,38 @@ def _goto_from_matrix( ) return self._goto_stub.GoToCartesian(request) + def _check_goto_parameters(self, duration: float, target: Any, q0: Optional[List[float]] = None) -> None: + """Check the validity of the parameters for the `goto` method. + + Args: + duration: The time in seconds for the movement to be completed. + target: The target position, either a list of joint positions or a 4x4 pose matrix. + q0: An optional initial joint configuration for inverse kinematics. Defaults to None. + + Raises: + TypeError: If the target is not a list or a NumPy matrix. + ValueError: If the target list has a length other than 7, or the pose matrix is not of + shape (4, 4). + ValueError: If the duration is set to 0. + ValueError: If the length of `q0` is not 7. + """ + if not (isinstance(target, list) or isinstance(target, np.ndarray)): + raise TypeError(f"Invalid target: must be either a list or a np matrix, got {type(target)} instead.") + + elif isinstance(target, list) and not (len(target) == 7): + raise ValueError(f"The joints list should be of length 7 (got {len(target)} instead).") + elif isinstance(target, np.ndarray) and not (target.shape == (4, 4)): + raise ValueError(f"The pose matrix should be of shape (4, 4) (got {target.shape} instead).") + + elif q0 is not None: + if not isinstance(q0, list): + raise TypeError("Invalid q0: must be a list.") + elif len(q0) != 7: + raise ValueError(f"q0 should be of length 7 (got {len(q0)} instead)!") + + elif duration == 0: + raise ValueError("duration cannot be set to 0.") + def _goto_single_joint( self, arm_joint: int, @@ -912,8 +937,15 @@ def send_cartesian_interpolation( the current end-effector position and the target position. If the end-effector is further than this distance from the target after the movement, the movement is repeated until the precision is met. Defaults to 0.003. + + Raises: + TypeError: If the target is not a NumPy matrix. + ValueError: If the target shape is not (4, 4). + ValueError: If the duration is set to 0. """ self.cancel_all_goto() + if not isinstance(target, np.ndarray): + raise TypeError(f"target should be a NumPy array (got {type(target)} instead)!") if target.shape != (4, 4): raise ValueError("target shape should be (4, 4) (got {target.shape} instead)!") if duration == 0: diff --git a/src/reachy2_sdk/parts/goto_based_part.py b/src/reachy2_sdk/parts/goto_based_part.py index 80f43d69..231834c0 100644 --- a/src/reachy2_sdk/parts/goto_based_part.py +++ b/src/reachy2_sdk/parts/goto_based_part.py @@ -5,8 +5,8 @@ import logging import time -from abc import ABC -from typing import List, Optional +from abc import ABC, abstractmethod +from typing import Any, List, Optional from reachy2_sdk_api.goto_pb2 import GoalStatus, GoToAck, GoToId from reachy2_sdk_api.goto_pb2_grpc import GoToServiceStub @@ -143,3 +143,8 @@ def _wait_goto(self, id: GoToId) -> None: return self._logger_goto.info(f"Movement with {id} finished.") + + @abstractmethod + def _check_goto_parameters(self, duration: float, target: Any, q0: Optional[List[float]] = None) -> None: + """Check the validity of the parameters for a goto movement.""" + pass diff --git a/src/reachy2_sdk/parts/head.py b/src/reachy2_sdk/parts/head.py index e012002f..67c05f0c 100644 --- a/src/reachy2_sdk/parts/head.py +++ b/src/reachy2_sdk/parts/head.py @@ -3,7 +3,7 @@ Handles all specific methods to a Head. """ -from typing import Any, List, overload +from typing import Any, List, Optional, overload import grpc import numpy as np @@ -171,19 +171,19 @@ def goto( degrees (bool, optional): Specifies if the RPY values in `target` are in degrees. Defaults to True. Raises: - ValueError: If the `duration` is set to 0, or if the input type for `target` is invalid. + TypeError : If the input type for `target` is invalid + ValueError: If the `duration` is set to 0. Returns: GoToId: The unique identifier for the movement command. """ - if duration == 0: - raise ValueError("duration cannot be set to 0.") - if not self.neck.is_on(): self._logger.warning("head.neck is off. No command sent.") return GoToId(id=-1) - if isinstance(target, list) and len(target) == 3: + self._check_goto_parameters(duration, target) + + if isinstance(target, list): if degrees: target = np.deg2rad(target).tolist() joints_goal = NeckOrientation( @@ -197,8 +197,6 @@ def goto( ) elif isinstance(target, pyQuat): joints_goal = NeckOrientation(rotation=Rotation3d(q=Quaternion(w=target.w, x=target.x, y=target.y, z=target.z))) - else: - raise ValueError("Invalid input type for orientation. Must be either a list of 3 floats or a pyQuat.") request = GoToRequest( joints_goal=JointsGoal( @@ -222,6 +220,27 @@ def goto( self._wait_goto(response) return response + def _check_goto_parameters(self, duration: float, target: Any, q0: Optional[List[float]] = None) -> None: + """Check the validity of the parameters for the `goto` method. + + Args: + duration: The time in seconds for the movement to be completed. + target: The target position, either a list of joint positions or a quaternion. + + Raises: + TypeError: If the target is not a list or a quaternion. + ValueError: If the target list has a length other than 3. + ValueError: If the duration is set to 0. + """ + if not (isinstance(target, pyQuat) or isinstance(target, list)): + raise TypeError(f"Invalid orientation: must be either a list or a quaternion, got {type(target)}.") + + elif isinstance(target, list) and len(target) != 3: + raise ValueError(f"The joints list should be of length 3, got {len(target)}.") + + elif duration == 0: + raise ValueError("duration cannot be set to 0.") + def _goto_single_joint( self, neck_joint: int, diff --git a/src/reachy2_sdk/parts/joints_based_part.py b/src/reachy2_sdk/parts/joints_based_part.py index 2caa5736..bfaf811e 100644 --- a/src/reachy2_sdk/parts/joints_based_part.py +++ b/src/reachy2_sdk/parts/joints_based_part.py @@ -84,7 +84,7 @@ def set_torque_limits(self, torque_limit: int) -> None: specified as a float or int. """ if not isinstance(torque_limit, float | int): - raise ValueError(f"Expected one of: float, int for torque_limit, got {type(torque_limit).__name__}") + raise TypeError(f"Expected one of: float, int for torque_limit, got {type(torque_limit).__name__}") if not (0 <= torque_limit <= 100): raise ValueError(f"torque_limit must be in [0, 100], got {torque_limit}.") req = TorqueLimitRequest( @@ -101,7 +101,7 @@ def set_speed_limits(self, speed_limit: int) -> None: specified as a float or int. """ if not isinstance(speed_limit, float | int): - raise ValueError(f"Expected one of: float, int for speed_limit, got {type(speed_limit).__name__}") + raise TypeError(f"Expected one of: float, int for speed_limit, got {type(speed_limit).__name__}") if not (0 <= speed_limit <= 100): raise ValueError(f"speed_limit must be in [0, 100], got {speed_limit}.") req = SpeedLimitRequest( diff --git a/src/reachy2_sdk/utils/utils.py b/src/reachy2_sdk/utils/utils.py index 7517aa10..e3825293 100644 --- a/src/reachy2_sdk/utils/utils.py +++ b/src/reachy2_sdk/utils/utils.py @@ -59,7 +59,7 @@ def list_to_arm_position(positions: List[float], degrees: bool = True) -> ArmPos Args: positions: A list of float values representing joint positions. The list should contain 7 values - in the following order: [shoulder.pitch, shoulder.yaw, elbow.pitch, elbow.yaw, wrist.roll, wrist.pitch, wrist.yaw]. + in the following order: [shoulder_pitch, shoulder_roll, elbow_yaw, elbow_pitch, wrist_roll, wrist_pitch,wrist_yaw]. degrees: A flag indicating whether the input joint positions are in degrees. If set to `True`, the input positions are in degrees. Defaults to `True`. @@ -100,7 +100,7 @@ def arm_position_to_list(arm_pos: ArmPosition, degrees: bool = True) -> List[flo Returns: A list of joint positions based on the ArmPosition, returned in the following order: - [shoulder.pitch, shoulder.yaw, elbow.yaw, elbow.pitch, wrist.roll, wrist.pitch, wrist.yaw]. + [shoulder_pitch, shoulder_roll, elbow_yaw, elbow_pitch, wrist_roll, wrist_pitch, wrist_yaw]. """ positions = [] diff --git a/tests/units/offline/test_arm.py b/tests/units/offline/test_arm.py index cc1f440c..89354226 100644 --- a/tests/units/offline/test_arm.py +++ b/tests/units/offline/test_arm.py @@ -101,6 +101,19 @@ def test_class() -> None: assert isinstance(arm._actuators, dict) with pytest.raises(ValueError): + arm._check_goto_parameters(duration=0, target=[0, 0, 0, 0, 0, 0, 0]) + with pytest.raises(ValueError): + arm._check_goto_parameters(duration=2, target=[0, 0, 0, 0]) + with pytest.raises(TypeError): + arm._check_goto_parameters(duration=2, target="default") + with pytest.raises(ValueError): + arm._check_goto_parameters(duration=2, target=np.eye(3)) + with pytest.raises(TypeError): + arm._check_goto_parameters(duration=2, target=np.eye(4), q0=np.eye(4)) + with pytest.raises(ValueError): + arm._check_goto_parameters(duration=2, target=np.eye(4), q0=[0, 0, 0, 0, 0, 0]) + + with pytest.raises(TypeError): arm.set_speed_limits("wrong value") with pytest.raises(ValueError): @@ -109,7 +122,7 @@ def test_class() -> None: with pytest.raises(ValueError): arm.set_speed_limits(-10) - with pytest.raises(ValueError): + with pytest.raises(TypeError): arm.set_torque_limits("wrong value") with pytest.raises(ValueError): diff --git a/tests/units/offline/test_head.py b/tests/units/offline/test_head.py index 90be0e4b..10538632 100644 --- a/tests/units/offline/test_head.py +++ b/tests/units/offline/test_head.py @@ -1,4 +1,5 @@ import grpc +import numpy as np import pytest from google.protobuf.wrappers_pb2 import BoolValue, FloatValue from pyquaternion import Quaternion @@ -120,6 +121,13 @@ def test_class() -> None: assert isinstance(head._actuators, dict) with pytest.raises(ValueError): + head._check_goto_parameters(duration=0, target=[0, 0, 0]) + with pytest.raises(ValueError): + head._check_goto_parameters(duration=2, target=[0, 0, 0, 0]) + with pytest.raises(TypeError): + head._check_goto_parameters(duration=2, target=np.eye(4)) + + with pytest.raises(TypeError): head.set_speed_limits("wrong value") with pytest.raises(ValueError): @@ -128,7 +136,7 @@ def test_class() -> None: with pytest.raises(ValueError): head.set_speed_limits(-10) - with pytest.raises(ValueError): + with pytest.raises(TypeError): head.set_torque_limits("wrong value") with pytest.raises(ValueError): diff --git a/tests/units/offline/test_orbita2d.py b/tests/units/offline/test_orbita2d.py index cc537fca..3e06338d 100644 --- a/tests/units/offline/test_orbita2d.py +++ b/tests/units/offline/test_orbita2d.py @@ -84,7 +84,7 @@ def test_orbita2d() -> None: orbita2d.temperatures["motor_1"] == temperature.motor_1.value orbita2d.temperatures["motor_2"] == temperature.motor_1.value - with pytest.raises(ValueError): + with pytest.raises(TypeError): orbita2d.set_speed_limits("wrong value") with pytest.raises(ValueError): @@ -93,7 +93,7 @@ def test_orbita2d() -> None: with pytest.raises(ValueError): orbita2d.set_speed_limits(-10) - with pytest.raises(ValueError): + with pytest.raises(TypeError): orbita2d.set_torque_limits("wrong value") with pytest.raises(ValueError): diff --git a/tests/units/offline/test_orbita3d.py b/tests/units/offline/test_orbita3d.py index a90ec9c1..5847d94a 100644 --- a/tests/units/offline/test_orbita3d.py +++ b/tests/units/offline/test_orbita3d.py @@ -87,7 +87,7 @@ def test_class() -> None: orbita3d.temperatures["motor_2"] == temperature.motor_1.value orbita3d.temperatures["motor_3"] == temperature.motor_3.value - with pytest.raises(ValueError): + with pytest.raises(TypeError): orbita3d.set_speed_limits("wrong value") with pytest.raises(ValueError): @@ -96,7 +96,7 @@ def test_class() -> None: with pytest.raises(ValueError): orbita3d.set_speed_limits(-10) - with pytest.raises(ValueError): + with pytest.raises(TypeError): orbita3d.set_torque_limits("wrong value") with pytest.raises(ValueError):