reachy2_sdk.parts.arm

   1"""Reachy Arm module.
   2
   3Handles all specific methods to an Arm (left and/or right).
   4"""
   5
   6import time
   7from typing import Any, Dict, List, Optional, overload
   8
   9import grpc
  10import numpy as np
  11import numpy.typing as npt
  12from google.protobuf.wrappers_pb2 import FloatValue
  13from reachy2_sdk_api.arm_pb2 import Arm as Arm_proto
  14from reachy2_sdk_api.arm_pb2 import (  # ArmLimits,; ArmTemperatures,
  15    ArmCartesianGoal,
  16    ArmComponentsCommands,
  17    ArmEndEffector,
  18    ArmFKRequest,
  19    ArmIKRequest,
  20    ArmJointGoal,
  21    ArmJoints,
  22    ArmState,
  23    ArmStatus,
  24    CustomArmJoints,
  25)
  26from reachy2_sdk_api.arm_pb2_grpc import ArmServiceStub
  27from reachy2_sdk_api.goto_pb2 import (
  28    CartesianGoal,
  29    CustomJointGoal,
  30    EllipticalGoToParameters,
  31    GoToId,
  32    GoToRequest,
  33    JointsGoal,
  34)
  35from reachy2_sdk_api.goto_pb2_grpc import GoToServiceStub
  36from reachy2_sdk_api.hand_pb2 import Hand as HandState
  37from reachy2_sdk_api.hand_pb2 import Hand as Hand_proto
  38from reachy2_sdk_api.hand_pb2 import HandType
  39from reachy2_sdk_api.kinematics_pb2 import Matrix4x4
  40
  41from ..grippers.parallel_gripper import ParallelGripper
  42from ..orbita.orbita2d import Orbita2d
  43from ..orbita.orbita3d import Orbita3d
  44from ..utils.utils import (
  45    arm_position_to_list,
  46    get_grpc_arc_direction,
  47    get_grpc_interpolation_mode,
  48    get_grpc_interpolation_space,
  49    list_to_arm_position,
  50    matrix_from_euler_angles,
  51    recompose_matrix,
  52    rotate_in_self,
  53    translate_in_self,
  54)
  55from .goto_based_part import IGoToBasedPart
  56from .hand import Hand
  57from .joints_based_part import JointsBasedPart
  58
  59
  60class Arm(JointsBasedPart, IGoToBasedPart):
  61    """Reachy Arm module.
  62
  63    Handles specific functionalities for the arm (left and/or right), including:
  64    - Forward and inverse kinematics
  65    - Goto functions for movement
  66    - Turning the arm on and off
  67    - Cartesian interpolation for movements
  68
  69    Attributes:
  70        shoulder (Orbita2d): The shoulder actuator of the arm.
  71        elbow (Orbita2d): The elbow actuator of the arm.
  72        wrist (Orbita3d): The wrist actuator of the arm.
  73        gripper (Optional[Hand]): The gripper of the arm, if initialized.
  74    """
  75
  76    def __init__(
  77        self,
  78        arm_msg: Arm_proto,
  79        initial_state: ArmState,
  80        grpc_channel: grpc.Channel,
  81        goto_stub: GoToServiceStub,
  82    ) -> None:
  83        """Initialize an Arm instance.
  84
  85        This constructor sets up the arm's gRPC communication and initializes its actuators
  86        (shoulder, elbow, and wrist). Optionally, a gripper can also be configured.
  87
  88        Args:
  89            arm_msg: The protobuf message containing the arm's configuration details.
  90            initial_state: The initial state of the arm's actuators.
  91            grpc_channel: The gRPC channel used for communication with the arm's server.
  92            goto_stub: The gRPC stub for controlling goto movements.
  93        """
  94        JointsBasedPart.__init__(self, arm_msg, grpc_channel, ArmServiceStub(grpc_channel))
  95        IGoToBasedPart.__init__(self, self._part_id, goto_stub)
  96
  97        self._setup_arm(arm_msg, initial_state)
  98        self._gripper: Optional[Hand] = None
  99
 100        self._actuators: Dict[str, Orbita2d | Orbita3d | Hand] = {}
 101        self._actuators["shoulder"] = self.shoulder
 102        self._actuators["elbow"] = self.elbow
 103        self._actuators["wrist"] = self.wrist
 104
 105    def _setup_arm(self, arm: Arm_proto, initial_state: ArmState) -> None:
 106        """Initialize the arm's actuators (shoulder, elbow, and wrist) based on the arm's description and initial state.
 107
 108        Args:
 109            arm: The arm description used to set up the actuators, including the shoulder,
 110                elbow, and wrist. The method creates instances of `Orbita2d` for the shoulder and
 111                elbow, and an instance of `Orbita3d` for the wrist.
 112            initial_state: The initial state of the arm's actuators, containing the starting
 113                positions or states of the shoulder, elbow, and wrist. This information is used to
 114                initialize the corresponding actuators.
 115        """
 116        description = arm.description
 117        self._shoulder = Orbita2d(
 118            uid=description.shoulder.id.id,
 119            name=description.shoulder.id.name,
 120            axis1=description.shoulder.axis_1,
 121            axis2=description.shoulder.axis_2,
 122            initial_state=initial_state.shoulder_state,
 123            grpc_channel=self._grpc_channel,
 124            part=self,
 125            joints_position_order=[ArmJoints.SHOULDER_PITCH, ArmJoints.SHOULDER_ROLL],
 126        )
 127        self._elbow = Orbita2d(
 128            uid=description.elbow.id.id,
 129            name=description.elbow.id.name,
 130            axis1=description.elbow.axis_1,
 131            axis2=description.elbow.axis_2,
 132            initial_state=initial_state.elbow_state,
 133            grpc_channel=self._grpc_channel,
 134            part=self,
 135            joints_position_order=[ArmJoints.ELBOW_YAW, ArmJoints.ELBOW_PITCH],
 136        )
 137        self._wrist = Orbita3d(
 138            uid=description.wrist.id.id,
 139            name=description.wrist.id.name,
 140            initial_state=initial_state.wrist_state,
 141            grpc_channel=self._grpc_channel,
 142            part=self,
 143            joints_position_order=[ArmJoints.WRIST_ROLL, ArmJoints.WRIST_PITCH, ArmJoints.WRIST_YAW],
 144        )
 145
 146    def _init_hand(self, hand: Hand_proto, hand_initial_state: HandState) -> None:
 147        if hand.type == HandType.PARALLEL_GRIPPER:
 148            self._gripper = ParallelGripper(hand, hand_initial_state, self._grpc_channel, self._goto_stub)
 149            self._actuators["gripper"] = self._gripper
 150
 151    @property
 152    def shoulder(self) -> Orbita2d:
 153        """Get the shoulder actuator of the arm."""
 154        return self._shoulder
 155
 156    @property
 157    def elbow(self) -> Orbita2d:
 158        """Get the elbow actuator of the arm."""
 159        return self._elbow
 160
 161    @property
 162    def wrist(self) -> Orbita3d:
 163        """Get the wrist actuator of the arm."""
 164        return self._wrist
 165
 166    @property
 167    def gripper(self) -> Optional[Hand]:
 168        """Get the gripper of the arm, or None if not set."""
 169        return self._gripper
 170
 171    def __repr__(self) -> str:
 172        """Clean representation of an Arm."""
 173        s = "\n\t".join([act_name + ": " + str(actuator) for act_name, actuator in self._actuators.items()])
 174        return f"""<Arm on={self.is_on()} actuators=\n\t{
 175            s
 176        }\n>"""
 177
 178    def turn_on(self) -> None:
 179        """Turn on all motors of the part, making all arm motors stiff.
 180
 181        If a gripper is present, it will also be turned on.
 182        """
 183        if self._gripper is not None:
 184            self._gripper._turn_on()
 185        super().turn_on()
 186
 187    def turn_off(self) -> None:
 188        """Turn off all motors of the part, making all arm motors compliant.
 189
 190        If a gripper is present, it will also be turned off.
 191        """
 192        if self._gripper is not None:
 193            self._gripper._turn_off()
 194        super().turn_off()
 195
 196    def turn_off_smoothly(self) -> None:
 197        """Gradually reduce the torque limit of all motors over 3 seconds before turning them off.
 198
 199        This function decreases the torque limit in steps until the motors are turned off.
 200        It then restores the torque limit to its original value.
 201        """
 202        torque_limit_low = 35
 203        torque_limit_high = 100
 204        duration = 3
 205
 206        self.set_torque_limits(torque_limit_low)
 207        self.goto_posture(duration=duration, wait_for_goto_end=False)
 208
 209        countingTime = 0
 210        while countingTime < duration:
 211            time.sleep(1)
 212            torque_limit_low -= 10
 213            self.set_torque_limits(torque_limit_low)
 214            countingTime += 1
 215
 216        super().turn_off()
 217        self.set_torque_limits(torque_limit_high)
 218
 219    def _turn_on(self) -> None:
 220        """Turn on all motors of the part.
 221
 222        This will make all arm motors stiff. If a gripper is present, it will also be turned on.
 223        """
 224        if self._gripper is not None:
 225            self._gripper._turn_on()
 226        super()._turn_on()
 227
 228    def _turn_off(self) -> None:
 229        """Turn off all motors of the part.
 230
 231        This will make all arm motors compliant. If a gripper is present, it will also be turned off.
 232        """
 233        if self._gripper is not None:
 234            self._gripper._turn_off()
 235        super()._turn_off()
 236
 237    def is_on(self, check_gripper: bool = True) -> bool:
 238        """Check if all actuators of the arm are stiff.
 239
 240        Returns:
 241            `True` if all actuators of the arm are stiff, `False` otherwise.
 242        """
 243        if not check_gripper:
 244            for actuator in [self._actuators[act] for act in self._actuators.keys() if act not in ["gripper"]]:
 245                if not actuator.is_on():
 246                    return False
 247            return True
 248        return super().is_on()
 249
 250    def is_off(self, check_gripper: bool = True) -> bool:
 251        """Check if all actuators of the arm are compliant.
 252
 253        Returns:
 254            `True` if all actuators of the arm are compliant, `False` otherwise.
 255        """
 256        if not check_gripper:
 257            for actuator in [self._actuators[act] for act in self._actuators.keys() if act not in ["gripper"]]:
 258                if not actuator.is_off():
 259                    return False
 260            return True
 261        return super().is_off()
 262
 263    def get_current_positions(self, degrees: bool = True) -> List[float]:
 264        """Return the current joint positions of the arm, either in degrees or radians.
 265
 266        Args:
 267            degrees: Specifies whether the joint positions should be returned in degrees.
 268                If set to `True`, the positions are returned in degrees; otherwise, they are returned in radians.
 269                Defaults to `True`.
 270
 271        Returns:
 272            A list of float values representing the current joint positions of the arm in the
 273            following order: [shoulder_pitch, shoulder_roll, elbow_yaw, elbow_pitch, wrist_roll, wrist_pitch,
 274            wrist_yaw].
 275        """
 276        response = self._stub.GetJointPosition(self._part_id)
 277        positions: List[float] = arm_position_to_list(response, degrees)
 278        return positions
 279
 280    def forward_kinematics(
 281        self, joints_positions: Optional[List[float]] = None, degrees: bool = True
 282    ) -> npt.NDArray[np.float64]:
 283        """Compute the forward kinematics of the arm and return a 4x4 pose matrix.
 284
 285        The pose matrix is expressed in Reachy coordinate system.
 286
 287        Args:
 288            joints_positions: A list of float values representing the positions of the joints
 289                in the arm. If not provided, the current robot joints positions are used. Defaults to None.
 290            degrees: Indicates whether the joint positions are in degrees or radians.
 291                If `True`, the positions are in degrees; if `False`, in radians. Defaults to True.
 292
 293        Returns:
 294            A 4x4 pose matrix as a NumPy array, expressed in Reachy coordinate system.
 295
 296        Raises:
 297            ValueError: If `joints_positions` is provided and its length is not 7.
 298            ValueError: If no solution is found for the given joint positions.
 299        """
 300        req_params = {
 301            "id": self._part_id,
 302        }
 303        if joints_positions is None:
 304            present_joints_positions = [
 305                joint.present_position for orbita in self._actuators.values() for joint in orbita._joints.values()
 306            ]
 307            req_params["position"] = list_to_arm_position(present_joints_positions, degrees)
 308
 309        else:
 310            if len(joints_positions) != 7:
 311                raise ValueError(f"joints_positions should be length 7 (got {len(joints_positions)} instead)!")
 312            req_params["position"] = list_to_arm_position(joints_positions, degrees)
 313        req = ArmFKRequest(**req_params)
 314        resp = self._stub.ComputeArmFK(req)
 315        if not resp.success:
 316            raise ValueError(f"No solution found for the given joints ({joints_positions})!")
 317
 318        return np.array(resp.end_effector.pose.data).reshape((4, 4))
 319
 320    def inverse_kinematics(
 321        self,
 322        target: npt.NDArray[np.float64],
 323        q0: Optional[List[float]] = None,
 324        degrees: bool = True,
 325    ) -> List[float]:
 326        """Compute a joint configuration to reach a specified target pose for the arm end-effector.
 327
 328        Args:
 329            target: A 4x4 homogeneous pose matrix representing the target pose in
 330                Reachy coordinate system, provided as a NumPy array.
 331            q0: An optional initial joint configuration for the arm. If provided, the
 332                algorithm will use it as a starting point for finding a solution. Defaults to None.
 333            degrees: Indicates whether the returned joint angles should be in degrees or radians.
 334                If `True`, angles are in degrees; if `False`, in radians. Defaults to True.
 335            round: Number of decimal places to round the computed joint angles to before
 336                returning. If None, no rounding is performed. Defaults to None.
 337
 338        Returns:
 339            A list of joint angles representing the solution to reach the target pose, in the following order:
 340                [shoulder_pitch, shoulder_roll, elbo_yaw, elbow_pitch, wrist.roll, wrist.pitch, wrist.yaw].
 341
 342        Raises:
 343            ValueError: If the target shape is not (4, 4).
 344            ValueError: If the length of `q0` is not 7.
 345            ValueError: If vectorized kinematics is attempted (unsupported).
 346            ValueError: If no solution is found for the given target.
 347        """
 348        if target.shape != (4, 4):
 349            raise ValueError("target shape should be (4, 4) (got {target.shape} instead)!")
 350
 351        if q0 is not None and (len(q0) != 7):
 352            raise ValueError(f"q0 should be length 7 (got {len(q0)} instead)!")
 353
 354        if isinstance(q0, np.ndarray) and len(q0.shape) > 1:
 355            raise ValueError("Vectorized kinematics not supported!")
 356
 357        req_params = {
 358            "target": ArmEndEffector(
 359                pose=Matrix4x4(data=target.flatten().tolist()),
 360            ),
 361            "id": self._part_id,
 362        }
 363
 364        if q0 is not None:
 365            req_params["q0"] = list_to_arm_position(q0, degrees)
 366
 367        else:
 368            present_joints_positions = [
 369                joint.present_position for orbita in self._actuators.values() for joint in orbita._joints.values()
 370            ]
 371            req_params["q0"] = list_to_arm_position(present_joints_positions, degrees)
 372
 373        req = ArmIKRequest(**req_params)
 374        resp = self._stub.ComputeArmIK(req)
 375
 376        if not resp.success:
 377            raise ValueError(f"No solution found for the given target ({target})!")
 378
 379        answer: List[float] = arm_position_to_list(resp.arm_position, degrees)
 380        return answer
 381
 382    @overload
 383    def goto(
 384        self,
 385        target: List[float],
 386        duration: float = 2,
 387        wait: bool = False,
 388        interpolation_space: str = "joint_space",
 389        interpolation_mode: str = "minimum_jerk",
 390        degrees: bool = True,
 391        q0: Optional[List[float]] = None,
 392    ) -> GoToId:
 393        ...  # pragma: no cover
 394
 395    @overload
 396    def goto(
 397        self,
 398        target: npt.NDArray[np.float64],
 399        duration: float = 2,
 400        wait: bool = False,
 401        interpolation_space: str = "joint_space",
 402        interpolation_mode: str = "minimum_jerk",
 403        degrees: bool = True,
 404        q0: Optional[List[float]] = None,
 405        arc_direction: str = "above",
 406        secondary_radius: Optional[float] = None,
 407    ) -> GoToId:
 408        ...  # pragma: no cover
 409
 410    def goto(
 411        self,
 412        target: Any,
 413        duration: float = 2,
 414        wait: bool = False,
 415        interpolation_space: str = "joint_space",
 416        interpolation_mode: str = "minimum_jerk",
 417        degrees: bool = True,
 418        q0: Optional[List[float]] = None,
 419        arc_direction: str = "above",
 420        secondary_radius: Optional[float] = None,
 421    ) -> GoToId:
 422        """Move the arm to a specified target position, either in joint space or Cartesian space.
 423
 424        This function allows the arm to move to a specified target using either:
 425        - A list of 7 joint positions, or
 426        - A 4x4 pose matrix representing the desired end-effector position.
 427
 428        The function also supports an optional initial configuration `q0` for
 429        computing the inverse kinematics solution when the target is in Cartesian space.
 430
 431        Args:
 432            target: The target position. It can either be a list of 7 joint values (for joint space)
 433                    or a 4x4 NumPy array (for Cartesian space).
 434            duration: The time in seconds for the movement to be completed. Defaults to 2.
 435            wait: If True, the function waits until the movement is completed before returning.
 436                    Defaults to False.
 437            interpolation_space: The space in which the interpolation should be performed. It can
 438                    be either "joint_space" or "cartesian_space". Defaults to "joint_space".
 439            interpolation_mode: The interpolation method to be used. It can be either "minimum_jerk",
 440                    "linear" or "elliptical". Defaults to "minimum_jerk".
 441            degrees: If True, the joint values in the `target` argument are treated as degrees.
 442                    Defaults to True.
 443            q0: An optional list of 7 joint values representing the initial configuration
 444                    for inverse kinematics. Defaults to None.
 445            arc_direction: The direction of the arc to be followed during elliptical interpolation.
 446                    Can be "above", "below", "front", "back", "left" or "right" . Defaults to "above".
 447            secondary_radius: The secondary radius of the ellipse for elliptical interpolation, in meters.
 448
 449        Returns:
 450            GoToId: The unique GoToId identifier for the movement command.
 451
 452        Raises:
 453            TypeError: If the `target` is neither a list nor a pose matrix.
 454            TypeError: If the `q0` is not a list.
 455            ValueError: If the `target` list has a length other than 7, or the pose matrix is not
 456                of shape (4, 4).
 457            ValueError: If the `q0` list has a length other than 7.
 458            ValueError: If the `duration` is set to 0.
 459        """
 460        self._check_goto_parameters(target, duration, q0)
 461
 462        if self.is_off(check_gripper=False):
 463            self._logger.warning(f"{self._part_id.name} is off. Goto not sent.")
 464            return GoToId(id=-1)
 465
 466        if interpolation_space == "joint_space" and interpolation_mode == "elliptical":
 467            self._logger.warning("Elliptical interpolation is not supported in joint space. Switching to linear.")
 468            interpolation_mode = "linear"
 469        if secondary_radius is not None and secondary_radius > 0.3:
 470            self._logger.warning("Interpolation secondary_radius was too large, reduced to 0.3")
 471            secondary_radius = 0.3
 472
 473        if isinstance(target, list) and len(target) == 7:
 474            response = self._goto_joints(
 475                target,
 476                duration,
 477                interpolation_space,
 478                interpolation_mode,
 479                degrees,
 480            )
 481        elif isinstance(target, np.ndarray) and target.shape == (4, 4):
 482            response = self._goto_from_matrix(
 483                target, duration, interpolation_space, interpolation_mode, q0, arc_direction, secondary_radius
 484            )
 485
 486        if response.id == -1:
 487            self._logger.error("Target was not reachable. No command sent.")
 488        elif wait:
 489            self._wait_goto(response, duration)
 490
 491        return response
 492
 493    def _goto_joints(
 494        self,
 495        target: List[float],
 496        duration: float,
 497        interpolation_space: str,
 498        interpolation_mode: str,
 499        degrees: bool,
 500    ) -> GoToId:
 501        """Handle movement to a specified position in joint space.
 502
 503        Args:
 504            target: A list of 7 joint positions to move the arm to.
 505            duration: The time in seconds for the movement to be completed.
 506            interpolation_space: The space in which the interpolation should be performed.
 507                    Only "joint_space" is supported for joints target.
 508            interpolation_mode: The interpolation method to be used. Can be "minimum_jerk" or "linear".
 509            degrees: If True, the joint positions are interpreted as degrees; otherwise, as radians.
 510
 511        Returns:
 512            GoToId: A unique identifier for the movement command.
 513        """
 514        if isinstance(target, np.ndarray):
 515            target = target.tolist()
 516        arm_pos = list_to_arm_position(target, degrees)
 517
 518        if interpolation_space == "cartesian_space":
 519            self._logger.warning(
 520                "cartesian_space interpolation is not supported using joints target. Switching to joint_space interpolation."
 521            )
 522            interpolation_space == "joint_space"
 523        if interpolation_mode == "elliptical":
 524            self._logger.warning("Elliptical interpolation is not supported in joint space. Switching to linear.")
 525            interpolation_mode = "linear"
 526
 527        req_params = {
 528            "joints_goal": JointsGoal(
 529                arm_joint_goal=ArmJointGoal(id=self._part_id, joints_goal=arm_pos, duration=FloatValue(value=duration))
 530            ),
 531            "interpolation_space": get_grpc_interpolation_space(interpolation_space),
 532            "interpolation_mode": get_grpc_interpolation_mode(interpolation_mode),
 533        }
 534
 535        request = GoToRequest(**req_params)
 536
 537        return self._goto_stub.GoToJoints(request)
 538
 539    def _goto_from_matrix(
 540        self,
 541        target: npt.NDArray[np.float64],
 542        duration: float,
 543        interpolation_space: str,
 544        interpolation_mode: str,
 545        q0: Optional[List[float]],
 546        arc_direction: str,
 547        secondary_radius: Optional[float],
 548    ) -> GoToId:
 549        """Handle movement to a Cartesian target using a 4x4 transformation matrix.
 550
 551        This function computes and sends a command to move the arm to a Cartesian target specified by a
 552        4x4 homogeneous transformation matrix. Optionally, an initial joint configuration (`q0`) can be provided
 553        for the inverse kinematics calculation.
 554
 555        Args:
 556            target: A 4x4 NumPy array representing the Cartesian target pose.
 557            duration: The time in seconds for the movement to be completed.
 558            interpolation_space: The space in which the interpolation should be performed. Can be "joint_space"
 559                    or "cartesian_space".
 560            interpolation_mode: The interpolation method to be used. Can be "minimum_jerk", "linear" or "elliptical".
 561            q0: An optional list of 7 joint positions representing the initial configuration. Defaults to None.
 562            arc_direction: The direction of the arc to be followed during elliptical interpolation. Can be "above",
 563                    "below", "front", "back", "left" or "right".
 564            secondary_radius: The secondary radius of the ellipse for elliptical interpolation, in meters.
 565
 566        Returns:
 567            GoToId: A unique identifier for the movement command.
 568
 569        Raises:
 570            ValueError: If the length of `q0` is not 7.
 571        """
 572        goal_pose = Matrix4x4(data=target.flatten().tolist())
 573
 574        req_params = {
 575            "cartesian_goal": CartesianGoal(
 576                arm_cartesian_goal=ArmCartesianGoal(
 577                    id=self._part_id,
 578                    goal_pose=goal_pose,
 579                    duration=FloatValue(value=duration),
 580                    q0=list_to_arm_position(q0) if q0 is not None else None,
 581                )
 582            ),
 583            "interpolation_space": get_grpc_interpolation_space(interpolation_space),
 584            "interpolation_mode": get_grpc_interpolation_mode(interpolation_mode),
 585        }
 586
 587        if interpolation_mode == "elliptical":
 588            ellipse_params = {
 589                "arc_direction": get_grpc_arc_direction(arc_direction),
 590            }
 591            if secondary_radius is not None:
 592                ellipse_params["secondary_radius"] = FloatValue(value=secondary_radius)
 593            elliptical_params = EllipticalGoToParameters(**ellipse_params)
 594            req_params["elliptical_parameters"] = elliptical_params
 595
 596        request = GoToRequest(**req_params)
 597
 598        return self._goto_stub.GoToCartesian(request)
 599
 600    def _check_goto_parameters(self, target: Any, duration: Optional[float] = 0, q0: Optional[List[float]] = None) -> None:
 601        """Check the validity of the parameters for the `goto` method.
 602
 603        Args:
 604            duration: The time in seconds for the movement to be completed.
 605            target: The target position, either a list of joint positions or a 4x4 pose matrix.
 606            q0: An optional initial joint configuration for inverse kinematics. Defaults to None.
 607
 608        Raises:
 609            TypeError: If the target is not a list or a NumPy matrix.
 610            ValueError: If the target list has a length other than 7, or the pose matrix is not of
 611                shape (4, 4).
 612            ValueError: If the duration is set to 0.
 613            ValueError: If the length of `q0` is not 7.
 614        """
 615        if not (isinstance(target, list) or isinstance(target, np.ndarray)):
 616            raise TypeError(f"Invalid target: must be either a list or a np matrix, got {type(target)} instead.")
 617
 618        elif isinstance(target, list) and not (len(target) == 7):
 619            raise ValueError(f"The joints list should be of length 7 (got {len(target)} instead).")
 620        elif isinstance(target, np.ndarray) and not (target.shape == (4, 4)):
 621            raise ValueError(f"The pose matrix should be of shape (4, 4) (got {target.shape} instead).")
 622
 623        elif q0 is not None:
 624            if not isinstance(q0, list):
 625                raise TypeError("Invalid q0: must be a list.")
 626            elif len(q0) != 7:
 627                raise ValueError(f"q0 should be of length 7 (got {len(q0)} instead)!")
 628
 629        elif duration == 0:
 630            raise ValueError("duration cannot be set to 0.")
 631
 632    def _goto_single_joint(
 633        self,
 634        arm_joint: int,
 635        goal_position: float,
 636        duration: float = 2,
 637        wait: bool = False,
 638        interpolation_mode: str = "minimum_jerk",
 639        degrees: bool = True,
 640    ) -> GoToId:
 641        """Move a single joint of the arm to a specified position.
 642
 643        The function allows for optional parameters for duration, interpolation mode, and waiting for completion.
 644
 645        Args:
 646            arm_joint: The specific joint of the arm to move, identified by an integer value.
 647            goal_position: The target position for the specified arm joint, given as a float.
 648                The value can be in radians or degrees, depending on the `degrees` parameter.
 649            duration: The time duration in seconds for the joint to reach the specified goal
 650                position. Defaults to 2.
 651            wait: Determines whether the program should wait for the movement to finish before
 652                returning. If set to `True`, the program waits for the movement to complete before continuing
 653                execution. Defaults to `False`.
 654            interpolation_mode: The type of interpolation to use when moving the arm's joint.
 655                Can be 'minimum_jerk' or 'linear'. Defaults to 'minimum_jerk'.
 656            degrees: Specifies whether the joint positions are in degrees. If set to `True`,
 657                the goal position is interpreted as degrees. Defaults to `True`.
 658
 659        Returns:
 660            A unique GoToId identifier corresponding to this specific goto movement.
 661        """
 662        if degrees:
 663            goal_position = np.deg2rad(goal_position)
 664        request = GoToRequest(
 665            joints_goal=JointsGoal(
 666                custom_joint_goal=CustomJointGoal(
 667                    id=self._part_id,
 668                    arm_joints=CustomArmJoints(joints=[arm_joint]),
 669                    joints_goals=[FloatValue(value=goal_position)],
 670                    duration=FloatValue(value=duration),
 671                )
 672            ),
 673            interpolation_mode=get_grpc_interpolation_mode(interpolation_mode),
 674        )
 675        response = self._goto_stub.GoToJoints(request)
 676
 677        if response.id == -1:
 678            self._logger.error(f"Position {goal_position} was not reachable. No command sent.")
 679        elif wait:
 680            self._wait_goto(response, duration)
 681        return response
 682
 683    def goto_posture(
 684        self,
 685        common_posture: str = "default",
 686        duration: float = 2,
 687        wait: bool = False,
 688        wait_for_goto_end: bool = True,
 689        interpolation_mode: str = "minimum_jerk",
 690        open_gripper: bool = False,
 691    ) -> GoToId:
 692        """Send all joints to standard positions with optional parameters for duration, waiting, and interpolation mode.
 693
 694        Args:
 695            common_posture: The standard positions to which all joints will be sent.
 696                It can be 'default' or 'elbow_90'. Defaults to 'default'.
 697            duration: The time duration in seconds for the robot to move to the specified posture.
 698                Defaults to 2.
 699            wait: Determines whether the program should wait for the movement to finish before
 700                returning. If set to `True`, the program waits for the movement to complete before continuing
 701                execution. Defaults to `False`.
 702            wait_for_goto_end: Specifies whether commands will be sent to a part immediately or
 703                only after all previous commands in the queue have been executed. If set to `False`, the program
 704                will cancel all executing moves and queues. Defaults to `True`.
 705            interpolation_mode: The type of interpolation used when moving the arm's joints.
 706                Can be 'minimum_jerk' or 'linear'. Defaults to 'minimum_jerk'.
 707            open_gripper: If `True`, the gripper will open, if `False`, it stays in its current position.
 708                Defaults to `False`.
 709
 710        Returns:
 711            A unique GoToId identifier for this specific movement.
 712        """
 713        joints = self.get_default_posture_joints(common_posture=common_posture)
 714        if self._gripper is not None and self._gripper.is_on() and open_gripper:
 715            self._gripper.open()
 716        if not wait_for_goto_end:
 717            self.cancel_all_goto()
 718        if self.is_on():
 719            return self.goto(joints, duration, wait, interpolation_mode=interpolation_mode)
 720        else:
 721            self._logger.warning(f"{self._part_id.name} is off. No command sent.")
 722        return GoToId(id=-1)
 723
 724    def get_default_posture_joints(self, common_posture: str = "default") -> List[float]:
 725        """Get the list of joint positions for default or elbow_90 poses.
 726
 727        Args:
 728            common_posture: The name of the posture to retrieve. Can be "default" or "elbow_90".
 729                Defaults to "default".
 730
 731        Returns:
 732            A list of joint positions in degrees for the specified posture.
 733
 734        Raises:
 735            ValueError: If `common_posture` is not "default" or "elbow_90".
 736        """
 737        if common_posture not in ["default", "elbow_90"]:
 738            raise ValueError(f"common_posture {common_posture} not supported! Should be 'default' or 'elbow_90'")
 739        if common_posture == "elbow_90":
 740            elbow_pitch = -90
 741        else:
 742            elbow_pitch = 0
 743        if self._part_id.name == "r_arm":
 744            return [0, 10, -10, elbow_pitch, 0, 0, 0]
 745        else:
 746            return [0, -10, 10, elbow_pitch, 0, 0, 0]
 747
 748    def get_default_posture_matrix(self, common_posture: str = "default") -> npt.NDArray[np.float64]:
 749        """Get the 4x4 pose matrix in Reachy coordinate system for a default robot posture.
 750
 751        Args:
 752            common_posture: The posture to retrieve. Can be "default" or "elbow_90".
 753                Defaults to "default".
 754
 755        Returns:
 756            The 4x4 homogeneous pose matrix for the specified posture in Reachy coordinate system.
 757        """
 758        joints = self.get_default_posture_joints(common_posture)
 759        return self.forward_kinematics(joints)
 760
 761    def get_translation_by(
 762        self,
 763        x: float,
 764        y: float,
 765        z: float,
 766        initial_pose: Optional[npt.NDArray[np.float64]] = None,
 767        frame: str = "robot",
 768    ) -> npt.NDArray[np.float64]:
 769        """Return a 4x4 matrix representing a pose translated by specified x, y, z values.
 770
 771        The translation is performed in either the robot or gripper coordinate system.
 772
 773        Args:
 774            x: Translation along the x-axis in meters (forwards direction) to apply
 775                to the pose matrix.
 776            y: Translation along the y-axis in meters (left direction) to apply
 777                to the pose matrix.
 778            z: Translation along the z-axis in meters (upwards direction) to apply
 779                to the pose matrix.
 780            initial_pose: A 4x4 matrix representing the initial pose of the end-effector in Reachy coordinate system,
 781                expressed as a NumPy array of type `np.float64`.
 782                If not provided, the current pose of the arm is used. Defaults to `None`.
 783            frame: The coordinate system in which the translation should be performed.
 784                Can be either "robot" or "gripper". Defaults to "robot".
 785
 786        Returns:
 787            A 4x4 pose matrix, expressed in Reachy coordinate system,
 788            translated by the specified x, y, z values from the initial pose.
 789
 790        Raises:
 791            ValueError: If the `frame` is not "robot" or "gripper".
 792        """
 793        if frame not in ["robot", "gripper"]:
 794            raise ValueError(f"Unknown frame {frame}! Should be 'robot' or 'gripper'")
 795
 796        if initial_pose is None:
 797            initial_pose = self.forward_kinematics()
 798
 799        pose = initial_pose.copy()
 800
 801        if frame == "robot":
 802            pose[0, 3] += x
 803            pose[1, 3] += y
 804            pose[2, 3] += z
 805        elif frame == "gripper":
 806            pose = translate_in_self(initial_pose, [x, y, z])
 807        return pose
 808
 809    def translate_by(
 810        self,
 811        x: float,
 812        y: float,
 813        z: float,
 814        duration: float = 2,
 815        wait: bool = False,
 816        frame: str = "robot",
 817        interpolation_space: str = "cartesian_space",
 818        interpolation_mode: str = "minimum_jerk",
 819        arc_direction: str = "above",
 820        secondary_radius: Optional[float] = None,
 821    ) -> GoToId:
 822        """Create a translation movement for the arm's end effector.
 823
 824        The movement is based on the last sent position or the current position.
 825
 826        Args:
 827            x: Translation along the x-axis in meters (forwards direction) to apply
 828                to the pose matrix.
 829            y: Translation along the y-axis in meters (left direction) to apply
 830                to the pose matrix.
 831            z: Translation along the z-axis in meters (vertical direction) to apply
 832                to the pose matrix.
 833            duration: Time duration in seconds for the translation movement to be completed.
 834                Defaults to 2.
 835            wait: Determines whether the program should wait for the movement to finish before
 836                returning. If set to `True`, the program waits for the movement to complete before continuing
 837                execution. Defaults to `False`.
 838            frame: The coordinate system in which the translation should be performed.
 839                Can be "robot" or "gripper". Defaults to "robot".
 840            interpolation_mode: The type of interpolation to be used when moving the arm's
 841                joints. Can be 'minimum_jerk' or 'linear'. Defaults to 'minimum_jerk'.
 842
 843        Returns:
 844            The GoToId of the movement command, created using the `goto_from_matrix` method with the
 845            translated pose computed in the specified frame.
 846
 847        Raises:
 848            ValueError: If the `frame` is not "robot" or "gripper".
 849        """
 850        try:
 851            goto = self.get_goto_queue()[-1]
 852        except IndexError:
 853            goto = self.get_goto_playing()
 854
 855        if goto.id != -1:
 856            joints_request = self._get_goto_request(goto)
 857        else:
 858            joints_request = None
 859
 860        if joints_request is not None:
 861            if joints_request.request.target.joints is not None:
 862                pose = self.forward_kinematics(joints_request.request.target.joints)
 863            else:
 864                pose = joints_request.request.target.pose
 865        else:
 866            pose = self.forward_kinematics()
 867
 868        pose = self.get_translation_by(x, y, z, initial_pose=pose, frame=frame)
 869        return self.goto(
 870            pose,
 871            duration=duration,
 872            wait=wait,
 873            interpolation_space=interpolation_space,
 874            interpolation_mode=interpolation_mode,
 875            arc_direction=arc_direction,
 876            secondary_radius=secondary_radius,
 877        )
 878
 879    def get_rotation_by(
 880        self,
 881        roll: float,
 882        pitch: float,
 883        yaw: float,
 884        initial_pose: Optional[npt.NDArray[np.float64]] = None,
 885        degrees: bool = True,
 886        frame: str = "robot",
 887    ) -> npt.NDArray[np.float64]:
 888        """Calculate a new pose matrix by rotating an initial pose matrix by specified roll, pitch, and yaw angles.
 889
 890        The rotation is performed in either the robot or gripper coordinate system.
 891
 892        Args:
 893            roll: Rotation around the x-axis in the Euler angles representation, specified
 894                in radians or degrees (based on the `degrees` parameter).
 895            pitch: Rotation around the y-axis in the Euler angles representation, specified
 896                in radians or degrees (based on the `degrees` parameter).
 897            yaw: Rotation around the z-axis in the Euler angles representation, specified
 898                in radians or degrees (based on the `degrees` parameter).
 899            initial_pose: A 4x4 matrix representing the initial
 900                pose of the end-effector, expressed as a NumPy array of type `np.float64`. If not provided,
 901                the current pose of the arm is used. Defaults to `None`.
 902            degrees: Specifies whether the rotation angles are provided in degrees. If set to
 903                `True`, the angles are interpreted as degrees. Defaults to `True`.
 904            frame: The coordinate system in which the rotation should be performed. Can be
 905                "robot" or "gripper". Defaults to "robot".
 906
 907        Returns:
 908            A 4x4 pose matrix, expressed in the Reachy coordinate system, rotated
 909            by the specified roll, pitch, and yaw angles from the initial pose, in the specified frame.
 910
 911        Raises:
 912            ValueError: If the `frame` is not "robot" or "gripper".
 913        """
 914        if frame not in ["robot", "gripper"]:
 915            raise ValueError(f"Unknown frame {frame}! Should be 'robot' or 'gripper'")
 916
 917        if initial_pose is None:
 918            initial_pose = self.forward_kinematics()
 919
 920        pose = initial_pose.copy()
 921        rotation = matrix_from_euler_angles(roll, pitch, yaw, degrees=degrees)
 922
 923        if frame == "robot":
 924            pose_rotation = np.eye(4)
 925            pose_rotation[:3, :3] = pose.copy()[:3, :3]
 926            pose_translation = pose.copy()[:3, 3]
 927            pose_rotation = rotation @ pose_rotation
 928            pose = recompose_matrix(pose_rotation[:3, :3], pose_translation)
 929        elif frame == "gripper":
 930            pose = rotate_in_self(initial_pose, [roll, pitch, yaw], degrees=degrees)
 931
 932        return pose
 933
 934    def rotate_by(
 935        self,
 936        roll: float,
 937        pitch: float,
 938        yaw: float,
 939        duration: float = 2,
 940        wait: bool = False,
 941        degrees: bool = True,
 942        frame: str = "robot",
 943        interpolation_mode: str = "minimum_jerk",
 944    ) -> GoToId:
 945        """Create a rotation movement for the arm's end effector based on the specified roll, pitch, and yaw angles.
 946
 947        The rotation is performed in either the robot or gripper frame.
 948
 949        Args:
 950            roll: Rotation around the x-axis in the Euler angles representation, specified
 951                in radians or degrees (based on the `degrees` parameter).
 952            pitch: Rotation around the y-axis in the Euler angles representation, specified
 953                in radians or degrees (based on the `degrees` parameter).
 954            yaw: Rotation around the z-axis in the Euler angles representation, specified
 955                in radians or degrees (based on the `degrees` parameter).
 956            duration: Time duration in seconds for the rotation movement to be completed.
 957                Defaults to 2.
 958            wait: Determines whether the program should wait for the movement to finish before
 959                returning. If set to `True`, the program waits for the movement to complete before continuing
 960                execution. Defaults to `False`.
 961            degrees: Specifies whether the rotation angles are provided in degrees. If set to
 962                `True`, the angles are interpreted as degrees. Defaults to `True`.
 963            frame: The coordinate system in which the rotation should be performed. Can be
 964                "robot" or "gripper". Defaults to "robot".
 965            interpolation_mode: The type of interpolation to be used when moving the arm's
 966                joints. Can be 'minimum_jerk' or 'linear'. Defaults to 'minimum_jerk'.
 967
 968        Returns:
 969            The GoToId of the movement command, created by calling the `goto_from_matrix` method with
 970            the rotated pose computed in the specified frame.
 971
 972        Raises:
 973            ValueError: If the `frame` is not "robot" or "gripper".
 974        """
 975        if frame not in ["robot", "gripper"]:
 976            raise ValueError(f"Unknown frame {frame}! Should be 'robot' or 'gripper'")
 977
 978        try:
 979            goto = self.get_goto_queue()[-1]
 980        except IndexError:
 981            goto = self.get_goto_playing()
 982
 983        if goto.id != -1:
 984            joints_request = self._get_goto_request(goto)
 985        else:
 986            joints_request = None
 987
 988        if joints_request is not None:
 989            if joints_request.request.target.joints is not None:
 990                pose = self.forward_kinematics(joints_request.request.target.joints)
 991            else:
 992                pose = joints_request.request.target.pose
 993        else:
 994            pose = self.forward_kinematics()
 995
 996        pose = self.get_rotation_by(roll, pitch, yaw, initial_pose=pose, degrees=degrees, frame=frame)
 997        return self.goto(pose, duration=duration, wait=wait, interpolation_mode=interpolation_mode)
 998
 999    # @property
1000    # def joints_limits(self) -> ArmLimits:
1001    #     """Get limits of all the part's joints"""
1002    #     limits = self._arm_stub.GetJointsLimits(self._part_id)
1003    #     return limits
1004
1005    # @property
1006    # def temperatures(self) -> ArmTemperatures:
1007    #     """Get temperatures of all the part's motors"""
1008    #     temperatures = self._arm_stub.GetTemperatures(self._part_id)
1009    #     return temperatures
1010
1011    def _get_goal_positions_message(self) -> ArmComponentsCommands:
1012        """Get the ArmComponentsCommands message to send the goal positions to the actuator."""
1013        commands = {}
1014        for actuator_name, actuator in self._actuators.items():
1015            if actuator_name != "gripper":
1016                actuator_command = actuator._get_goal_positions_message()
1017                if actuator_command is not None:
1018                    commands[f"{actuator_name}_command"] = actuator_command
1019        return ArmComponentsCommands(**commands)
1020
1021    def _clean_outgoing_goal_positions(self) -> None:
1022        """Clean the outgoing goal positions."""
1023        for actuator in [self._actuators[act] for act in self._actuators.keys() if act not in ["gripper"]]:
1024            actuator._clean_outgoing_goal_positions()
1025
1026    def _post_send_goal_positions(self) -> None:
1027        """Monitor the joint positions to check if they reach the specified goals."""
1028        for actuator in [self._actuators[act] for act in self._actuators.keys() if act not in ["gripper"]]:
1029            actuator._post_send_goal_positions()
1030
1031    def send_goal_positions(self, check_positions: bool = False) -> None:
1032        """Send goal positions to the arm's joints, including the gripper.
1033
1034        If goal positions have been specified for any joint of the part, sends them to the robot.
1035
1036        Args :
1037            check_positions: A boolean indicating whether to check the positions after sending the command.
1038                Defaults to True.
1039        """
1040        super().send_goal_positions(check_positions)
1041        if self.gripper is not None:
1042            self.gripper.send_goal_positions(check_positions)
1043
1044    def _update_with(self, new_state: ArmState) -> None:
1045        """Update the arm with a newly received (partial) state from the gRPC server.
1046
1047        Updating the shoulder, elbow, and wrist states accordingly.
1048
1049        Args:
1050            new_state: current state of the arm, including the states of the shoulder, elbow, and wrist.
1051        """
1052        self.shoulder._update_with(new_state.shoulder_state)
1053        self.elbow._update_with(new_state.elbow_state)
1054        self.wrist._update_with(new_state.wrist_state)
1055
1056    def _update_audit_status(self, new_status: ArmStatus) -> None:
1057        """Update the audit status of different components based on a new overall status.
1058
1059        Args:
1060            new_status: new status of the shoulder, elbow, and  wrist.
1061        """
1062        self.shoulder._update_audit_status(new_status.shoulder_status)
1063        self.elbow._update_audit_status(new_status.elbow_status)
1064        self.wrist._update_audit_status(new_status.wrist_status)