diff --git a/setup.cfg b/setup.cfg index 281d45ef..650df92e 100644 --- a/setup.cfg +++ b/setup.cfg @@ -20,7 +20,7 @@ install_requires = grpcio>=1.59.0, <=1.62.2 pyquaternion==0.9.9 opencv-python>=4.8.0, <4.9.0 - reachy2-sdk-api>=1.0.16, <1.1.0 + reachy2-sdk-api>=1.0.17, <1.1.0 [options.packages.find] where = src diff --git a/src/examples/3_arm_and_gripper.ipynb b/src/examples/3_arm_and_gripper.ipynb index fc9c2ecc..08682a39 100644 --- a/src/examples/3_arm_and_gripper.ipynb +++ b/src/examples/3_arm_and_gripper.ipynb @@ -105,7 +105,7 @@ "id": "10", "metadata": {}, "source": [ - "### Joint space" + "### Joint space goal" ] }, { @@ -433,7 +433,7 @@ "id": "43", "metadata": {}, "source": [ - "### Cartesian space" + "### Cartesian space goal" ] }, { @@ -540,7 +540,8 @@ "metadata": {}, "outputs": [], "source": [ - "reachy.l_arm.goto(new_pose)" + "reachy.l_arm.goto(new_pose)\n", + "reachy.l_arm.goto_posture()" ] }, { @@ -549,6 +550,151 @@ "metadata": {}, "source": [ "To simplify your life, you have access to functions to easily compute translation or rotation. \n", + "But first, have a look the interpolation space!" + ] + }, + { + "cell_type": "markdown", + "id": "884dfc68", + "metadata": {}, + "source": [ + "### Interpolation space" + ] + }, + { + "cell_type": "markdown", + "id": "7a4f0d79", + "metadata": {}, + "source": [ + "Let's make things a little more complicated now. \n", + "Sending a command in a space (joint or cartesian) does not mean the interpolation to reach the goal position is made in the same space. \n", + "\n", + "***What does that mean?*** \n", + "By default, the interpolation (all the intermediate positions the arm is going to take before reaching the goal position) is made in joint space: if the initial position of the `r_arm.shoulder.pitch` is `30` and we want to reach a position so that `r_arm.shoulder.pitch` is `15`, the goto will calculate the interpolation from 30 to 15 for the joint, **even if the goal position was given in cartesian_space**. The movement will be made regardless the intermediate positions of the end-effector in the cartesian space, only the intermediate positions of the joints are considered.\n", + "\n", + "> **Default interpolation space is `joint_space`**" + ] + }, + { + "cell_type": "markdown", + "id": "0572d110", + "metadata": {}, + "source": [ + "Here is an example to go through this. \n", + "From the `elbow_90` posture, we calculate a new pose that is 20cm ahead:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "ac548ed0", + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "from reachy2_sdk.utils.utils import get_pose_matrix\n", + "\n", + "initial_pose = get_pose_matrix([0.25, -0.3, -0.3], [0, -90, 0])\n", + "modified_pose = np.copy(initial_pose)\n", + "modified_pose[0, 3] += 0.25" + ] + }, + { + "cell_type": "markdown", + "id": "81b47732", + "metadata": {}, + "source": [ + "We send the arm to the `elbow_90` posture first:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "132db3d7", + "metadata": {}, + "outputs": [], + "source": [ + "reachy.r_arm.goto(initial_pose)" + ] + }, + { + "cell_type": "markdown", + "id": "2a5f57e9", + "metadata": {}, + "source": [ + "Then we send the arm to the new position, 20cm ahead. Watch carefully the trajectory of the gripper when moving to the new goal position:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "95fed794", + "metadata": {}, + "outputs": [], + "source": [ + "reachy.r_arm.goto(modified_pose)" + ] + }, + { + "cell_type": "markdown", + "id": "cec15f4e", + "metadata": {}, + "source": [ + "The gripper is going slightly down before reaching the target. This is because the movement was interpolated in joint space. \n", + "\n", + "Put back the robot in the initial_pose, and set the `interpolation_space` argument to `cartesian_space` to reach the modified goal:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "07c84f62", + "metadata": {}, + "outputs": [], + "source": [ + "reachy.r_arm.goto(initial_pose)\n", + "reachy.r_arm.goto(modified_pose, interpolation_space=\"cartesian_space\")" + ] + }, + { + "cell_type": "markdown", + "id": "f99a2c13", + "metadata": {}, + "source": [ + "The gripper is not following the same trajectory as previsouly, as the interpolation is now made in cartesian space." + ] + }, + { + "cell_type": "markdown", + "id": "0a296552", + "metadata": {}, + "source": [ + "### Relative moves: translate_by / rotate_by" + ] + }, + { + "cell_type": "markdown", + "id": "9bb293a9", + "metadata": {}, + "source": [ + "The `translate_by` and `rotate_by` methods are here to simplify simple translations and rotations. \n", + "\n", + "Note that if you use the `translate_by` method, which is a goto-based method, the interpolation space will be set bu default to cartesian_space." + ] + }, + { + "cell_type": "markdown", + "id": "323cf49c", + "metadata": {}, + "source": [ + "> **Default interpolation space for translate_by is `cartesian_space`**" + ] + }, + { + "cell_type": "markdown", + "id": "1e7bbced", + "metadata": {}, + "source": [ "Use the `translate_by(...)` method to send the gripper back to the previous pose, asking for a translation 10cm back (-10cm on Reachy's x axis):" ] }, @@ -559,7 +705,7 @@ "metadata": {}, "outputs": [], "source": [ - "reachy.l_arm.translate_by(-0.1, 0, 0, frame='robot')" + "reachy.r_arm.translate_by(-0.25, 0, 0, frame='robot')" ] }, { @@ -577,7 +723,7 @@ "metadata": {}, "outputs": [], "source": [ - "reachy.l_arm.rotate_by(10, 0, 0, frame='gripper')" + "reachy.r_arm.rotate_by(10, 0, 0, frame='gripper')" ] }, { @@ -606,7 +752,7 @@ "metadata": {}, "outputs": [], "source": [ - "reachy.l_arm.get_translation_by(-0.1, 0.2, -0.1, frame='gripper')" + "reachy.r_arm.get_translation_by(-0.1, 0.2, -0.1, frame='gripper')" ] }, { @@ -616,7 +762,17 @@ "metadata": {}, "outputs": [], "source": [ - "reachy.l_arm.get_rotation_by(-10, 20, 0, frame='gripper')" + "reachy.r_arm.get_rotation_by(-10, 20, 0, frame='gripper')" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "6d777afc", + "metadata": {}, + "outputs": [], + "source": [ + "reachy.r_arm.goto_posture()" ] }, { diff --git a/src/examples/4_head_control.ipynb b/src/examples/4_head_control.ipynb index 5ab7cf75..4fb24352 100644 --- a/src/examples/4_head_control.ipynb +++ b/src/examples/4_head_control.ipynb @@ -1,7 +1,6 @@ { "cells": [ { - "attachments": {}, "cell_type": "markdown", "id": "0", "metadata": {}, @@ -10,7 +9,6 @@ ] }, { - "attachments": {}, "cell_type": "markdown", "id": "1", "metadata": {}, @@ -21,7 +19,6 @@ ] }, { - "attachments": {}, "cell_type": "markdown", "id": "2", "metadata": {}, @@ -30,7 +27,6 @@ ] }, { - "attachments": {}, "cell_type": "markdown", "id": "3", "metadata": {}, @@ -51,7 +47,6 @@ ] }, { - "attachments": {}, "cell_type": "markdown", "id": "5", "metadata": {}, @@ -70,7 +65,6 @@ ] }, { - "attachments": {}, "cell_type": "markdown", "id": "7", "metadata": {}, @@ -96,7 +90,6 @@ ] }, { - "attachments": {}, "cell_type": "markdown", "id": "9", "metadata": {}, @@ -105,7 +98,6 @@ ] }, { - "attachments": {}, "cell_type": "markdown", "id": "10", "metadata": {}, @@ -114,7 +106,6 @@ ] }, { - "attachments": {}, "cell_type": "markdown", "id": "11", "metadata": {}, @@ -125,7 +116,6 @@ ] }, { - "attachments": {}, "cell_type": "markdown", "id": "12", "metadata": {}, @@ -154,7 +144,6 @@ ] }, { - "attachments": {}, "cell_type": "markdown", "id": "14", "metadata": {}, @@ -179,7 +168,6 @@ ] }, { - "attachments": {}, "cell_type": "markdown", "id": "16", "metadata": {}, @@ -188,7 +176,6 @@ ] }, { - "attachments": {}, "cell_type": "markdown", "id": "17", "metadata": {}, @@ -203,7 +190,6 @@ ] }, { - "attachments": {}, "cell_type": "markdown", "id": "19", "metadata": {}, @@ -271,7 +257,6 @@ ] }, { - "attachments": {}, "cell_type": "markdown", "id": "21", "metadata": {}, @@ -302,7 +287,6 @@ ] }, { - "attachments": {}, "cell_type": "markdown", "id": "23", "metadata": {}, @@ -325,7 +309,6 @@ ] }, { - "attachments": {}, "cell_type": "markdown", "id": "29", "metadata": {}, @@ -344,6 +327,56 @@ " \n", "reachy.turn_off_smoothly()" ] + }, + { + "cell_type": "markdown", + "id": "5b478c4e", + "metadata": {}, + "source": [ + "## Antennas goto" + ] + }, + { + "cell_type": "markdown", + "id": "12b1a208", + "metadata": {}, + "source": [ + "The antennas can be accessed through the head, using `reachy.head.l_antenna` and `reachy.head.r_antenna`:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "0af15dc0", + "metadata": {}, + "outputs": [], + "source": [ + "print(reachy.head.r_antenna)\n", + "print(reachy.head.l_antenna)" + ] + }, + { + "cell_type": "markdown", + "id": "f45b3140", + "metadata": {}, + "source": [ + "The methods to control the antennas are quite close to the one available for the head:\n", + "- using the `goto` methods, which works like in the [goto_introduction](2_goto_introduction.ipynb) example\n", + "- controlling the joint goal positions, namely **reachy.head.r_antenna** and **reachy.head.l_antenna**" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "e9c33233", + "metadata": {}, + "outputs": [], + "source": [ + "reachy.head.r_antenna.goto(20, duration=0.5)\n", + "reachy.head.l_antenna.goto(-20, duration=0.5)\n", + "reachy.head.r_antenna.goto(0, duration=0.5)\n", + "reachy.head.l_antenna.goto(0, duration=0.5)" + ] } ], "metadata": { diff --git a/src/reachy2_sdk/__init__.py b/src/reachy2_sdk/__init__.py index 4a4af7ae..86eed50b 100644 --- a/src/reachy2_sdk/__init__.py +++ b/src/reachy2_sdk/__init__.py @@ -20,4 +20,4 @@ from .reachy_sdk import ReachySDK # noqa: F401 -__version__ = "1.0.9" +__version__ = "1.0.10" diff --git a/src/reachy2_sdk/components/antenna.py b/src/reachy2_sdk/components/antenna.py new file mode 100644 index 00000000..1ad9eca4 --- /dev/null +++ b/src/reachy2_sdk/components/antenna.py @@ -0,0 +1,264 @@ +"""Reachy Antenna module. + +Handles all specific methods to Antennas. +""" + +import logging +from typing import Any, Dict, List, Optional + +import numpy as np +from google.protobuf.wrappers_pb2 import FloatValue +from grpc import Channel +from reachy2_sdk_api.component_pb2 import ComponentId +from reachy2_sdk_api.dynamixel_motor_pb2 import DynamixelMotor as DynamixelMotor_proto +from reachy2_sdk_api.dynamixel_motor_pb2 import ( + DynamixelMotorState, + DynamixelMotorStatus, +) +from reachy2_sdk_api.goto_pb2 import GoToId, GoToRequest, JointsGoal +from reachy2_sdk_api.goto_pb2_grpc import GoToServiceStub +from reachy2_sdk_api.head_pb2 import AntennaJointGoal + +from ..dynamixel.dynamixel_motor import DynamixelMotor +from ..parts.part import Part +from ..utils.utils import get_grpc_interpolation_mode +from .goto_based_component import IGoToBasedComponent + + +class Antenna(IGoToBasedComponent): + """The Antenna class represents any antenna of the robot's head.""" + + def __init__( + self, + uid: int, + name: str, + initial_state: DynamixelMotorState, + grpc_channel: Channel, + goto_stub: GoToServiceStub, + part: Part, + ): + """Initialize the Antenna with its initial state and configuration. + + Args: + uid: The unique identifier of the component. + name: The name of the joint. + initial_state: A dictionary containing the initial state of the joint, with + each entry representing a specific parameter of the joint (e.g., present position). + grpc_channel: The gRPC channel used to communicate with the DynamixelMotor service. + goto_stub: The gRPC stub for controlling goto movements. + part: The part to which this joint belongs. + """ + self._logger = logging.getLogger(__name__) + IGoToBasedComponent.__init__(self, ComponentId(id=uid, name=name), goto_stub) + self._part = part + self._error_status: Optional[str] = None + self._joints: Dict[str, Any] = {} + if name == "antenna_left": + self._name = "l_antenna" + else: + self._name = "r_antenna" + self._joints[self._name] = DynamixelMotor(uid, name, initial_state, grpc_channel) + + def _check_goto_parameters(self, target: Any, duration: Optional[float], 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, float) or isinstance(target, int)): + raise TypeError(f"Antenna's goto target must be either a float or int, got {type(target)}.") + + elif duration == 0: + raise ValueError("duration cannot be set to 0.") + + def goto_posture( + self, + common_posture: str = "default", + duration: float = 2, + wait: bool = False, + wait_for_goto_end: bool = True, + interpolation_mode: str = "minimum_jerk", + ) -> GoToId: + """Send the antenna to standard positions within the specified duration. + + The default posture sets the antenna is 0.0. + + Args: + common_posture: The standard positions to which all joints will be sent. + It can be 'default' or 'elbow_90'. Defaults to 'default'. + duration: The time duration in seconds for the robot to move to the specified posture. + Defaults to 2. + wait: Determines whether the program should wait for the movement to finish before + returning. If set to `True`, the program waits for the movement to complete before continuing + execution. Defaults to `False`. + wait_for_goto_end: Specifies whether commands will be sent to a part immediately or + only after all previous commands in the queue have been executed. If set to `False`, the program + will cancel all executing moves and queues. Defaults to `True`. + interpolation_mode: The type of interpolation used when moving the arm's joints. + Can be 'minimum_jerk' or 'linear'. Defaults to 'minimum_jerk'. + + Returns: + The unique GoToId associated with the movement command. + """ + if not wait_for_goto_end: + self.cancel_all_goto() + if self.is_on(): + return self.goto(0, duration, wait, interpolation_mode) + else: + self._logger.warning(f"{self._name} is off. No command sent.") + return GoToId(id=-1) + + def goto( + self, + target: float, + duration: float = 2.0, + wait: bool = False, + interpolation_mode: str = "minimum_jerk", + degrees: bool = True, + ) -> GoToId: + """Send the antenna to a specified goal position. + + Args: + target: The desired goal position for the antenna. + duration: The time in seconds for the movement to be completed. Defaults to 2. + wait: If True, the function waits until the movement is completed before returning. + Defaults to False. + interpolation_mode: The interpolation method to be used. It can be either "minimum_jerk" + or "linear". Defaults to "minimum_jerk". + degrees: If True, the joint value in the `target` argument is treated as degrees. + Defaults to True. + + Raises: + 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 not self.is_on(): + self._logger.warning(f"head.{self._name} is off. No command sent.") + return GoToId(id=-1) + + self._check_goto_parameters(target, duration) + + if degrees: + target = np.deg2rad(target) + + request = GoToRequest( + joints_goal=JointsGoal( + antenna_joint_goal=AntennaJointGoal( + id=self._part._part_id, + antenna=DynamixelMotor_proto( + id=ComponentId(id=self._joints[self._name]._id, name=self._joints[self._name]._name), + ), + joint_goal=FloatValue(value=target), + duration=FloatValue(value=duration), + ) + ), + interpolation_mode=get_grpc_interpolation_mode(interpolation_mode), + ) + + response = self._goto_stub.GoToJoints(request) + + if response.id == -1: + self._logger.error(f"Position {target} was not reachable. No command sent.") + elif wait: + self._wait_goto(response, duration) + return response + + def __repr__(self) -> str: + """Clean representation of the Antenna only joint (DynamixelMotor).""" + return str(self._joints[self._name].__repr__()) + + def turn_on(self) -> None: + """Turn on the antenna's motor.""" + self._joints[self._name].turn_on() + + def turn_off(self) -> None: + """Turn off the antenna's motor.""" + self._joints[self._name].turn_off() + + def is_on(self) -> bool: + """Check if the antenna is currently stiff. + + Returns: + `True` if the antenna's motor is stiff (not compliant), `False` otherwise. + """ + return bool(self._joints[self._name].is_on()) + + @property + def present_position(self) -> float: + """Get the present position of the joint in degrees.""" + return float(self._joints[self._name].present_position) + + @property + def goal_position(self) -> float: + """Get the goal position of the joint in degrees.""" + return float(self._joints[self._name].goal_position) + + @goal_position.setter + def goal_position(self, value: float | int) -> None: + """Set the goal position of the joint in degrees. + + The goal position is not send to the joint immediately, it is stored locally until the `send_goal_positions` method + is called. + + Args: + value: The goal position to set, specified as a float or int. + + Raises: + TypeError: If the provided value is not a float or int. + """ + self._joints[self._name].goal_position = value + + def send_goal_positions(self, check_positions: bool = True) -> None: + """Send goal positions to the motor. + + If goal positions have been specified, sends them to the motor. + Args : + check_positions: A boolean indicating whether to check the positions after sending the command. + Defaults to True. + """ + self._joints[self._name].send_goal_positions(check_positions) + + def set_speed_limits(self, speed_limit: float | int) -> None: + """Set the speed limit as a percentage of the maximum speed the motor. + + Args: + speed_limit: The desired speed limit as a percentage (0-100) of the maximum speed. Can be + specified as a float or int. + """ + self._joints[self._name].set_speed_limits(speed_limit) + + def _update_with(self, new_state: DynamixelMotorState) -> None: + """Update the present and goal positions of the joint with new state values. + + Args: + new_state: A dictionary containing the new state values for the joint. The keys should include + "present_position" and "goal_position", with corresponding FloatValue objects as values. + """ + self._joints[self._name]._update_with(new_state) + + @property + def audit(self) -> Optional[str]: + """Get the current audit status of the actuator. + + Returns: + The audit status as a string, representing the latest error or status + message, or `None` if there is no error. + """ + pass + + def _update_audit_status(self, new_status: DynamixelMotorStatus) -> None: + """Update the audit status based on the new status data. + + Args: + new_status: The new status data, as a DynamixelMotorStatus object, containing error details. + """ + pass diff --git a/src/reachy2_sdk/components/goto_based_component.py b/src/reachy2_sdk/components/goto_based_component.py new file mode 100644 index 00000000..b8cf514c --- /dev/null +++ b/src/reachy2_sdk/components/goto_based_component.py @@ -0,0 +1,57 @@ +"""Reachy IGoToBasedComponent interface. + +Handles common interface for components performing movement using goto mechanism. +""" + +from typing import List + +from reachy2_sdk_api.component_pb2 import ComponentId +from reachy2_sdk_api.goto_pb2 import GoToAck, GoToId +from reachy2_sdk_api.goto_pb2_grpc import GoToServiceStub + +from ..utils.goto_based_element import IGoToBasedElement + + +class IGoToBasedComponent(IGoToBasedElement): + """Interface for components of Reachy that use goto functions. + + The `IGoToBasedComponent` class defines a common interface for handling goto-based movements. It is + designed to be implemented by components of the robot that perform movements via the goto mechanism. + It is used by the Antenna class to handle goto-based movements, and is based on the IGoToBasedElement. + """ + + def __init__( + self, + component_id: ComponentId, + goto_stub: GoToServiceStub, + ) -> None: + """Initialize the IGoToBasedComponent interface. + + Sets up the common attributes needed for handling goto-based movements. This includes + associating the component with the interface and setting up the gRPC stub for performing + goto commands. + + Args: + component_id: The robot component that uses this interface. + goto_stub: The gRPC stub used to send goto commands to the robot component. + """ + super().__init__(component_id, goto_stub) + + def get_goto_playing(self) -> GoToId: + """Return the GoToId of the currently playing goto movement on a specific component.""" + response = self._goto_stub.GetComponentGoToPlaying(self._element_id) + return response + + def get_goto_queue(self) -> List[GoToId]: + """Return a list of all GoToIds waiting to be played on a specific component.""" + response = self._goto_stub.GetComponentGoToQueue(self._element_id) + return [goal_id for goal_id in response.goto_ids] + + def cancel_all_goto(self) -> GoToAck: + """Request the cancellation of all playing and waiting goto commands for a specific component. + + Returns: + A GoToAck acknowledging the cancellation of all goto commands. + """ + response = self._goto_stub.CancelComponentAllGoTo(self._element_id) + return response diff --git a/src/reachy2_sdk/dynamixel/dynamixel_motor.py b/src/reachy2_sdk/dynamixel/dynamixel_motor.py new file mode 100644 index 00000000..76014f1e --- /dev/null +++ b/src/reachy2_sdk/dynamixel/dynamixel_motor.py @@ -0,0 +1,181 @@ +"""Reachy DynamixelMotor module. + +Handles all specific methods to DynamixelMotor. +""" + +import logging +from typing import Optional + +from google.protobuf.wrappers_pb2 import BoolValue, FloatValue +from grpc import Channel +from reachy2_sdk_api.component_pb2 import ComponentId +from reachy2_sdk_api.dynamixel_motor_pb2 import ( + DynamixelMotorCommand, + DynamixelMotorsCommand, + DynamixelMotorState, +) +from reachy2_sdk_api.dynamixel_motor_pb2_grpc import DynamixelMotorServiceStub + +from ..orbita.utils import to_internal_position, to_position + + +class DynamixelMotor: + """The DynamixelMotor class represents any Dynamixel motor. + + The DynamixelMotor class is used to store the up-to-date state of the motor, especially: + - its present_position (RO) + - its goal_position (RW) + """ + + def __init__( + self, + uid: int, + name: str, + initial_state: DynamixelMotorState, + grpc_channel: Channel, + ): + """Initialize the DynamixelMotor with its initial state and configuration. + + This sets up the motor by assigning its state based on the provided initial values. + + Args: + uid: The unique identifier of the component. + name: The name of the component. + initial_state: A dictionary containing the initial state of the joint, with + each entry representing a specific parameter of the joint (e.g., present position). + grpc_channel: The gRPC channel used to communicate with the DynamixelMotor service. + """ + self._logger = logging.getLogger(__name__) + self._name = name + self._id = uid + self._stub = DynamixelMotorServiceStub(grpc_channel) + self._update_with(initial_state) + self._outgoing_goal_position: Optional[float] = None + + def __repr__(self) -> str: + """Clean representation of the DynamixelMotor.""" + repr_template = "" + return repr_template.format( + dxl_on=self.is_on(), + present_position=round(self.present_position, 2), + goal_position=round(self.goal_position, 2), + ) + + def turn_on(self) -> None: + """Turn on the motor.""" + self._set_compliant(False) + + def turn_off(self) -> None: + """Turn off the motor.""" + self._set_compliant(True) + + def is_on(self) -> bool: + """Check if the dynamixel motor is currently stiff. + + Returns: + `True` if the motor is stiff (not compliant), `False` otherwise. + """ + return not self._compliant + + @property + def present_position(self) -> float: + """Get the present position of the joint in degrees.""" + return to_position(self._present_position) + + @property + def goal_position(self) -> float: + """Get the goal position of the joint in degrees.""" + return to_position(self._goal_position) + + @goal_position.setter + def goal_position(self, value: float | int) -> None: + """Set the goal position of the joint in degrees. + + The goal position is not send to the joint immediately, it is stored locally until the `send_goal_positions` method + is called. + + Args: + value: The goal position to set, specified as a float or int. + + Raises: + TypeError: If the provided value is not a float or int. + """ + if isinstance(value, float) | isinstance(value, int): + self._outgoing_goal_position = to_internal_position(value) + else: + raise TypeError("goal_position must be a float or int") + + def _set_compliant(self, compliant: bool) -> None: + """Set the compliance mode of the motor. + + Compliance mode determines whether the motor is stiff or compliant. + + Args: + compliant: A boolean value indicating whether to set the motor to + compliant (`True`) or stiff (`False`). + """ + command = DynamixelMotorsCommand( + cmd=[ + DynamixelMotorCommand( + id=ComponentId(id=self._id), + compliant=BoolValue(value=compliant), + ) + ] + ) + self._stub.SendCommand(command) + + def send_goal_positions(self, check_positions: bool = True) -> None: + """Send goal positions to the motor. + + If goal positions have been specified, sends them to the motor. + Args : + check_positions: A boolean indicating whether to check the positions after sending the command. + Defaults to True. + """ + if self._outgoing_goal_position is not None: + command = DynamixelMotorsCommand( + cmd=[ + DynamixelMotorCommand( + id=ComponentId(id=self._id), + goal_position=FloatValue(value=self._outgoing_goal_position), + ) + ] + ) + self._outgoing_goal_position = None + self._stub.SendCommand(command) + if check_positions: + # self._post_send_goal_positions() + pass + + def set_speed_limits(self, speed_limit: float | int) -> None: + """Set the speed limit as a percentage of the maximum speed the motor. + + Args: + speed_limit: The desired speed limit as a percentage (0-100) of the maximum speed. Can be + specified as a float or int. + """ + if not isinstance(speed_limit, float | int): + 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}.") + speed_limit = speed_limit / 100.0 + command = DynamixelMotorsCommand( + cmd=[ + DynamixelMotorCommand( + id=ComponentId(id=self._id), + speed_limit=FloatValue(value=speed_limit), + ) + ] + ) + self._stub.SendCommand(command) + + def _update_with(self, new_state: DynamixelMotorState) -> None: + """Update the present and goal positions of the joint with new state values. + + Args: + new_state: A dictionary containing the new state values for the joint. The keys should include + "present_position" and "goal_position", with corresponding FloatValue objects as values. + """ + self._present_position = new_state.present_position.value + self._goal_position = new_state.goal_position.value + self._compliant = new_state.compliant.value diff --git a/src/reachy2_sdk/parts/arm.py b/src/reachy2_sdk/parts/arm.py index 381af0ec..595514c3 100644 --- a/src/reachy2_sdk/parts/arm.py +++ b/src/reachy2_sdk/parts/arm.py @@ -10,7 +10,6 @@ import numpy as np import numpy.typing as npt from google.protobuf.wrappers_pb2 import FloatValue -from pyquaternion import Quaternion from reachy2_sdk_api.arm_pb2 import Arm as Arm_proto from reachy2_sdk_api.arm_pb2 import ( # ArmLimits,; ArmTemperatures, ArmCartesianGoal, @@ -27,6 +26,7 @@ from reachy2_sdk_api.goto_pb2 import ( CartesianGoal, CustomJointGoal, + EllipticalGoToParameters, GoToId, GoToRequest, JointsGoal, @@ -40,9 +40,9 @@ from ..orbita.orbita3d import Orbita3d from ..utils.utils import ( arm_position_to_list, - decompose_matrix, + get_grpc_arc_direction, get_grpc_interpolation_mode, - get_normal_vector, + get_grpc_interpolation_space, list_to_arm_position, matrix_from_euler_angles, recompose_matrix, @@ -89,7 +89,7 @@ def __init__( goto_stub: The gRPC stub for controlling goto movements. """ JointsBasedPart.__init__(self, arm_msg, grpc_channel, ArmServiceStub(grpc_channel)) - IGoToBasedPart.__init__(self, self, goto_stub) + IGoToBasedPart.__init__(self, self._part_id, goto_stub) self._setup_arm(arm_msg, initial_state) self._gripper: Optional[Hand] = None @@ -374,6 +374,7 @@ def goto( target: List[float], duration: float = 2, wait: bool = False, + interpolation_space: str = "joint_space", interpolation_mode: str = "minimum_jerk", degrees: bool = True, q0: Optional[List[float]] = None, @@ -386,9 +387,12 @@ def goto( target: npt.NDArray[np.float64], duration: float = 2, wait: bool = False, + interpolation_space: str = "joint_space", interpolation_mode: str = "minimum_jerk", degrees: bool = True, q0: Optional[List[float]] = None, + arc_direction: str = "above", + secondary_radius: Optional[float] = None, ) -> GoToId: ... # pragma: no cover @@ -397,9 +401,12 @@ def goto( target: Any, duration: float = 2, wait: bool = False, + interpolation_space: str = "joint_space", interpolation_mode: str = "minimum_jerk", degrees: bool = True, q0: Optional[List[float]] = None, + arc_direction: str = "above", + secondary_radius: Optional[float] = None, ) -> GoToId: """Move the arm to a specified target position, either in joint space or Cartesian space. @@ -416,12 +423,17 @@ def goto( duration: The time in seconds for the movement to be completed. Defaults to 2. wait: If True, the function waits until the movement is completed before returning. Defaults to False. + interpolation_space: The space in which the interpolation should be performed. It can + be either "joint_space" or "cartesian_space". Defaults to "joint_space". interpolation_mode: The interpolation method to be used. It can be either "minimum_jerk" or "linear". Defaults to "minimum_jerk". degrees: If True, the joint values in the `target` argument are treated as degrees. Defaults to True. q0: An optional list of 7 joint values representing the initial configuration for inverse kinematics. Defaults to None. + arc_direction: The direction of the arc to be followed during elliptical interpolation. + Can be "above", "below", "front", "back", "left" or "right" . Defaults to "above". + secondary_radius: The secondary radius of the ellipse for elliptical interpolation, in meters. Returns: GoToId: The unique GoToId identifier for the movement command. @@ -440,10 +452,25 @@ def goto( self._logger.warning(f"{self._part_id.name} is off. Goto not sent.") return GoToId(id=-1) + if interpolation_space == "joint_space" and interpolation_mode == "elliptical": + self._logger.warning("Elliptical interpolation is not supported in joint space. Switching to linear.") + interpolation_mode = "linear" + if secondary_radius is not None and secondary_radius > 0.3: + self._logger.warning("Interpolation secondary_radius was too large, reduced to 0.3") + secondary_radius = 0.3 + if isinstance(target, list) and len(target) == 7: - response = self._goto_joints(target, duration, interpolation_mode, degrees) + response = self._goto_joints( + target, + duration, + interpolation_space, + interpolation_mode, + degrees, + ) elif isinstance(target, np.ndarray) and target.shape == (4, 4): - response = self._goto_from_matrix(target, duration, interpolation_mode, q0) + response = self._goto_from_matrix( + target, duration, interpolation_space, interpolation_mode, q0, arc_direction, secondary_radius + ) if response.id == -1: self._logger.error("Target was not reachable. No command sent.") @@ -452,12 +479,21 @@ def goto( return response - def _goto_joints(self, target: List[float], duration: float, interpolation_mode: str, degrees: bool) -> GoToId: + def _goto_joints( + self, + target: List[float], + duration: float, + interpolation_space: str, + interpolation_mode: str, + degrees: bool, + ) -> GoToId: """Handle movement to a specified position in joint space. Args: target: A list of 7 joint positions to move the arm to. duration: The time in seconds for the movement to be completed. + interpolation_space: The space in which the interpolation should be performed. + Only "joint_space" is supported for joints target. interpolation_mode: The interpolation method to be used. Can be "minimum_jerk" or "linear". degrees: If True, the joint positions are interpreted as degrees; otherwise, as radians. @@ -467,16 +503,37 @@ def _goto_joints(self, target: List[float], duration: float, interpolation_mode: if isinstance(target, np.ndarray): target = target.tolist() arm_pos = list_to_arm_position(target, degrees) - request = GoToRequest( - joints_goal=JointsGoal( + + if interpolation_space == "cartesian_space": + self._logger.warning( + "cartesian_space interpolation is not supported using joints target. Switching to joint_space interpolation." + ) + interpolation_space == "joint_space" + if interpolation_mode == "elliptical": + self._logger.warning("Elliptical interpolation is not supported in joint space. Switching to linear.") + interpolation_mode = "linear" + + req_params = { + "joints_goal": JointsGoal( arm_joint_goal=ArmJointGoal(id=self._part_id, joints_goal=arm_pos, duration=FloatValue(value=duration)) ), - interpolation_mode=get_grpc_interpolation_mode(interpolation_mode), - ) + "interpolation_space": get_grpc_interpolation_space(interpolation_space), + "interpolation_mode": get_grpc_interpolation_mode(interpolation_mode), + } + + request = GoToRequest(**req_params) + return self._goto_stub.GoToJoints(request) def _goto_from_matrix( - self, target: npt.NDArray[np.float64], duration: float, interpolation_mode: str, q0: Optional[List[float]] + self, + target: npt.NDArray[np.float64], + duration: float, + interpolation_space: str, + interpolation_mode: str, + q0: Optional[List[float]], + arc_direction: str, + secondary_radius: Optional[float], ) -> GoToId: """Handle movement to a Cartesian target using a 4x4 transformation matrix. @@ -487,8 +544,13 @@ def _goto_from_matrix( Args: target: A 4x4 NumPy array representing the Cartesian target pose. duration: The time in seconds for the movement to be completed. - interpolation_mode: The interpolation method to be used. Can be "minimum_jerk" or "linear". + interpolation_space: The space in which the interpolation should be performed. Can be "joint_space" + or "cartesian_space". + interpolation_mode: The interpolation method to be used. Can be "minimum_jerk", "linear" or "elliptical". q0: An optional list of 7 joint positions representing the initial configuration. Defaults to None. + arc_direction: The direction of the arc to be followed during elliptical interpolation. Can be "above", + "below", "front", "back", "left" or "right". + secondary_radius: The secondary radius of the ellipse for elliptical interpolation, in meters. Returns: GoToId: A unique identifier for the movement command. @@ -497,8 +559,9 @@ def _goto_from_matrix( ValueError: If the length of `q0` is not 7. """ goal_pose = Matrix4x4(data=target.flatten().tolist()) - request = GoToRequest( - cartesian_goal=CartesianGoal( + + req_params = { + "cartesian_goal": CartesianGoal( arm_cartesian_goal=ArmCartesianGoal( id=self._part_id, goal_pose=goal_pose, @@ -506,8 +569,21 @@ def _goto_from_matrix( q0=list_to_arm_position(q0) if q0 is not None else None, ) ), - interpolation_mode=get_grpc_interpolation_mode(interpolation_mode), - ) + "interpolation_space": get_grpc_interpolation_space(interpolation_space), + "interpolation_mode": get_grpc_interpolation_mode(interpolation_mode), + } + + if interpolation_mode == "elliptical": + ellipse_params = { + "arc_direction": get_grpc_arc_direction(arc_direction), + } + if secondary_radius is not None: + ellipse_params["secondary_radius"] = FloatValue(value=secondary_radius) + elliptical_params = EllipticalGoToParameters(**ellipse_params) + req_params["elliptical_parameters"] = elliptical_params + + request = GoToRequest(**req_params) + return self._goto_stub.GoToCartesian(request) def _check_goto_parameters(self, target: Any, duration: Optional[float] = 0, q0: Optional[List[float]] = None) -> None: @@ -629,7 +705,7 @@ def goto_posture( if not wait_for_goto_end: self.cancel_all_goto() if self.is_on(): - return self.goto(joints, duration, wait, interpolation_mode) + return self.goto(joints, duration, wait, interpolation_mode=interpolation_mode) else: self._logger.warning(f"{self._part_id.name} is off. No command sent.") return GoToId(id=-1) @@ -727,7 +803,10 @@ def translate_by( duration: float = 2, wait: bool = False, frame: str = "robot", + interpolation_space: str = "cartesian_space", interpolation_mode: str = "minimum_jerk", + arc_direction: str = "above", + secondary_radius: Optional[float] = None, ) -> GoToId: """Create a translation movement for the arm's end effector. @@ -768,12 +847,23 @@ def translate_by( joints_request = None if joints_request is not None: - pose = self.forward_kinematics(joints_request.request.goal_positions) + if joints_request.request.target.joints is not None: + pose = self.forward_kinematics(joints_request.request.target.joints) + else: + pose = joints_request.request.target.pose else: pose = self.forward_kinematics() pose = self.get_translation_by(x, y, z, initial_pose=pose, frame=frame) - return self.goto(pose, duration=duration, wait=wait, interpolation_mode=interpolation_mode) + return self.goto( + pose, + duration=duration, + wait=wait, + interpolation_space=interpolation_space, + interpolation_mode=interpolation_mode, + arc_direction=arc_direction, + secondary_radius=secondary_radius, + ) def get_rotation_by( self, @@ -885,7 +975,10 @@ def rotate_by( joints_request = None if joints_request is not None: - pose = self.forward_kinematics(joints_request.request.goal_positions) + if joints_request.request.target.joints is not None: + pose = self.forward_kinematics(joints_request.request.target.joints) + else: + pose = joints_request.request.target.pose else: pose = self.forward_kinematics() @@ -904,236 +997,6 @@ def rotate_by( # temperatures = self._arm_stub.GetTemperatures(self._part_id) # return temperatures - def send_cartesian_interpolation( - self, - target: npt.NDArray[np.float64], - duration: float = 2, - arc_direction: Optional[str] = None, - elliptic_radius: Optional[float] = None, - interpolation_frequency: float = 120, - precision_distance_xyz: float = 0.003, - ) -> None: - """Perform Cartesian interpolation and move the arm towards a target pose. - - The function uses linear or elliptical interpolation for translation to reach or get close - to the specified target pose. - - Args: - target: A 4x4 homogeneous pose matrix representing the desired - position and orientation in the Reachy coordinate system, provided as a NumPy array. - duration: The expected time in seconds for the arm to reach the target position - from its current position. Defaults to 2. - arc_direction: The direction for elliptic interpolation when moving - the arm towards the target pose. Can be 'above', 'below', 'right', 'left', 'front', - or 'back'. If not specified, a linear interpolation is computed. - elliptic_radius: The second radius of the computed ellipse for elliptical - interpolation. The first radius is the distance between the current pose and the - target pose. If not specified, a circular interpolation is used. - interpolation_frequency: The number of intermediate points used to interpolate - the movement in Cartesian space between the initial and target poses. Defaults to 120. - precision_distance_xyz: The maximum allowed distance in meters in the XYZ space between - 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: - raise ValueError("duration cannot be set to 0.") - if self.is_off(): - self._logger.warning(f"{self._part_id.name} is off. Commands not sent.") - return - try: - self.inverse_kinematics(target) - except ValueError: - raise ValueError(f"Target pose: \n{target}\n is not reachable!") - - origin_matrix = self.forward_kinematics() - nb_steps = int(duration * interpolation_frequency) - time_step = duration / nb_steps - - q1, trans1 = decompose_matrix(origin_matrix) - q2, trans2 = decompose_matrix(target) - - if arc_direction is None: - self._send_linear_interpolation(trans1, trans2, q1, q2, nb_steps=nb_steps, time_step=time_step) - - else: - self._send_elliptical_interpolation( - trans1, - trans2, - q1, - q2, - arc_direction=arc_direction, - secondary_radius=elliptic_radius, - nb_steps=nb_steps, - time_step=time_step, - ) - - current_pose = self.forward_kinematics() - current_precision_distance_xyz = np.linalg.norm(current_pose[:3, 3] - target[:3, 3]) - if current_precision_distance_xyz > precision_distance_xyz: - request = ArmCartesianGoal( - id=self._part_id, - goal_pose=Matrix4x4(data=target.flatten().tolist()), - ) - self._stub.SendArmCartesianGoal(request) - time.sleep(time_step) - - current_pose = self.forward_kinematics() - current_precision_distance_xyz = np.linalg.norm(current_pose[:3, 3] - target[:3, 3]) - self._logger.info(f"l2 xyz distance to goal: {current_precision_distance_xyz}") - - def _send_linear_interpolation( - self, - origin_trans: npt.NDArray[np.float64], - target_trans: npt.NDArray[np.float64], - origin_rot: Quaternion, - target_rot: Quaternion, - nb_steps: int, - time_step: float, - ) -> None: - """Generate linear interpolation between two poses over a specified number of steps. - - The function performs linear interpolation for both translation and rotation between - the origin and target poses. - - Args: - origin_trans: The original translation vector in 3D space, - given as a NumPy array of type `np.float64`, containing the translation components - along the x, y, and z axes in meters. - target_trans: The target translation vector in 3D space, given - as a NumPy array of type `np.float64`, representing the desired final translation in meters. - origin_rot: The initial rotation quaternion, used as the starting point for - the rotation interpolation. - target_rot: The target rotation quaternion for the interpolation. - nb_steps: The number of steps or intervals for the interpolation process, determining - how many intermediate points will be calculated between the origin and target poses. - time_step: The time interval in seconds between each step of the interpolation. - """ - for t in np.linspace(0, 1, nb_steps): - # Linear interpolation for translation - trans_interpolated = (1 - t) * origin_trans + t * target_trans - - # SLERP for rotation interpolation - q_interpolated = Quaternion.slerp(origin_rot, target_rot, t) - rot_interpolated = q_interpolated.rotation_matrix - - # Recompose the interpolated matrix - interpolated_matrix = recompose_matrix(rot_interpolated, trans_interpolated) - - request = ArmCartesianGoal( - id=self._part_id, - goal_pose=Matrix4x4(data=interpolated_matrix.flatten().tolist()), - ) - self._stub.SendArmCartesianGoal(request) - time.sleep(time_step) - - def _send_elliptical_interpolation( - self, - origin_trans: npt.NDArray[np.float64], - target_trans: npt.NDArray[np.float64], - origin_rot: Quaternion, - target_rot: Quaternion, - arc_direction: str, - secondary_radius: Optional[float], - nb_steps: int, - time_step: float, - ) -> None: - """Generate elliptical interpolation between two poses for the arm. - - The function performs elliptical interpolation for both translation and rotation from the - origin to the target pose. - - Args: - origin_trans: The initial translation vector of the arm, - given as a NumPy array of type `np.float64`, containing the x, y, and z coordinates. - target_trans: The target translation vector that the robot - end-effector should reach, provided as a NumPy array of type `np.float64` with the x, - y, and z coordinates. - origin_rot: The initial orientation (rotation) of the end-effector. - target_rot: The target orientation (rotation) that the end-effector should reach. - arc_direction: The direction of the elliptical interpolation, which can be 'above', - 'below', 'right', 'left', 'front', or 'back'. - secondary_radius: The radius of the secondary axis of the ellipse. If not - provided, it defaults to the primary radius, which is based on the distance between the - origin and target poses. - nb_steps: The number of steps for the interpolation, determining how many - intermediate poses will be generated between the origin and target. - time_step: The time interval in seconds between each interpolation step. - """ - vector_target_origin = target_trans - origin_trans - - center = (origin_trans + target_trans) / 2 - radius = float(np.linalg.norm(vector_target_origin) / 2) - - vector_origin_center = origin_trans - center - vector_target_center = target_trans - center - - if np.isclose(radius, 0, atol=1e-03): - self._logger.warning(f"{self._part_id.name} is already at the target pose. No command sent.") - return - if secondary_radius is None: - secondary_radius = radius - if secondary_radius is not None and secondary_radius > 0.3: - self._logger.warning("interpolation elliptic_radius was too large, reduced to 0.3") - secondary_radius = 0.3 - - normal = get_normal_vector(vector=vector_target_origin, arc_direction=arc_direction) - - if normal is None: - self._logger.warning("arc_direction has no solution. Executing linear interpolation instead.") - self._send_linear_interpolation( - origin_trans=origin_trans, - target_trans=target_trans, - origin_rot=origin_rot, - target_rot=target_rot, - nb_steps=nb_steps, - time_step=time_step, - ) - return - - cos_angle = np.dot(vector_origin_center, vector_target_center) / ( - np.linalg.norm(vector_origin_center) * np.linalg.norm(vector_target_center) - ) - angle = np.arccos(np.clip(cos_angle, -1, 1)) - - for t in np.linspace(0, 1, nb_steps): - # Interpolated angles - theta = t * angle - - # Rotation of origin_vector around the circle center in the plan defined by 'normal' - q1 = Quaternion(axis=normal, angle=theta) - rotation_matrix = q1.rotation_matrix - - # Interpolated point in plan - trans_interpolated = np.dot(rotation_matrix, vector_origin_center) - # Adjusting the ellipse - ellipse_interpolated = trans_interpolated * np.array([1, 1, secondary_radius / radius]) - trans_interpolated = ellipse_interpolated + center - - # SLERP for the rotation - q_interpolated = Quaternion.slerp(origin_rot, target_rot, t) - rot_interpolated = q_interpolated.rotation_matrix - - # Recompose the interpolated matrix - interpolated_matrix = recompose_matrix(rot_interpolated, trans_interpolated) - - request = ArmCartesianGoal( - id=self._part_id, - goal_pose=Matrix4x4(data=interpolated_matrix.flatten().tolist()), - ) - self._stub.SendArmCartesianGoal(request) - time.sleep(time_step) - def send_goal_positions(self, check_positions: bool = True) -> None: """Send goal positions to the gripper and actuators if the parts are on. diff --git a/src/reachy2_sdk/parts/goto_based_part.py b/src/reachy2_sdk/parts/goto_based_part.py index f46e9ef0..800218b1 100644 --- a/src/reachy2_sdk/parts/goto_based_part.py +++ b/src/reachy2_sdk/parts/goto_based_part.py @@ -3,26 +3,16 @@ Handles common interface for parts performing movement using goto mechanism. """ -import logging -import time -from abc import ABC, abstractmethod -from typing import Any, List, Optional +from typing import List -from reachy2_sdk_api.goto_pb2 import GoalStatus, GoToAck, GoToId +from reachy2_sdk_api.goto_pb2 import GoToAck, GoToId from reachy2_sdk_api.goto_pb2_grpc import GoToServiceStub +from reachy2_sdk_api.part_pb2 import PartId -from ..utils.utils import ( - JointsRequest, - OdometryRequest, - SimplifiedRequest, - arm_position_to_list, - ext_euler_angles_to_list, - get_interpolation_mode, -) -from .part import Part +from ..utils.goto_based_element import IGoToBasedElement -class IGoToBasedPart(ABC): +class IGoToBasedPart(IGoToBasedElement): """Interface for parts of Reachy that use goto functions. The `IGoToBasedPart` class defines a common interface for handling goto-based movements. It is @@ -32,7 +22,7 @@ class IGoToBasedPart(ABC): def __init__( self, - part: Part, + part: PartId, goto_stub: GoToServiceStub, ) -> None: """Initialize the IGoToBasedPart interface. @@ -45,18 +35,16 @@ def __init__( part: The robot part that uses this interface, such as an Arm or Head. goto_stub: The gRPC stub used to send goto commands to the robot part. """ - 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 + super().__init__(part, goto_stub) def get_goto_playing(self) -> GoToId: """Return the GoToId of the currently playing goto movement on a specific part.""" - response = self._goto_stub.GetPartGoToPlaying(self.part._part_id) + response = self._goto_stub.GetPartGoToPlaying(self._element_id) return response def get_goto_queue(self) -> List[GoToId]: """Return a list of all GoToIds waiting to be played on a specific part.""" - response = self._goto_stub.GetPartGoToQueue(self.part._part_id) + response = self._goto_stub.GetPartGoToQueue(self._element_id) return [goal_id for goal_id in response.goto_ids] def cancel_all_goto(self) -> GoToAck: @@ -65,148 +53,5 @@ def cancel_all_goto(self) -> GoToAck: Returns: A GoToAck acknowledging the cancellation of all goto commands. """ - response = self._goto_stub.CancelPartAllGoTo(self.part._part_id) + response = self._goto_stub.CancelPartAllGoTo(self._element_id) return response - - def _get_goto_request(self, goto_id: GoToId) -> Optional[SimplifiedRequest]: - """Retrieve the details of a goto command based on its GoToId. - - Args: - goto_id: The ID of the goto command for which details are requested. - - Returns: - A `SimplifiedRequest` object containing the part name, joint goal positions - (in degrees), movement duration, and interpolation mode. - Returns `None` if the robot is not connected or if the `goto_id` is invalid. - - Raises: - TypeError: If `goto_id` is not an instance of `GoToId`. - ValueError: If `goto_id` is -1, indicating an invalid command. - """ - if not isinstance(goto_id, GoToId): - raise TypeError(f"goto_id must be a GoToId, got {type(goto_id).__name__}") - if goto_id.id == -1: - raise ValueError("No answer was found for given move, goto_id is -1") - - response = self._goto_stub.GetGoToRequest(goto_id) - - if response.HasField("joints_goal"): - if response.joints_goal.HasField("arm_joint_goal"): - part = response.joints_goal.arm_joint_goal.id.name - mode = get_interpolation_mode(response.interpolation_mode.interpolation_type) - goal_positions = arm_position_to_list(response.joints_goal.arm_joint_goal.joints_goal, degrees=True) - duration = response.joints_goal.arm_joint_goal.duration.value - elif response.joints_goal.HasField("neck_joint_goal"): - part = response.joints_goal.neck_joint_goal.id.name - mode = get_interpolation_mode(response.interpolation_mode.interpolation_type) - goal_positions = ext_euler_angles_to_list( - response.joints_goal.neck_joint_goal.joints_goal.rotation.rpy, degrees=True - ) - duration = response.joints_goal.neck_joint_goal.duration.value - - joints_request = JointsRequest( - goal_positions=goal_positions, - duration=duration, - mode=mode, - ) - - full_request = SimplifiedRequest( - part=part, - request=joints_request, - ) - elif response.HasField("odometry_goal"): - part = response.odometry_goal.odometry_goal.id.name - odom_goal_positions = {} - odom_goal_positions["x"] = response.odometry_goal.odometry_goal.direction.x.value - odom_goal_positions["y"] = response.odometry_goal.odometry_goal.direction.y.value - odom_goal_positions["theta"] = response.odometry_goal.odometry_goal.direction.theta.value - odom_request = OdometryRequest( - goal_positions=odom_goal_positions, - timeout=response.odometry_goal.timeout.value, - distance_tolerance=response.odometry_goal.distance_tolerance.value, - angle_tolerance=response.odometry_goal.angle_tolerance.value, - ) - full_request = SimplifiedRequest( - part=part, - request=odom_request, - ) - - return full_request - - def _is_goto_finished(self, id: GoToId) -> bool: - """Check if the goto movement has been completed or cancelled. - - Returns: - `True` if the goto has been played or cancelled, `False` otherwise. - """ - state = self._goto_stub.GetGoToState(id) - result = bool( - state.goal_status == GoalStatus.STATUS_ABORTED - or state.goal_status == GoalStatus.STATUS_CANCELED - or state.goal_status == GoalStatus.STATUS_SUCCEEDED - ) - return result - - def _wait_goto(self, id: GoToId, duration: float) -> None: - """Wait for a goto to finish. timeout is in seconds.""" - self._logger_goto.info(f"Waiting for movement with {id}.") - - if not self._is_goto_already_over(id, duration): - info_gotos = [self._get_goto_request(id)] - ids_queue = self.get_goto_queue() - for goto_id in ids_queue: - info_gotos.append(self._get_goto_request(goto_id)) - - timeout = 1 # adding one more sec - for igoto in info_gotos: - if igoto is not None: - if type(igoto.request) is JointsRequest: - timeout += igoto.request.duration - elif type(igoto.request) is OdometryRequest: - timeout += igoto.request.timeout - - 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 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.") - - def _is_goto_already_over(self, id: GoToId, timeout: float) -> bool: - """Check if the goto movement is already over.""" - t0 = time.time() - id_playing = self.get_goto_playing() - while id_playing.id == -1: - time.sleep(0.005) - id_playing = self.get_goto_playing() - - if self._is_goto_finished(id): - return True - - # manage an id_playing staying at -1 - if time.time() - t0 > timeout: - self._logger_goto.warning(f"Waiting time for movement with {id} is timeout.") - return True - return False - - @abstractmethod - def _check_goto_parameters(self, target: Any, duration: Optional[float], q0: Optional[List[float]] = None) -> None: - """Check the validity of the parameters for a goto movement.""" - pass # pragma: no cover - - @abstractmethod - def goto_posture( - self, - common_posture: str = "default", - duration: float = 2, - wait: bool = False, - wait_for_goto_end: bool = True, - interpolation_mode: str = "minimum_jerk", - ) -> GoToId: - """Send all joints to standard positions with optional parameters for duration, waiting, and interpolation mode.""" - pass # pragma: no cover diff --git a/src/reachy2_sdk/parts/head.py b/src/reachy2_sdk/parts/head.py index fddafc5b..3d8d9489 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, Optional, overload +from typing import Any, Dict, List, Optional, overload import grpc import numpy as np @@ -30,6 +30,7 @@ from reachy2_sdk_api.head_pb2_grpc import HeadServiceStub from reachy2_sdk_api.kinematics_pb2 import ExtEulerAngles, Point, Quaternion, Rotation3d +from ..components.antenna import Antenna from ..orbita.orbita3d import Orbita3d from ..utils.utils import get_grpc_interpolation_mode, quaternion_from_euler_angles from .goto_based_part import IGoToBasedPart @@ -66,12 +67,14 @@ def __init__( goto_stub: The GoToServiceStub used to handle goto-based movements for the head. """ JointsBasedPart.__init__(self, head_msg, grpc_channel, HeadServiceStub(grpc_channel)) - IGoToBasedPart.__init__(self, self, goto_stub) + IGoToBasedPart.__init__(self, self._part_id, goto_stub) + + self._neck: Optional[Orbita3d] = None + self._l_antenna: Optional[Antenna] = None + self._r_antenna: Optional[Antenna] = None + self._actuators: Dict[str, Any] = {} self._setup_head(head_msg, initial_state) - self._actuators = { - "neck": self.neck, - } def _setup_head(self, head: Head_proto, initial_state: HeadState) -> None: """Set up the head with its actuators. @@ -83,14 +86,36 @@ def _setup_head(self, head: Head_proto, initial_state: HeadState) -> None: initial_state: A HeadState object representing the initial state of the head's actuators. """ description = head.description - self._neck = Orbita3d( - uid=description.neck.id.id, - name=description.neck.id.name, - initial_state=initial_state.neck_state, - grpc_channel=self._grpc_channel, - part=self, - joints_position_order=[NeckJoints.ROLL, NeckJoints.PITCH, NeckJoints.YAW], - ) + if description.HasField("neck"): + self._neck = Orbita3d( + uid=description.neck.id.id, + name=description.neck.id.name, + initial_state=initial_state.neck_state, + grpc_channel=self._grpc_channel, + part=self, + joints_position_order=[NeckJoints.ROLL, NeckJoints.PITCH, NeckJoints.YAW], + ) + self._actuators["neck"] = self._neck + if description.HasField("l_antenna"): + self._l_antenna = Antenna( + uid=description.l_antenna.id.id, + name=description.l_antenna.id.name, + initial_state=initial_state.l_antenna_state, + grpc_channel=self._grpc_channel, + goto_stub=self._goto_stub, + part=self, + ) + self._actuators["l_antenna"] = self._l_antenna + if description.HasField("r_antenna"): + self._r_antenna = Antenna( + uid=description.r_antenna.id.id, + name=description.r_antenna.id.name, + initial_state=initial_state.r_antenna_state, + grpc_channel=self._grpc_channel, + goto_stub=self._goto_stub, + part=self, + ) + self._actuators["r_antenna"] = self._r_antenna def __repr__(self) -> str: """Clean representation of an Head.""" @@ -100,10 +125,20 @@ def __repr__(self) -> str: }\n>""" @property - def neck(self) -> Orbita3d: + def neck(self) -> Optional[Orbita3d]: """Get the neck actuator of the head.""" return self._neck + @property + def l_antenna(self) -> Optional[Antenna]: + """Get the left antenna actuator of the head.""" + return self._l_antenna + + @property + def r_antenna(self) -> Optional[Antenna]: + """Get the right antenna actuator of the head.""" + return self._r_antenna + def get_current_orientation(self) -> pyQuat: """Get the current orientation of the head. @@ -119,6 +154,8 @@ def get_current_positions(self, degrees: bool = True) -> List[float]: Returns: A list of the current neck joint positions in the order [roll, pitch, yaw]. """ + if self.neck is None: + return [] roll = self.neck._joints["roll"].present_position pitch = self.neck._joints["pitch"].present_position yaw = self.neck._joints["yaw"].present_position @@ -161,14 +198,16 @@ def goto( This method moves the neck either to a given roll-pitch-yaw (RPY) position or to a quaternion orientation. Args: - target (Any): The desired orientation for the neck. Can either be: + target: The desired orientation for the neck. Can either be: - A list of three floats [roll, pitch, yaw] representing the RPY orientation (in degrees if `degrees=True`). - A pyQuat object representing a quaternion. - duration (float, optional): Time in seconds for the movement. Defaults to 2.0. - wait (bool, optional): Whether to wait for the movement to complete before returning. Defaults to False. - interpolation_mode (str, optional): The type of interpolation to be used for the movement. - Can be "minimum_jerk" or other modes. Defaults to "minimum_jerk". - degrees (bool, optional): Specifies if the RPY values in `target` are in degrees. Defaults to True. + duration: The time in seconds for the movement to be completed. Defaults to 2. + wait: If True, the function waits until the movement is completed before returning. + Defaults to False. + interpolation_mode: The interpolation method to be used. It can be either "minimum_jerk" + or "linear". Defaults to "minimum_jerk". + degrees: If True, the RPY values in the `target` argument are treated as degrees. + Defaults to True. Raises: TypeError : If the input type for `target` is invalid @@ -177,7 +216,7 @@ def goto( Returns: GoToId: The unique identifier for the movement command. """ - if not self.neck.is_on(): + if self.neck is not None and not self.neck.is_on(): self._logger.warning("head.neck is off. No command sent.") return GoToId(id=-1) @@ -314,7 +353,7 @@ def look_at( """ if duration == 0: raise ValueError("duration cannot be set to 0.") - if self.neck.is_off(): + if self.neck is not None and self.neck.is_off(): self._logger.warning("head.neck is off. No command sent.") return GoToId(id=-1) @@ -427,13 +466,14 @@ def goto_posture( Returns: The unique GoToId associated with the movement command. - - Raises: - ValueError: If the neck is off and the command cannot be sent. """ if not wait_for_goto_end: self.cancel_all_goto() - if self.neck.is_on(): + if self.l_antenna is not None and self.l_antenna.is_on(): + self.l_antenna.goto_posture(common_posture, duration, wait, wait_for_goto_end, interpolation_mode) + if self.r_antenna is not None and self.r_antenna.is_on(): + self.r_antenna.goto_posture(common_posture, duration, wait, wait_for_goto_end, interpolation_mode) + if self.neck is not None and self.neck.is_on(): return self.goto([0, -10, 0], duration, wait, interpolation_mode) else: self._logger.warning("Head is off. No command sent.") @@ -460,7 +500,8 @@ def _update_with(self, new_state: HeadState) -> None: Args: new_state: A HeadState object representing the new state of the head's actuators. """ - self.neck._update_with(new_state.neck_state) + for actuator_name, actuator in self._actuators.items(): + actuator._update_with(getattr(new_state, f"{actuator_name}_state")) def _update_audit_status(self, new_status: HeadStatus) -> None: """Update the audit status of the neck with the new status from the gRPC server. @@ -468,4 +509,5 @@ def _update_audit_status(self, new_status: HeadStatus) -> None: Args: new_status: A HeadStatus object representing the new status of the neck. """ - self.neck._update_audit_status(new_status.neck_status) + for actuator_name, actuator in self._actuators.items(): + actuator._update_audit_status(getattr(new_status, f"{actuator_name}_status")) diff --git a/src/reachy2_sdk/parts/joints_based_part.py b/src/reachy2_sdk/parts/joints_based_part.py index eb2b7082..03ba1efd 100644 --- a/src/reachy2_sdk/parts/joints_based_part.py +++ b/src/reachy2_sdk/parts/joints_based_part.py @@ -56,7 +56,10 @@ def joints(self) -> CustomDict[str, OrbitaJoint]: _joints: CustomDict[str, OrbitaJoint] = CustomDict({}) for actuator_name, actuator in self._actuators.items(): for joint in actuator._joints.values(): - _joints[actuator_name + "." + joint._axis_type] = joint + if hasattr(joint, "_axis_type"): + _joints[actuator_name + "." + joint._axis_type] = joint + else: + _joints[actuator_name] = joint return _joints @abstractmethod diff --git a/src/reachy2_sdk/parts/mobile_base.py b/src/reachy2_sdk/parts/mobile_base.py index 2e1b02f1..5713d15d 100644 --- a/src/reachy2_sdk/parts/mobile_base.py +++ b/src/reachy2_sdk/parts/mobile_base.py @@ -69,7 +69,7 @@ def __init__( """ self._logger = logging.getLogger(__name__) super().__init__(mb_msg, grpc_channel, MobileBaseUtilityServiceStub(grpc_channel)) - IGoToBasedPart.__init__(self, self, goto_stub) + IGoToBasedPart.__init__(self, self._part_id, goto_stub) self._mobility_stub = MobileBaseMobilityServiceStub(grpc_channel) @@ -348,15 +348,16 @@ def translate_by( angle_tolerance = None if odom_request is not None: - base_odom = odom_request.request.goal_positions - angle_tolerance = odom_request.request.angle_tolerance + base_odom = odom_request.request.target + angle_tolerance = deg2rad(odom_request.request.angle_tolerance) else: base_odom = self.odometry - base_odom["theta"] = deg2rad(base_odom["theta"]) - theta_goal = base_odom["theta"] + theta_goal = deg2rad(base_odom["theta"]) x_goal = base_odom["x"] + (x * np.cos(theta_goal) - y * np.sin(theta_goal)) y_goal = base_odom["y"] + (x * np.sin(theta_goal) + y * np.cos(theta_goal)) + + print(f"x_goal : {x_goal}, y_goal : {y_goal}, theta_goal : {theta_goal}") return self.goto( x_goal, y_goal, @@ -400,8 +401,7 @@ def rotate_by( distance_tolerance = 0.05 if odom_request is not None: - base_odom = odom_request.request.goal_positions - base_odom["theta"] = rad2deg(base_odom["theta"]) + base_odom = odom_request.request.target if angle_tolerance is None: angle_tolerance = odom_request.request.angle_tolerance distance_tolerance = odom_request.request.distance_tolerance @@ -565,7 +565,7 @@ def _check_goto_parameters(self, target: Any, duration: Optional[float] = None, odom_request = None if odom_request is not None: - base_odom = odom_request.request.goal_positions + base_odom = odom_request.request.target x_offset = abs(target[0] - base_odom["x"]) y_offset = abs(target[1] - base_odom["y"]) else: diff --git a/src/reachy2_sdk/reachy_sdk.py b/src/reachy2_sdk/reachy_sdk.py index d56c9c40..4e25941c 100644 --- a/src/reachy2_sdk/reachy_sdk.py +++ b/src/reachy2_sdk/reachy_sdk.py @@ -21,7 +21,6 @@ from google.protobuf.empty_pb2 import Empty from google.protobuf.timestamp_pb2 import Timestamp from grpc._channel import _InactiveRpcError -from numpy import rad2deg from reachy2_sdk_api import reachy_pb2, reachy_pb2_grpc from reachy2_sdk_api.goto_pb2 import GoalStatus, GoToAck, GoToGoalStatus, GoToId from reachy2_sdk_api.goto_pb2_grpc import GoToServiceStub @@ -38,14 +37,8 @@ from .parts.joints_based_part import JointsBasedPart from .parts.mobile_base import MobileBase from .utils.custom_dict import CustomDict -from .utils.utils import ( - JointsRequest, - OdometryRequest, - SimplifiedRequest, - arm_position_to_list, - ext_euler_angles_to_list, - get_interpolation_mode, -) +from .utils.goto_based_element import process_goto_request +from .utils.utils import SimplifiedRequest GoToHomeId = namedtuple("GoToHomeId", ["head", "r_arm", "l_arm"]) """Named tuple for easy access to goto request on full body""" @@ -765,46 +758,7 @@ def get_goto_request(self, goto_id: GoToId) -> Optional[SimplifiedRequest]: response = self._goto_stub.GetGoToRequest(goto_id) - if response.HasField("joints_goal"): - if response.joints_goal.HasField("arm_joint_goal"): - part = response.joints_goal.arm_joint_goal.id.name - mode = get_interpolation_mode(response.interpolation_mode.interpolation_type) - goal_positions = arm_position_to_list(response.joints_goal.arm_joint_goal.joints_goal, degrees=True) - duration = response.joints_goal.arm_joint_goal.duration.value - elif response.joints_goal.HasField("neck_joint_goal"): - part = response.joints_goal.neck_joint_goal.id.name - mode = get_interpolation_mode(response.interpolation_mode.interpolation_type) - goal_positions = ext_euler_angles_to_list( - response.joints_goal.neck_joint_goal.joints_goal.rotation.rpy, degrees=True - ) - duration = response.joints_goal.neck_joint_goal.duration.value - - joints_request = JointsRequest( - goal_positions=goal_positions, - duration=duration, - mode=mode, - ) - - full_request = SimplifiedRequest( - part=part, - request=joints_request, - ) - elif response.HasField("odometry_goal"): - part = response.odometry_goal.odometry_goal.id.name - odom_goal_positions = {} - odom_goal_positions["x"] = response.odometry_goal.odometry_goal.direction.x.value - odom_goal_positions["y"] = response.odometry_goal.odometry_goal.direction.y.value - odom_goal_positions["theta"] = rad2deg(response.odometry_goal.odometry_goal.direction.theta.value) - odom_request = OdometryRequest( - goal_positions=odom_goal_positions, - timeout=response.odometry_goal.timeout.value, - distance_tolerance=response.odometry_goal.distance_tolerance.value, - angle_tolerance=rad2deg(response.odometry_goal.angle_tolerance.value), - ) - full_request = SimplifiedRequest( - part=part, - request=odom_request, - ) + full_request = process_goto_request(response) return full_request diff --git a/src/reachy2_sdk/utils/goto_based_element.py b/src/reachy2_sdk/utils/goto_based_element.py new file mode 100644 index 00000000..ab31af2a --- /dev/null +++ b/src/reachy2_sdk/utils/goto_based_element.py @@ -0,0 +1,283 @@ +"""Reachy IGoToBasedElement interface. + +Handles common interface for elements (parts or components) performing movements using goto mechanism. +""" + +import logging +import time +from abc import ABC, abstractmethod +from typing import Any, List, Optional + +import numpy as np +from reachy2_sdk_api.component_pb2 import ComponentId +from reachy2_sdk_api.goto_pb2 import GoalStatus, GoToAck, GoToId, GoToRequest +from reachy2_sdk_api.goto_pb2_grpc import GoToServiceStub +from reachy2_sdk_api.part_pb2 import PartId + +from ..utils.utils import ( + EllipticalParameters, + JointsRequest, + OdometryRequest, + SimplifiedRequest, + TargetJointsRequest, + arm_position_to_list, + ext_euler_angles_to_list, + get_arc_direction, + get_interpolation_mode, + get_interpolation_space, +) + + +class IGoToBasedElement(ABC): + """Interface for elements (parts or components) of Reachy that use goto functions. + + The `IGoToBasedElements` class defines a common interface for handling goto-based movements. It is + designed to be implemented by any parts or components of the robot that perform movements via the goto mechanism. + """ + + def __init__( + self, + element_id: ComponentId | PartId, + goto_stub: GoToServiceStub, + ) -> None: + """Initialize the IGoToBasedElement interface. + + Sets up the common attributes needed for handling goto-based movements. This includes + associating the component with the interface and setting up the gRPC stub for performing + goto commands. + + Args: + element_id: The robot component or part that uses this interface. + goto_stub: The gRPC stub used to send goto commands to the robot element. + """ + self._element_id = element_id + self._goto_stub = goto_stub + self._logger_goto = logging.getLogger(__name__) # not using self._logger to avoid name conflict in multiple inheritance + + @abstractmethod + def get_goto_playing(self) -> GoToId: + """Return the GoToId of the currently playing goto movement on a specific element.""" + pass + + @abstractmethod + def get_goto_queue(self) -> List[GoToId]: + """Return a list of all GoToIds waiting to be played on a specific element.""" + pass + + @abstractmethod + def cancel_all_goto(self) -> GoToAck: + """Request the cancellation of all playing and waiting goto commands for a specific element. + + Returns: + A GoToAck acknowledging the cancellation of all goto commands. + """ + pass + + def _get_goto_request(self, goto_id: GoToId) -> Optional[SimplifiedRequest]: + """Retrieve the details of a goto command based on its GoToId. + + Args: + goto_id: The ID of the goto command for which details are requested. + + Returns: + A `SimplifiedRequest` object containing the element name, joint goal positions + (in degrees), movement duration, and interpolation mode. + Returns `None` if the robot is not connected or if the `goto_id` is invalid. + + Raises: + TypeError: If `goto_id` is not an instance of `GoToId`. + ValueError: If `goto_id` is -1, indicating an invalid command. + """ + if not isinstance(goto_id, GoToId): + raise TypeError(f"goto_id must be a GoToId, got {type(goto_id).__name__}") + if goto_id.id == -1: + raise ValueError("No answer was found for given move, goto_id is -1") + + response = self._goto_stub.GetGoToRequest(goto_id) + + full_request = process_goto_request(response) + + return full_request + + def _is_goto_finished(self, id: GoToId) -> bool: + """Check if the goto movement has been completed or cancelled. + + Returns: + `True` if the goto has been played or cancelled, `False` otherwise. + """ + state = self._goto_stub.GetGoToState(id) + result = bool( + state.goal_status == GoalStatus.STATUS_ABORTED + or state.goal_status == GoalStatus.STATUS_CANCELED + or state.goal_status == GoalStatus.STATUS_SUCCEEDED + ) + return result + + def _wait_goto(self, id: GoToId, duration: float) -> None: + """Wait for a goto to finish. timeout is in seconds.""" + self._logger_goto.info(f"Waiting for movement with {id}.") + + if not self._is_goto_already_over(id, duration): + info_gotos = [self._get_goto_request(id)] + ids_queue = self.get_goto_queue() + for goto_id in ids_queue: + info_gotos.append(self._get_goto_request(goto_id)) + + timeout = 1 # adding one more sec + for igoto in info_gotos: + if igoto is not None: + if type(igoto.request) is JointsRequest: + timeout += igoto.request.duration + elif type(igoto.request) is OdometryRequest: + timeout += igoto.request.timeout + + 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 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.") + + def _is_goto_already_over(self, id: GoToId, timeout: float) -> bool: + """Check if the goto movement is already over.""" + t0 = time.time() + id_playing = self.get_goto_playing() + while id_playing.id == -1: + time.sleep(0.005) + id_playing = self.get_goto_playing() + + if self._is_goto_finished(id): + return True + + # manage an id_playing staying at -1 + if time.time() - t0 > timeout: + self._logger_goto.warning(f"Waiting time for movement with {id} is timeout.") + return True + return False + + @abstractmethod + def _check_goto_parameters(self, target: Any, duration: Optional[float], q0: Optional[List[float]] = None) -> None: + """Check the validity of the parameters for a goto movement.""" + pass # pragma: no cover + + @abstractmethod + def goto_posture( + self, + common_posture: str = "default", + duration: float = 2, + wait: bool = False, + wait_for_goto_end: bool = True, + interpolation_mode: str = "minimum_jerk", + ) -> GoToId: + """Send all joints to standard positions with optional parameters for duration, waiting, and interpolation mode.""" + pass # pragma: no cover + + +def process_goto_request(response: GoToRequest) -> Optional[SimplifiedRequest]: + """Process the response from a goto request and return a SimplifiedRequest object.""" + full_request = None + if response.HasField("cartesian_goal"): + part = response.cartesian_goal.arm_cartesian_goal.id.name + mode = get_interpolation_mode(response.interpolation_mode.interpolation_type) + interpolation_space = get_interpolation_space(response.interpolation_space.interpolation_space) + duration = response.cartesian_goal.arm_cartesian_goal.duration.value + target_pose = np.reshape(response.cartesian_goal.arm_cartesian_goal.goal_pose.data, (4, 4)) + + if mode == "elliptical": + arc_direction = get_arc_direction(response.elliptical_parameters.arc_direction) + secondary_radius = response.elliptical_parameters.secondary_radius.value + elliptical_params = EllipticalParameters(arc_direction, secondary_radius) + else: + elliptical_params = None + + target = TargetJointsRequest( + joints=None, + pose=target_pose, + ) + + joints_request = JointsRequest( + target=target, + duration=duration, + mode=mode, + interpolation_space=interpolation_space, + elliptical_parameters=elliptical_params, + ) + + full_request = SimplifiedRequest( + part=part, + request=joints_request, + ) + + elif response.HasField("joints_goal"): + if response.joints_goal.HasField("arm_joint_goal"): + part = response.joints_goal.arm_joint_goal.id.name + mode = get_interpolation_mode(response.interpolation_mode.interpolation_type) + interpolation_space = get_interpolation_space(response.interpolation_space.interpolation_space) + duration = response.joints_goal.arm_joint_goal.duration.value + target_joints = arm_position_to_list(response.joints_goal.arm_joint_goal.joints_goal, degrees=True) + elliptical_params = None + elif response.joints_goal.HasField("neck_joint_goal"): + part = response.joints_goal.neck_joint_goal.id.name + mode = get_interpolation_mode(response.interpolation_mode.interpolation_type) + interpolation_space = get_interpolation_space(response.interpolation_space.interpolation_space) + target_joints = ext_euler_angles_to_list( + response.joints_goal.neck_joint_goal.joints_goal.rotation.rpy, degrees=True + ) + duration = response.joints_goal.neck_joint_goal.duration.value + elliptical_params = None + elif response.joints_goal.HasField("antenna_joint_goal"): + part = response.joints_goal.antenna_joint_goal.antenna.id.name + if part == "antenna_right": + part = "r_antenna" + elif part == "antenna_left": + part = "l_antenna" + mode = get_interpolation_mode(response.interpolation_mode.interpolation_type) + interpolation_space = "joint_space" + target_joints = np.rad2deg(response.joints_goal.antenna_joint_goal.joint_goal.value) + duration = response.joints_goal.antenna_joint_goal.duration.value + elliptical_params = None + + target = TargetJointsRequest( + joints=target_joints, + pose=None, + ) + + joints_request = JointsRequest( + target=target, + duration=duration, + mode=mode, + interpolation_space=interpolation_space, + elliptical_parameters=elliptical_params, + ) + + full_request = SimplifiedRequest( + part=part, + request=joints_request, + ) + + elif response.HasField("odometry_goal"): + part = response.odometry_goal.odometry_goal.id.name + odom_goal_positions = {} + odom_goal_positions["x"] = response.odometry_goal.odometry_goal.direction.x.value + odom_goal_positions["y"] = response.odometry_goal.odometry_goal.direction.y.value + odom_goal_positions["theta"] = np.rad2deg(response.odometry_goal.odometry_goal.direction.theta.value) + odom_request = OdometryRequest( + target=odom_goal_positions, + timeout=response.odometry_goal.timeout.value, + distance_tolerance=response.odometry_goal.distance_tolerance.value, + angle_tolerance=np.rad2deg(response.odometry_goal.angle_tolerance.value), + ) + full_request = SimplifiedRequest( + part=part, + request=odom_request, + ) + + else: + raise ValueError("No valid request found in the response") + + return full_request diff --git a/src/reachy2_sdk/utils/utils.py b/src/reachy2_sdk/utils/utils.py index 325a0575..f886db94 100644 --- a/src/reachy2_sdk/utils/utils.py +++ b/src/reachy2_sdk/utils/utils.py @@ -16,17 +16,29 @@ from google.protobuf.wrappers_pb2 import FloatValue from pyquaternion import Quaternion from reachy2_sdk_api.arm_pb2 import ArmPosition -from reachy2_sdk_api.goto_pb2 import GoToInterpolation, InterpolationMode +from reachy2_sdk_api.goto_pb2 import ( + ArcDirection, + GoToInterpolation, + GoToInterpolationSpace, + InterpolationMode, + InterpolationSpace, +) from reachy2_sdk_api.kinematics_pb2 import ExtEulerAngles, Rotation3d from reachy2_sdk_api.orbita2d_pb2 import Pose2d SimplifiedRequest = namedtuple("SimplifiedRequest", ["part", "request"]) """Named tuple for easy access to request variables""" -JointsRequest = namedtuple("JointsRequest", ["goal_positions", "duration", "mode"]) +JointsRequest = namedtuple("JointsRequest", ["target", "duration", "mode", "interpolation_space", "elliptical_parameters"]) """Named tuple for easy access to request variables""" -OdometryRequest = namedtuple("OdometryRequest", ["goal_positions", "timeout", "distance_tolerance", "angle_tolerance"]) +TargetJointsRequest = namedtuple("TargetJointsRequest", ["joints", "pose"]) +"""Named tuple for easy access to target details""" + +OdometryRequest = namedtuple("OdometryRequest", ["target", "timeout", "distance_tolerance", "angle_tolerance"]) +"""Named tuple for easy access to request variables""" + +EllipticalParameters = namedtuple("EllipticalParameters", ["arc_direction", "secondary_radius"]) """Named tuple for easy access to request variables""" @@ -147,50 +159,188 @@ def get_grpc_interpolation_mode(interpolation_mode: str) -> GoToInterpolation: Args: interpolation_mode: A string representing the type of interpolation to be used. It can be either - "minimum_jerk" or "linear". + "minimum_jerk", "linear" or "elliptical". Returns: An instance of the GoToInterpolation class with the interpolation type set based on the input interpolation_mode string. Raises: - ValueError: If the interpolation_mode is not "minimum_jerk" or "linear". + ValueError: If the interpolation_mode is not "minimum_jerk", "linear" or "elliptical". """ - if interpolation_mode not in ["minimum_jerk", "linear"]: - raise ValueError(f"Interpolation mode {interpolation_mode} not supported! Should be 'minimum_jerk' or 'linear'") + if interpolation_mode not in ["minimum_jerk", "linear", "elliptical"]: + raise ValueError( + f"Interpolation mode {interpolation_mode} not supported! Should be 'minimum_jerk', 'linear' or 'elliptical'" + ) if interpolation_mode == "minimum_jerk": interpolation_mode = InterpolationMode.MINIMUM_JERK - else: + elif interpolation_mode == "linear": interpolation_mode = InterpolationMode.LINEAR + else: + interpolation_mode = InterpolationMode.ELLIPTICAL return GoToInterpolation(interpolation_type=interpolation_mode) +def get_grpc_interpolation_space(interpolation_space: str) -> GoToInterpolationSpace: + """Convert a given interpolation space string to a corresponding GoToInterpolationSpace object. + + Args: + interpolation_space: A string representing the interpolation space to be used. It can be either + "joint_space" or "cartesian_space". + + Returns: + An instance of the GoToInterpolationSpace class with the interpolation type set based on the input + interpolation_space string. + + Raises: + ValueError: If the interpolation_space is not "joint_space" or "cartesian_space". + """ + if interpolation_space not in ["joint_space", "cartesian_space"]: + raise ValueError( + f"Interpolation space {interpolation_space} not supported! Should be 'joint_space' or 'cartesian_space'" + ) + + if interpolation_space == "joint_space": + interpolation_space = InterpolationSpace.JOINT_SPACE + else: + interpolation_space = InterpolationSpace.CARTESIAN_SPACE + return GoToInterpolationSpace(interpolation_space=interpolation_space) + + def get_interpolation_mode(interpolation_mode: InterpolationMode) -> str: """Convert an interpolation mode enum to a string representation. Args: interpolation_mode: The interpolation mode given as InterpolationMode. The supported interpolation - modes are MINIMUM_JERK and LINEAR. + modes are MINIMUM_JERK, LINEAR and ELLIPTICAL. Returns: A string representing the interpolation mode based on the input interpolation_mode. Returns - "minimum_jerk" if the mode is InterpolationMode.MINIMUM_JERK, and "linear" if it is - InterpolationMode.LINEAR. + "minimum_jerk" if the mode is InterpolationMode.MINIMUM_JERK, "linear" if it is + InterpolationMode.LINEAR, and "elliptical" if it is InterpolationMode.ELLIPTICAL. Raises: - ValueError: If the interpolation_mode is not InterpolationMode.MINIMUM_JERK or InterpolationMode.LINEAR. + ValueError: If the interpolation_mode is not InterpolationMode.MINIMUM_JERK, InterpolationMode.LINEAR + or InterpolationMode.ELLIPTICAL. """ - if interpolation_mode not in [InterpolationMode.MINIMUM_JERK, InterpolationMode.LINEAR]: - raise ValueError(f"Interpolation mode {interpolation_mode} not supported! Should be 'minimum_jerk' or 'linear'") + if interpolation_mode not in [InterpolationMode.MINIMUM_JERK, InterpolationMode.LINEAR, InterpolationMode.ELLIPTICAL]: + raise ValueError( + f"Interpolation mode {interpolation_mode} not supported! Should be 'minimum_jerk', 'linear' or 'elliptical'" + ) if interpolation_mode == InterpolationMode.MINIMUM_JERK: mode = "minimum_jerk" - else: + elif interpolation_mode == InterpolationMode.LINEAR: mode = "linear" + else: + mode = "elliptical" return mode +def get_interpolation_space(interpolation_space: InterpolationSpace) -> str: + """Convert an interpolation space enum to a string representation. + + Args: + interpolation_space: The interpolation space given as InterpolationSpace. The supported interpolation + modes are JOINT_SPACE and CARTESIAN_SPACE. + + Returns: + A string representing the interpolation mode based on the input interpolation_space. Returns + "joint_space" if the mode is InterpolationSpace.JOINT_SPACE, and "cartesian_space" if it is + InterpolationSpace.CARTESIAN_SPACE. + + Raises: + ValueError: If the interpolation_space is not InterpolationSpace.JOINT_SPACE or InterpolationSpace.CARTESIAN_SPACE. + """ + if interpolation_space not in [InterpolationSpace.JOINT_SPACE, InterpolationSpace.CARTESIAN_SPACE]: + raise ValueError( + f"Interpolation space {interpolation_space} not supported! Should be 'joint_space' or 'cartesian_space'" + ) + + if interpolation_space == InterpolationSpace.CARTESIAN_SPACE: + space = "cartesian_space" + else: + space = "joint_space" + return space + + +def get_grpc_arc_direction(arc_direction: str) -> ArcDirection: + """Convert a given arc direction string to a corresponding ArcDirection object. + + Args: + arc_direction: A string representing the direction of the arc. It can be one of the following options: + "above", "below", "front", "back", "right", or "left". + + Returns: + An instance of the ArcDirection class with the direction set based on the input arc_direction string. + + Raises: + ValueError: If the arc_direction is not one of "above", "below", "front", "back", "right", or "left". + """ + if arc_direction not in ["above", "below", "front", "back", "right", "left"]: + raise ValueError( + f"Arc direction {arc_direction} not supported! Should be 'above', 'below', 'front', 'back', 'right' or 'left'" + ) + + if arc_direction == "above": + arc_direction = ArcDirection.ABOVE + elif arc_direction == "below": + arc_direction = ArcDirection.BELOW + elif arc_direction == "front": + arc_direction = ArcDirection.FRONT + elif arc_direction == "back": + arc_direction = ArcDirection.BACK + elif arc_direction == "right": + arc_direction = ArcDirection.RIGHT + else: + arc_direction = ArcDirection.LEFT + return arc_direction + + +def get_arc_direction(arc_direction: ArcDirection) -> str: + """Convert an arc direction enum to a string representation. + + Args: + arc_direction: The arc direction given as ArcDirection. The supported arc directions are ABOVE, BELOW, FRONT, + BACK, RIGHT, and LEFT. + + Returns: + A string representing the arc direction based on the input arc_direction. Returns "above" if the direction is + ArcDirection.ABOVE, "below" if it is ArcDirection.BELOW, "front" if it is ArcDirection.FRONT, "back" if it is + ArcDirection.BACK, "right" if it is ArcDirection.RIGHT, and "left" if it is ArcDirection.LEFT. + + Raises: + ValueError: If the arc_direction is not ArcDirection.ABOVE, ArcDirection.BELOW, ArcDirection.FRONT, ArcDirection.BACK, + ArcDirection.RIGHT, or ArcDirection.LEFT. + """ + if arc_direction not in [ + ArcDirection.ABOVE, + ArcDirection.BELOW, + ArcDirection.FRONT, + ArcDirection.BACK, + ArcDirection.RIGHT, + ArcDirection.LEFT, + ]: + raise ValueError( + f"Arc direction {arc_direction} not supported! Should be 'above', 'below', 'front', 'back', 'right' or 'left'" + ) + + if arc_direction == ArcDirection.ABOVE: + direction = "above" + elif arc_direction == ArcDirection.BELOW: + direction = "below" + elif arc_direction == ArcDirection.FRONT: + direction = "front" + elif arc_direction == ArcDirection.BACK: + direction = "back" + elif arc_direction == ArcDirection.RIGHT: + direction = "right" + else: + direction = "left" + return direction + + def decompose_matrix(matrix: npt.NDArray[np.float64]) -> Tuple[Quaternion, npt.NDArray[np.float64]]: """Decompose a homogeneous 4x4 matrix into rotation (represented as a quaternion) and translation components. diff --git a/tests/units/offline/test_antenna.py b/tests/units/offline/test_antenna.py new file mode 100644 index 00000000..be08ab99 --- /dev/null +++ b/tests/units/offline/test_antenna.py @@ -0,0 +1,87 @@ +import grpc +import pytest +from google.protobuf.wrappers_pb2 import BoolValue, FloatValue +from reachy2_sdk_api.component_pb2 import ComponentId, PIDGains +from reachy2_sdk_api.dynamixel_motor_pb2 import DynamixelMotorState + +from reachy2_sdk.components.antenna import Antenna +from reachy2_sdk.orbita.utils import to_position + + +@pytest.mark.offline +def test_class() -> None: + grpc_channel = grpc.insecure_channel("dummy:5050") + + compliance = BoolValue(value=True) + + pid = PIDGains(p=FloatValue(value=0), i=FloatValue(value=0), d=FloatValue(value=0)) + + motor_id = ComponentId(id=1, name="motor") + temperature = FloatValue(value=0) + speed_limit = FloatValue(value=0) + torque_limit = FloatValue(value=0) + present_position = FloatValue(value=0.2) + goal_position = FloatValue(value=0.3) + present_speed = FloatValue(value=0) + present_load = FloatValue(value=0) + motor_state = DynamixelMotorState( + id=motor_id, + compliant=compliance, + present_position=present_position, + goal_position=goal_position, + temperature=temperature, + pid=pid, + speed_limit=speed_limit, + torque_limit=torque_limit, + present_speed=present_speed, + present_load=present_load, + ) + + uid = 2 + name = "antenna" + antenna = Antenna(uid=uid, name=name, initial_state=motor_state, grpc_channel=grpc_channel, goto_stub=None, part=None) + + assert antenna.__repr__() != "" + + assert antenna.goal_position == to_position(goal_position.value) + assert antenna.present_position == to_position(present_position.value) + + antenna._check_goto_parameters(duration=1, target=3.0) + + with pytest.raises(ValueError): + antenna._check_goto_parameters(duration=0, target=2.0) + with pytest.raises(TypeError): + antenna._check_goto_parameters(duration=2, target=[2.0]) + with pytest.raises(TypeError): + antenna._check_goto_parameters(duration=2, target="error") + + compliance = BoolValue(value=False) + + pid = PIDGains(p=FloatValue(value=0), i=FloatValue(value=0), d=FloatValue(value=0)) + + motor_id = ComponentId(id=1, name="motor") + temperature = FloatValue(value=0) + speed_limit = FloatValue(value=0) + torque_limit = FloatValue(value=0) + present_position = FloatValue(value=0.2) + goal_position = FloatValue(value=0.3) + present_speed = FloatValue(value=0) + present_load = FloatValue(value=0) + motor_state = DynamixelMotorState( + id=motor_id, + compliant=compliance, + present_position=present_position, + goal_position=goal_position, + temperature=temperature, + pid=pid, + speed_limit=speed_limit, + torque_limit=torque_limit, + present_speed=present_speed, + present_load=present_load, + ) + antenna._update_with(motor_state) + + with pytest.raises(ValueError): + antenna.goto(2.0, duration=0) + with pytest.raises(TypeError): + antenna.goto([20], duration=0) diff --git a/tests/units/offline/test_dynamixel_motor.py b/tests/units/offline/test_dynamixel_motor.py new file mode 100644 index 00000000..4cd2393c --- /dev/null +++ b/tests/units/offline/test_dynamixel_motor.py @@ -0,0 +1,91 @@ +import grpc +import pytest +from google.protobuf.wrappers_pb2 import BoolValue, FloatValue +from reachy2_sdk_api.component_pb2 import ComponentId, PIDGains +from reachy2_sdk_api.dynamixel_motor_pb2 import DynamixelMotorState + +from reachy2_sdk.dynamixel.dynamixel_motor import DynamixelMotor +from reachy2_sdk.orbita.utils import to_position + + +@pytest.mark.offline +def test_class() -> None: + grpc_channel = grpc.insecure_channel("dummy:5050") + + compliance = BoolValue(value=True) + + pid = PIDGains(p=FloatValue(value=0), i=FloatValue(value=0), d=FloatValue(value=0)) + + motor_id = ComponentId(id=1, name="motor") + temperature = FloatValue(value=0) + speed_limit = FloatValue(value=0) + torque_limit = FloatValue(value=0) + present_position = FloatValue(value=0.2) + goal_position = FloatValue(value=0.3) + present_speed = FloatValue(value=0) + present_load = FloatValue(value=0) + motor_state = DynamixelMotorState( + id=motor_id, + compliant=compliance, + present_position=present_position, + goal_position=goal_position, + temperature=temperature, + pid=pid, + speed_limit=speed_limit, + torque_limit=torque_limit, + present_speed=present_speed, + present_load=present_load, + ) + motor = DynamixelMotor(uid=motor_id.id, name=motor_id.name, initial_state=motor_state, grpc_channel=grpc_channel) + + assert motor.__repr__() != "" + + assert not motor.is_on() + + motor.send_goal_positions() + + # use to_position() to convert radian to degree + assert motor.goal_position == to_position(goal_position.value) + assert motor.present_position == to_position(present_position.value) + + # updating values + compliance = BoolValue(value=False) + + pid = PIDGains(p=FloatValue(value=0), i=FloatValue(value=0), d=FloatValue(value=0)) + + motor_id = ComponentId(id=1, name="motor") + temperature = FloatValue(value=0) + speed_limit = FloatValue(value=0) + torque_limit = FloatValue(value=0) + present_position = FloatValue(value=0.5) + goal_position = FloatValue(value=0.4) + present_speed = FloatValue(value=0) + present_load = FloatValue(value=0) + motor_state = DynamixelMotorState( + id=motor_id, + compliant=compliance, + present_position=present_position, + goal_position=goal_position, + temperature=temperature, + pid=pid, + speed_limit=speed_limit, + torque_limit=torque_limit, + present_speed=present_speed, + present_load=present_load, + ) + + motor._update_with(motor_state) + + assert motor.goal_position == to_position(goal_position.value) + assert motor.present_position == to_position(present_position.value) + + assert motor.is_on() + + with pytest.raises(TypeError): + motor.set_speed_limits("wrong value") + + with pytest.raises(ValueError): + motor.set_speed_limits(120) + + with pytest.raises(ValueError): + motor.set_speed_limits(-10) diff --git a/tests/units/offline/test_head.py b/tests/units/offline/test_head.py index 916d6844..2799579b 100644 --- a/tests/units/offline/test_head.py +++ b/tests/units/offline/test_head.py @@ -3,19 +3,25 @@ import pytest from google.protobuf.wrappers_pb2 import BoolValue, FloatValue from pyquaternion import Quaternion -from reachy2_sdk_api.component_pb2 import PIDGains -from reachy2_sdk_api.dynamixel_motor_pb2 import DynamixelMotorStatus +from reachy2_sdk_api.component_pb2 import ComponentId, PIDGains +from reachy2_sdk_api.dynamixel_motor_pb2 import ( + DynamixelMotor, + DynamixelMotorState, + DynamixelMotorStatus, +) from reachy2_sdk_api.error_pb2 import Error from reachy2_sdk_api.head_pb2 import Head as Head_proto -from reachy2_sdk_api.head_pb2 import HeadState, HeadStatus +from reachy2_sdk_api.head_pb2 import HeadDescription, HeadState, HeadStatus from reachy2_sdk_api.kinematics_pb2 import ExtEulerAngles, Rotation3d from reachy2_sdk_api.orbita3d_pb2 import ( Float3d, + Orbita3d, Orbita3dState, Orbita3dStatus, PID3d, Vector3d, ) +from reachy2_sdk_api.part_pb2 import PartId from reachy2_sdk.orbita.utils import to_position from reachy2_sdk.parts.head import Head @@ -51,7 +57,14 @@ def test_class() -> None: present_speed=present_speed, present_load=present_load, ) - head_proto = Head_proto() + + head_proto = Head_proto( + part_id=PartId(id=1, name="head"), + description=HeadDescription( + neck=Orbita3d(id=ComponentId(id=10, name="neck")), + ), + ) + head_state = HeadState(neck_state=neck_state) head = Head(head_msg=head_proto, initial_state=head_state, grpc_channel=grpc_channel, goto_stub=None) @@ -201,3 +214,207 @@ def test_class() -> None: head_status = HeadStatus(neck_status=orbita3d_status, l_antenna_status=dynamixel_status, r_antenna_status=dynamixel_status) head._update_audit_status(head_status) assert head.neck.audit == "orbita3d error" + + +@pytest.mark.offline +def test_class_with_antennas() -> None: + grpc_channel = grpc.insecure_channel("dummy:5050") + + compliance = BoolValue(value=True) + + pid = PID3d( + motor_1=PIDGains(p=FloatValue(value=0), i=FloatValue(value=0), d=FloatValue(value=0)), + motor_2=PIDGains(p=FloatValue(value=0), i=FloatValue(value=0), d=FloatValue(value=0)), + motor_3=PIDGains(p=FloatValue(value=0), i=FloatValue(value=0), d=FloatValue(value=0)), + ) + + temperature = Float3d(motor_1=FloatValue(value=0), motor_2=FloatValue(value=0), motor_3=FloatValue(value=0)) + speed_limit = Float3d(motor_1=FloatValue(value=0), motor_2=FloatValue(value=0), motor_3=FloatValue(value=0)) + torque_limit = Float3d(motor_1=FloatValue(value=0), motor_2=FloatValue(value=0), motor_3=FloatValue(value=0)) + present_speed = Vector3d(x=FloatValue(value=0), y=FloatValue(value=0), z=FloatValue(value=0)) + present_load = Vector3d(x=FloatValue(value=0), y=FloatValue(value=0), z=FloatValue(value=0)) + present_rot = Rotation3d(rpy=ExtEulerAngles(roll=FloatValue(value=1), pitch=FloatValue(value=2), yaw=FloatValue(value=3))) + goal_rot = Rotation3d(rpy=ExtEulerAngles(roll=FloatValue(value=4), pitch=FloatValue(value=5), yaw=FloatValue(value=6))) + neck_state = Orbita3dState( + compliant=compliance, + present_position=present_rot, + goal_position=goal_rot, + temperature=temperature, + pid=pid, + speed_limit=speed_limit, + torque_limit=torque_limit, + present_speed=present_speed, + present_load=present_load, + ) + + compliance1 = BoolValue(value=True) + pid1 = PIDGains(p=FloatValue(value=0), i=FloatValue(value=0), d=FloatValue(value=0)) + motor1_id = ComponentId(id=1, name="motor1") + temperature1 = FloatValue(value=0) + speed_limit1 = FloatValue(value=0) + torque_limit1 = FloatValue(value=0) + present_position1 = FloatValue(value=0.2) + goal_position1 = FloatValue(value=0.3) + present_speed1 = FloatValue(value=0) + present_load1 = FloatValue(value=0) + motor_state1 = DynamixelMotorState( + id=motor1_id, + compliant=compliance1, + present_position=present_position1, + goal_position=goal_position1, + temperature=temperature1, + pid=pid1, + speed_limit=speed_limit1, + torque_limit=torque_limit1, + present_speed=present_speed1, + present_load=present_load1, + ) + motor1 = DynamixelMotor(id=motor1_id) + + compliance2 = BoolValue(value=True) + pid2 = PIDGains(p=FloatValue(value=0), i=FloatValue(value=0), d=FloatValue(value=0)) + motor2_id = ComponentId(id=2, name="motor2") + temperature2 = FloatValue(value=0) + speed_limit2 = FloatValue(value=0) + torque_limit2 = FloatValue(value=0) + present_position2 = FloatValue(value=0.2) + goal_position2 = FloatValue(value=0.3) + present_speed2 = FloatValue(value=0) + present_load2 = FloatValue(value=0) + motor_state2 = DynamixelMotorState( + id=motor2_id, + compliant=compliance2, + present_position=present_position2, + goal_position=goal_position2, + temperature=temperature2, + pid=pid2, + speed_limit=speed_limit2, + torque_limit=torque_limit2, + present_speed=present_speed2, + present_load=present_load2, + ) + motor2 = DynamixelMotor(id=motor2_id) + + head_proto = Head_proto( + part_id=PartId(id=1, name="head"), + description=HeadDescription( + neck=Orbita3d(id=ComponentId(id=10, name="neck")), + l_antenna=motor1, + r_antenna=motor2, + ), + ) + + head_state = HeadState(neck_state=neck_state, l_antenna_state=motor_state1, r_antenna_state=motor_state2) + head = Head(head_msg=head_proto, initial_state=head_state, grpc_channel=grpc_channel, goto_stub=None) + + assert head.__repr__() != "" + + assert not head.neck.is_on() + assert head.is_off() + assert not head.is_on() + + head.send_goal_positions() + + assert len(head._actuators) == 3 + assert isinstance(head._actuators, dict) + + # use to_position() to convert radian to degree + assert head.neck.roll.goal_position == to_position(goal_rot.rpy.roll.value) + assert head.neck.roll.present_position == to_position(present_rot.rpy.roll.value) + assert head.neck.pitch.goal_position == to_position(goal_rot.rpy.pitch.value) + assert head.neck.pitch.present_position == to_position(present_rot.rpy.pitch.value) + assert head.neck.yaw.goal_position == to_position(goal_rot.rpy.yaw.value) + assert head.neck.yaw.present_position == to_position(present_rot.rpy.yaw.value) + + assert head.joints["neck.pitch"]._axis_type == "pitch" + assert head.joints["neck.pitch"].goal_position == to_position(goal_rot.rpy.pitch.value) + assert head.joints["neck.pitch"].present_position == to_position(present_rot.rpy.pitch.value) + + assert head.joints["neck.yaw"]._axis_type == "yaw" + assert head.joints["neck.yaw"].goal_position == to_position(goal_rot.rpy.yaw.value) + assert head.joints["neck.yaw"].present_position == to_position(present_rot.rpy.yaw.value) + + assert head.joints["neck.roll"]._axis_type == "roll" + assert head.joints["neck.roll"].goal_position == to_position(goal_rot.rpy.roll.value) + assert head.joints["neck.roll"].present_position == to_position(present_rot.rpy.roll.value) + + assert head.joints["l_antenna"].goal_position == to_position(goal_position1.value) + assert head.joints["l_antenna"].present_position == to_position(present_position1.value) + + assert head.joints["r_antenna"].goal_position == to_position(goal_position2.value) + assert head.joints["r_antenna"].present_position == to_position(present_position2.value) + + # updating values + compliance = BoolValue(value=False) + + pid = PID3d( + motor_1=PIDGains(p=FloatValue(value=0), i=FloatValue(value=0), d=FloatValue(value=0)), + motor_2=PIDGains(p=FloatValue(value=0), i=FloatValue(value=0), d=FloatValue(value=0)), + motor_3=PIDGains(p=FloatValue(value=0), i=FloatValue(value=0), d=FloatValue(value=0)), + ) + + temperature = Float3d(motor_1=FloatValue(value=0), motor_2=FloatValue(value=0), motor_3=FloatValue(value=0)) + speed_limit = Float3d(motor_1=FloatValue(value=0), motor_2=FloatValue(value=0), motor_3=FloatValue(value=0)) + torque_limit = Float3d(motor_1=FloatValue(value=0), motor_2=FloatValue(value=0), motor_3=FloatValue(value=0)) + present_speed = Vector3d(x=FloatValue(value=0), y=FloatValue(value=0), z=FloatValue(value=0)) + present_load = Vector3d(x=FloatValue(value=0), y=FloatValue(value=0), z=FloatValue(value=0)) + present_rot = Rotation3d(rpy=ExtEulerAngles(roll=FloatValue(value=7), pitch=FloatValue(value=8), yaw=FloatValue(value=9))) + goal_rot = Rotation3d(rpy=ExtEulerAngles(roll=FloatValue(value=10), pitch=FloatValue(value=11), yaw=FloatValue(value=12))) + neck_state = Orbita3dState( + compliant=compliance, + present_position=present_rot, + goal_position=goal_rot, + temperature=temperature, + pid=pid, + speed_limit=speed_limit, + torque_limit=torque_limit, + present_speed=present_speed, + present_load=present_load, + ) + + compliance = BoolValue(value=False) + + pid = PID3d( + motor_1=PIDGains(p=FloatValue(value=0), i=FloatValue(value=0), d=FloatValue(value=0)), + motor_2=PIDGains(p=FloatValue(value=0), i=FloatValue(value=0), d=FloatValue(value=0)), + motor_3=PIDGains(p=FloatValue(value=0), i=FloatValue(value=0), d=FloatValue(value=0)), + ) + + compliance3 = BoolValue(value=False) + pid3 = PIDGains(p=FloatValue(value=0), i=FloatValue(value=0), d=FloatValue(value=0)) + motor2_id = ComponentId(id=2, name="motor2") + temperature3 = FloatValue(value=0) + speed_limit3 = FloatValue(value=0) + torque_limit3 = FloatValue(value=0) + present_position3 = FloatValue(value=0.2) + goal_position3 = FloatValue(value=0.3) + present_speed3 = FloatValue(value=0) + present_load3 = FloatValue(value=0) + motor_state3 = DynamixelMotorState( + id=motor2_id, + compliant=compliance3, + present_position=present_position3, + goal_position=goal_position3, + temperature=temperature3, + pid=pid3, + speed_limit=speed_limit3, + torque_limit=torque_limit3, + present_speed=present_speed3, + present_load=present_load3, + ) + + head_state = HeadState(neck_state=neck_state, l_antenna_state=motor_state3, r_antenna_state=motor_state2) + + head._update_with(head_state) + + assert head.neck.is_on() + assert head.l_antenna.is_on() + assert not head.r_antenna.is_on() + assert not head.is_on() + assert not head.is_off() + + assert head.joints["l_antenna"].goal_position == to_position(goal_position3.value) + assert head.joints["l_antenna"].present_position == to_position(present_position3.value) + + assert head.joints["r_antenna"].goal_position == to_position(goal_position2.value) + assert head.joints["r_antenna"].present_position == to_position(present_position2.value) diff --git a/tests/units/online/test_advanced_goto_functions.py b/tests/units/online/test_advanced_goto_functions.py index dde16238..0818648b 100644 --- a/tests/units/online/test_advanced_goto_functions.py +++ b/tests/units/online/test_advanced_goto_functions.py @@ -246,19 +246,19 @@ def test_get_goto_request(reachy_sdk_zeroed: ReachySDK) -> None: ans1 = reachy_sdk_zeroed.get_goto_request(req1) assert ans1.part == "head" - assert np.allclose(ans1.request.goal_positions, [30, 0, 0], atol=1e-03) + assert np.allclose(ans1.request.target.joints, [30, 0, 0], atol=1e-03) assert ans1.request.duration == 5 assert ans1.request.mode == "minimum_jerk" ans2 = reachy_sdk_zeroed.get_goto_request(req2) assert ans2.part == "l_arm" - assert np.allclose(ans2.request.goal_positions, [10, 10, 15, -20, 15, -15, -10], atol=1e-03) + assert np.allclose(ans2.request.target.joints, [10, 10, 15, -20, 15, -15, -10], atol=1e-03) assert ans2.request.duration == 7 assert ans2.request.mode == "linear" ans3 = reachy_sdk_zeroed.get_goto_request(req3) assert ans3.part == "r_arm" - assert np.allclose(ans3.request.goal_positions, [0, 10, 20, -40, 10, 10, -15], atol=1e-03) + assert np.allclose(ans3.request.target.joints, [0, 10, 20, -40, 10, 10, -15], atol=1e-03) assert ans3.request.duration == 10 assert ans3.request.mode == "minimum_jerk" @@ -338,7 +338,7 @@ def test_reachy_goto_posture(reachy_sdk_zeroed: ReachySDK) -> None: ans_r = reachy_sdk_zeroed.get_goto_request(req_r) assert ans_r.part == "r_arm" - assert np.allclose(ans_r.request.goal_positions, zero_r_arm, atol=1e-03) + assert np.allclose(ans_r.request.target.joints, zero_r_arm, atol=1e-03) assert ans_r.request.duration == 2 assert ans_r.request.mode == "minimum_jerk" @@ -383,7 +383,7 @@ def test_reachy_goto_posture(reachy_sdk_zeroed: ReachySDK) -> None: ans_l2 = reachy_sdk_zeroed.get_goto_request(req_l2) assert ans_l2.part == "l_arm" - assert np.allclose(ans_l2.request.goal_positions, zero_l_arm, atol=1e-03) + assert np.allclose(ans_l2.request.target.joints, zero_l_arm, atol=1e-03) assert ans_l2.request.duration == 1 assert ans_l2.request.mode == "linear" @@ -422,7 +422,7 @@ def test_reachy_goto_posture(reachy_sdk_zeroed: ReachySDK) -> None: ans_l3 = reachy_sdk_zeroed.get_goto_request(req_l3) assert ans_l3.part == "l_arm" - assert np.allclose(ans_l3.request.goal_positions, elbow_90_l_arm, atol=1e-03) + assert np.allclose(ans_l3.request.target.joints, elbow_90_l_arm, atol=1e-03) assert ans_l3.request.duration == 2 assert ans_l3.request.mode == "minimum_jerk" @@ -673,8 +673,16 @@ def test_translate_by_robot_frame(reachy_sdk_zeroed: ReachySDK) -> None: req4 = reachy_sdk_zeroed.r_arm.goto([-10, -15, 20, -110, 0, 0, 0]) req5 = reachy_sdk_zeroed.r_arm.translate_by(0.1, -0.1, -0.1) - pose4 = reachy_sdk_zeroed.r_arm.forward_kinematics(reachy_sdk_zeroed.get_goto_request(req4).request.goal_positions) - pose5 = reachy_sdk_zeroed.r_arm.forward_kinematics(reachy_sdk_zeroed.get_goto_request(req5).request.goal_positions) + get_req4 = reachy_sdk_zeroed.get_goto_request(req4) + if get_req4.request.target.joints is not None: + pose4 = reachy_sdk_zeroed.r_arm.forward_kinematics(get_req4.request.target.joints) + else: + pose4 = get_req4.request.target.pose + get_req5 = reachy_sdk_zeroed.get_goto_request(req5) + if get_req5.request.target.joints is not None: + pose5 = reachy_sdk_zeroed.r_arm.forward_kinematics(get_req5.request.target.joints) + else: + pose5 = get_req5.request.target.pose assert np.allclose(pose4[:3, :3], pose5[:3, :3], atol=1e-03) assert np.isclose(pose4[0, 3] + 0.1, pose5[0, 3], atol=1e-03) assert np.isclose(pose4[1, 3] - 0.1, pose5[1, 3], atol=1e-03) @@ -683,7 +691,7 @@ def test_translate_by_robot_frame(reachy_sdk_zeroed: ReachySDK) -> None: req6 = reachy_sdk_zeroed.r_arm.translate_by(0, 0, -0.1) with pytest.raises(ValueError): - reachy_sdk_zeroed.r_arm.forward_kinematics(reachy_sdk_zeroed.get_goto_request(req6).request.goal_positions) + reachy_sdk_zeroed.r_arm.forward_kinematics(reachy_sdk_zeroed.get_goto_request(req6).request.target.pose) @pytest.mark.online @@ -709,8 +717,16 @@ def test_translate_by_gripper_frame(reachy_sdk_zeroed: ReachySDK) -> None: req4 = reachy_sdk_zeroed.r_arm.goto([-10, -15, 20, -110, 0, 0, 0]) req5 = reachy_sdk_zeroed.r_arm.translate_by(0.1, -0.1, -0.1, frame="gripper") - pose4 = reachy_sdk_zeroed.r_arm.forward_kinematics(reachy_sdk_zeroed.get_goto_request(req4).request.goal_positions) - pose5 = reachy_sdk_zeroed.r_arm.forward_kinematics(reachy_sdk_zeroed.get_goto_request(req5).request.goal_positions) + get_req4 = reachy_sdk_zeroed.get_goto_request(req4) + if get_req4.request.target.joints is not None: + pose4 = reachy_sdk_zeroed.r_arm.forward_kinematics(get_req4.request.target.joints) + else: + pose4 = get_req4.request.target.pose + get_req5 = reachy_sdk_zeroed.get_goto_request(req5) + if get_req5.request.target.joints is not None: + pose5 = reachy_sdk_zeroed.r_arm.forward_kinematics(get_req5.request.target.joints) + else: + pose5 = get_req5.request.target.pose translation5 = np.eye(4) translation5[0, 3] = 0.1 translation5[1, 3] = -0.1 @@ -723,17 +739,26 @@ def test_translate_by_gripper_frame(reachy_sdk_zeroed: ReachySDK) -> None: req6 = reachy_sdk_zeroed.r_arm.goto([-10, -15, 30, -70, 0, 10, 0]) req7 = reachy_sdk_zeroed.r_arm.translate_by(0.15, 0, 0.05, frame="gripper") - pose6 = reachy_sdk_zeroed.r_arm.forward_kinematics(reachy_sdk_zeroed.get_goto_request(req6).request.goal_positions) - pose7 = reachy_sdk_zeroed.r_arm.forward_kinematics(reachy_sdk_zeroed.get_goto_request(req7).request.goal_positions) + get_req6 = reachy_sdk_zeroed.get_goto_request(req6) + if get_req6.request.target.joints is not None: + pose6 = reachy_sdk_zeroed.r_arm.forward_kinematics(get_req6.request.target.joints) + else: + pose6 = get_req6.request.target.pose + get_req7 = reachy_sdk_zeroed.get_goto_request(req7) + if get_req7.request.target.joints is not None: + pose7 = reachy_sdk_zeroed.r_arm.forward_kinematics(get_req7.request.target.joints) + else: + pose7 = get_req7.request.target.pose translation7 = np.eye(4) translation7[0, 3] = 0.15 translation7[2, 3] = 0.05 assert np.allclose(pose6 @ translation7, pose7, atol=1e-03) - req8 = reachy_sdk_zeroed.r_arm.translate_by(0, -0.3, -0.3, frame="gripper") + req8 = reachy_sdk_zeroed.r_arm.translate_by(0, -0.3, -0.3, frame="gripper", wait=True) - with pytest.raises(ValueError): - reachy_sdk_zeroed.r_arm.forward_kinematics(reachy_sdk_zeroed.get_goto_request(req8).request.goal_positions) + assert not np.allclose( + reachy_sdk_zeroed.get_goto_request(req8).request.target.pose, reachy_sdk_zeroed.r_arm.forward_kinematics() + ) @pytest.mark.online @@ -760,7 +785,11 @@ def test_rotate_by_robot_frame(reachy_sdk_zeroed: ReachySDK) -> None: req4 = reachy_sdk_zeroed.r_arm.goto([-10, 10, 20, -110, 0, 0, 0]) req5 = reachy_sdk_zeroed.r_arm.rotate_by(15, -10, -5, frame="robot") - pose5 = reachy_sdk_zeroed.r_arm.forward_kinematics(reachy_sdk_zeroed.get_goto_request(req5).request.goal_positions) + get_req5 = reachy_sdk_zeroed.get_goto_request(req5) + if get_req5.request.target.joints is not None: + pose5 = reachy_sdk_zeroed.r_arm.forward_kinematics(get_req5.request.target.joints) + else: + pose5 = get_req5.request.target.pose expected_pose5 = np.array( [ [-0.62069375, -0.4494677, -0.64243136, 0.32550951], @@ -777,7 +806,11 @@ def test_rotate_by_robot_frame(reachy_sdk_zeroed: ReachySDK) -> None: req6 = reachy_sdk_zeroed.r_arm.goto([-10, 10, 30, -70, 0, 10, 0]) req7 = reachy_sdk_zeroed.r_arm.rotate_by(0.15, 0, 0.05) - pose7 = reachy_sdk_zeroed.r_arm.forward_kinematics(reachy_sdk_zeroed.get_goto_request(req7).request.goal_positions) + get_req7 = reachy_sdk_zeroed.get_goto_request(req7) + if get_req7.request.target.joints is not None: + pose7 = reachy_sdk_zeroed.r_arm.forward_kinematics(get_req7.request.target.joints) + else: + pose7 = get_req7.request.target.pose expected_pose7 = np.array( [ [0.20722949, -0.6570895, -0.72476846, 0.33380879], @@ -821,7 +854,11 @@ def test_rotate_by_gripper_frame(reachy_sdk_zeroed: ReachySDK) -> None: req4 = reachy_sdk_zeroed.r_arm.goto([-10, 10, 10, -110, 0, 0, 0]) req5 = reachy_sdk_zeroed.r_arm.rotate_by(15, -10, -5, frame="gripper") - pose5 = reachy_sdk_zeroed.r_arm.forward_kinematics(reachy_sdk_zeroed.get_goto_request(req5).request.goal_positions) + get_req5 = reachy_sdk_zeroed.get_goto_request(req5) + if get_req5.request.target.joints is not None: + pose5 = reachy_sdk_zeroed.r_arm.forward_kinematics(get_req5.request.target.joints) + else: + pose5 = get_req5.request.target.pose expected_pose5 = np.array( [ [-0.58122566, -0.57576842, -0.57503692, 0.35318658], @@ -838,7 +875,11 @@ def test_rotate_by_gripper_frame(reachy_sdk_zeroed: ReachySDK) -> None: req6 = reachy_sdk_zeroed.r_arm.goto([-10, 10, 15, -70, 0, 10, 0]) req7 = reachy_sdk_zeroed.r_arm.rotate_by(0.15, 0, 0.05, frame="gripper") - pose7 = reachy_sdk_zeroed.r_arm.forward_kinematics(reachy_sdk_zeroed.get_goto_request(req7).request.goal_positions) + get_req7 = reachy_sdk_zeroed.get_goto_request(req7) + if get_req7.request.target.joints is not None: + pose7 = reachy_sdk_zeroed.r_arm.forward_kinematics(get_req7.request.target.joints) + else: + pose7 = get_req7.request.target.pose expected_pose7 = np.array( [ [0.27965445, -0.44712639, -0.84963015, 0.38450476], diff --git a/tests/units/online/test_antennas_movements.py b/tests/units/online/test_antennas_movements.py new file mode 100644 index 00000000..3dee171d --- /dev/null +++ b/tests/units/online/test_antennas_movements.py @@ -0,0 +1,62 @@ +import time + +import numpy as np +import pytest + +from reachy2_sdk.reachy_sdk import ReachySDK + + +@pytest.mark.online +def test_basic_antennas(reachy_sdk_zeroed: ReachySDK) -> None: + l_goal_position = -20 + r_goal_position = 50 + reachy_sdk_zeroed.head.l_antenna.goal_position = l_goal_position + reachy_sdk_zeroed.head.r_antenna.goal_position = r_goal_position + reachy_sdk_zeroed.send_goal_positions() + time.sleep(1) + assert np.isclose(reachy_sdk_zeroed.head.l_antenna.present_position, l_goal_position, 1e-03) + assert np.isclose(reachy_sdk_zeroed.head.r_antenna.present_position, r_goal_position, 1e-03) + + +@pytest.mark.online +def test_antennas_goto(reachy_sdk_zeroed: ReachySDK) -> None: + reachy_sdk_zeroed.head.l_antenna.goto(-20) + reachy_sdk_zeroed.head.r_antenna.goto(50, wait=True) + assert np.isclose(reachy_sdk_zeroed.head.l_antenna.present_position, -20, 1e-03) + assert np.isclose(reachy_sdk_zeroed.head.r_antenna.present_position, 50, 1e-03) + + l_id = reachy_sdk_zeroed.head.l_antenna.goto(40) + r_id = reachy_sdk_zeroed.head.r_antenna.goto(-30, interpolation_mode="linear", duration=1) + l_req = reachy_sdk_zeroed.get_goto_request(l_id) + r_req = reachy_sdk_zeroed.get_goto_request(r_id) + + assert l_req.part == "l_antenna" + assert np.isclose(l_req.request.target.joints, 40.0, 1e-03) + assert np.isclose(l_req.request.duration, 2.0, 1e-03) + assert l_req.request.mode == "minimum_jerk" + assert l_req.request.interpolation_space == "joint_space" + + assert r_req.part == "r_antenna" + assert np.isclose(r_req.request.target.joints, -30.0, 1e-03) + assert np.isclose(r_req.request.duration, 1.0, 1e-03) + assert r_req.request.mode == "linear" + assert r_req.request.interpolation_space == "joint_space" + + l_id = reachy_sdk_zeroed.head.l_antenna.goto(10, wait=True) + assert reachy_sdk_zeroed.is_goto_finished(l_id) + assert np.isclose(reachy_sdk_zeroed.head.l_antenna.present_position, 10, 1e-03) + + reachy_sdk_zeroed.head.r_antenna.goto(20) + reachy_sdk_zeroed.head.r_antenna.goto(40) + r_id = reachy_sdk_zeroed.head.r_antenna.goto(0) + reachy_sdk_zeroed.head.r_antenna.goto(30) + time.sleep(0.1) + assert len(reachy_sdk_zeroed.head.r_antenna.get_goto_queue()) == 3 + reachy_sdk_zeroed.cancel_goto_by_id(r_id) + time.sleep(0.1) + assert len(reachy_sdk_zeroed.head.r_antenna.get_goto_queue()) == 2 + reachy_sdk_zeroed.head.r_antenna.cancel_all_goto() + time.sleep(0.1) + assert len(reachy_sdk_zeroed.head.r_antenna.get_goto_queue()) == 0 + assert reachy_sdk_zeroed.is_goto_finished(r_id) + assert reachy_sdk_zeroed.head.r_antenna.get_goto_playing().id == -1 diff --git a/tests/units/online/test_audit.py b/tests/units/online/test_audit.py index c56c9830..f9324041 100644 --- a/tests/units/online/test_audit.py +++ b/tests/units/online/test_audit.py @@ -1,5 +1,3 @@ -import time - import pytest from reachy2_sdk.reachy_sdk import ReachySDK @@ -25,7 +23,7 @@ def test_audit(reachy_sdk_zeroed: ReachySDK) -> None: "elbow": reachy_sdk_zeroed.l_arm.elbow.audit, "wrist": reachy_sdk_zeroed.l_arm.wrist.audit, } - assert reachy_sdk_zeroed.head.audit == {"neck": reachy_sdk_zeroed.head.neck.audit} + assert reachy_sdk_zeroed.head.audit == {"neck": reachy_sdk_zeroed.head.neck.audit, "l_antenna": None, "r_antenna": None} assert reachy_sdk_zeroed.audit["r_arm"] == reachy_sdk_zeroed.r_arm.audit assert reachy_sdk_zeroed.audit["l_arm"] == reachy_sdk_zeroed.l_arm.audit diff --git a/tests/units/online/test_interpolations.py b/tests/units/online/test_interpolations.py index 0dcbb214..81493bd5 100644 --- a/tests/units/online/test_interpolations.py +++ b/tests/units/online/test_interpolations.py @@ -11,52 +11,8 @@ from .test_basic_movements import build_pose_matrix -class LoopThread: - def __init__(self, reachy: ReachySDK, arm: str): - self.reachy = reachy - self.arm = arm - self.running = False - self.forward_list = [] - self.thread = Thread( - target=self.loop_forward_function, - args=( - self.reachy, - self.arm, - ), - ) - - def start(self): - self.running = True - self.thread.start() - - def stop(self): - self.running = False - self.thread.join() - - def loop_forward_function(self, reachy: ReachySDK, arm: str): - while self.running: - if arm == "r_arm": - self.forward_list.append(reachy.r_arm.forward_kinematics()) - else: - self.forward_list.append(reachy.l_arm.forward_kinematics()) - time.sleep(0.5) - - @pytest.mark.online -def test_send_cartesian_interpolation_linear(reachy_sdk_zeroed: ReachySDK) -> None: - def test_pose_trajectory( - A: npt.NDArray[np.float64], B: npt.NDArray[np.float64], duration: float - ) -> List[npt.NDArray[np.float64]]: - reachy_sdk_zeroed.r_arm.goto(A, wait=True) - t = LoopThread(reachy_sdk_zeroed, "r_arm") - t.start() - tic = time.time() - reachy_sdk_zeroed.r_arm.send_cartesian_interpolation(B, duration=duration, precision_distance_xyz=0.005) - elapsed_time = time.time() - tic - t.stop() - assert np.isclose(elapsed_time, duration, 1) - return t.forward_list - +def test_goto_cartesian_interpolation_linear(reachy_sdk_zeroed: ReachySDK) -> None: xA = 0.3 yA = -0.2 zA = -0.3 @@ -65,16 +21,20 @@ def test_pose_trajectory( zB = zA A = build_pose_matrix(xA, yA, zA) B = build_pose_matrix(xB, yB, zB) - inter_poses = test_pose_trajectory(A, B, 3.0) + reachy_sdk_zeroed.r_arm.goto(A, wait=True) + B_gotoid = reachy_sdk_zeroed.r_arm.goto(B, interpolation_space="cartesian_space", interpolation_mode="linear") + inter_poses = [] + while not reachy_sdk_zeroed.is_goto_finished(B_gotoid): + inter_poses.append(reachy_sdk_zeroed.r_arm.forward_kinematics()) B_forward = reachy_sdk_zeroed.r_arm.forward_kinematics() - assert np.allclose(B_forward, B, atol=1e-03) + assert np.allclose(B_forward, B, atol=1e-02) for pose in inter_poses: assert np.isclose(pose[0, 3], xB, 1e-03) assert (pose[1, 3] <= max(yA, yB) or np.isclose(pose[1, 3], max(yA, yB), 1e-03)) and ( pose[1, 3] >= min(yA, yB) or np.isclose(pose[1, 3], min(yA, yB), 1e-03) ) assert np.isclose(pose[2, 3], zB, 1e-03) - assert np.allclose(pose[:3, :3], B_forward[:3, :3], atol=1e-03) + assert np.allclose(pose[:3, :3], B_forward[:3, :3], atol=1e-02) xA = 0.3 yA = -0.3 @@ -84,9 +44,13 @@ def test_pose_trajectory( zB = -0.1 A = build_pose_matrix(xA, yA, zA) B = build_pose_matrix(xB, yB, zB) - inter_poses = test_pose_trajectory(A, B, 2.0) + reachy_sdk_zeroed.r_arm.goto(A, wait=True) + B_gotoid = reachy_sdk_zeroed.r_arm.goto(B, interpolation_space="cartesian_space", interpolation_mode="linear") + inter_poses = [] + while not reachy_sdk_zeroed.is_goto_finished(B_gotoid): + inter_poses.append(reachy_sdk_zeroed.r_arm.forward_kinematics()) B_forward = reachy_sdk_zeroed.r_arm.forward_kinematics() - assert np.allclose(B_forward, B, atol=1e-03) + assert np.allclose(B_forward, B, atol=1e-02) for pose in inter_poses: assert (pose[0, 3] <= max(xA, xB) or np.isclose(pose[0, 3], max(xA, xB), 1e-03)) and ( pose[0, 3] >= min(xA, xB) or np.isclose(pose[0, 3], min(xA, xB), 1e-03) @@ -95,26 +59,11 @@ def test_pose_trajectory( assert (pose[2, 3] <= max(zA, zB) or np.isclose(pose[2, 3], max(zA, zB), 1e-03)) and ( pose[2, 3] >= min(zA, zB) or np.isclose(pose[2, 3], max(zA, zB), 1e-03) ) - assert np.allclose(pose[:3, :3], B_forward[:3, :3], atol=1e-03) + assert np.allclose(pose[:3, :3], B_forward[:3, :3], atol=1e-02) @pytest.mark.online -def test_send_cartesian_interpolation_circular(reachy_sdk_zeroed: ReachySDK) -> None: - def test_pose_trajectory( - A: npt.NDArray[np.float64], B: npt.NDArray[np.float64], duration: float, arc_direction: str - ) -> List[npt.NDArray[np.float64]]: - reachy_sdk_zeroed.r_arm.goto(A, wait=True) - t = LoopThread(reachy_sdk_zeroed, "r_arm") - t.start() - tic = time.time() - reachy_sdk_zeroed.r_arm.send_cartesian_interpolation( - B, duration=duration, arc_direction=arc_direction, precision_distance_xyz=0.005 - ) - elapsed_time = time.time() - tic - t.stop() - assert np.isclose(elapsed_time, duration, 1) - return t.forward_list - +def test_goto_cartesian_interpolation_elliptical(reachy_sdk_zeroed: ReachySDK) -> None: xA = 0.3 yA = -0.2 zA = -0.3 @@ -125,9 +74,15 @@ def test_pose_trajectory( B = build_pose_matrix(xB, yB, zB) # Test below - inter_poses = test_pose_trajectory(A, B, 3.0, arc_direction="below") + reachy_sdk_zeroed.r_arm.goto(A, wait=True) + B_gotoid = reachy_sdk_zeroed.r_arm.goto( + B, interpolation_space="cartesian_space", interpolation_mode="elliptical", arc_direction="below" + ) + inter_poses = [] + while not reachy_sdk_zeroed.is_goto_finished(B_gotoid): + inter_poses.append(reachy_sdk_zeroed.r_arm.forward_kinematics()) B_forward = reachy_sdk_zeroed.r_arm.forward_kinematics() - assert np.allclose(B_forward, B, atol=1e-03) + assert np.allclose(B_forward, B, atol=1e-02) went_down = False for pose in inter_poses: assert np.isclose(pose[0, 3], xB, 1e-03) @@ -138,15 +93,21 @@ def test_pose_trajectory( assert (pose[2, 3] <= zB or np.isclose(pose[2, 3], zB, 1e-03)) and ( pose[2, 3] >= z_limit or np.isclose(pose[2, 3], z_limit, 1e-03) ) - assert np.allclose(pose[:3, :3], B_forward[:3, :3], atol=1e-03) + assert np.allclose(pose[:3, :3], B_forward[:3, :3], atol=1e-02) if pose[2, 3] < z_limit + 0.05: went_down = True assert went_down # Test above - inter_poses = test_pose_trajectory(A, B, 3.0, arc_direction="above") + reachy_sdk_zeroed.r_arm.goto(A, wait=True) + B_gotoid = reachy_sdk_zeroed.r_arm.goto( + B, interpolation_space="cartesian_space", interpolation_mode="elliptical", arc_direction="above" + ) + inter_poses = [] + while not reachy_sdk_zeroed.is_goto_finished(B_gotoid): + inter_poses.append(reachy_sdk_zeroed.r_arm.forward_kinematics()) B_forward = reachy_sdk_zeroed.r_arm.forward_kinematics() - assert np.allclose(B_forward, B, atol=1e-03) + assert np.allclose(B_forward, B, atol=1e-02) went_up = False for pose in inter_poses: assert np.isclose(pose[0, 3], xB, 1e-03) @@ -157,15 +118,21 @@ def test_pose_trajectory( assert (pose[2, 3] >= zB or np.isclose(pose[2, 3], zB, 1e-03)) and ( pose[2, 3] <= z_limit or np.isclose(pose[2, 3], z_limit, 1e-03) ) - assert np.allclose(pose[:3, :3], B_forward[:3, :3], atol=1e-03) + assert np.allclose(pose[:3, :3], B_forward[:3, :3], atol=1e-02) if pose[2, 3] > z_limit - 0.05: went_up = True assert went_up # Test front - inter_poses = test_pose_trajectory(A, B, 3.0, arc_direction="front") + reachy_sdk_zeroed.r_arm.goto(A, wait=True) + B_gotoid = reachy_sdk_zeroed.r_arm.goto( + B, interpolation_space="cartesian_space", interpolation_mode="elliptical", arc_direction="front" + ) + inter_poses = [] + while not reachy_sdk_zeroed.is_goto_finished(B_gotoid): + inter_poses.append(reachy_sdk_zeroed.r_arm.forward_kinematics()) B_forward = reachy_sdk_zeroed.r_arm.forward_kinematics() - assert np.allclose(B_forward, B, atol=1e-03) + assert np.allclose(B_forward, B, atol=1e-02) went_front = False for pose in inter_poses: x_limit = xB + abs(yA - yB) / 2 @@ -178,13 +145,19 @@ def test_pose_trajectory( pose[1, 3] >= min(yA, yB) or np.isclose(pose[1, 3], min(yA, yB), 1e-03) ) assert np.isclose(pose[2, 3], zB, 1e-03) - assert np.allclose(pose[:3, :3], B_forward[:3, :3], atol=1e-03) + assert np.allclose(pose[:3, :3], B_forward[:3, :3], atol=1e-02) assert went_front # Test back - inter_poses = test_pose_trajectory(A, B, 3.0, arc_direction="back") + reachy_sdk_zeroed.r_arm.goto(A, wait=True) + B_gotoid = reachy_sdk_zeroed.r_arm.goto( + B, interpolation_space="cartesian_space", interpolation_mode="elliptical", arc_direction="back" + ) + inter_poses = [] + while not reachy_sdk_zeroed.is_goto_finished(B_gotoid): + inter_poses.append(reachy_sdk_zeroed.r_arm.forward_kinematics()) B_forward = reachy_sdk_zeroed.r_arm.forward_kinematics() - assert np.allclose(B_forward, B, atol=1e-03) + assert np.allclose(B_forward, B, atol=1e-02) went_back = False for pose in inter_poses: x_limit = xB - abs(yA - yB) / 2 @@ -197,13 +170,19 @@ def test_pose_trajectory( pose[1, 3] >= min(yA, yB) or np.isclose(pose[1, 3], min(yA, yB), 1e-03) ) assert np.isclose(pose[2, 3], zB, 1e-03) - assert np.allclose(pose[:3, :3], B_forward[:3, :3], atol=1e-03) + assert np.allclose(pose[:3, :3], B_forward[:3, :3], atol=1e-02) assert went_back # Test right - inter_poses = test_pose_trajectory(A, B, 3.0, arc_direction="right") + reachy_sdk_zeroed.r_arm.goto(A, wait=True) + B_gotoid = reachy_sdk_zeroed.r_arm.goto( + B, interpolation_space="cartesian_space", interpolation_mode="elliptical", arc_direction="right" + ) + inter_poses = [] + while not reachy_sdk_zeroed.is_goto_finished(B_gotoid): + inter_poses.append(reachy_sdk_zeroed.r_arm.forward_kinematics()) B_forward = reachy_sdk_zeroed.r_arm.forward_kinematics() - assert np.allclose(B_forward, B, atol=1e-03) + assert np.allclose(B_forward, B, atol=1e-02) for pose in inter_poses: assert np.isclose(pose[0, 3], xB, 1e-03) assert (pose[1, 3] <= max(yA, yB) or np.isclose(pose[1, 3], max(yA, yB), 1e-03)) and ( @@ -212,12 +191,18 @@ def test_pose_trajectory( assert (pose[2, 3] <= max(zA, zB) or np.isclose(pose[2, 3], max(zA, zB), 1e-03)) and ( pose[2, 3] >= min(zA, zB) or np.isclose(pose[2, 3], max(zA, zB), 1e-03) ) - assert np.allclose(pose[:3, :3], B_forward[:3, :3], atol=1e-03) + assert np.allclose(pose[:3, :3], B_forward[:3, :3], atol=1e-02) # Test left - inter_poses = test_pose_trajectory(A, B, 3.0, arc_direction="left") + reachy_sdk_zeroed.r_arm.goto(A, wait=True) + B_gotoid = reachy_sdk_zeroed.r_arm.goto( + B, interpolation_space="cartesian_space", interpolation_mode="elliptical", arc_direction="left" + ) + inter_poses = [] + while not reachy_sdk_zeroed.is_goto_finished(B_gotoid): + inter_poses.append(reachy_sdk_zeroed.r_arm.forward_kinematics()) B_forward = reachy_sdk_zeroed.r_arm.forward_kinematics() - assert np.allclose(B_forward, B, atol=1e-03) + assert np.allclose(B_forward, B, atol=1e-02) for pose in inter_poses: assert np.isclose(pose[0, 3], xB, 1e-03) assert (pose[1, 3] <= max(yA, yB) or np.isclose(pose[1, 3], max(yA, yB), 1e-03)) and ( @@ -226,4 +211,209 @@ def test_pose_trajectory( assert (pose[2, 3] <= max(zA, zB) or np.isclose(pose[2, 3], max(zA, zB), 1e-03)) and ( pose[2, 3] >= min(zA, zB) or np.isclose(pose[2, 3], max(zA, zB), 1e-03) ) - assert np.allclose(pose[:3, :3], B_forward[:3, :3], atol=1e-03) + assert np.allclose(pose[:3, :3], B_forward[:3, :3], atol=1e-02) + + +@pytest.mark.online +def test_goto_cartesian_interpolation_elliptical_radius(reachy_sdk_zeroed: ReachySDK) -> None: + xA = 0.3 + yA = -0.2 + zA = -0.2 + xB = xA + yB = -0.4 + zB = zA + A = build_pose_matrix(xA, yA, zA) + B = build_pose_matrix(xB, yB, zB) + + # Test below + reachy_sdk_zeroed.r_arm.goto(A, wait=True) + secondary_radius = 0.2 + B_gotoid = reachy_sdk_zeroed.r_arm.goto( + B, + interpolation_space="cartesian_space", + interpolation_mode="elliptical", + arc_direction="below", + secondary_radius=secondary_radius, + ) + inter_poses = [] + while not reachy_sdk_zeroed.is_goto_finished(B_gotoid): + inter_poses.append(reachy_sdk_zeroed.r_arm.forward_kinematics()) + B_forward = reachy_sdk_zeroed.r_arm.forward_kinematics() + assert np.allclose(B_forward, B, atol=1e-02) + min_z = zA + z_limit = zB - secondary_radius + for pose in inter_poses: + assert np.isclose(pose[0, 3], xB, 1e-03) + assert (pose[1, 3] <= max(yA, yB) or np.isclose(pose[1, 3], max(yA, yB), 1e-03)) and ( + pose[1, 3] >= min(yA, yB) or np.isclose(pose[1, 3], min(yA, yB), 1e-03) + ) + assert (pose[2, 3] <= zB or np.isclose(pose[2, 3], zB, 1e-03)) and ( + pose[2, 3] >= z_limit or np.isclose(pose[2, 3], z_limit, 1e-03) + ) + assert np.allclose(pose[:3, :3], B_forward[:3, :3], atol=1e-02) + if pose[2, 3] < min_z: + min_z = pose[2, 3] + assert np.isclose(min_z, z_limit, 1e-03) + + # Test above + reachy_sdk_zeroed.r_arm.goto(A, wait=True) + secondary_radius = 0.05 + B_gotoid = reachy_sdk_zeroed.r_arm.goto( + B, + interpolation_space="cartesian_space", + interpolation_mode="elliptical", + arc_direction="above", + secondary_radius=secondary_radius, + ) + inter_poses = [] + while not reachy_sdk_zeroed.is_goto_finished(B_gotoid): + inter_poses.append(reachy_sdk_zeroed.r_arm.forward_kinematics()) + B_forward = reachy_sdk_zeroed.r_arm.forward_kinematics() + assert np.allclose(B_forward, B, atol=1e-02) + max_z = zA + z_limit = zB + secondary_radius + for pose in inter_poses: + assert np.isclose(pose[0, 3], xB, 1e-03) + assert (pose[1, 3] <= max(yA, yB) or np.isclose(pose[1, 3], max(yA, yB), 1e-03)) and ( + pose[1, 3] >= min(yA, yB) or np.isclose(pose[1, 3], min(yA, yB), 1e-03) + ) + assert (pose[2, 3] >= zB or np.isclose(pose[2, 3], zB, 1e-03)) and ( + pose[2, 3] <= z_limit or np.isclose(pose[2, 3], z_limit, 1e-03) + ) + assert np.allclose(pose[:3, :3], B_forward[:3, :3], atol=1e-02) + if pose[2, 3] > max_z: + max_z = pose[2, 3] + assert np.isclose(max_z, z_limit, 1e-03) + + xA = 0.4 + yA = -0.2 + zA = -0.3 + xB = xA + yB = -0.4 + zB = zA + A = build_pose_matrix(xA, yA, zA) + B = build_pose_matrix(xB, yB, zB) + + # Test front + reachy_sdk_zeroed.r_arm.goto(A, wait=True) + B_gotoid = reachy_sdk_zeroed.r_arm.goto( + B, interpolation_space="cartesian_space", interpolation_mode="elliptical", arc_direction="front" + ) + inter_poses = [] + while not reachy_sdk_zeroed.is_goto_finished(B_gotoid): + inter_poses.append(reachy_sdk_zeroed.r_arm.forward_kinematics()) + B_forward = reachy_sdk_zeroed.r_arm.forward_kinematics() + assert np.allclose(B_forward, B, atol=1e-02) + max_x = xA + x_limit = xB + abs(yA - yB) / 2 + for pose in inter_poses: + assert (pose[0, 3] >= xB or np.isclose(pose[0, 3], xB, 1e-03)) and ( + pose[0, 3] <= x_limit or np.isclose(pose[0, 3], x_limit, 1e-03) + ) + if pose[0, 3] > max_x: + max_x = pose[0, 3] + assert (pose[1, 3] <= max(yA, yB) or np.isclose(pose[1, 3], max(yA, yB), 1e-03)) and ( + pose[1, 3] >= min(yA, yB) or np.isclose(pose[1, 3], min(yA, yB), 1e-03) + ) + assert np.isclose(pose[2, 3], zB, 1e-03) + assert np.allclose(pose[:3, :3], B_forward[:3, :3], atol=1e-02) + assert np.isclose(max_x, x_limit, 1e-03) + + # Test back + reachy_sdk_zeroed.r_arm.goto(A, wait=True) + secondary_radius = 0.2 + B_gotoid = reachy_sdk_zeroed.r_arm.goto( + B, + interpolation_space="cartesian_space", + interpolation_mode="elliptical", + arc_direction="back", + secondary_radius=secondary_radius, + ) + inter_poses = [] + while not reachy_sdk_zeroed.is_goto_finished(B_gotoid): + inter_poses.append(reachy_sdk_zeroed.r_arm.forward_kinematics()) + B_forward = reachy_sdk_zeroed.r_arm.forward_kinematics() + assert np.allclose(B_forward, B, atol=1e-02) + min_x = xA + x_limit = xB - secondary_radius + for pose in inter_poses: + assert (pose[0, 3] <= xB or np.isclose(pose[0, 3], xB, 1e-03)) and ( + pose[0, 3] >= x_limit or np.isclose(pose[0, 3], x_limit, 1e-03) + ) + if pose[0, 3] < min_x: + min_x = pose[0, 3] + assert (pose[1, 3] <= max(yA, yB) or np.isclose(pose[1, 3], max(yA, yB), 1e-03)) and ( + pose[1, 3] >= min(yA, yB) or np.isclose(pose[1, 3], min(yA, yB), 1e-03) + ) + assert np.isclose(pose[2, 3], zB, 1e-03) + assert np.allclose(pose[:3, :3], B_forward[:3, :3], atol=1e-02) + assert np.isclose(min_x, x_limit, 1e-03) + + xA = 0.2 + yA = -0.3 + zA = -0.3 + xB = 0.4 + yB = yA + zB = zA + A = build_pose_matrix(xA, yA, zA) + B = build_pose_matrix(xB, yB, zB) + + # Test right + reachy_sdk_zeroed.r_arm.goto(A, wait=True) + secondary_radius = 0.1 + B_gotoid = reachy_sdk_zeroed.r_arm.goto( + B, + interpolation_space="cartesian_space", + interpolation_mode="elliptical", + arc_direction="right", + secondary_radius=secondary_radius, + ) + inter_poses = [] + while not reachy_sdk_zeroed.is_goto_finished(B_gotoid): + inter_poses.append(reachy_sdk_zeroed.r_arm.forward_kinematics()) + B_forward = reachy_sdk_zeroed.r_arm.forward_kinematics() + assert np.allclose(B_forward, B, atol=1e-02) + min_y = yA + y_limit = yB - secondary_radius + for pose in inter_poses: + assert np.isclose(pose[2, 3], zB, 1e-03) + assert (pose[1, 3] >= y_limit or np.isclose(pose[1, 3], y_limit, 1e-03)) and ( + pose[1, 3] <= yB or np.isclose(pose[1, 3], yB, 1e-03) + ) + assert (pose[0, 3] <= max(xA, xB) or np.isclose(pose[0, 3], max(xA, xB), 1e-03)) and ( + pose[0, 3] >= min(xA, xB) or np.isclose(pose[0, 3], max(xA, xB), 1e-03) + ) + if pose[1, 3] < min_y: + min_y = pose[1, 3] + assert np.allclose(pose[:3, :3], B_forward[:3, :3], atol=1e-02) + assert np.isclose(min_y, y_limit, 1e-03) + + # Test left + reachy_sdk_zeroed.r_arm.goto(A, wait=True) + secondary_radius = 0.25 + B_gotoid = reachy_sdk_zeroed.r_arm.goto( + B, + interpolation_space="cartesian_space", + interpolation_mode="elliptical", + arc_direction="left", + secondary_radius=secondary_radius, + ) + inter_poses = [] + while not reachy_sdk_zeroed.is_goto_finished(B_gotoid): + inter_poses.append(reachy_sdk_zeroed.r_arm.forward_kinematics()) + B_forward = reachy_sdk_zeroed.r_arm.forward_kinematics() + assert np.allclose(B_forward, B, atol=1e-02) + max_y = yA + y_limit = yB + secondary_radius + for pose in inter_poses: + assert np.isclose(pose[2, 3], zB, 1e-03) + assert (pose[1, 3] >= yB or np.isclose(pose[1, 3], yB, 1e-03)) and ( + pose[1, 3] <= y_limit or np.isclose(pose[1, 3], y_limit, 1e-03) + ) + assert (pose[0, 3] <= max(xA, xB) or np.isclose(pose[0, 3], max(xA, xB), 1e-03)) and ( + pose[0, 3] >= min(xA, xB) or np.isclose(pose[0, 3], max(xA, xB), 1e-03) + ) + if pose[1, 3] > max_y: + max_y = pose[1, 3] + assert np.allclose(pose[:3, :3], B_forward[:3, :3], atol=1e-02) + assert np.isclose(max_y, y_limit, 1e-03) diff --git a/tests/units/online/test_mobile_base.py b/tests/units/online/test_mobile_base.py index b2812a3b..7bec40c4 100644 --- a/tests/units/online/test_mobile_base.py +++ b/tests/units/online/test_mobile_base.py @@ -288,9 +288,9 @@ def test_mobile_base_goto(reachy_sdk_zeroed: ReachySDK) -> None: assert is_goto_finished(reachy_sdk_zeroed, goto3) request3 = reachy_sdk_zeroed.get_goto_request(goto3) assert request3.part == "mobile_base" - assert np.isclose(request3.request.goal_positions["x"], 0.0, atol=1e-03) - assert np.isclose(request3.request.goal_positions["y"], -0.4, atol=1e-03) - assert np.isclose(request3.request.goal_positions["theta"], 30, atol=1e-03) + assert np.isclose(request3.request.target["x"], 0.0, atol=1e-03) + assert np.isclose(request3.request.target["y"], -0.4, atol=1e-03) + assert np.isclose(request3.request.target["theta"], 30, atol=1e-03) assert np.isclose(request3.request.distance_tolerance, 0.05, atol=1e-03) assert np.isclose(request3.request.angle_tolerance, 5, atol=1e-03) @@ -301,9 +301,9 @@ def test_mobile_base_goto(reachy_sdk_zeroed: ReachySDK) -> None: assert is_goto_finished(reachy_sdk_zeroed, goto4) request4 = reachy_sdk_zeroed.get_goto_request(goto4) assert request4.part == "mobile_base" - assert np.isclose(request4.request.goal_positions["x"], -0.2, atol=1e-03) - assert np.isclose(request4.request.goal_positions["y"], 0.3, atol=1e-03) - assert np.isclose(request4.request.goal_positions["theta"], 70, atol=1e-03) + assert np.isclose(request4.request.target["x"], -0.2, atol=1e-03) + assert np.isclose(request4.request.target["y"], 0.3, atol=1e-03) + assert np.isclose(request4.request.target["theta"], 70, atol=1e-03) assert np.isclose(request4.request.distance_tolerance, 0.02, atol=1e-03) assert np.isclose(request4.request.angle_tolerance, 2, atol=1e-03) odom = reachy_sdk_zeroed.mobile_base.get_current_odometry() @@ -314,9 +314,9 @@ def test_mobile_base_goto(reachy_sdk_zeroed: ReachySDK) -> None: goto5 = reachy_sdk_zeroed.mobile_base.goto(x=0, y=0.3, theta=np.deg2rad(60), distance_tolerance=0.02, degrees=False) request5 = reachy_sdk_zeroed.get_goto_request(goto5) assert request5.part == "mobile_base" - assert np.isclose(request5.request.goal_positions["x"], 0, atol=1e-03) - assert np.isclose(request5.request.goal_positions["y"], 0.3, atol=1e-03) - assert np.isclose(request5.request.goal_positions["theta"], 60, atol=1e-03) + assert np.isclose(request5.request.target["x"], 0, atol=1e-03) + assert np.isclose(request5.request.target["y"], 0.3, atol=1e-03) + assert np.isclose(request5.request.target["theta"], 60, atol=1e-03) assert np.isclose(request5.request.distance_tolerance, 0.02, atol=1e-03) assert np.isclose(request5.request.angle_tolerance, 5, atol=1e-03) @@ -329,9 +329,9 @@ def test_mobile_base_goto(reachy_sdk_zeroed: ReachySDK) -> None: ) request6 = reachy_sdk_zeroed.get_goto_request(goto6) assert request6.part == "mobile_base" - assert np.isclose(request6.request.goal_positions["x"], 0, atol=1e-03) - assert np.isclose(request6.request.goal_positions["y"], 0.3, atol=1e-03) - assert np.isclose(request6.request.goal_positions["theta"], 40, atol=1e-03) + assert np.isclose(request6.request.target["x"], 0, atol=1e-03) + assert np.isclose(request6.request.target["y"], 0.3, atol=1e-03) + assert np.isclose(request6.request.target["theta"], 40, atol=1e-03) assert np.isclose(request6.request.distance_tolerance, 0.02, atol=1e-03) assert np.isclose(request6.request.angle_tolerance, 2, atol=1e-03) @@ -359,9 +359,9 @@ def test_mobile_base_goto_timeout(reachy_sdk_zeroed: ReachySDK) -> None: request1 = reachy_sdk_zeroed.get_goto_request(goto1) assert request1.part == "mobile_base" - assert np.isclose(request1.request.goal_positions["x"], 0.8, atol=1e-03) - assert np.isclose(request1.request.goal_positions["y"], 0.5, atol=1e-03) - assert np.isclose(request1.request.goal_positions["theta"], 80, atol=1e-03) + assert np.isclose(request1.request.target["x"], 0.8, atol=1e-03) + assert np.isclose(request1.request.target["y"], 0.5, atol=1e-03) + assert np.isclose(request1.request.target["theta"], 80, atol=1e-03) assert np.isclose(request1.request.timeout, 1, atol=1e-03) tic = time.time() @@ -376,9 +376,9 @@ def test_mobile_base_goto_timeout(reachy_sdk_zeroed: ReachySDK) -> None: request2 = reachy_sdk_zeroed.get_goto_request(goto2) assert request1.part == "mobile_base" - assert np.isclose(request2.request.goal_positions["x"], 0.0, atol=1e-03) - assert np.isclose(request2.request.goal_positions["y"], -0.2, atol=1e-03) - assert np.isclose(request2.request.goal_positions["theta"], 0, atol=1e-03) + assert np.isclose(request2.request.target["x"], 0.0, atol=1e-03) + assert np.isclose(request2.request.target["y"], -0.2, atol=1e-03) + assert np.isclose(request2.request.target["theta"], 0, atol=1e-03) assert np.isclose(request2.request.timeout, 0.8, atol=1e-03) @@ -409,9 +409,9 @@ def test_mobile_base_goto_tolerances(reachy_sdk_zeroed: ReachySDK) -> None: request1 = reachy_sdk_zeroed.get_goto_request(goto1) assert request1.part == "mobile_base" - assert np.isclose(request1.request.goal_positions["x"], 0.9, atol=1e-03) - assert np.isclose(request1.request.goal_positions["y"], 0.9, atol=1e-03) - assert np.isclose(request1.request.goal_positions["theta"], 80, atol=1e-03) + assert np.isclose(request1.request.target["x"], 0.9, atol=1e-03) + assert np.isclose(request1.request.target["y"], 0.9, atol=1e-03) + assert np.isclose(request1.request.target["theta"], 80, atol=1e-03) assert np.isclose(request1.request.distance_tolerance, 0.2, atol=1e-03) assert np.isclose(request1.request.angle_tolerance, 5, atol=1e-03) @@ -475,9 +475,9 @@ def test_mobile_base_translate_by(reachy_sdk_zeroed: ReachySDK) -> None: assert is_goto_finished(reachy_sdk_zeroed, trans0) request0 = reachy_sdk_zeroed.get_goto_request(trans0) assert request0.part == "mobile_base" - assert np.isclose(request0.request.goal_positions["x"], odom["x"] + 0.1, atol=1e-02) - assert np.isclose(request0.request.goal_positions["y"], odom["x"] + 0.1, atol=1e-02) - assert np.isclose(request0.request.goal_positions["theta"], odom["theta"], atol=1e-02) + assert np.isclose(request0.request.target["x"], odom["x"] + 0.1, atol=1e-02) + assert np.isclose(request0.request.target["y"], odom["x"] + 0.1, atol=1e-02) + assert np.isclose(request0.request.target["theta"], odom["theta"], atol=1e-02) assert np.isclose(request0.request.distance_tolerance, 0.05, atol=1e-03) assert np.isclose(request0.request.angle_tolerance, 5, atol=1e-03) @@ -490,16 +490,16 @@ def test_mobile_base_translate_by(reachy_sdk_zeroed: ReachySDK) -> None: request2 = reachy_sdk_zeroed.get_goto_request(trans2) assert request1.part == "mobile_base" - assert np.isclose(request1.request.goal_positions["x"], 0.9, atol=1e-03) - assert np.isclose(request1.request.goal_positions["y"], -0.1, atol=1e-03) - assert np.isclose(request1.request.goal_positions["theta"], 0, atol=1e-03) + assert np.isclose(request1.request.target["x"], 0.9, atol=1e-03) + assert np.isclose(request1.request.target["y"], -0.1, atol=1e-03) + assert np.isclose(request1.request.target["theta"], 0, atol=1e-03) assert np.isclose(request1.request.distance_tolerance, 0.05, atol=1e-03) assert np.isclose(request1.request.angle_tolerance, 2, atol=1e-03) assert request2.part == "mobile_base" - assert np.isclose(request2.request.goal_positions["x"], 1.4, atol=1e-03) - assert np.isclose(request2.request.goal_positions["y"], 0.2, atol=1e-03) - assert np.isclose(request2.request.goal_positions["theta"], 0, atol=1e-03) + assert np.isclose(request2.request.target["x"], 1.4, atol=1e-03) + assert np.isclose(request2.request.target["y"], 0.2, atol=1e-03) + assert np.isclose(request2.request.target["theta"], 0, atol=1e-03) assert np.isclose(request2.request.distance_tolerance, 0.1, atol=1e-03) assert np.isclose(request2.request.angle_tolerance, 2, atol=1e-03) @@ -510,9 +510,9 @@ def test_mobile_base_translate_by(reachy_sdk_zeroed: ReachySDK) -> None: request3 = reachy_sdk_zeroed.get_goto_request(trans3) assert request3.part == "mobile_base" - assert np.isclose(request3.request.goal_positions["x"], 0.917, atol=1e-03) - assert np.isclose(request3.request.goal_positions["y"], 0.521, atol=1e-03) - assert np.isclose(request3.request.goal_positions["theta"], 50, atol=1e-03) + assert np.isclose(request3.request.target["x"], 0.917, atol=1e-03) + assert np.isclose(request3.request.target["y"], 0.521, atol=1e-03) + assert np.isclose(request3.request.target["theta"], 50, atol=1e-03) assert np.isclose(request3.request.distance_tolerance, 0.05, atol=1e-03) assert np.isclose(request3.request.angle_tolerance, 5, atol=1e-03) assert np.isclose(request3.request.timeout, 50, atol=1e-03) @@ -531,9 +531,9 @@ def test_mobile_base_rotate_by(reachy_sdk_zeroed: ReachySDK) -> None: assert is_goto_finished(reachy_sdk_zeroed, rot0) request0 = reachy_sdk_zeroed.get_goto_request(rot0) assert request0.part == "mobile_base" - assert np.isclose(request0.request.goal_positions["x"], 0, atol=1e-03) - assert np.isclose(request0.request.goal_positions["y"], 0, atol=1e-03) - assert np.isclose(request0.request.goal_positions["theta"], 35, atol=1e-03) + assert np.isclose(request0.request.target["x"], 0, atol=1e-03) + assert np.isclose(request0.request.target["y"], 0, atol=1e-03) + assert np.isclose(request0.request.target["theta"], 35, atol=1e-03) assert np.isclose(request0.request.distance_tolerance, 0.05, atol=1e-03) assert np.isclose(request0.request.angle_tolerance, 5, atol=1e-03) @@ -546,16 +546,16 @@ def test_mobile_base_rotate_by(reachy_sdk_zeroed: ReachySDK) -> None: request2 = reachy_sdk_zeroed.get_goto_request(rot2) assert request1.part == "mobile_base" - assert np.isclose(request1.request.goal_positions["x"], 0.1, atol=1e-03) - assert np.isclose(request1.request.goal_positions["y"], 0, atol=1e-03) - assert np.isclose(request1.request.goal_positions["theta"], 60, atol=1e-03) + assert np.isclose(request1.request.target["x"], 0.1, atol=1e-03) + assert np.isclose(request1.request.target["y"], 0, atol=1e-03) + assert np.isclose(request1.request.target["theta"], 60, atol=1e-03) assert np.isclose(request1.request.distance_tolerance, 0.02, atol=1e-03) assert np.isclose(request1.request.angle_tolerance, 2, atol=1e-03) assert request2.part == "mobile_base" - assert np.isclose(request2.request.goal_positions["x"], 0.1, atol=1e-03) - assert np.isclose(request2.request.goal_positions["y"], 0, atol=1e-03) - assert np.isclose(request2.request.goal_positions["theta"], 40, atol=1e-03) + assert np.isclose(request2.request.target["x"], 0.1, atol=1e-03) + assert np.isclose(request2.request.target["y"], 0, atol=1e-03) + assert np.isclose(request2.request.target["theta"], 40, atol=1e-03) assert np.isclose(request2.request.distance_tolerance, 0.02, atol=1e-03) assert np.isclose(request2.request.angle_tolerance, 3, atol=1e-03) @@ -573,9 +573,9 @@ def test_mobile_base_rotate_by(reachy_sdk_zeroed: ReachySDK) -> None: request3 = reachy_sdk_zeroed.get_goto_request(rot3) assert request3.part == "mobile_base" - assert np.isclose(request3.request.goal_positions["x"], odom["x"], atol=1e-03) - assert np.isclose(request3.request.goal_positions["y"], odom["y"], atol=1e-03) - assert np.isclose(request3.request.goal_positions["theta"], odom["theta"] - 40, atol=1e-03) + assert np.isclose(request3.request.target["x"], odom["x"], atol=1e-03) + assert np.isclose(request3.request.target["y"], odom["y"], atol=1e-03) + assert np.isclose(request3.request.target["theta"], odom["theta"] - 40, atol=1e-03) assert np.isclose(request3.request.distance_tolerance, 0.05, atol=1e-03) assert np.isclose(request3.request.angle_tolerance, 5, atol=1e-03) assert np.isclose(request3.request.timeout, 50, atol=1e-03) @@ -597,9 +597,9 @@ def test_set_max_xy_goto(reachy_sdk_zeroed: ReachySDK) -> None: time.sleep(0.5) assert request1.part == "mobile_base" - assert np.isclose(request1.request.goal_positions["x"], 1.5, atol=1e-03) - assert np.isclose(request1.request.goal_positions["y"], 0.2, atol=1e-03) - assert np.isclose(request1.request.goal_positions["theta"], 10, atol=1e-03) + assert np.isclose(request1.request.target["x"], 1.5, atol=1e-03) + assert np.isclose(request1.request.target["y"], 0.2, atol=1e-03) + assert np.isclose(request1.request.target["theta"], 10, atol=1e-03) assert np.isclose(request1.request.distance_tolerance, 0.05, atol=1e-03) assert np.isclose(request1.request.angle_tolerance, 5, atol=1e-03)