reachy2_sdk.orbita.orbita3d

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