reachy2_sdk.dynamixel.dynamixel_motor

  1"""Reachy DynamixelMotor module.
  2
  3Handles all specific methods to DynamixelMotor.
  4"""
  5
  6import logging
  7from typing import Optional
  8
  9from google.protobuf.wrappers_pb2 import BoolValue, FloatValue
 10from grpc import Channel
 11from reachy2_sdk_api.component_pb2 import ComponentId
 12from reachy2_sdk_api.dynamixel_motor_pb2 import (
 13    DynamixelMotorCommand,
 14    DynamixelMotorsCommand,
 15    DynamixelMotorState,
 16)
 17from reachy2_sdk_api.dynamixel_motor_pb2_grpc import DynamixelMotorServiceStub
 18
 19from ..orbita.utils import to_internal_position, to_position
 20
 21
 22class DynamixelMotor:
 23    """The DynamixelMotor class represents any Dynamixel motor.
 24
 25    The DynamixelMotor class is used to store the up-to-date state of the motor, especially:
 26    - its present_position (RO)
 27    - its goal_position (RW)
 28    """
 29
 30    def __init__(
 31        self,
 32        uid: int,
 33        name: str,
 34        initial_state: DynamixelMotorState,
 35        grpc_channel: Channel,
 36    ):
 37        """Initialize the DynamixelMotor with its initial state and configuration.
 38
 39        This sets up the motor by assigning its state based on the provided initial values.
 40
 41        Args:
 42            uid: The unique identifier of the component.
 43            name: The name of the component.
 44            initial_state: A dictionary containing the initial state of the joint, with
 45                each entry representing a specific parameter of the joint (e.g., present position).
 46            grpc_channel: The gRPC channel used to communicate with the DynamixelMotor service.
 47        """
 48        self._logger = logging.getLogger(__name__)
 49        self._name = name
 50        self._id = uid
 51        self._stub = DynamixelMotorServiceStub(grpc_channel)
 52        self._update_with(initial_state)
 53        self._outgoing_goal_position: Optional[float] = None
 54
 55    def __repr__(self) -> str:
 56        """Clean representation of the DynamixelMotor."""
 57        repr_template = "<DynamixelMotor on={dxl_on} present_position={present_position} goal_position={goal_position} >"
 58        return repr_template.format(
 59            dxl_on=self.is_on(),
 60            present_position=round(self.present_position, 2),
 61            goal_position=round(self.goal_position, 2),
 62        )
 63
 64    def turn_on(self) -> None:
 65        """Turn on the motor."""
 66        self._set_compliant(False)
 67
 68    def turn_off(self) -> None:
 69        """Turn off the motor."""
 70        self._set_compliant(True)
 71
 72    def is_on(self) -> bool:
 73        """Check if the dynamixel motor is currently stiff.
 74
 75        Returns:
 76            `True` if the motor is stiff (not compliant), `False` otherwise.
 77        """
 78        return not self._compliant
 79
 80    @property
 81    def present_position(self) -> float:
 82        """Get the present position of the joint in degrees."""
 83        return to_position(self._present_position)
 84
 85    @property
 86    def goal_position(self) -> float:
 87        """Get the goal position of the joint in degrees."""
 88        return to_position(self._goal_position)
 89
 90    @goal_position.setter
 91    def goal_position(self, value: float | int) -> None:
 92        """Set the goal position of the joint in degrees.
 93
 94        The goal position is not send to the joint immediately, it is stored locally until the `send_goal_positions` method
 95        is called.
 96
 97        Args:
 98            value: The goal position to set, specified as a float or int.
 99
100        Raises:
101            TypeError: If the provided value is not a float or int.
102        """
103        if isinstance(value, float) or isinstance(value, int):
104            self._outgoing_goal_position = to_internal_position(value)
105        else:
106            raise TypeError("goal_position must be a float or int")
107
108    def _set_compliant(self, compliant: bool) -> None:
109        """Set the compliance mode of the motor.
110
111        Compliance mode determines whether the motor is stiff or compliant.
112
113        Args:
114            compliant: A boolean value indicating whether to set the motor to
115                compliant (`True`) or stiff (`False`).
116        """
117        command = DynamixelMotorsCommand(
118            cmd=[
119                DynamixelMotorCommand(
120                    id=ComponentId(id=self._id),
121                    compliant=BoolValue(value=compliant),
122                )
123            ]
124        )
125        self._stub.SendCommand(command)
126
127    def _get_goal_positions_message(self, check_positions: bool = True) -> Optional[DynamixelMotorsCommand]:
128        """Get the DynamixelMotorsCommand message to send the goal positions to the actuator."""
129        if self._outgoing_goal_position is not None:
130            if not self.is_on():
131                self._logger.warning(f"{self._name} is off. Command not sent.")
132                return None
133            command = DynamixelMotorsCommand(
134                cmd=[
135                    DynamixelMotorCommand(
136                        id=ComponentId(id=self._id),
137                        goal_position=FloatValue(value=self._outgoing_goal_position),
138                    )
139                ]
140            )
141            return command
142        return None
143
144    def _clean_outgoing_goal_positions(self) -> None:
145        """Clean the outgoing goal positions."""
146        self._outgoing_goal_position = None
147
148    def send_goal_positions(self, check_positions: bool = False) -> None:
149        """Send goal positions to the motor.
150
151        If goal positions have been specified, sends them to the motor.
152        Args :
153            check_positions: A boolean indicating whether to check the positions after sending the command.
154                Defaults to True.
155        """
156        command = self._get_goal_positions_message()
157        if command is not None:
158            self._clean_outgoing_goal_positions()
159            self._stub.SendCommand(command)
160            if check_positions:
161                pass
162
163    def set_speed_limits(self, speed_limit: float | int) -> None:
164        """Set the speed limit as a percentage of the maximum speed the motor.
165
166        Args:
167            speed_limit: The desired speed limit as a percentage (0-100) of the maximum speed. Can be
168                specified as a float or int.
169        """
170        if not isinstance(speed_limit, float | int):
171            raise TypeError(f"Expected one of: float, int for speed_limit, got {type(speed_limit).__name__}")
172        if not (0 <= speed_limit <= 100):
173            raise ValueError(f"speed_limit must be in [0, 100], got {speed_limit}.")
174        speed_limit = speed_limit / 100.0
175        command = DynamixelMotorsCommand(
176            cmd=[
177                DynamixelMotorCommand(
178                    id=ComponentId(id=self._id),
179                    speed_limit=FloatValue(value=speed_limit),
180                )
181            ]
182        )
183        self._stub.SendCommand(command)
184
185    def _update_with(self, new_state: DynamixelMotorState) -> None:
186        """Update the present and goal positions of the joint with new state values.
187
188        Args:
189            new_state: A dictionary containing the new state values for the joint. The keys should include
190                "present_position" and "goal_position", with corresponding FloatValue objects as values.
191        """
192        self._present_position = new_state.present_position.value
193        self._goal_position = new_state.goal_position.value
194        self._compliant = new_state.compliant.value