reachy2_sdk.orbita.orbita_joint

  1"""Reachy OrbitaJoint module.
  2
  3Handles all specific methods to OrbitaJoint.
  4"""
  5
  6from typing import Any, Dict
  7
  8from google.protobuf.wrappers_pb2 import FloatValue
  9from reachy2_sdk_api.goto_pb2 import GoToId
 10
 11from .utils import to_internal_position, to_position
 12
 13
 14class OrbitaJoint:
 15    """The OrbitaJoint class represents any Orbita2d or Orbita3d joint.
 16
 17    The OrbitaJoint class is used to store the up-to-date state of the joint, especially:
 18    - its present_position (RO)
 19    - its goal_position (RW)
 20    """
 21
 22    def __init__(
 23        self,
 24        initial_state: Dict[str, FloatValue],
 25        axis_type: str,
 26        actuator: Any,
 27        position_order_in_part: int,
 28    ) -> None:
 29        """Initialize the OrbitaJoint with its initial state and configuration.
 30
 31        This sets up the joint by assigning its actuator, axis type, and position order within
 32        the part, and updates its state based on the provided initial values.
 33
 34        Args:
 35            initial_state: A dictionary containing the initial state of the joint, with
 36                each entry representing a specific parameter of the joint (e.g., present position).
 37            axis_type: The type of axis for the joint (e.g., roll, pitch, yaw).
 38            actuator: The actuator to which this joint belongs.
 39            position_order_in_part: The position order of this joint in the overall part's
 40                list of joints.
 41        """
 42        self._actuator = actuator
 43        self._axis_type = axis_type
 44        self._position_order_in_part = position_order_in_part
 45
 46        self._update_with(initial_state)
 47
 48    def __repr__(self) -> str:
 49        """Clean representation of the OrbitaJoint."""
 50        repr_template = (
 51            '<OrbitaJoint axis_type="{axis_type}" present_position={present_position} goal_position={goal_position} >'
 52        )
 53        return repr_template.format(
 54            axis_type=self._axis_type,
 55            present_position=round(self.present_position, 2),
 56            goal_position=round(self.goal_position, 2),
 57        )
 58
 59    @property
 60    def present_position(self) -> float:
 61        """Get the present position of the joint in degrees."""
 62        return to_position(self._present_position)
 63
 64    @property
 65    def goal_position(self) -> float:
 66        """Get the goal position of the joint in degrees."""
 67        return to_position(self._goal_position)
 68
 69    @goal_position.setter
 70    def goal_position(self, value: float | int) -> None:
 71        """Set the goal position of the joint in degrees.
 72
 73        The goal position is not send to the joint immediately, it is stored locally until the `send_goal_positions` method
 74        is called.
 75
 76        Args:
 77            value: The goal position to set, specified as a float or int.
 78
 79        Raises:
 80            TypeError: If the provided value is not a float or int.
 81        """
 82        if isinstance(value, float) | isinstance(value, int):
 83            self._actuator._set_outgoing_goal_position(self._axis_type, to_internal_position(value))
 84        else:
 85            raise TypeError("goal_position must be a float or int")
 86
 87    def goto(
 88        self,
 89        goal_position: float,
 90        duration: float = 2,
 91        wait: bool = False,
 92        interpolation_mode: str = "minimum_jerk",
 93        degrees: bool = True,
 94    ) -> GoToId:
 95        """Send the joint to the specified goal position within a given duration.
 96
 97        Acts like a "goto" movement on the part, where "goto" movements for joints are queued on the part they belong to.
 98
 99        Args:
100            goal_position: The target position to move the joint to.
101            duration: The time in seconds for the joint to reach the goal position. Defaults to 2.
102            wait: Whether to wait for the movement to finish before continuing. Defaults to False.
103            interpolation_mode: The type of interpolation to use for the movement, either "minimum_jerk" or "linear".
104                Defaults to "minimum_jerk".
105            degrees: Whether the goal position is specified in degrees. If True, the position is interpreted as degrees.
106                Defaults to True.
107
108        Returns:
109            The GoToId associated with the movement command.
110        """
111        if self._actuator._part.is_off():
112            self._actuator._logger.warning(f"{self._actuator._part._part_id.name} is off. No command sent.")
113            return GoToId(id=-1)
114
115        return self._actuator._part._goto_single_joint(
116            self._position_order_in_part,
117            goal_position=goal_position,
118            duration=duration,
119            wait=wait,
120            interpolation_mode=interpolation_mode,
121            degrees=degrees,
122        )
123
124    def _update_with(self, new_state: Dict[str, FloatValue]) -> None:
125        """Update the present and goal positions of the joint with new state values.
126
127        Args:
128            new_state: A dictionary containing the new state values for the joint. The keys should include
129                "present_position" and "goal_position", with corresponding FloatValue objects as values.
130        """
131        self._present_position = new_state["present_position"].value
132        self._goal_position = new_state["goal_position"].value