reachy2_sdk.orbita.orbita2d

  1"""Reachy Orbita2d module.
  2
  3Handles all specific methods to Orbita2d.
  4"""
  5
  6from typing import Any, Dict, List, Optional
  7
  8from google.protobuf.wrappers_pb2 import FloatValue
  9from grpc import Channel
 10from reachy2_sdk_api.component_pb2 import ComponentId
 11from reachy2_sdk_api.orbita2d_pb2 import (
 12    Axis,
 13    Float2d,
 14    Orbita2dCommand,
 15    Orbita2dsCommand,
 16    Orbita2dState,
 17    PID2d,
 18    Pose2d,
 19    Vector2d,
 20)
 21from reachy2_sdk_api.orbita2d_pb2_grpc import Orbita2dServiceStub
 22
 23from ..parts.part import Part
 24from .orbita import Orbita
 25from .orbita_axis import OrbitaAxis
 26from .orbita_joint import OrbitaJoint
 27from .orbita_motor import OrbitaMotor
 28
 29
 30class Orbita2d(Orbita):
 31    """The Orbita2d class represents any Orbita2d actuator and its joints, motors and axis.
 32
 33    The Orbita2d class is used to store the up-to-date state of the actuator, especially:
 34    - its compliancy
 35    - its joints state
 36    - its motors state
 37    - its axis state
 38
 39    You can access properties of the motors from the actuators with function that act on all the actuator's motors:
 40    - speed limit (in percentage, for all motors of the actuator)
 41    - torque limit (in percentage, for all motors of the actuator)
 42    - pid (for all motors of the actuator)
 43    - compliancy (for all motors of the actuator)
 44
 45    Lower properties that are read-only but acessible at actuator level:
 46    - temperatures (temperatures of all motors of the actuator)
 47    """
 48
 49    def __init__(
 50        self,
 51        uid: int,
 52        name: str,
 53        axis1: Axis,
 54        axis2: Axis,
 55        initial_state: Orbita2dState,
 56        grpc_channel: Channel,
 57        part: Part,
 58        joints_position_order: List[int],
 59    ):
 60        """Initialize the Orbita2d actuator with its joints, motors, and axes.
 61
 62        Args:
 63            uid: The unique identifier for the actuator.
 64            name: The name of the actuator.
 65            axis1: The first axis of the actuator, typically representing roll, pitch, or yaw.
 66            axis2: The second axis of the actuator, typically representing roll, pitch, or yaw.
 67            initial_state: The initial state of the Orbita2d actuator, containing the states
 68                of the joints, motors, and axes.
 69            grpc_channel: The gRPC communication channel used for interfacing with the
 70                Orbita2d actuator.
 71            part: The robot part that this actuator belongs to.
 72            joints_position_order: A list defining the order of the joint positions in the
 73                containing part, used to map the actuator's joint positions correctly.
 74        """
 75        super().__init__(uid, name, "2d", Orbita2dServiceStub(grpc_channel), part)
 76
 77        axis1_name = Axis.DESCRIPTOR.values_by_number[axis1].name.lower()
 78        axis2_name = Axis.DESCRIPTOR.values_by_number[axis2].name.lower()
 79
 80        init_state: Dict[str, Dict[str, FloatValue]] = self._create_dict_state(initial_state)
 81
 82        setattr(
 83            self,
 84            axis1_name,
 85            OrbitaJoint(
 86                initial_state=init_state["axis_1"],
 87                axis_type=axis1_name,
 88                actuator=self,
 89                position_order_in_part=joints_position_order[0],
 90            ),
 91        )
 92        setattr(
 93            self,
 94            axis2_name,
 95            OrbitaJoint(
 96                initial_state=init_state["axis_2"],
 97                axis_type=axis2_name,
 98                actuator=self,
 99                position_order_in_part=joints_position_order[1],
100            ),
101        )
102        self._joints = {
103            "axis_1": getattr(self, axis1_name),
104            "axis_2": getattr(self, axis2_name),
105        }
106        self._axis_name_by_joint = {v: k for k, v in self._joints.items()}
107
108        self.__motor_1 = OrbitaMotor(initial_state=init_state["motor_1"], actuator=self)
109        self.__motor_2 = OrbitaMotor(initial_state=init_state["motor_2"], actuator=self)
110        self._motors = {"motor_1": self.__motor_1, "motor_2": self.__motor_2}
111
112        self.__x = OrbitaAxis(initial_state=init_state["x"])
113        self.__y = OrbitaAxis(initial_state=init_state["y"])
114        self._axis = {"x": self.__x, "y": self.__y}
115
116    def _create_dict_state(self, initial_state: Orbita2dState) -> Dict[str, Dict[str, FloatValue]]:  # noqa: C901
117        """Create a dictionary representation of the state for the actuator.
118
119        The method processes the fields in the given Orbita2dState and converts them into a nested dictionary
120        structure, where the top-level keys are the axis, motor and joints names, and the inner dictionaries contain
121        field names and corresponding FloatValue objects.
122
123        Args:
124            initial_state: An Orbita2dState object representing the initial state of the actuator.
125
126        Returns:
127            A dictionary where the keys represent the axis, motors and joints, and the values are dictionaries
128            containing field names and corresponding FloatValue objects.
129
130        Raises:
131            ValueError: If the field type is not recognized or supported.
132        """
133        init_state: Dict[str, Dict[str, FloatValue]] = {}
134
135        for field, value in initial_state.ListFields():
136            if field.name == "compliant":
137                self._compliant = value.value
138                init_state["motor_1"][field.name] = value
139                init_state["motor_2"][field.name] = value
140            else:
141                if isinstance(value, Pose2d):
142                    for axis, val in value.ListFields():
143                        if axis.name not in init_state:
144                            init_state[axis.name] = {}
145                        init_state[axis.name][field.name] = val
146                if isinstance(value, Float2d | PID2d):
147                    for motor, val in value.ListFields():
148                        if motor.name not in init_state:
149                            init_state[motor.name] = {}
150                        init_state[motor.name][field.name] = val
151                if isinstance(value, Vector2d):
152                    for axis, val in value.ListFields():
153                        if axis.name not in init_state:
154                            init_state[axis.name] = {}
155                        init_state[axis.name][field.name] = val
156        return init_state
157
158    def __setattr__(self, __name: str, __value: Any) -> None:
159        """Custom attribute setting to prevent modification of specific attributes.
160
161        This method overrides the default behavior of setting attributes to ensure that
162        certain attributes ('roll', 'pitch', 'yaw') cannot be modified after being set initially.
163        If an attempt is made to set these attributes again, an AttributeError is raised.
164
165        Args:
166            __name: The name of the attribute.
167            __value: The value to assign to the attribute.
168
169        Raises:
170            AttributeError: If trying to set the value of 'roll', 'pitch', or 'yaw' after they are already set.
171        """
172        if __name in ["roll", "pitch", "yaw"]:
173            if hasattr(self, __name):
174                raise AttributeError(f"can't set attribute '{__name}'")
175        super().__setattr__(__name, __value)
176
177    def send_goal_positions(self, check_positions: bool = False) -> None:
178        """Send goal positions to the actuator's joints.
179
180        If goal positions have been specified for any joint of this actuator, sends them to the actuator.
181
182        Args :
183            check_positions: A boolean indicating whether to check the positions after sending the command.
184                Defaults to True.
185        """
186        command = self._get_goal_positions_message()
187        if command is not None:
188            self._clean_outgoing_goal_positions()
189            self._stub.SendCommand(command)
190            if check_positions:
191                self._post_send_goal_positions()
192
193    def _get_goal_positions_message(self) -> Optional[Orbita2dsCommand]:
194        """Get the Orbita2dsCommand message to send the goal positions to the actuator."""
195        if self._outgoing_goal_positions:
196            if self.is_off():
197                self._logger.warning(f"{self._name} is off. Command not sent.")
198                return None
199
200            req_pos = {}
201            for joint_axis in self._joints.keys():
202                if joint_axis in self._outgoing_goal_positions:
203                    req_pos[joint_axis] = FloatValue(value=self._outgoing_goal_positions[joint_axis])
204            pose = Pose2d(**req_pos)
205
206            command = Orbita2dsCommand(
207                cmd=[
208                    Orbita2dCommand(
209                        id=ComponentId(id=self._id),
210                        goal_position=pose,
211                    )
212                ]
213            )
214            return command
215        return None
216
217    def _clean_outgoing_goal_positions(self) -> None:
218        """Clean the outgoing goal positions."""
219        self._outgoing_goal_positions = {}
220
221    def set_speed_limits(self, speed_limit: float | int) -> None:
222        """Set the speed limit as a percentage of the maximum speed for all motors of the actuator.
223
224        Args:
225            speed_limit: The desired speed limit as a percentage (0-100) of the maximum speed. Can be
226                specified as a float or int.
227        """
228        super().set_speed_limits(speed_limit)
229        speed_limit = speed_limit / 100.0
230        command = Orbita2dsCommand(
231            cmd=[
232                Orbita2dCommand(
233                    id=ComponentId(id=self._id),
234                    speed_limit=Float2d(
235                        motor_1=FloatValue(value=speed_limit),
236                        motor_2=FloatValue(value=speed_limit),
237                    ),
238                )
239            ]
240        )
241        self._stub.SendCommand(command)
242
243    def set_torque_limits(self, torque_limit: float | int) -> None:
244        """Set the torque limit as a percentage of the maximum torque for all motors of the actuator.
245
246        Args:
247            torque_limit: The desired torque limit as a percentage (0-100) of the maximum torque. Can be
248                specified as a float or int.
249        """
250        super().set_torque_limits(torque_limit)
251        torque_limit = torque_limit / 100.0
252        command = Orbita2dsCommand(
253            cmd=[
254                Orbita2dCommand(
255                    id=ComponentId(id=self._id),
256                    torque_limit=Float2d(
257                        motor_1=FloatValue(value=torque_limit),
258                        motor_2=FloatValue(value=torque_limit),
259                    ),
260                )
261            ]
262        )
263        self._stub.SendCommand(command)