reachy2_sdk.grippers.gripper_joint

  1"""Reachy GripperJoint module.
  2
  3Handles all specific methods to a GripperJoint.
  4"""
  5
  6import logging
  7from collections import deque
  8from typing import Deque, Optional
  9
 10import numpy as np
 11from reachy2_sdk_api.hand_pb2 import HandState
 12
 13from ..orbita.utils import to_internal_position, to_position
 14
 15
 16class GripperJoint:
 17    """Class to represent any gripper's joint of the robot.
 18
 19    The `GripperJoint` class provides methods to get output values of the joint, such as the opening,
 20    present position, and goal position. It also allows setting the goal position and checking the
 21    joint's movement status.
 22
 23    Attributes:
 24        opening: The opening of the joint as a percentage (0-100), rounded to two decimal places.
 25        present_position: The current position of the joint in degrees.
 26        goal_position: The target goal position of the joint in degrees.
 27    """
 28
 29    def __init__(
 30        self,
 31        initial_state: HandState,
 32    ):
 33        """Initialize the GripperJoint with its initial state.
 34
 35        This sets up the joint by assigning its state based on the provided initial values.
 36
 37        Args:
 38            initial_state: A HandState containing the initial state of the joint.
 39        """
 40        self._logger = logging.getLogger(__name__)
 41
 42        self._is_moving = False
 43        self._nb_steps_to_ignore = 0
 44        self._steps_ignored = 0
 45        self._last_present_positions_queue_size = 10
 46        self._last_present_positions: Deque[float] = deque(maxlen=self._last_present_positions_queue_size)
 47
 48        self._update_with(initial_state)
 49        self._last_present_positions.append(self._present_position)
 50        self._outgoing_goal_positions: Optional[float] = None
 51
 52    def __repr__(self) -> str:
 53        """Clean representation of a GripperJoint."""
 54        repr_template = "<GripperJoint on={is_on} present_position={present_position} goal_position={goal_position} >"
 55        return repr_template.format(
 56            is_on=self.is_on(),
 57            present_position=round(self.present_position, 2),
 58            goal_position=round(self.goal_position, 2),
 59        )
 60
 61    @property
 62    def opening(self) -> float:
 63        """Get the opening of the joint as a percentage.
 64
 65        Returns:
 66            The joint opening as a percentage (0-100), rounded to two decimal places.
 67        """
 68        return float(round(self._opening * 100, 2))
 69
 70    @property
 71    def present_position(self) -> float:
 72        """Get the current position of the joint.
 73
 74        Returns:
 75            The present position of the joint in degrees.
 76        """
 77        return to_position(self._present_position)
 78
 79    @property
 80    def goal_position(self) -> float:
 81        """Get the goal position of the joint.
 82
 83        Returns:
 84            The goal position of the joint in degrees.
 85        """
 86        return to_position(self._goal_position)
 87
 88    @goal_position.setter
 89    def goal_position(self, value: float | int) -> None:
 90        """Set the goal position for the joint.
 91
 92        Args:
 93            value: The goal position to set, specified as a float or int.
 94
 95        Raises:
 96            TypeError: If the provided value is not a float or int.
 97        """
 98        if isinstance(value, float) | isinstance(value, int):
 99            self._outgoing_goal_positions = to_internal_position(value)
100        else:
101            raise TypeError("goal_position must be a float or int")
102
103    def is_on(self) -> bool:
104        """Check if the joint is stiff.
105
106        Returns:
107            `True` if the joint is on (not compliant), `False` otherwise.
108        """
109        return not self._compliant
110
111    def is_off(self) -> bool:
112        """Check if the joint is compliant.
113
114        Returns:
115            `True` if the joint is off (compliant), `False` otherwise.
116        """
117        return bool(self._compliant)
118
119    def is_moving(self) -> bool:
120        """Check if the joint is currently moving.
121
122        Returns:
123            `True` if the joint is moving, `False` otherwise.
124        """
125        return self._is_moving
126
127    def _check_joint_movement(self) -> None:
128        """Check if the joint is still moving based on the present position.
129
130        This method updates the movement status by comparing the current position to the last few positions.
131        If the position has not changed significantly, the joint is considered to have stopped moving.
132        """
133        present_position = self._present_position
134        if (
135            len(self._last_present_positions) >= self._last_present_positions_queue_size
136            and np.isclose(present_position, self._last_present_positions[-1], np.deg2rad(0.1))
137            and np.isclose(present_position, self._last_present_positions[-2], np.deg2rad(0.1))
138        ):
139            self._is_moving = False
140            self._last_present_positions.clear()
141        self._last_present_positions.append(present_position)
142
143    def _update_with(self, new_state: HandState) -> None:
144        """Update the joint with a newly received (partial) state from the gRPC server.
145
146        This method updates the present position, goal position, opening, and compliance status.
147        It also checks if the joint is still moving based on the new state.
148
149        Args:
150            new_state: A HandState object representing the new state of the hand.
151        """
152        self._present_position = new_state.present_position.parallel_gripper.position.value
153        self._goal_position = new_state.goal_position.parallel_gripper.position.value
154        self._opening = new_state.opening.value
155        self._compliant = new_state.compliant.value
156        if self._is_moving:
157            self._check_joint_movement()