diff --git a/src/reachy2_sdk/parts/head.py b/src/reachy2_sdk/parts/head.py index f5fe618f..993fab62 100644 --- a/src/reachy2_sdk/parts/head.py +++ b/src/reachy2_sdk/parts/head.py @@ -29,10 +29,9 @@ ) from reachy2_sdk_api.head_pb2_grpc import HeadServiceStub from reachy2_sdk_api.kinematics_pb2 import ExtEulerAngles, Point, Quaternion, Rotation3d -from scipy.spatial.transform import Rotation as R from ..orbita.orbita3d import Orbita3d -from ..utils.utils import get_grpc_interpolation_mode +from ..utils.utils import get_grpc_interpolation_mode, quaternion_from_euler from .goto_based_part import IGoToBasedPart from .joints_based_part import JointsBasedPart @@ -135,7 +134,8 @@ def goto( wait: bool = False, interpolation_mode: str = "minimum_jerk", degrees: bool = True, - ) -> GoToId: ... + ) -> GoToId: + ... @overload def goto( @@ -145,7 +145,8 @@ def goto( wait: bool = False, interpolation_mode: str = "minimum_jerk", degrees: bool = True, - ) -> GoToId: ... + ) -> GoToId: + ... def goto( self, @@ -355,27 +356,14 @@ def rotate_by( if not degrees: roll, pitch, yaw = np.rad2deg([roll, pitch, yaw]) - actual_rpy = self.get_current_positions() - current_rotation = R.from_euler("XYZ", actual_rpy, degrees=True) - + current_quaternion = self.get_current_orientation() + additional_quaternion = quaternion_from_euler(roll, pitch, yaw, degrees=True) if frame == "head": - additional_rotation = R.from_euler("xyz", [roll, pitch, yaw], degrees=True) - new_rotation = current_rotation * additional_rotation + target = current_quaternion * additional_quaternion elif frame == "robot": - additional_rotation = R.from_euler("XYZ", [roll, pitch, yaw], degrees=True) - new_rotation = additional_rotation * current_rotation - - target_rpy = new_rotation.as_euler("XYZ", degrees=True) - target = np.deg2rad(target_rpy).tolist() - joints_goal = NeckOrientation( - rotation=Rotation3d( - rpy=ExtEulerAngles( - roll=FloatValue(value=target[0]), - pitch=FloatValue(value=target[1]), - yaw=FloatValue(value=target[2]), - ) - ) - ) + target = additional_quaternion * current_quaternion + + joints_goal = NeckOrientation(rotation=Rotation3d(q=Quaternion(w=target.w, x=target.x, y=target.y, z=target.z))) request = GoToRequest( joints_goal=JointsGoal( diff --git a/src/reachy2_sdk/utils/utils.py b/src/reachy2_sdk/utils/utils.py index 3336b331..6ac4dc8f 100644 --- a/src/reachy2_sdk/utils/utils.py +++ b/src/reachy2_sdk/utils/utils.py @@ -293,6 +293,34 @@ def get_pose_matrix(position: List[float], rotation: List[float], degrees: bool return pose +def quaternion_from_euler(roll: float, pitch: float, yaw: float, degrees: bool = True) -> Quaternion: + """Convert Euler angles to a quaternion using the pyquaternion library.""" + if degrees: + roll = np.deg2rad(roll) + pitch = np.deg2rad(pitch) + yaw = np.deg2rad(yaw) + + qx = Quaternion(axis=[1, 0, 0], angle=roll) + qy = Quaternion(axis=[0, 1, 0], angle=pitch) + qz = Quaternion(axis=[0, 0, 1], angle=yaw) + + quaternion = qz * qy * qx + + return quaternion + + +def euler_from_quaternion(quaternion: Quaternion, degrees: bool = True) -> Tuple[float, float, float]: + """Convert a quaternion back to Euler angles.""" + yaw, pitch, roll = quaternion.yaw_pitch_roll + + if degrees: + roll = np.rad2deg(roll) + pitch = np.rad2deg(pitch) + yaw = np.rad2deg(yaw) + + return roll, pitch, yaw + + def rotate_in_self(frame: npt.NDArray[np.float64], rotation: List[float], degrees: bool = True) -> npt.NDArray[np.float64]: """Return a new homogeneous 4x4 pose matrix that is the input matrix rotated in itself. diff --git a/tests/units/online/test_advanced_goto_functions.py b/tests/units/online/test_advanced_goto_functions.py index 0a4057ce..82579030 100644 --- a/tests/units/online/test_advanced_goto_functions.py +++ b/tests/units/online/test_advanced_goto_functions.py @@ -852,36 +852,35 @@ def test_rotate_by_gripper_frame(reachy_sdk_zeroed: ReachySDK) -> None: @pytest.mark.online def test_head_rotation(reachy_sdk_zeroed: ReachySDK) -> None: - reachy_sdk_zeroed.head.goto_posture("default") - reachy_sdk_zeroed.head.rotate_by(roll=-10, yaw=90, frame="robot") + reachy_sdk_zeroed.head.goto([0, 0, 90], duration=1, wait=True) current_orientation = reachy_sdk_zeroed.head.get_current_positions() - assert np.allclose(current_orientation, [0, 0, 90], atol=1e-03) + assert np.allclose(current_orientation, [0, 0, 90], atol=5) - reachy_sdk_zeroed.head.rotate_by(roll=30, frame="head") + reachy_sdk_zeroed.head.rotate_by(roll=30, duration=1, frame="head", wait=True) current_orientation = reachy_sdk_zeroed.head.get_current_positions() - assert np.allclose(current_orientation, [0, 30, 90], atol=1e-03) + assert np.allclose(current_orientation, [0, 30, 90], atol=5) - reachy_sdk_zeroed.head.rotate_by(roll=-30, frame="head") + reachy_sdk_zeroed.head.rotate_by(roll=-30, duration=1, frame="head", wait=True) current_orientation = reachy_sdk_zeroed.head.get_current_positions() - assert np.allclose(current_orientation, [0, 0, 90], atol=1e-03) + assert np.allclose(current_orientation, [0, 0, 90], atol=5) - reachy_sdk_zeroed.head.rotate_by(roll=30, frame="robot") + reachy_sdk_zeroed.head.rotate_by(roll=30, duration=1, frame="robot", wait=True) current_orientation = reachy_sdk_zeroed.head.get_current_positions() - assert np.allclose(current_orientation, [30, 0, 90], atol=1e-03) + assert np.allclose(current_orientation, [30, 0, 90], atol=5) - reachy_sdk_zeroed.head.rotate_by(roll=-30, frame="robot") - reachy_sdk_zeroed.head.rotate_by(pitch=30, frame="head") + reachy_sdk_zeroed.head.rotate_by(roll=-30, duration=1, frame="robot", wait=True) + reachy_sdk_zeroed.head.rotate_by(pitch=30, duration=1, frame="head", wait=True) current_orientation = reachy_sdk_zeroed.head.get_current_positions() - assert np.allclose(current_orientation, [-30, 0, 90], atol=1e-03) + assert np.allclose(current_orientation, [-30, 0, 90], atol=5) - reachy_sdk_zeroed.head.rotate_by(pitch=-30, frame="head") - reachy_sdk_zeroed.head.rotate_by(pitch=30, frame="robot") + reachy_sdk_zeroed.head.rotate_by(pitch=-30, duration=1, frame="head", wait=True) + reachy_sdk_zeroed.head.rotate_by(pitch=30, duration=1, frame="robot", wait=True) current_orientation = reachy_sdk_zeroed.head.get_current_positions() - assert np.allclose(current_orientation, [0, 30, 90], atol=1e-03) + assert np.allclose(current_orientation, [0, 30, 90], atol=5) - reachy_sdk_zeroed.head.rotate_by(yaw=30, frame="head") + reachy_sdk_zeroed.head.rotate_by(yaw=30, duration=1, frame="head", wait=True) current_orientation = reachy_sdk_zeroed.head.get_current_positions() - assert np.allclose(current_orientation, [0, 30, 120], atol=1e-03) + assert np.allclose(current_orientation, [0, 30, 120], atol=5) with pytest.raises(ValueError): reachy_sdk_zeroed.head.rotate_by(roll=30, frame="head", duration=0)