reachy2_sdk.utils.utils

  1"""Reachy utils module.
  2
  3This module contains various useful functions especially:
  4- angle conversion from/to degree/radian
  5- enum conversion to string
  6- matrix decomposition/recomposition
  7- pose matrix creation
  8- various grpc messages conversion
  9"""
 10
 11from collections import namedtuple
 12from typing import Any, List, Optional, Tuple
 13
 14import numpy as np
 15import numpy.typing as npt
 16from google.protobuf.wrappers_pb2 import FloatValue
 17from pyquaternion import Quaternion
 18from reachy2_sdk_api.arm_pb2 import ArmPosition
 19from reachy2_sdk_api.goto_pb2 import (
 20    ArcDirection,
 21    GoToInterpolation,
 22    GoToInterpolationSpace,
 23    InterpolationMode,
 24    InterpolationSpace,
 25)
 26from reachy2_sdk_api.kinematics_pb2 import ExtEulerAngles, Rotation3d
 27from reachy2_sdk_api.orbita2d_pb2 import Pose2d
 28
 29SimplifiedRequest = namedtuple("SimplifiedRequest", ["part", "request"])
 30"""Named tuple for easy access to request variables"""
 31
 32JointsRequest = namedtuple("JointsRequest", ["target", "duration", "mode", "interpolation_space", "elliptical_parameters"])
 33"""Named tuple for easy access to request variables"""
 34
 35TargetJointsRequest = namedtuple("TargetJointsRequest", ["joints", "pose"])
 36"""Named tuple for easy access to target details"""
 37
 38OdometryRequest = namedtuple("OdometryRequest", ["target", "timeout", "distance_tolerance", "angle_tolerance"])
 39"""Named tuple for easy access to request variables"""
 40
 41EllipticalParameters = namedtuple("EllipticalParameters", ["arc_direction", "secondary_radius"])
 42"""Named tuple for easy access to request variables"""
 43
 44
 45def convert_to_radians(angles_list: List[float]) -> Any:
 46    """Convert a list of angles from degrees to radians.
 47
 48    Args:
 49        angles_list: A list of angles in degrees to convert to radians.
 50
 51    Returns:
 52        A list of angles converted to radians.
 53    """
 54    a = np.array(angles_list)
 55    a = np.deg2rad(a)
 56
 57    return a.tolist()
 58
 59
 60def convert_to_degrees(angles_list: List[float]) -> Any:
 61    """Convert a list of angles from radians to degrees.
 62
 63    Args:
 64        angles_list: A list of angles in radians to convert to degrees.
 65
 66    Returns:
 67        A list of angles converted to degrees.
 68    """
 69    a = np.array(angles_list)
 70    a = np.rad2deg(a)
 71
 72    return a.tolist()
 73
 74
 75def list_to_arm_position(positions: List[float], degrees: bool = True) -> ArmPosition:
 76    """Convert a list of joint positions to an ArmPosition message, considering whether the positions are in degrees or radians.
 77
 78    Args:
 79        positions: A list of float values representing joint positions. The list should contain 7 values
 80            in the following order: [shoulder_pitch, shoulder_roll, elbow_yaw, elbow_pitch, wrist_roll, wrist_pitch,wrist_yaw].
 81        degrees: A flag indicating whether the input joint positions are in degrees. If set to `True`,
 82            the input positions are in degrees. Defaults to `True`.
 83
 84    Returns:
 85        An ArmPosition message containing the shoulder position, elbow position, and wrist position
 86        based on the input list of joint positions.
 87    """
 88    if degrees:
 89        positions = convert_to_radians(positions)
 90    arm_pos = ArmPosition(
 91        shoulder_position=Pose2d(
 92            axis_1=FloatValue(value=positions[0]),
 93            axis_2=FloatValue(value=positions[1]),
 94        ),
 95        elbow_position=Pose2d(
 96            axis_1=FloatValue(value=positions[2]),
 97            axis_2=FloatValue(value=positions[3]),
 98        ),
 99        wrist_position=Rotation3d(
100            rpy=ExtEulerAngles(
101                roll=FloatValue(value=positions[4]),
102                pitch=FloatValue(value=positions[5]),
103                yaw=FloatValue(value=positions[6]),
104            )
105        ),
106    )
107
108    return arm_pos
109
110
111def arm_position_to_list(arm_pos: ArmPosition, degrees: bool = True) -> List[float]:
112    """Convert an ArmPosition message to a list of joint positions, with an option to return the result in degrees.
113
114    Args:
115        arm_pos: An ArmPosition message containing shoulder, elbow, and wrist positions.
116        degrees: Specifies whether the joint positions should be returned in degrees. If set to `True`,
117            the positions are converted to degrees. Defaults to `True`.
118
119    Returns:
120        A list of joint positions based on the ArmPosition, returned in the following order:
121        [shoulder_pitch, shoulder_roll, elbow_yaw, elbow_pitch, wrist_roll, wrist_pitch, wrist_yaw].
122    """
123    positions = []
124
125    for _, value in arm_pos.shoulder_position.ListFields():
126        positions.append(value.value)
127    for _, value in arm_pos.elbow_position.ListFields():
128        positions.append(value.value)
129    for _, value in arm_pos.wrist_position.rpy.ListFields():
130        positions.append(value.value)
131
132    if degrees:
133        positions = convert_to_degrees(positions)
134
135    return positions
136
137
138def ext_euler_angles_to_list(euler_angles: ExtEulerAngles, degrees: bool = True) -> List[float]:
139    """Convert an ExtEulerAngles 3D rotation message to a list of joint positions.
140
141    Args:
142        euler_angles: An ExtEulerAngles object representing a 3D rotation message.
143        degrees: Specifies whether the output should be in degrees. If set to `True`, the function
144            converts the angles to degrees before returning the list. Defaults to `True`.
145
146    Returns:
147        A list of joint positions representing the Euler angles in the order [roll, pitch, yaw].
148    """
149    positions = [euler_angles.roll.value, euler_angles.pitch.value, euler_angles.yaw.value]
150
151    if degrees:
152        positions = convert_to_degrees(positions)
153
154    return positions
155
156
157def get_grpc_interpolation_mode(interpolation_mode: str) -> GoToInterpolation:
158    """Convert a given interpolation mode string to a corresponding GoToInterpolation object.
159
160    Args:
161        interpolation_mode: A string representing the type of interpolation to be used. It can be either
162            "minimum_jerk", "linear" or "elliptical".
163
164    Returns:
165        An instance of the GoToInterpolation class with the interpolation type set based on the input
166        interpolation_mode string.
167
168    Raises:
169        ValueError: If the interpolation_mode is not "minimum_jerk", "linear" or "elliptical".
170    """
171    if interpolation_mode not in ["minimum_jerk", "linear", "elliptical"]:
172        raise ValueError(
173            f"Interpolation mode {interpolation_mode} not supported! Should be 'minimum_jerk', 'linear' or 'elliptical'"
174        )
175
176    if interpolation_mode == "minimum_jerk":
177        interpolation_mode = InterpolationMode.MINIMUM_JERK
178    elif interpolation_mode == "linear":
179        interpolation_mode = InterpolationMode.LINEAR
180    else:
181        interpolation_mode = InterpolationMode.ELLIPTICAL
182    return GoToInterpolation(interpolation_type=interpolation_mode)
183
184
185def get_grpc_interpolation_space(interpolation_space: str) -> GoToInterpolationSpace:
186    """Convert a given interpolation space string to a corresponding GoToInterpolationSpace object.
187
188    Args:
189        interpolation_space: A string representing the interpolation space to be used. It can be either
190            "joint_space" or "cartesian_space".
191
192    Returns:
193        An instance of the GoToInterpolationSpace class with the interpolation type set based on the input
194        interpolation_space string.
195
196    Raises:
197        ValueError: If the interpolation_space is not "joint_space" or "cartesian_space".
198    """
199    if interpolation_space not in ["joint_space", "cartesian_space"]:
200        raise ValueError(
201            f"Interpolation space {interpolation_space} not supported! Should be 'joint_space' or 'cartesian_space'"
202        )
203
204    if interpolation_space == "joint_space":
205        interpolation_space = InterpolationSpace.JOINT_SPACE
206    else:
207        interpolation_space = InterpolationSpace.CARTESIAN_SPACE
208    return GoToInterpolationSpace(interpolation_space=interpolation_space)
209
210
211def get_interpolation_mode(interpolation_mode: InterpolationMode) -> str:
212    """Convert an interpolation mode enum to a string representation.
213
214    Args:
215        interpolation_mode: The interpolation mode given as InterpolationMode. The supported interpolation
216            modes are MINIMUM_JERK, LINEAR and ELLIPTICAL.
217
218    Returns:
219        A string representing the interpolation mode based on the input interpolation_mode. Returns
220        "minimum_jerk" if the mode is InterpolationMode.MINIMUM_JERK, "linear" if it is
221        InterpolationMode.LINEAR, and "elliptical" if it is InterpolationMode.ELLIPTICAL.
222
223    Raises:
224        ValueError: If the interpolation_mode is not InterpolationMode.MINIMUM_JERK, InterpolationMode.LINEAR
225        or InterpolationMode.ELLIPTICAL.
226    """
227    if interpolation_mode not in [InterpolationMode.MINIMUM_JERK, InterpolationMode.LINEAR, InterpolationMode.ELLIPTICAL]:
228        raise ValueError(
229            f"Interpolation mode {interpolation_mode} not supported! Should be 'minimum_jerk', 'linear' or 'elliptical'"
230        )
231
232    if interpolation_mode == InterpolationMode.MINIMUM_JERK:
233        mode = "minimum_jerk"
234    elif interpolation_mode == InterpolationMode.LINEAR:
235        mode = "linear"
236    else:
237        mode = "elliptical"
238    return mode
239
240
241def get_interpolation_space(interpolation_space: InterpolationSpace) -> str:
242    """Convert an interpolation space enum to a string representation.
243
244    Args:
245        interpolation_space: The interpolation space given as InterpolationSpace. The supported interpolation
246            modes are JOINT_SPACE and CARTESIAN_SPACE.
247
248    Returns:
249        A string representing the interpolation mode based on the input interpolation_space. Returns
250        "joint_space" if the mode is InterpolationSpace.JOINT_SPACE, and "cartesian_space" if it is
251        InterpolationSpace.CARTESIAN_SPACE.
252
253    Raises:
254        ValueError: If the interpolation_space is not InterpolationSpace.JOINT_SPACE or InterpolationSpace.CARTESIAN_SPACE.
255    """
256    if interpolation_space not in [InterpolationSpace.JOINT_SPACE, InterpolationSpace.CARTESIAN_SPACE]:
257        raise ValueError(
258            f"Interpolation space {interpolation_space} not supported! Should be 'joint_space' or 'cartesian_space'"
259        )
260
261    if interpolation_space == InterpolationSpace.CARTESIAN_SPACE:
262        space = "cartesian_space"
263    else:
264        space = "joint_space"
265    return space
266
267
268def get_grpc_arc_direction(arc_direction: str) -> ArcDirection:
269    """Convert a given arc direction string to a corresponding ArcDirection object.
270
271    Args:
272        arc_direction: A string representing the direction of the arc. It can be one of the following options:
273            "above", "below", "front", "back", "right", or "left".
274
275    Returns:
276        An instance of the ArcDirection class with the direction set based on the input arc_direction string.
277
278    Raises:
279        ValueError: If the arc_direction is not one of "above", "below", "front", "back", "right", or "left".
280    """
281    if arc_direction not in ["above", "below", "front", "back", "right", "left"]:
282        raise ValueError(
283            f"Arc direction {arc_direction} not supported! Should be 'above', 'below', 'front', 'back', 'right' or 'left'"
284        )
285
286    if arc_direction == "above":
287        arc_direction = ArcDirection.ABOVE
288    elif arc_direction == "below":
289        arc_direction = ArcDirection.BELOW
290    elif arc_direction == "front":
291        arc_direction = ArcDirection.FRONT
292    elif arc_direction == "back":
293        arc_direction = ArcDirection.BACK
294    elif arc_direction == "right":
295        arc_direction = ArcDirection.RIGHT
296    else:
297        arc_direction = ArcDirection.LEFT
298    return arc_direction
299
300
301def get_arc_direction(arc_direction: ArcDirection) -> str:
302    """Convert an arc direction enum to a string representation.
303
304    Args:
305        arc_direction: The arc direction given as ArcDirection. The supported arc directions are ABOVE, BELOW, FRONT,
306            BACK, RIGHT, and LEFT.
307
308    Returns:
309        A string representing the arc direction based on the input arc_direction. Returns "above" if the direction is
310        ArcDirection.ABOVE, "below" if it is ArcDirection.BELOW, "front" if it is ArcDirection.FRONT, "back" if it is
311        ArcDirection.BACK, "right" if it is ArcDirection.RIGHT, and "left" if it is ArcDirection.LEFT.
312
313    Raises:
314        ValueError: If the arc_direction is not ArcDirection.ABOVE, ArcDirection.BELOW, ArcDirection.FRONT, ArcDirection.BACK,
315        ArcDirection.RIGHT, or ArcDirection.LEFT.
316    """
317    if arc_direction not in [
318        ArcDirection.ABOVE,
319        ArcDirection.BELOW,
320        ArcDirection.FRONT,
321        ArcDirection.BACK,
322        ArcDirection.RIGHT,
323        ArcDirection.LEFT,
324    ]:
325        raise ValueError(
326            f"Arc direction {arc_direction} not supported! Should be 'above', 'below', 'front', 'back', 'right' or 'left'"
327        )
328
329    if arc_direction == ArcDirection.ABOVE:
330        direction = "above"
331    elif arc_direction == ArcDirection.BELOW:
332        direction = "below"
333    elif arc_direction == ArcDirection.FRONT:
334        direction = "front"
335    elif arc_direction == ArcDirection.BACK:
336        direction = "back"
337    elif arc_direction == ArcDirection.RIGHT:
338        direction = "right"
339    else:
340        direction = "left"
341    return direction
342
343
344def decompose_matrix(matrix: npt.NDArray[np.float64]) -> Tuple[Quaternion, npt.NDArray[np.float64]]:
345    """Decompose a homogeneous 4x4 matrix into rotation (represented as a quaternion) and translation components.
346
347    Args:
348        matrix: A homogeneous 4x4 matrix represented as a NumPy array of type np.float64.
349
350    Returns:
351        A tuple containing a Quaternion representing the rotation component and a NumPy array
352        representing the translation component of the input matrix.
353    """
354    rotation_matrix = matrix[:3, :3]
355    translation = matrix[:3, 3]
356
357    # increase tolerance to avoid errors when checking if the matrix is a valid rotation matrix
358    # See https://github.com/KieranWynn/pyquaternion/pull/44
359    rotation = Quaternion(matrix=rotation_matrix, atol=1e-05, rtol=1e-05)
360    return rotation, translation
361
362
363def recompose_matrix(rotation: npt.NDArray[np.float64], translation: npt.NDArray[np.float64]) -> npt.NDArray[np.float64]:
364    """Recompose a homogeneous 4x4 matrix from rotation (quaternion) and translation components.
365
366    Args:
367        rotation: A 3x3 rotation matrix represented as a NumPy array of type np.float64.
368        translation: A vector representing the displacement in space, containing the x, y, and z
369            components of the translation vector.
370
371    Returns:
372        A homogeneous 4x4 matrix composed from the provided rotation and translation components.
373    """
374    matrix = np.eye(4)
375    matrix[:3, :3] = rotation  # .as_matrix()
376    matrix[:3, 3] = translation
377    return matrix
378
379
380def matrix_from_euler_angles(roll: float, pitch: float, yaw: float, degrees: bool = True) -> npt.NDArray[np.float64]:
381    """Create a 4x4 homogeneous rotation matrix from roll, pitch, and yaw angles, with an option to input angles in degrees.
382
383    Args:
384        roll: The rotation angle around the x-axis in the Euler angles representation.
385        pitch: The rotation angle around the y-axis in the Euler angles representation.
386        yaw: The rotation angle around the z-axis in the Euler angles representation.
387        degrees: Specifies whether the input angles (roll, pitch, yaw) are in degrees. If set to `True`,
388            the input angles are expected to be in degrees. Defaults to `True`.
389
390    Returns:
391        A 4x4 homogeneous rotation matrix created from the input roll, pitch, and yaw angles.
392    """
393    if degrees:
394        roll = np.deg2rad(roll)
395        pitch = np.deg2rad(pitch)
396        yaw = np.deg2rad(yaw)
397
398    R_x = np.array(
399        [[1, 0, 0, 0], [0, np.cos(roll), -np.sin(roll), 0], [0, np.sin(roll), np.cos(roll), 0], [0, 0, 0, 1]], dtype=np.float64
400    )
401
402    R_y = np.array(
403        [[np.cos(pitch), 0, np.sin(pitch), 0], [0, 1, 0, 0], [-np.sin(pitch), 0, np.cos(pitch), 0], [0, 0, 0, 1]],
404        dtype=np.float64,
405    )
406
407    R_z = np.array(
408        [[np.cos(yaw), -np.sin(yaw), 0, 0], [np.sin(yaw), np.cos(yaw), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], dtype=np.float64
409    )
410
411    rotation_matrix = R_z @ R_y @ R_x
412    return rotation_matrix
413
414
415def get_pose_matrix(position: List[float], rotation: List[float], degrees: bool = True) -> npt.NDArray[np.float64]:
416    """Create a 4x4 pose matrix from a position vector and "roll, pitch, yaw" angles (rotation).
417
418    Args:
419        position: A list of size 3 representing the requested position of the end effector in the Reachy coordinate system.
420        rotation: A list of size 3 representing the requested orientation of the end effector in the Reachy coordinate system.
421            The rotation is given as intrinsic angles, executed in roll, pitch, yaw order.
422        degrees: Specifies whether the input angles are in degrees. If set to `True`, the angles are interpreted as degrees.
423            If set to `False`, they are interpreted as radians. Defaults to `True`.
424
425    Returns:
426        The constructed 4x4 pose matrix.
427
428    Raises:
429        TypeError: If `position` is not a list of floats or integers.
430        TypeError: If `rotation` is not a list of floats or integers.
431        ValueError: If the length of `position` is not 3.
432        ValueError: If the length of `rotation` is not 3.
433    """
434    if not (isinstance(position, np.ndarray) or isinstance(position, list)) or not all(
435        isinstance(pos, float | int) for pos in position
436    ):
437        raise TypeError(f"position should be a list of float, got {position}")
438    if not (isinstance(rotation, np.ndarray) or isinstance(rotation, list)) or not all(
439        isinstance(rot, float | int) for rot in rotation
440    ):
441        raise TypeError(f"rotation should be a list of float, got {rotation}")
442    if len(position) != 3:
443        raise ValueError("position should be a list of len 3")
444    if len(rotation) != 3:
445        raise ValueError("rotation should be a list of len 3")
446
447    pose = matrix_from_euler_angles(rotation[0], rotation[1], rotation[2], degrees=degrees)
448    pose[:3, 3] = np.array(position)
449    return pose
450
451
452def quaternion_from_euler_angles(roll: float, pitch: float, yaw: float, degrees: bool = True) -> Quaternion:
453    """Convert Euler angles (intrinsic XYZ order) to a quaternion using the pyquaternion library.
454
455    Args:
456        roll (float): Rotation angle around the X-axis (roll), in degrees by default.
457        pitch (float): Rotation angle around the Y-axis (pitch), in degrees by default.
458        yaw (float): Rotation angle around the Z-axis (yaw), in degrees by default.
459        degrees (bool): If True, the input angles are interpreted as degrees. If False, they are
460            interpreted as radians. Defaults to True.
461
462    Returns:
463        Quaternion: The quaternion representing the combined rotation in 3D space.
464    """
465    if degrees:
466        roll = np.deg2rad(roll)
467        pitch = np.deg2rad(pitch)
468        yaw = np.deg2rad(yaw)
469
470    qx = Quaternion(axis=[1, 0, 0], angle=roll)
471    qy = Quaternion(axis=[0, 1, 0], angle=pitch)
472    qz = Quaternion(axis=[0, 0, 1], angle=yaw)
473
474    quaternion = qx * qy * qz
475
476    return quaternion
477
478
479def rotate_in_self(frame: npt.NDArray[np.float64], rotation: List[float], degrees: bool = True) -> npt.NDArray[np.float64]:
480    """Return a new homogeneous 4x4 pose matrix that is the input matrix rotated in itself.
481
482    Args:
483        frame: The input frame, given as a 4x4 homogeneous matrix.
484        rotation: A list of size 3 representing the rotation to be applied. The rotation is given as intrinsic angles,
485            executed in roll, pitch, yaw order.
486        degrees: Specifies whether the input angles are in degrees. If set to `True`, the angles are interpreted as degrees.
487            If set to `False`, they are interpreted as radians. Defaults to `True`.
488
489    Returns:
490        A new 4x4 homogeneous matrix after applying the specified rotation.
491    """
492    new_frame = frame.copy()
493
494    toOrigin = np.eye(4)
495    toOrigin[:3, :3] = new_frame[:3, :3]
496    toOrigin[:3, 3] = new_frame[:3, 3]
497    toOrigin = np.linalg.inv(toOrigin)
498
499    new_frame = toOrigin @ new_frame
500    new_frame = get_pose_matrix([0.0, 0.0, 0.0], rotation, degrees=degrees) @ new_frame
501    new_frame = np.linalg.inv(toOrigin) @ new_frame
502
503    return new_frame
504
505
506def translate_in_self(frame: npt.NDArray[np.float64], translation: List[float]) -> npt.NDArray[np.float64]:
507    """Return a new homogeneous 4x4 pose matrix that is the input frame translated along its own axes.
508
509    Args:
510        frame: The input frame, given as a 4x4 homogeneous matrix.
511        translation: A list of size 3 representing the translation to be applied, given as [x, y, z].
512
513    Returns:
514        A new homogeneous 4x4 pose matrix after translating the input frame along its own axes.
515    """
516    new_frame = frame.copy()
517
518    toOrigin = np.eye(4)
519    toOrigin[:3, :3] = new_frame[:3, :3]
520    toOrigin[:3, 3] = new_frame[:3, 3]
521    toOrigin = np.linalg.inv(toOrigin)
522
523    new_frame = toOrigin @ new_frame
524    new_frame = get_pose_matrix(translation, [0, 0, 0]) @ new_frame
525    new_frame = np.linalg.inv(toOrigin) @ new_frame
526
527    return new_frame
528
529
530def invert_affine_transformation_matrix(matrix: npt.NDArray[np.float64]) -> npt.NDArray[np.float64]:
531    """Invert a 4x4 homogeneous transformation matrix.
532
533    The function computes the inverse by transposing the rotation component and adjusting the translation component.
534
535    Args:
536        matrix: A 4x4 NumPy array representing a homogeneous transformation matrix.
537
538    Returns:
539        A new 4x4 homogeneous matrix that is the inverse of the input matrix.
540
541    Raises:
542        ValueError: If the input matrix is not 4x4.
543    """
544    if matrix.shape != (4, 4):
545        raise ValueError("matrix should be 4x4")
546
547    inv_matrix = np.eye(4)
548    inv_matrix[:3, :3] = matrix[:3, :3].T
549    inv_matrix[:3, 3] = -matrix[:3, :3].T @ matrix[:3, 3]
550    return inv_matrix
551
552
553def get_normal_vector(vector: npt.NDArray[np.float64], arc_direction: str) -> Optional[npt.NDArray[np.float64]]:
554    """Calculate a normal vector to a given vector based on a specified direction.
555
556    Args:
557        vector: A vector [x, y, z] in 3D space.
558        arc_direction: The desired direction for the normal vector. It can be one of the following options:
559            'above', 'below', 'front', 'back', 'right', or 'left'.
560
561    Returns:
562        The normal vector [x, y, z] to the given vector in the specified direction. Returns `None` if the
563        normal vector cannot be computed or if the vector is in the requested arc_direction.
564
565    Raises:
566        ValueError: If the arc_direction is not one of 'above', 'below', 'front', 'back', 'right', or 'left'.
567    """
568    match arc_direction:
569        case "above":
570            if abs(vector[0]) < 0.001 and abs(vector[1]) < 0.001:
571                return None
572            normal = np.cross(vector, [0, 0, -1])
573        case "below":
574            if abs(vector[0]) < 0.001 and abs(vector[1]) < 0.001:
575                return None
576            normal = np.cross(vector, [0, 0, 1])
577        case "left":
578            if abs(vector[0]) < 0.001 and abs(vector[2]) < 0.001:
579                return None
580            normal = np.cross(vector, [0, -1, 0])
581        case "right":
582            if abs(vector[0]) < 0.001 and abs(vector[2]) < 0.001:
583                return None
584            normal = np.cross(vector, [0, 1, 0])
585        case "front":
586            if abs(vector[1]) < 0.001 and abs(vector[2]) < 0.001:
587                return None
588            normal = np.cross(vector, [-1, 0, 0])
589        case "back":
590            if abs(vector[1]) < 0.001 and abs(vector[2]) < 0.001:
591                return None
592            normal = np.cross(vector, [1, 0, 0])
593        case _:
594            raise ValueError(
595                f"arc_direction '{arc_direction}' not supported! Should be one of: "
596                "'above', 'below', 'front', 'back', 'right' or 'left'"
597            )
598
599    if np.linalg.norm(normal) == 0:
600        # Return None if the vector is in the requested arc_direction
601        return None
602
603    normal = normal / np.linalg.norm(normal)
604    return normal