Skip to content

Commit

Permalink
Merge pull request #431 from pollen-robotics/387-orbita-valueerror-sh…
Browse files Browse the repository at this point in the history
…ould-be-typeerror

Switch to TypeErrors
  • Loading branch information
ClaireHzl authored Oct 24, 2024
2 parents cb1f721 + 7fe6d86 commit d97a57f
Show file tree
Hide file tree
Showing 15 changed files with 138 additions and 58 deletions.
17 changes: 9 additions & 8 deletions src/reachy2_sdk/orbita/orbita.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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}.")

Expand All @@ -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}.")

Expand Down
9 changes: 5 additions & 4 deletions src/reachy2_sdk/orbita/orbita2d.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
"""
Expand Down
9 changes: 5 additions & 4 deletions src/reachy2_sdk/orbita/orbita3d.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
"""
Expand Down
4 changes: 2 additions & 2 deletions src/reachy2_sdk/orbita/orbita_axis.py
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
4 changes: 2 additions & 2 deletions src/reachy2_sdk/orbita/orbita_joint.py
Original file line number Diff line number Diff line change
Expand Up @@ -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__(
Expand Down
10 changes: 5 additions & 5 deletions src/reachy2_sdk/orbita/orbita_motor.py
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
58 changes: 45 additions & 13 deletions src/reachy2_sdk/parts/arm.py
Original file line number Diff line number Diff line change
Expand Up @@ -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).
Expand Down Expand Up @@ -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.")
Expand Down Expand Up @@ -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(
Expand All @@ -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,
Expand Down Expand Up @@ -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:
Expand Down
9 changes: 7 additions & 2 deletions src/reachy2_sdk/parts/goto_based_part.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
35 changes: 27 additions & 8 deletions src/reachy2_sdk/parts/head.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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(
Expand All @@ -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(
Expand All @@ -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,
Expand Down
4 changes: 2 additions & 2 deletions src/reachy2_sdk/parts/joints_based_part.py
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand All @@ -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(
Expand Down
4 changes: 2 additions & 2 deletions src/reachy2_sdk/utils/utils.py
Original file line number Diff line number Diff line change
Expand Up @@ -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`.
Expand Down Expand Up @@ -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 = []

Expand Down
15 changes: 14 additions & 1 deletion tests/units/offline/test_arm.py
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand All @@ -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):
Expand Down
Loading

0 comments on commit d97a57f

Please sign in to comment.