Fondamentaux du quaternion

Objectif : Apprendre les bases de l’utilisation des quaternions dans ROS 2.

Niveau du didacticiel : Intermédiaire

Durée : 10 minutes

Contenu

Arrière-plan
Conditions préalables
Composants d’un quaternion
Types de quaternions dans ROS 2
Opérations de quaternions
Résumé

Arrière-plan 

Un quaternion est une représentation à 4 tuples de l’orientation, qui est plus concise qu’une matrice de rotation. Les quaternions sont très efficaces pour analyser des situations où des rotations en trois dimensions sont impliquées. Les quaternions sont largement utilisés dans la robotique, la mécanique quantique, la vision par ordinateur et l’animation 3D.

Vous pouvez en savoir plus sur le concept mathématique sous-jacent sur Wikipedia. Vous pouvez également jeter un œil à une série de vidéos explorables Visualizing quaternions réalisées par 3blue1brown.

Dans ce didacticiel, vous apprendrez comment les quaternions et les méthodes de conversion fonctionnent dans ROS 2.

Conditions préalables 

Cependant, ce n’est pas une exigence stricte et vous pouvez vous en tenir à toute autre bibliothèque de transformation géométrique qui vous convient le mieux. Vous pouvez jeter un œil à des bibliothèques comme transforms3d, scipy.spatial.transform, pytransform3d, numpy-quaternion ou blender.mathutils.

Composants d’un quaternion 

ROS 2 utilise des quaternions pour suivre et appliquer des rotations. Un quaternion a 4 composantes (x, y, z, w). Dans ROS 2, w est en dernier, mais dans certaines bibliothèques comme Eigen, w peut être placé en première position. Le quaternion unitaire couramment utilisé qui ne produit aucune rotation autour des axes x/y/z est (0, 0, 0, 1), et peut être créé de la manière suivante :

#include <tf2/LinearMath/Quaternion.h>
...

tf2::Quaternion q;
// Create a quaternion from roll/pitch/yaw in radians (0, 0, 0)
q.setRPY(0, 0, 0);
// Print the quaternion components (0, 0, 0, 1)
RCLCPP_INFO(this->get_logger(), "%f %f %f %f",
            q.x(), q.y(), q.z(), q.w());

La magnitude d’un quaternion doit toujours être un. Si des erreurs numériques provoquent une magnitude de quaternion autre que un, ROS 2 imprimera des avertissements. Pour éviter ces avertissements, normalisez le quaternion :

q.normalize();

Types de quaternions dans ROS 2 

ROS 2 utilise deux types de données quaternions : tf2::Quaternion et son équivalent geometry_msgs::msg::Quaternion. Pour convertir entre eux en C++, utilisez les méthodes de tf2_geometry_msgs.

C++

#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
...

tf2::Quaternion tf2_quat, tf2_quat_from_msg;
tf2_quat.setRPY(roll, pitch, yaw);
// Convert tf2::Quaternion to geometry_msgs::msg::Quaternion
geometry_msgs::msg::Quaternion msg_quat = tf2::toMsg(tf2_quat);

// Convert geometry_msgs::msg::Quaternion to tf2::Quaternion
tf2::convert(msg_quat, tf2_quat_from_msg);
// or
tf2::fromMsg(msg_quat, tf2_quat_from_msg);

Python

from geometry_msgs.msg import Quaternion
...

# Create a list of floats, which is compatible with tf2
# Quaternion methods
quat_tf = [0.0, 1.0, 0.0, 0.0]

# Convert a list to geometry_msgs.msg.Quaternion
msg_quat = Quaternion(x=quat_tf[0], y=quat_tf[1], z=quat_tf[2], w=quat_tf[3])

Opérations de quaternions 

1 Pensez en RPY puis convertissez en quaternion 

Il est facile pour nous de penser à des rotations autour d’axes, mais difficile de penser en termes de quaternions. Une suggestion est de calculer les rotations cibles en termes de roulis (autour d’un axe X), de tangage (autour de l’axe Y) et de lacet (autour de l’axe Z), puis de les convertir en un quaternion.

# quaternion_from_euler method is available in turtle_tf2_py/turtle_tf2_py/turtle_tf2_broadcaster.py
q = quaternion_from_euler(1.5707, 0, -1.5707)
print(f'The quaternion representation is x: {q[0]} y: {q[1]} z: {q[2]} w: {q[3]}.')

2 Appliquer une rotation de quaternion 

Pour appliquer la rotation d’un quaternion à une pose, il suffit de multiplier le quaternion précédent de la pose par le quaternion représentant la rotation souhaitée. L’ordre de cette multiplication est important.

C++

#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
...

tf2::Quaternion q_orig, q_rot, q_new;

q_orig.setRPY(0.0, 0.0, 0.0);
// Rotate the previous pose by 180* about X
q_rot.setRPY(3.14159, 0.0, 0.0);
q_new = q_rot * q_orig;
q_new.normalize();

Python

q_orig = quaternion_from_euler(0, 0, 0)
# Rotate the previous pose by 180* about X
q_rot = quaternion_from_euler(3.14159, 0, 0)
q_new = quaternion_multiply(q_rot, q_orig)

3 Inversion d’un quaternion 

Un moyen simple d’inverser un quaternion est de nier la composante w :

q[3] = -q[3]

4 Rotations relatives 

Supposons que vous ayez deux quaternions du même cadre, q_1 et q_2. Vous voulez trouver la rotation relative, q_r, qui convertit q_1 en q_2 de la manière suivante :

q_2 = q_r * q_1

Vous pouvez résoudre pour q r similaire à la résolution d’une équation matricielle. Inverse q_ 1 et multiplie à droite les deux côtés. Là encore, l’ordre de multiplication est important :

q_r = q_2 * q_1_inverse

Voici un exemple pour obtenir la rotation relative de la pose précédente du robot à la pose actuelle du robot en python :

def quaternion_multiply(q0, q1):
    """
    Multiplies two quaternions.

    Input
    :param q0: A 4 element array containing the first quaternion (q01, q11, q21, q31)
    :param q1: A 4 element array containing the second quaternion (q02, q12, q22, q32)

    Output
    :return: A 4 element array containing the final quaternion (q03,q13,q23,q33)

    """
    # Extract the values from q0
    w0 = q0[0]
    x0 = q0[1]
    y0 = q0[2]
    z0 = q0[3]

    # Extract the values from q1
    w1 = q1[0]
    x1 = q1[1]
    y1 = q1[2]
    z1 = q1[3]

    # Computer the product of the two quaternions, term by term
    q0q1_w = w0 * w1 - x0 * x1 - y0 * y1 - z0 * z1
    q0q1_x = w0 * x1 + x0 * w1 + y0 * z1 - z0 * y1
    q0q1_y = w0 * y1 - x0 * z1 + y0 * w1 + z0 * x1
    q0q1_z = w0 * z1 + x0 * y1 - y0 * x1 + z0 * w1

    # Create a 4 element array containing the final quaternion
    final_quaternion = np.array([q0q1_w, q0q1_x, q0q1_y, q0q1_z])

    # Return a 4 element array containing the final quaternion (q02,q12,q22,q32)
    return final_quaternion

q1_inv[0] = prev_pose.pose.orientation.x
q1_inv[1] = prev_pose.pose.orientation.y
q1_inv[2] = prev_pose.pose.orientation.z
q1_inv[3] = -prev_pose.pose.orientation.w # Negate for inverse

q2[0] = current_pose.pose.orientation.x
q2[1] = current_pose.pose.orientation.y
q2[2] = current_pose.pose.orientation.z
q2[3] = current_pose.pose.orientation.w

qr = quaternion_multiply(q2, q1_inv)

Résumé 

Dans ce didacticiel, vous avez découvert les concepts fondamentaux d’un quaternion et ses opérations mathématiques associées, telles que l’inversion et la rotation. Vous avez également découvert ses exemples d’utilisation dans ROS 2 et les méthodes de conversion entre deux classes Quaternion distinctes.