reachy2_sdk.grippers.parallel_gripper

  1"""Reachy ParallelGripper module.
  2
  3Handles all specific methods to a ParallelGripper.
  4"""
  5
  6from typing import Any, List, Optional
  7
  8import grpc
  9import numpy as np
 10from google.protobuf.wrappers_pb2 import FloatValue
 11from reachy2_sdk_api.goto_pb2 import GoToId, GoToRequest, JointsGoal
 12from reachy2_sdk_api.goto_pb2_grpc import GoToServiceStub
 13from reachy2_sdk_api.hand_pb2 import Hand as Hand_proto
 14from reachy2_sdk_api.hand_pb2 import (
 15    HandJointGoal,
 16    HandPosition,
 17    HandPositionRequest,
 18    HandState,
 19    HandStatus,
 20    ParallelGripperPosition,
 21)
 22from reachy2_sdk_api.hand_pb2_grpc import HandServiceStub
 23
 24from ..parts.hand import Hand
 25from ..utils.utils import get_grpc_interpolation_mode
 26from .gripper_joint import GripperJoint
 27
 28
 29class ParallelGripper(Hand):
 30    """Class for controlling the Reachy's parallel gripper.
 31
 32    The `ParallelGripper` class provides methods to control the gripper of Reachy, including opening and closing
 33    the hand, setting the goal position, and checking the hand's state. It also manages the hand's
 34    compliance status (whether it is stiff or free). It implements all specific behaviors for the parallel gripper.
 35
 36    Attributes:
 37        opening: The opening of the hand as a percentage (0-100), rounded to two decimal places.
 38        present_position: The current position of the hand in degrees.
 39        goal_position: The target goal position of the hand in degrees.
 40    """
 41
 42    def __init__(
 43        self,
 44        hand_msg: Hand_proto,
 45        initial_state: HandState,
 46        grpc_channel: grpc.Channel,
 47        goto_stub: GoToServiceStub,
 48    ) -> None:
 49        """Initialize the ParallelGripper component.
 50
 51        Sets up the necessary attributes and configuration for the hand, including the gRPC
 52        stub and initial state.
 53
 54        Args:
 55            hand_msg: The Hand_proto object containing the configuration details for the hand.
 56            initial_state: The initial state of the hand, represented as a HandState object.
 57            grpc_channel: The gRPC channel used to communicate with the hand's gRPC service.
 58            goto_stub: The gRPC stub for controlling goto movements.
 59        """
 60        super().__init__(hand_msg, grpc_channel, goto_stub)
 61        self._stub = HandServiceStub(grpc_channel)
 62
 63        self._joints = {"finger": GripperJoint(initial_state)}
 64
 65    def __repr__(self) -> str:
 66        """Clean representation of a ParallelGripper."""
 67        s = "\n\t".join([str(joint) for joint in self._joints.values()])
 68        return f"""<ParallelGripper on={self.is_on()} joints=\n\t{
 69            s
 70        }\n>"""
 71
 72    @property
 73    def opening(self) -> float:
 74        """Get the opening of the parallel gripper only joint as a percentage.
 75
 76        Returns:
 77            The hand opening as a percentage (0-100), rounded to two decimal places.
 78        """
 79        return float(self._joints["finger"].opening)
 80
 81    @property
 82    def present_position(self) -> float:
 83        """Get the current position of the parallel gripper only joint.
 84
 85        Returns:
 86            The present position of the hand in degrees.
 87        """
 88        return float(self._joints["finger"].present_position)
 89
 90    @property
 91    def goal_position(self) -> float:
 92        """Get the goal position of the parallel gripper only joint.
 93
 94        Returns:
 95            The goal position of the hand in degrees.
 96        """
 97        return float(self._joints["finger"].goal_position)
 98
 99    @goal_position.setter
100    def goal_position(self, value: float | int) -> None:
101        """Set the goal position for the parallel gripper only joint.
102
103        Args:
104            value: The goal position to set, specified as a float or int.
105
106        Raises:
107            TypeError: If the provided value is not a float or int.
108        """
109        self._joints["finger"].goal_position = value
110
111    def _check_goto_parameters(self, target: Any, duration: Optional[float] = 0, q0: Optional[List[float]] = None) -> None:
112        """Check the validity of the parameters for the `goto` method.
113
114        Args:
115            duration: The time in seconds for the movement to be completed.
116            target: The target position, either a float or int.
117            q0: An optional initial joint configuration for inverse kinematics (not used for the hand). Defaults to None.
118
119        Raises:
120            TypeError: If the target is not a float or a int.
121            ValueError: If the duration is set to 0.
122        """
123        if not (isinstance(target, float) or isinstance(target, int)):
124            raise TypeError(f"Invalid target: must be either a float or a int, got {type(target)} instead.")
125
126        elif duration == 0:
127            raise ValueError("duration cannot be set to 0.")
128
129    def get_current_opening(self) -> float:
130        """Get the current opening of the parallel gripper only joint.
131
132        Returns:
133            The current opening of the hand as a percentage (0-100).
134        """
135        return self.opening
136
137    def set_opening(self, percentage: float) -> None:
138        """Set the opening value for the parallel gripper only joint.
139
140        Args:
141            percentage: The desired opening percentage of the hand, ranging from 0 to 100.
142
143        Raises:
144            ValueError: If the percentage is not between 0 and 100.
145            RuntimeError: If the gripper is off and the opening value cannot be set.
146        """
147        if not 0.0 <= percentage <= 100.0:
148            raise ValueError(f"Percentage should be between 0 and 100, not {percentage}")
149        if self.is_off():
150            raise RuntimeError("Gripper is off. Opening value not sent.")
151
152        self._stub.SetHandPosition(
153            HandPositionRequest(
154                id=self._part_id,
155                position=HandPosition(
156                    parallel_gripper=ParallelGripperPosition(opening_percentage=FloatValue(value=percentage / 100.0))
157                ),
158            )
159        )
160        self._joints["finger"]._is_moving = True
161
162    def _get_goal_positions_message(self) -> Optional[HandPositionRequest]:
163        """Get the HandPositionRequest message to send the goal positions to the actuator."""
164        if self._joints["finger"]._outgoing_goal_positions is not None:
165            if self.is_off():
166                self._logger.warning(f"{self._part_id.name} is off. Command not sent.")
167                return None
168            command = HandPositionRequest(
169                id=self._part_id,
170                position=HandPosition(
171                    parallel_gripper=ParallelGripperPosition(
172                        position=FloatValue(value=self._joints["finger"]._outgoing_goal_positions)
173                    )
174                ),
175            )
176            self._joints["finger"]._is_moving = True
177            return command
178        return None
179
180    def goto_posture(
181        self,
182        common_posture: str = "default",
183        duration: float = 2,
184        wait: bool = False,
185        wait_for_goto_end: bool = True,
186        interpolation_mode: str = "minimum_jerk",
187    ) -> GoToId:
188        """Send the gripper to default open posture with optional parameters for duration, waiting, and interpolation mode.
189
190        Args:
191            common_posture: The standard posture. It can be 'default' or 'elbow_90'. Defaults to 'default'.
192                Modifying the posture has no effect on the hand.
193            duration: The time duration in seconds for the robot to move to the specified posture.
194                Defaults to 2.
195            wait: Determines whether the program should wait for the movement to finish before
196                returning. If set to `True`, the program waits for the movement to complete before continuing
197                execution. Defaults to `False`.
198            wait_for_goto_end: Specifies whether commands will be sent to a part immediately or
199                only after all previous commands in the queue have been executed. If set to `False`, the program
200                will cancel all executing moves and queues. Defaults to `True`.
201            interpolation_mode: The type of interpolation used when moving the gripper.
202                Can be 'minimum_jerk' or 'linear'. Defaults to 'minimum_jerk'.
203
204        Returns:
205            A unique GoToId identifier for this specific movement.
206        """
207        if not wait_for_goto_end:
208            self.cancel_all_goto()
209        if self.is_on():
210            return self.goto(100.0, duration, wait, percentage=True, interpolation_mode=interpolation_mode)
211        else:
212            self._logger.warning(f"{self._part_id.name} is off. No command sent.")
213        return GoToId(id=-1)
214
215    def goto(
216        self,
217        target: float | int,
218        duration: float = 2,
219        wait: bool = False,
220        interpolation_mode: str = "minimum_jerk",
221        degrees: bool = True,
222        percentage: float = False,
223    ) -> GoToId:
224        """Move the hand to a specified goal position.
225
226        Args:
227            target: The target position. It can either be a float or int.
228            duration: The time in seconds for the movement to be completed. Defaults to 2.
229            wait: If True, the function waits until the movement is completed before returning.
230                    Defaults to False.
231            interpolation_mode: The interpolation method to be used. It can be either "minimum_jerk"
232                    or "linear". Defaults to "minimum_jerk".
233            degrees: If True, the joint values in the `target` argument are treated as degrees.
234                    Defaults to True.
235            percentage: If True, the target value is treated as a percentage of opening. Defaults to False.
236
237        Returns:
238            GoToId: The unique GoToId identifier for the movement command.
239        """
240        self._check_goto_parameters(target, duration)
241
242        if self.is_off():
243            self._logger.warning(f"{self._part_id.name} is off. Goto not sent.")
244            return GoToId(id=-1)
245
246        if degrees and not percentage:
247            target = np.deg2rad(target)
248
249        if percentage:
250            parallel_gripper_target = ParallelGripperPosition(opening_percentage=FloatValue(value=target / 100.0))
251        else:
252            parallel_gripper_target = ParallelGripperPosition(position=FloatValue(value=target))
253
254        request = GoToRequest(
255            joints_goal=JointsGoal(
256                hand_joint_goal=HandJointGoal(
257                    goal_request=HandPositionRequest(
258                        id=self._part_id,
259                        position=HandPosition(parallel_gripper=parallel_gripper_target),
260                    ),
261                    duration=FloatValue(value=duration),
262                )
263            ),
264            interpolation_mode=get_grpc_interpolation_mode(interpolation_mode),
265        )
266
267        response = self._goto_stub.GoToJoints(request)
268
269        if response.id == -1:
270            self._logger.error(f"Position {target} was not reachable. No command sent.")
271        elif wait:
272            self._wait_goto(response, duration)
273        self._joints["finger"]._is_moving = True
274        if interpolation_mode == "minimum_jerk":
275            self._nb_steps_to_ignore = 10
276        return response
277
278    def _update_with(self, new_state: HandState) -> None:
279        """Update the hand with a newly received (partial) state from the gRPC server.
280
281        This method updates the present position, goal position, opening, and compliance status.
282        It also checks if the hand is still moving based on the new state.
283
284        Args:
285            new_state: A HandState object representing the new state of the hand.
286        """
287        self._joints["finger"]._update_with(new_state)
288
289    def _update_audit_status(self, new_status: HandStatus) -> None:
290        """Update the audit status with the new status received from the gRPC server.
291
292        Args:
293            new_status: A HandStatus object representing the new status of the hand.
294        """
295        pass  # pragma: no cover
296
297    @property
298    def status(self) -> Optional[str]:
299        """Get the current audit status of the actuator.
300
301        Returns:
302            The audit status as a string, representing the latest error or status
303            message, or `None` if there is no error.
304        """
305        pass