reachy2_sdk.parts.mobile_base

  1"""Reachy MobileBase module.
  2
  3Handles all specific methods to a MobileBase.
  4"""
  5
  6import logging
  7import time
  8from typing import Any, Dict, List, Optional
  9
 10import grpc
 11import numpy as np
 12from google.protobuf.wrappers_pb2 import FloatValue
 13from numpy import deg2rad, rad2deg, round
 14from reachy2_sdk_api.goto_pb2 import GoToId, GoToRequest, OdometryGoal
 15from reachy2_sdk_api.goto_pb2_grpc import GoToServiceStub
 16from reachy2_sdk_api.mobile_base_mobility_pb2 import (
 17    DirectionVector,
 18    TargetDirectionCommand,
 19)
 20from reachy2_sdk_api.mobile_base_mobility_pb2_grpc import MobileBaseMobilityServiceStub
 21from reachy2_sdk_api.mobile_base_utility_pb2 import (
 22    ControlModeCommand,
 23    ControlModePossiblities,
 24)
 25from reachy2_sdk_api.mobile_base_utility_pb2 import MobileBase as MobileBase_proto
 26from reachy2_sdk_api.mobile_base_utility_pb2 import (
 27    MobileBaseState,
 28    MobileBaseStatus,
 29    ZuuuModeCommand,
 30    ZuuuModePossiblities,
 31)
 32from reachy2_sdk_api.mobile_base_utility_pb2_grpc import MobileBaseUtilityServiceStub
 33
 34from ..sensors.lidar import Lidar
 35from .goto_based_part import IGoToBasedPart
 36from .part import Part
 37
 38
 39class MobileBase(Part, IGoToBasedPart):
 40    """MobileBase class for controlling Reachy's mobile base.
 41
 42    This class provides methods to interact with and control the mobile base of a Reachy robot. It allows
 43    users to access essential information such as battery voltage and odometry, as well as send commands
 44    to move the base to specified positions or velocities. The class supports different drive modes and
 45    control modes, and provides methods for resetting the base's odometry.
 46
 47    Attributes:
 48        lidar: Lidar object for handling safety features.
 49    """
 50
 51    def __init__(
 52        self,
 53        mb_msg: MobileBase_proto,
 54        initial_state: MobileBaseState,
 55        grpc_channel: grpc.Channel,
 56        goto_stub: GoToServiceStub,
 57    ) -> None:
 58        """Initialize the MobileBase with its gRPC communication and configuration.
 59
 60        This sets up the gRPC communication channel and service stubs for controlling the
 61        mobile base, initializes the drive and control modes.
 62        It also sets up the LIDAR safety monitoring.
 63
 64        Args:
 65            mb_msg: A MobileBase_proto message containing the configuration details for the mobile base.
 66            initial_state: The initial state of the mobile base, as a MobileBaseState object.
 67            grpc_channel: The gRPC channel used to communicate with the mobile base service.
 68            goto_stub: The gRPC service stub for the GoTo service.
 69        """
 70        self._logger = logging.getLogger(__name__)
 71        super().__init__(mb_msg, grpc_channel, MobileBaseUtilityServiceStub(grpc_channel))
 72        IGoToBasedPart.__init__(self, self._part_id, goto_stub)
 73
 74        self._mobility_stub = MobileBaseMobilityServiceStub(grpc_channel)
 75
 76        self._drive_mode: str = ZuuuModePossiblities.keys()[initial_state.zuuu_mode.mode].lower()
 77        self._control_mode: str = ControlModePossiblities.keys()[initial_state.control_mode.mode].lower()
 78        self._battery_level = 30.0
 79
 80        self._max_xy_vel = 0.61
 81        self._max_rot_vel = 114.0
 82        self._max_xy_goto = 1.0
 83
 84        self.lidar = Lidar(initial_state.lidar_safety, grpc_channel, self)
 85
 86        self._update_with(initial_state)
 87
 88    def __repr__(self) -> str:
 89        """Clean representation of a mobile base."""
 90        repr_template = (
 91            "<MobileBase on={on} \n" " lidar_safety_enabled={lidar_safety_enabled} \n" " battery_voltage={battery_voltage}>"
 92        )
 93        return repr_template.format(
 94            on=self.is_on(),
 95            lidar_safety_enabled=self.lidar.safety_enabled,
 96            battery_voltage=self.battery_voltage,
 97        )
 98
 99    @property
100    def battery_voltage(self) -> float:
101        """Return the battery voltage.
102
103        The battery should be recharged if the voltage reaches 24.5V or below. If the battery level is low,
104        a warning message is logged.
105
106        Returns:
107            The current battery voltage as a float, rounded to one decimal place.
108        """
109        battery_level = float(round(self._battery_level, 1))
110        if battery_level < 24.5:
111            self._logger.warning(f"Low battery level: {battery_level}V. Consider recharging.")
112        return float(round(self._battery_level, 1))
113
114    @property
115    def odometry(self) -> Dict[str, float]:
116        """Return the odometry of the base.
117
118        The odometry includes the x and y positions in meters and theta in degrees, along with the
119        velocities in the x, y directions in meters per degrees and the angular velocity in degrees per second.
120
121        Returns:
122            A dictionary containing the current odometry with keys 'x', 'y', 'theta', 'vx', 'vy', and 'vtheta',
123            each rounded to three decimal places.
124        """
125        response = self._stub.GetOdometry(self._part_id)
126        odom = {
127            "x": response.x.value,
128            "y": response.y.value,
129            "theta": rad2deg(response.theta.value),
130            "vx": response.vx.value,
131            "vy": response.vy.value,
132            "vtheta": rad2deg(response.vtheta.value),
133        }
134        return odom
135
136    @property
137    def last_cmd_vel(self) -> Dict[str, float]:
138        """Return the last command velocity sent to the base.
139
140        The velocity includes the x and y components in meters per second and the theta component in degrees per second.
141
142        Returns:
143            A dictionary containing the last command velocity with keys 'x', 'y', and 'theta',
144            each rounded to three decimal places.
145        """
146        response = self._mobility_stub.GetLastDirection(self._part_id)
147        cmd_vel = {
148            "x": round(response.x.value, 3),
149            "y": round(response.y.value, 3),
150            "theta": round(rad2deg(response.theta.value), 3),
151        }
152        return cmd_vel
153
154    def _set_drive_mode(self, mode: str) -> None:
155        """Set the base's drive mode.
156
157        The drive mode must be one of the allowed modes, excluding 'speed' and 'goto'. If the mode is
158        valid, the base's drive mode is set accordingly.
159
160        Args:
161            mode: The desired drive mode as a string. Possible drive modes are:
162                ['cmd_vel', 'brake', 'free_wheel', 'emergency_stop', 'cmd_goto'].
163
164        Raises:
165            ValueError: If the specified drive mode is not one of the allowed modes.
166        """
167        all_drive_modes = [mode.lower() for mode in ZuuuModePossiblities.keys()][1:]
168        possible_drive_modes = [mode for mode in all_drive_modes if mode not in ("speed", "goto")]
169        if mode in possible_drive_modes:
170            req = ZuuuModeCommand(mode=getattr(ZuuuModePossiblities, mode.upper()))
171            self._stub.SetZuuuMode(req)
172            self._drive_mode = mode
173        else:
174            raise ValueError(f"Drive mode requested should be in {possible_drive_modes}!")
175
176    def _set_control_mode(self, mode: str) -> None:
177        """Set the base's control mode.
178
179        The control mode must be one of the allowed modes. If the mode is valid, the base's control mode is set accordingly.
180
181        Args:
182            mode: The desired control mode as a string. Possible control modes are: ['open_loop', 'pid']
183
184        Raises:
185            ValueError: If the specified control mode is not one of the allowed modes.
186        """
187        possible_control_modes = [mode.lower() for mode in ControlModePossiblities.keys()][1:]
188        if mode in possible_control_modes:
189            req = ControlModeCommand(mode=getattr(ControlModePossiblities, mode.upper()))
190            self._stub.SetControlMode(req)
191            self._control_mode = mode
192        else:
193            raise ValueError(f"Control mode requested should be in {possible_control_modes}!")
194
195    def is_on(self) -> bool:
196        """Check if the mobile base is currently stiff (not in free-wheel mode).
197
198        Returns:
199            `True` if the mobile base is not compliant (stiff), `False` otherwise.
200        """
201        return not self._drive_mode == "free_wheel"
202
203    def is_off(self) -> bool:
204        """Check if the mobile base is currently compliant (in free-wheel mode).
205
206        Returns:
207            True if the mobile base is compliant (in free-wheel mode), `False` otherwise.
208        """
209        if self._drive_mode == "free_wheel":
210            return True
211        return False
212
213    def get_current_odometry(self, degrees: bool = True) -> Dict[str, float]:
214        """Get the current odometry of the mobile base in its reference frame.
215
216        Args:
217            degrees (bool, optional): Whether to return the orientation (`theta` and `vtheta`) in degrees.
218                                    Defaults to True.
219
220        Returns:
221            Dict[str, float]: A dictionary containing the current odometry of the mobile base with:
222            - 'x': Position along the x-axis (in meters).
223            - 'y': Position along the y-axis (in meters).
224            - 'theta': Orientation (in degrees by default, radians if `degrees=False`).
225            - 'vx': Linear velocity along the x-axis (in meters per second).
226            - 'vy': Linear velocity along the y-axis (in meters per second).
227            - 'vtheta': Angular velocity (in degrees per second by default, radians if `degrees=False`).
228        """
229        current_state = self.odometry.copy()
230        if not degrees:
231            current_state["theta"] = deg2rad(current_state["theta"])
232            current_state["vtheta"] = deg2rad(current_state["vtheta"])
233
234        return current_state
235
236    def goto(
237        self,
238        x: float,
239        y: float,
240        theta: float,
241        wait: bool = False,
242        degrees: bool = True,
243        distance_tolerance: Optional[float] = 0.05,
244        angle_tolerance: Optional[float] = None,
245        timeout: float = 100,
246    ) -> GoToId:
247        """Send the mobile base to a specified target position.
248
249        The (x, y) coordinates define the position in Cartesian space, and theta specifies the orientation in degrees.
250        The zero position is set when the mobile base is started or when the `reset_odometry` method is called. A timeout
251        can be provided to avoid the mobile base getting stuck. The tolerance values define the acceptable margins for
252        reaching the target position.
253
254        Args:
255            x: The target x-coordinate in meters.
256            y: The target y-coordinate in meters.
257            theta: The target orientation in degrees.
258            wait: If True, the function waits until the movement is completed before returning.
259                    Defaults to False.
260            degrees: If True, the theta value and angle_tolerance are treated as degrees.
261                    Defaults to True.
262            distance_tolerance: Optional; the tolerance to the target position to consider the goto finished, in meters.
263            angle_tolerance: Optional; the angle tolerance to the target to consider the goto finished, in meters.
264            timeout: Optional; the maximum time allowed to reach the target, in seconds.
265
266        Returns:
267            GoToId: The unique GoToId identifier for the movement command.
268
269        Raises:
270            TypeError: If the target is not reached and the mobile base is stopped due to an obstacle.
271        """
272        if self.is_off():
273            self._logger.warning("Mobile base is off. Goto not sent.")
274            return
275
276        self._check_goto_parameters(target=[x, y, theta])
277        self._check_optional_goto_parameters(distance_tolerance, angle_tolerance, timeout)
278
279        if degrees:
280            theta = deg2rad(theta)
281            if angle_tolerance is not None:
282                angle_tolerance = deg2rad(angle_tolerance)
283
284        if angle_tolerance is None:
285            angle_tolerance = deg2rad(5.0)
286
287        vector_goal = TargetDirectionCommand(
288            id=self._part_id,
289            direction=DirectionVector(
290                x=FloatValue(value=x),
291                y=FloatValue(value=y),
292                theta=FloatValue(value=theta),
293            ),
294        )
295
296        odometry_goal = OdometryGoal(
297            odometry_goal=vector_goal,
298            distance_tolerance=FloatValue(value=distance_tolerance),
299            angle_tolerance=FloatValue(value=angle_tolerance),
300            timeout=FloatValue(value=timeout),
301        )
302
303        request = GoToRequest(
304            odometry_goal=odometry_goal,
305        )
306
307        response = self._goto_stub.GoToOdometry(request)
308
309        if response.id == -1:
310            self._logger.error(f"Unable to go to requested position x={x}, y={y}, theta={theta}. No command sent.")
311        elif wait:
312            self._wait_goto(response, timeout)
313
314        return response
315
316    def translate_by(
317        self,
318        x: float,
319        y: float,
320        wait: bool = False,
321        distance_tolerance: Optional[float] = 0.05,
322        timeout: float = 100,
323    ) -> GoToId:
324        """Send a target position relative to the current position of the mobile base.
325
326        The (x, y) coordinates specify the desired translation in the mobile base's Cartesian space.
327
328        Args:
329            x: The desired translation along the x-axis in meters.
330            y: The desired translation along the y-axis in meters.
331            wait:  If True, the function waits until the movement is completed before returning.
332            distance_tolerance: Optional; The distance tolerance to the target to consider the goto finished, in meters.
333            timeout: An optional timeout for reaching the target position, in seconds.
334
335        Returns:
336            The GoToId of the movement command, created using the `goto` method.
337        """
338        try:
339            goto = self.get_goto_queue()[-1]
340        except IndexError:
341            goto = self.get_goto_playing()
342
343        if goto.id != -1:
344            odom_request = self._get_goto_request(goto)
345        else:
346            odom_request = None
347
348        angle_tolerance = None
349
350        if odom_request is not None:
351            base_odom = odom_request.request.target
352            angle_tolerance = deg2rad(odom_request.request.angle_tolerance)
353        else:
354            base_odom = self.odometry
355
356        theta_goal = deg2rad(base_odom["theta"])
357        x_goal = base_odom["x"] + (x * np.cos(theta_goal) - y * np.sin(theta_goal))
358        y_goal = base_odom["y"] + (x * np.sin(theta_goal) + y * np.cos(theta_goal))
359
360        return self.goto(
361            x_goal,
362            y_goal,
363            theta_goal,
364            wait=wait,
365            distance_tolerance=distance_tolerance,
366            angle_tolerance=angle_tolerance,
367            degrees=False,
368            timeout=timeout,
369        )
370
371    def rotate_by(
372        self,
373        theta: float,
374        wait: bool = False,
375        degrees: bool = True,
376        angle_tolerance: Optional[float] = None,
377        timeout: float = 100,
378    ) -> GoToId:
379        """Send a target rotation relative to the current rotation of the mobile base.
380
381        The theta parameter defines the desired rotation in degrees.
382
383        Args:
384            theta: The desired rotation in degrees, relative to the current orientation.
385            wait: If True, the function waits until the rotation is completed before returning.
386            degrees: If True, the theta value and angle_tolerance are treated as degrees, otherwise as radians.
387            angle_tolerance: Optional; The angle tolerance to the target to consider the goto finished.
388            timeout: An optional timeout for completing the rotation, in seconds.
389        """
390        try:
391            goto = self.get_goto_queue()[-1]
392        except IndexError:
393            goto = self.get_goto_playing()
394
395        if goto.id != -1:
396            odom_request = self._get_goto_request(goto)
397        else:
398            odom_request = None
399
400        distance_tolerance = 0.05
401
402        if odom_request is not None:
403            base_odom = odom_request.request.target
404            if angle_tolerance is None:
405                angle_tolerance = odom_request.request.angle_tolerance
406            distance_tolerance = odom_request.request.distance_tolerance
407        else:
408            base_odom = self.odometry
409
410        if degrees:
411            theta = base_odom["theta"] + theta
412        else:
413            theta = deg2rad(base_odom["theta"]) + theta
414        x = base_odom["x"]
415        y = base_odom["y"]
416
417        return self.goto(
418            x,
419            y,
420            theta,
421            wait=wait,
422            degrees=degrees,
423            distance_tolerance=distance_tolerance,
424            angle_tolerance=angle_tolerance,
425            timeout=timeout,
426        )
427
428    def reset_odometry(self) -> None:
429        """Reset the odometry.
430
431        This method resets the mobile base's odometry, so that the current position is now (x, y, theta) = (0, 0, 0).
432        If any goto is being played, stop the goto and the queued ones.
433        """
434        if self.get_goto_playing().id != -1 or len(self.get_goto_queue()) != 0:
435            self._logger.warning(
436                "Odometry reset requested while goto in progress: aborting the current goto and all queued gotos."
437            )
438        self._stub.ResetOdometry(self._part_id)
439        time.sleep(0.05)
440
441    def set_goal_speed(self, vx: float | int = 0, vy: float | int = 0, vtheta: float | int = 0) -> None:
442        """Set the goal speed for the mobile base.
443
444        This method sets the target velocities for the mobile base's movement along the x and y axes, as well as
445        its rotational speed. The actual movement is executed after calling `send_speed_command`.
446
447        Args:
448            vx (float | int, optional): Linear velocity along the x-axis in meters per second. Defaults to 0.
449            vy (float | int, optional): Linear velocity along the y-axis in meters per second. Defaults to 0.
450            vtheta (float | int, optional): Rotational velocity (around the z-axis) in degrees per second. Defaults to 0.
451
452        Raises:
453            TypeError: If any of the velocity values (`vx`, `vy`, `vtheta`) are not of type `float` or `int`.
454
455        Notes:
456            - Use `send_speed_command` after this method to execute the movement.
457            - The velocities will be used to command the mobile base for a short duration (0.2 seconds).
458        """
459        for vel in [vx, vy, vtheta]:
460            if not isinstance(vel, float) | isinstance(vel, int):
461                raise TypeError("goal_speed must be a float or int")
462
463        self._x_vel_goal = vx
464        self._y_vel_goal = vy
465        self._rot_vel_goal = vtheta
466
467    def send_speed_command(self) -> None:
468        """Send the speed command to the mobile base, based on previously set goal speeds.
469
470        This method sends the velocity commands for the mobile base that were set with `set_goal_speed`.
471        The command will be executed for a duration of 200ms, which is predefined at the ROS level of the mobile base code.
472
473        Raises:
474            ValueError: If the absolute value of `x_vel`, `y_vel`, or `rot_vel` exceeds the configured maximum values.
475            Warning: If the mobile base is off, no command is sent, and a warning is logged.
476
477        Notes:
478            - This method is optimal for sending frequent speed instructions to the mobile base.
479            - The goal velocities must be set with `set_goal_speed` before calling this function.
480        """
481        if self.is_off():
482            self._logger.warning(f"{self._part_id.name} is off. speed_command not sent.")
483            return
484        for vel, value in {"x_vel": self._x_vel_goal, "y_vel": self._y_vel_goal}.items():
485            if abs(value) > self._max_xy_vel:
486                raise ValueError(f"The absolute value of {vel} should not be more than {self._max_xy_vel}, got {abs(value)}")
487
488        if abs(self._rot_vel_goal) > self._max_rot_vel:
489            raise ValueError(
490                f"The absolute value of rot_vel should not be more than {self._max_rot_vel}, got {abs(self._rot_vel_goal)}"
491            )
492
493        if self._drive_mode != "cmd_vel":
494            self._set_drive_mode("cmd_vel")
495
496        req = TargetDirectionCommand(
497            direction=DirectionVector(
498                x=FloatValue(value=self._x_vel_goal),
499                y=FloatValue(value=self._y_vel_goal),
500                theta=FloatValue(value=deg2rad(self._rot_vel_goal)),
501            )
502        )
503        self._mobility_stub.SendDirection(req)
504
505    def _update_with(self, new_state: MobileBaseState) -> None:
506        """Update the mobile base's state with newly received data from the gRPC server.
507
508        This method updates the battery level, LIDAR safety information, drive mode, and control mode
509        of the mobile base.
510
511        Args:
512            new_state: The new state of the mobile base, as a MobileBaseState object.
513        """
514        self._battery_level = new_state.battery_level.level.value
515        self.lidar._update_with(new_state.lidar_safety)
516        self._drive_mode = ZuuuModePossiblities.keys()[new_state.zuuu_mode.mode].lower()
517        self._control_mode = ControlModePossiblities.keys()[new_state.control_mode.mode].lower()
518
519    def _update_audit_status(self, new_status: MobileBaseStatus) -> None:
520        """Update the audit status of the mobile base.
521
522        This is a placeholder method and does not perform any actions.
523
524        Args:
525            new_status: The new status of the mobile base, as a MobileBaseStatus object.
526        """
527        pass  # pragma: no cover
528
529    def _set_speed_limits(self, value: int) -> None:
530        """Set the speed limits for the mobile base.
531
532        This method overrides the base class implementation to set speed limits.
533
534        Args:
535            value: The speed limit value to be set, as an integer.
536        """
537        return super()._set_speed_limits(value)
538
539    def _check_goto_parameters(self, target: Any, duration: Optional[float] = None, q0: Optional[List[float]] = None) -> None:
540        """Check the validity of the parameters for the `goto` method.
541
542        Args:
543            duration: Not required here.
544            target: The target goal, as a list [x, y, theta] in the odometry coordinate system.
545            q0: Not required here. Defaults to None.
546
547        Raises:
548            TypeError: If the x goal is not a float or int.
549            TypeError: If the y goal is not a float or int.
550            TypeError: If the theta goal is not a float or int.
551        """
552        self._check_type_float(target[0], "x")
553        self._check_type_float(target[1], "y")
554        self._check_type_float(target[2], "theta")
555
556        try:
557            goto = self.get_goto_queue()[-1]
558        except IndexError:
559            goto = self.get_goto_playing()
560
561        if goto.id != -1:
562            odom_request = self._get_goto_request(goto)
563        else:
564            odom_request = None
565
566        if odom_request is not None:
567            base_odom = odom_request.request.target
568            x_offset = abs(target[0] - base_odom["x"])
569            y_offset = abs(target[1] - base_odom["y"])
570        else:
571            x_offset = abs(target[0] - self.odometry["x"])
572            y_offset = abs(target[1] - self.odometry["y"])
573        for pos, value in {"x": x_offset, "y": y_offset}.items():
574            if abs(value) > self._max_xy_goto:
575                raise ValueError(f"The displacement in {pos} should not be more than {self._max_xy_goto}, got {abs(value)}")
576
577    def _check_optional_goto_parameters(
578        self, distance_tolerance: Optional[float], angle_tolerance: Optional[float], timeout: Optional[float]
579    ) -> None:
580        """Check the validity of the optional parameters for the `goto` method.
581
582        Args:
583            distance_tolerance: The distance tolerance value to be checked.
584            angle_tolerance: The angle tolerance value to be checked.
585            timeout: The timeout value to be checked.
586
587        Raises:
588            ValueError: If the distance_tolerance is negative.
589            ValueError: If the angle_tolerance is negative.
590            ValueError: If the timeout is negative or null.
591        """
592        if distance_tolerance is not None:
593            self._check_type_float(distance_tolerance, "distance_tolerance")
594            if distance_tolerance < 0:
595                raise ValueError(f"distance_tolerance must be a positive value, got {distance_tolerance}")
596        if angle_tolerance is not None:
597            self._check_type_float(angle_tolerance, "angle_tolerance")
598            if angle_tolerance < 0:
599                raise ValueError(f"angle_tolerance must be a positive value, got {angle_tolerance}")
600        if timeout is not None:
601            self._check_type_float(timeout, "timeout")
602            if timeout <= 0:
603                raise ValueError(f"timeout must be a positive value greater than 0, got {timeout}")
604
605    def _check_type_float(self, value: Any, arg_name: str) -> None:
606        """Check the type of the value parameter.
607
608        Args:
609            value: The value to be checked.
610
611        Raises:
612            TypeError: If the value is not a float or int.
613        """
614        if not (isinstance(value, float) | isinstance(value, int)):
615            raise TypeError(f"{arg_name} must be a float or int, got {type(value)} instead")
616
617    def set_max_xy_goto(self, value: float) -> None:
618        """Set the maximum displacement in the x and y directions for the mobile base.
619
620        Args:
621            value: The maximum displacement value to be set, in meters.
622        """
623        self._max_xy_goto = value
624
625    def goto_posture(
626        self,
627        common_posture: str = "default",
628        duration: float = 2,
629        wait: bool = False,
630        wait_for_goto_end: bool = True,
631        interpolation_mode: str = "minimum_jerk",
632    ) -> GoToId:
633        """Mobile base is not affected by goto_posture. No command is sent."""
634        return super().goto_posture(common_posture, duration, wait, wait_for_goto_end, interpolation_mode)