reachy2_sdk.parts.tripod

  1"""Reachy Tripod module.
  2
  3Handles all specific methods to the tripod.
  4"""
  5
  6import logging
  7from typing import Tuple
  8
  9import grpc
 10import numpy as np
 11from google.protobuf.wrappers_pb2 import FloatValue
 12from reachy2_sdk_api.part_pb2 import PartId
 13from reachy2_sdk_api.tripod_pb2 import Tripod as Tripod_proto
 14from reachy2_sdk_api.tripod_pb2 import TripodCommand, TripodState
 15from reachy2_sdk_api.tripod_pb2_grpc import TripodServiceStub
 16
 17
 18class Tripod:
 19    """The Tripod class represents the fixed tripod of the robot.
 20
 21    The Tripod class is used to update manually the robot tripod's height value.
 22    """
 23
 24    def __init__(
 25        self,
 26        proto_msg: Tripod_proto,
 27        initial_state: TripodState,
 28        grpc_channel: grpc.Channel,
 29    ):
 30        """Initialize the Tripod with its initial state and configuration.
 31
 32        This sets up the tripod by assigning its state based on the provided initial values.
 33
 34        Args:
 35            proto_msg: The protobuf message containing configuration details for the part.
 36            initial_state: The initial state of the tripod's joints.
 37            grpc_channel: The gRPC channel used to communicate with the DynamixelMotor service.
 38        """
 39        self._grpc_channel = grpc_channel
 40        self._stub = TripodServiceStub(grpc_channel)
 41        self._part_id = PartId(id=proto_msg.part_id.id, name=proto_msg.part_id.name)
 42        self._logger = logging.getLogger(__name__)
 43
 44        self._present_position: float
 45        self._goal_position: float
 46        self._update_with(initial_state)
 47
 48    def __repr__(self) -> str:
 49        """Clean representation of the Tripod."""
 50        repr_template = "<Tripod height={height} >"
 51        return repr_template.format(
 52            height=round(self.height, 3),
 53        )
 54
 55    @property
 56    def height(self) -> float:
 57        """Get the current height of the robot torso in meters."""
 58        return float(np.round(self._present_position, 3))
 59
 60    def set_height(self, height: float) -> None:
 61        """Set the height of the tripod.
 62
 63        Args:
 64            height: The height of the tripod in meters.
 65
 66        Raises:
 67            TypeError: If the height is not a float or int.
 68        """
 69        if not isinstance(height, float) and not isinstance(height, int):
 70            raise TypeError(f"height should be a float or int (got {type(height)} instead)!")
 71
 72        limit_min, limit_max = self.get_limits()
 73        if not limit_min <= height <= limit_max:
 74            self._logger.warning(f"Height value {height} is out of bounds. ")
 75            height = np.clip(height, limit_min, limit_max)
 76            self._logger.warning(f"Setting height to {height}.")
 77        command = TripodCommand(
 78            part_id=self._part_id,
 79            height_position=FloatValue(value=height),
 80        )
 81        self._stub.SendCommand(command)
 82
 83    def reset_height(self) -> None:
 84        """Reset the height of the tripod to its default position."""
 85        self._stub.ResetDefaultValues(self._part_id)
 86
 87    def get_limits(self) -> Tuple[float, float]:
 88        """Get the limits of the tripod's height.
 89
 90        Returns:
 91            A tuple containing the minimum and maximum height values.
 92        """
 93        response = self._stub.GetJointsLimits(self._part_id)
 94        return np.round(response.height_limit.min.value, 3), np.round(response.height_limit.max.value, 3)
 95
 96    def _update_with(self, new_state: TripodState) -> None:
 97        """Update the present and goal positions of the joint with new state values.
 98
 99        Args:
100            new_state: A TripodState containing the new state values for the joint. Should include
101                "present_position" and "goal_position", with corresponding FloatValue objects as values.
102        """
103        self._present_position = new_state.height.present_position.value
104        self._goal_position = new_state.height.goal_position.value