Quaternion

https://youtu.be/d4EgbgTm0Bg

https://youtu.be/zjMuIxRvygQ

How Reachy moves the head to look at point in space using quaternions

look_at() from the Head:

def look_at(self, x, y, z, duration, wait):
        """Make the head look at a 3D point in space.
        Args:
            x (float): x coordinates in space
            y (float): y coordinates in space
            z (float): z coordinates in space
            duration (float): move duration (in seconds)
            wait (bool): whether or not to wait for the end of the motion
        """
        q = self.neck.model.find_quaternion_transform([1, 0, 0], [x, y, z])
        return self.neck.orient(q, duration=duration, wait=wait)

First based on a point in space defined by (x, y, z), use find_quaternion_transform() to find the quaternion.

find_quaternion_transform() from the Actuator:

def find_quaternion_transform(self, vect_origin, vect_target):
        vo = np.array(vect_origin)
        if np.any(vo):
            vo = vo / LA.norm(vo)

        vt = np.array(vect_target)
        if np.any(vt):
            vt = vt / LA.norm(vt)

        V = np.cross(vo, vt)
        if np.any(V):
            V = V / LA.norm(V)

        alpha = np.arccos(np.dot(vo, vt))
        if np.isnan(alpha) or alpha < 1e-6:
            return Quaternion(1, 0, 0, 0)

        return Quaternion(axis=V, radians=alpha)

1. Normalize the vect_origin and vect_target vectors if not 0
2. Calculate the cross product of the normalized vect_origin and vect_target vectors; normalize the cross product if not 0. This is the axis, V.
3. Calculate the angle, alpha, based on tthe normalized vect_origin and vect_target vectors
4. If alpha is is not a number or is very small, return the multiplicative identity [1, (0, 0, 0)]
5. Return the quaternion based on the axis, V and the angle in radians, alpha

orient() from OrbitaActuator:

def orient(self, quat, duration, wait):
        """Orient orbita given a quaternion.
        Args:
            quat (pyquaternion.Quaterion): quaternion goal orientation
            duration (float): move duration (in seconds)
            wait (bool): whether or not to wait for the end of the motion
        Returns:
            reachy.trajectory.TrajectoryPlayer: trajectory player that can be used to monitor the trajectory, stop it, etc
        """
        thetas = self.model.get_angles_from_quaternion(quat.w, quat.x, quat.y, quat.z)
        # We used a reversed encoder so we need to inverse the angles
        return self.goto(thetas, duration=duration, wait=wait, interpolation_mode='minjerk')