7 add head movements functions

src/reachy_v2_sdk/head.py

@@ -6,11 +6,13 @@
 """
 import grpc
 
-from pyquaternion import Quaternion
+from pyquaternion import Quaternion as Quat
 
 from reachy_sdk_api_v2.head_pb2_grpc import HeadServiceStub
 from reachy_sdk_api_v2.head_pb2 import Head as Head_proto, HeadState
+from reachy_sdk_api_v2.head_pb2 import HeadTargetPoint, NeckGoal
 from reachy_sdk_api_v2.part_pb2 import PartId
+from reachy_sdk_api_v2.kinematics_pb2 import Point, Rotation3D, Quaternion, EulerAngles
 
 from .orbita3d import Orbita3d
 from .dynamixel_motor import DynamixelMotor
@@ -51,13 +53,18 @@ def _setup_head(self, head: Head_proto, initial_state: HeadState) -> None:
         )
 
     def look_at(self, x: float, y: float, z: float, duration: float) -> None:
-        pass
+        req = HeadTargetPoint(id=self.part_id, point=Point(x=x, y=y, z=z))
+        self._head_stub.LookAt(req)
 
-    def orient(self, q: Quaternion, duration: float) -> None:
-        pass
+    def orient(self, q: Quat, duration: float) -> None:
+        req = NeckGoal(id=self.part_id, rotation=Rotation3D(q=Quaternion(w=q.w, x=q.x, y=q.y, z=q.z)), duration=duration)
+        self._head_stub.GoToOrientation(req)
 
     def rotate_to(self, roll: float, pitch: float, yaw: float, duration: float) -> None:
-        pass
+        req = NeckGoal(
+            id=self.part_id, rotation=Rotation3D(rpy=EulerAngles(roll=roll, pitch=pitch, yaw=yaw)), duration=duration
+        )
+        self._head_stub.GoToOrientation(req)
 
     def turn_on(self) -> None:
         self._head_stub.TurnOn(self.part_id)