reachy2_sdk.components.antenna

  1"""Reachy Antenna module.
  2
  3Handles all specific methods to Antennas.
  4"""
  5
  6import logging
  7import time
  8from threading import Thread
  9from typing import Any, Dict, List, Optional
 10
 11import numpy as np
 12from google.protobuf.wrappers_pb2 import FloatValue
 13from grpc import Channel
 14from reachy2_sdk_api.component_pb2 import ComponentId
 15from reachy2_sdk_api.dynamixel_motor_pb2 import DynamixelMotor as DynamixelMotor_proto
 16from reachy2_sdk_api.dynamixel_motor_pb2 import (
 17    DynamixelMotorsCommand,
 18    DynamixelMotorState,
 19    DynamixelMotorStatus,
 20)
 21from reachy2_sdk_api.goto_pb2 import GoToId, GoToRequest, JointsGoal
 22from reachy2_sdk_api.goto_pb2_grpc import GoToServiceStub
 23from reachy2_sdk_api.head_pb2 import AntennaJointGoal
 24
 25from ..dynamixel.dynamixel_motor import DynamixelMotor
 26from ..parts.part import Part
 27from ..utils.utils import get_grpc_interpolation_mode
 28from .goto_based_component import IGoToBasedComponent
 29
 30
 31class Antenna(IGoToBasedComponent):
 32    """The Antenna class represents any antenna of the robot's head."""
 33
 34    def __init__(
 35        self,
 36        uid: int,
 37        name: str,
 38        initial_state: DynamixelMotorState,
 39        grpc_channel: Channel,
 40        goto_stub: GoToServiceStub,
 41        part: Part,
 42    ):
 43        """Initialize the Antenna with its initial state and configuration.
 44
 45        Args:
 46            uid: The unique identifier of the component.
 47            name: The name of the joint.
 48            initial_state: A dictionary containing the initial state of the joint, with
 49                each entry representing a specific parameter of the joint (e.g., present position).
 50            grpc_channel: The gRPC channel used to communicate with the DynamixelMotor service.
 51            goto_stub: The gRPC stub for controlling goto movements.
 52            part: The part to which this joint belongs.
 53        """
 54        self._logger = logging.getLogger(__name__)
 55        IGoToBasedComponent.__init__(self, ComponentId(id=uid, name=name), goto_stub)
 56        self._part = part
 57        self._error_status: Optional[str] = None
 58        self._joints: Dict[str, DynamixelMotor] = {}
 59        if name == "antenna_left":
 60            self._name = "l_antenna"
 61        else:
 62            self._name = "r_antenna"
 63        self._joints[self._name] = DynamixelMotor(uid, name, initial_state, grpc_channel)
 64
 65        self._thread_check_position: Optional[Thread] = None
 66        self._cancel_check = False
 67
 68    def _check_goto_parameters(self, target: Any, duration: Optional[float], q0: Optional[List[float]] = None) -> None:
 69        """Check the validity of the parameters for the `goto` method.
 70
 71        Args:
 72            duration: The time in seconds for the movement to be completed.
 73            target: The target position, either a float or int.
 74
 75        Raises:
 76            TypeError: If the target is not a list or a quaternion.
 77            ValueError: If the target list has a length other than 3.
 78            ValueError: If the duration is set to 0.
 79        """
 80        if not (isinstance(target, float) or isinstance(target, int)):
 81            raise TypeError(f"Antenna's goto target must be either a float or int, got {type(target)}.")
 82
 83        elif duration == 0:
 84            raise ValueError("duration cannot be set to 0.")
 85        elif duration is not None and duration < 0:
 86            raise ValueError("duration cannot be negative.")
 87
 88    def goto_posture(
 89        self,
 90        common_posture: str = "default",
 91        duration: float = 2,
 92        wait: bool = False,
 93        wait_for_goto_end: bool = True,
 94        interpolation_mode: str = "minimum_jerk",
 95    ) -> GoToId:
 96        """Send the antenna to standard positions within the specified duration.
 97
 98        The default posture sets the antenna is 0.0.
 99
100        Args:
101            common_posture: The standard positions to which all joints will be sent.
102                It can be 'default' or 'elbow_90'. Defaults to 'default'.
103            duration: The time duration in seconds for the robot to move to the specified posture.
104                Defaults to 2.
105            wait: Determines whether the program should wait for the movement to finish before
106                returning. If set to `True`, the program waits for the movement to complete before continuing
107                execution. Defaults to `False`.
108            wait_for_goto_end: Specifies whether commands will be sent to a part immediately or
109                only after all previous commands in the queue have been executed. If set to `False`, the program
110                will cancel all executing moves and queues. Defaults to `True`.
111            interpolation_mode: The type of interpolation used when moving the arm's joints.
112                Can be 'minimum_jerk' or 'linear'. Defaults to 'minimum_jerk'.
113
114        Returns:
115            The unique GoToId associated with the movement command.
116        """
117        if not wait_for_goto_end:
118            self.cancel_all_goto()
119        if self.is_on():
120            return self.goto(0, duration, wait, interpolation_mode)
121        else:
122            self._logger.warning(f"{self._name} is off. No command sent.")
123        return GoToId(id=-1)
124
125    def goto(
126        self,
127        target: float,
128        duration: float = 2.0,
129        wait: bool = False,
130        interpolation_mode: str = "minimum_jerk",
131        degrees: bool = True,
132    ) -> GoToId:
133        """Send the antenna to a specified goal position.
134
135        Args:
136            target: The desired goal position for the antenna.
137            duration: The time in seconds for the movement to be completed. Defaults to 2.
138            wait: If True, the function waits until the movement is completed before returning.
139                    Defaults to False.
140            interpolation_mode: The interpolation method to be used. It can be either "minimum_jerk"
141                    or "linear". Defaults to "minimum_jerk".
142            degrees: If True, the joint value in the `target` argument is treated as degrees.
143                    Defaults to True.
144
145        Raises:
146            TypeError : If the input type for `target` is invalid
147            ValueError: If the `duration` is set to 0.
148
149        Returns:
150            GoToId: The unique identifier for the movement command.
151        """
152        if not self.is_on():
153            self._logger.warning(f"head.{self._name} is off. No command sent.")
154            return GoToId(id=-1)
155
156        self._check_goto_parameters(target, duration)
157
158        if degrees:
159            target = np.deg2rad(target)
160
161        request = GoToRequest(
162            joints_goal=JointsGoal(
163                antenna_joint_goal=AntennaJointGoal(
164                    id=self._part._part_id,
165                    antenna=DynamixelMotor_proto(
166                        id=ComponentId(id=self._joints[self._name]._id, name=self._joints[self._name]._name),
167                    ),
168                    joint_goal=FloatValue(value=target),
169                    duration=FloatValue(value=duration),
170                )
171            ),
172            interpolation_mode=get_grpc_interpolation_mode(interpolation_mode),
173        )
174
175        response = self._goto_stub.GoToJoints(request)
176
177        if response.id == -1:
178            self._logger.error(f"Position {target} was not reachable. No command sent.")
179        elif wait:
180            self._wait_goto(response, duration)
181        return response
182
183    def __repr__(self) -> str:
184        """Clean representation of the Antenna only joint (DynamixelMotor)."""
185        s = "\n\t".join([str(joint) for joint in self._joints.values()])
186        return f"""<Antenna on={self.is_on()} joints=\n\t{
187            s
188        }\n>"""
189
190    def turn_on(self) -> None:
191        """Turn on the antenna's motor."""
192        self._joints[self._name].turn_on()
193
194    def turn_off(self) -> None:
195        """Turn off the antenna's motor."""
196        self._joints[self._name].turn_off()
197
198    def is_on(self) -> bool:
199        """Check if the antenna is currently stiff.
200
201        Returns:
202            `True` if the antenna's motor is stiff (not compliant), `False` otherwise.
203        """
204        return bool(self._joints[self._name].is_on())
205
206    def is_off(self) -> bool:
207        """Check if the antenna is currently stiff.
208
209        Returns:
210            `True` if the antenna's motor is stiff (not compliant), `False` otherwise.
211        """
212        return not bool(self._joints[self._name].is_on())
213
214    @property
215    def present_position(self) -> float:
216        """Get the present position of the joint in degrees."""
217        return float(self._joints[self._name].present_position)
218
219    @property
220    def goal_position(self) -> float:
221        """Get the goal position of the joint in degrees."""
222        return float(self._joints[self._name].goal_position)
223
224    @goal_position.setter
225    def goal_position(self, value: float | int) -> None:
226        """Set the goal position of the joint in degrees.
227
228        The goal position is not send to the joint immediately, it is stored locally until the `send_goal_positions` method
229        is called.
230
231        Args:
232            value: The goal position to set, specified as a float or int.
233
234        Raises:
235            TypeError: If the provided value is not a float or int.
236        """
237        self._joints[self._name].goal_position = value
238
239    def _get_goal_positions_message(self) -> Optional[DynamixelMotorsCommand]:
240        """Get the Orbita2dsCommand message to send the goal positions to the actuator."""
241        return self._joints[self._name]._get_goal_positions_message()
242
243    def _clean_outgoing_goal_positions(self) -> None:
244        """Clean the outgoing goal positions."""
245        self._joints[self._name]._clean_outgoing_goal_positions()
246
247    def _post_send_goal_positions(self) -> None:
248        """Start a background thread to check the goal positions after sending them.
249
250        This method stops any ongoing position check thread and starts a new thread
251        to monitor the current positions of the joints relative to their last goal positions.
252        """
253        self._cancel_check = True
254        if self._thread_check_position is not None and self._thread_check_position.is_alive():
255            self._thread_check_position.join()
256        self._thread_check_position = Thread(target=self._check_goal_positions, daemon=True)
257        self._thread_check_position.start()
258
259    def _check_goal_positions(self) -> None:
260        """Monitor the joint positions to check if they reach the specified goals.
261
262        This method checks the current positions of the joints and compares them to
263        the goal positions. If a position is significantly different from the goal after 1 second,
264        a warning is logged indicating that the position may be unreachable.
265        """
266        self._cancel_check = False
267        t1 = time.time()
268        while time.time() - t1 < 1:
269            time.sleep(0.0001)
270            if self._cancel_check:
271                # in case of multiple send_goal_positions we'll check the next call
272                return
273
274        # precision is low we are looking for unreachable positions
275        if not np.isclose(self._joints[self._name].present_position, self._joints[self._name].goal_position, atol=1):
276            self._logger.warning(
277                f"Required goal position ({round(self._joints[self._name].goal_position, 2)}) for {self._name} is unreachable."
278                f"\nCurrent position is ({round(self._joints[self._name].present_position, 2)})."
279            )
280
281    def send_goal_positions(self, check_positions: bool = False) -> None:
282        """Send goal positions to the motor.
283
284        If goal positions have been specified, sends them to the motor.
285        Args :
286            check_positions: A boolean indicating whether to check the positions after sending the command.
287                Defaults to True.
288        """
289        if self._joints[self._name]._outgoing_goal_position is not None:
290            if self.is_off():
291                self._logger.warning(f"{self._name} is off. Command not sent.")
292                return
293            self._joints[self._name].send_goal_positions(check_positions)
294
295    def set_speed_limits(self, speed_limit: float | int) -> None:
296        """Set the speed limit as a percentage of the maximum speed the motor.
297
298        Args:
299            speed_limit: The desired speed limit as a percentage (0-100) of the maximum speed. Can be
300                specified as a float or int.
301        """
302        self._joints[self._name].set_speed_limits(speed_limit)
303
304    def _update_with(self, new_state: DynamixelMotorState) -> None:
305        """Update the present and goal positions of the joint with new state values.
306
307        Args:
308            new_state: A dictionary containing the new state values for the joint. The keys should include
309                "present_position" and "goal_position", with corresponding FloatValue objects as values.
310        """
311        self._joints[self._name]._update_with(new_state)
312
313    @property
314    def status(self) -> Optional[str]:
315        """Get the current audit status of the actuator.
316
317        Returns:
318            The audit status as a string, representing the latest error or status
319            message, or `None` if there is no error.
320        """
321        pass
322
323    def _update_audit_status(self, new_status: DynamixelMotorStatus) -> None:
324        """Update the audit status based on the new status data.
325
326        Args:
327            new_status: The new status data, as a DynamixelMotorStatus object, containing error details.
328        """
329        pass