1
\$\begingroup\$

I regularly wish to convert poses (position 3d + orientation 3d = 6d total) between different coordinate frames. Use-cases are for example in game engines and simulations to compute the position of objects relative to one another, or for example in robotics (my use-case) to compute the position of an object relative to the gripper of a robot arm.

Weirdly enough, I haven't yet found a good Python library to perform such conversions. Particularly one that depends on the scientific Python stack (numpy, scipy, matplotlib, ...), so this is my attempt at writing code that does just that. While the pieces are there, I didn't add functionality to parse a tree/hierarchy of coordinate frames and work out the full transformation (a.k.a. do forward kinematics), because doing so efficiently depends on how the tree/hierarchy is represented.

What I would like reviewed is general code quality and clarity of the documentation.

The code:


# kinematics.py

import numpy as np


def transform(new_frame: np.array):
    """
    Compute the homogenious transformation matrix from the current coordinate
    system into a new coordinate system.

    Given the pose of the new reference frame ``new_frame`` in the current
    reference frame, compute the homogenious transformation matrix from the
    current reference frame into the new reference frame.
    ``transform(new_frame)`` can, for example, be used to get the transformation
    from the corrdinate frame of the current link in a kinematic chain to the
    next link in a kinematic chain. Here, the next link's origin (``new_frame``)
    is specified relative to the current link's origin, i.e., in the current
    link's coordinate system.


    Parameters
    ----------
    new_frame: np.array
        The pose of the new coordinate system's origin. This is a 6-dimensional
        vector consisting of the origin's position and the frame's orientation
        (xyz Euler Angles): [x, y, z, alpha, beta, gamma].

    Returns
    -------
    transformation_matrix : np.ndarray
        A 4x4 matrix representing the homogenious transformation.


    Notes
    -----
    For performance reasons, it is better to sequentially apply multiple
    transformations to a vector (or set of vectors) than to first multiply a
    sequence of transformations and then apply them to a vector afterwards.

    """

    new_frame = np.asarray(new_frame)
    alpha, beta, gamma = - new_frame[3:]

    rot_x = np.array([
        [1, 0,              0],
        [0, np.cos(alpha),  np.sin(alpha)],
        [0, -np.sin(alpha), np.cos(alpha)]
    ])
    rot_y = np.array([
        [np.cos(beta), 0, np.sin(beta)],
        [0, 1, 0],
        [-np.sin(beta), 0, np.cos(beta)]
    ])
    rot_z = np.array([
        [np.cos(gamma),  np.sin(gamma), 0],
        [-np.sin(gamma), np.cos(gamma), 0],
        [0, 0, 1]
    ])

    transform = np.eye(4)
    # Note: apply inverse rotation
    transform[:3, :3] = np.matmul(rot_z, np.matmul(rot_y, rot_x))
    transform[:3, 3] = - new_frame[:3]

    return transform


def inverseTransform(old_frame):
    """
    Compute the homogenious transformation matrix from the current coordinate
    system into the old coordinate system.

    Given the pose of the current reference frame in the old reference frame
    ``old_frame``, compute the homogenious transformation matrix from the new
    reference frame into the old reference frame. For example,
    ``inverseTransform(camera_frame)`` can, be used to compute the
    transformation from a camera's coordinate frame to the world's coordinate
    frame assuming the camera frame's pose is given in the world's coordinate
    system.

    Parameters
    ----------
    old_frame: {np.array, None}
        The pose of the old coordinate system's origin. This is a 6-dimensional
        vector consisting of the origin's position and the frame's orientation
        (xyz Euler Angles): [x, y, z, alpha, beta, gamma].

    Returns
    -------
    transformation_matrix : np.ndarray
        A 4x4 matrix representing the homogenious transformation.


    Notes
    -----
    For performance reasons, it is better to sequentially apply multiple
    transformations to a vector (or set of vectors) than to first multiply a
    sequence of transformations and then apply them to a vector afterwards.

    """

    old_frame = np.asarray(old_frame)
    alpha, beta, gamma = old_frame[3:]

    rot_x = np.array([
        [1, 0,              0],
        [0, np.cos(alpha),  np.sin(alpha)],
        [0, -np.sin(alpha), np.cos(alpha)]
    ])
    rot_y = np.array([
        [np.cos(beta), 0, np.sin(beta)],
        [0, 1, 0],
        [-np.sin(beta), 0, np.cos(beta)]
    ])
    rot_z = np.array([
        [np.cos(gamma),  np.sin(gamma), 0],
        [-np.sin(gamma), np.cos(gamma), 0],
        [0, 0, 1]
    ])

    transform = np.eye(4)
    transform[:3, :3] = np.matmul(rot_x, np.matmul(rot_y, rot_z))
    transform[:3, 3] = old_frame[:3]

    return transform


def transformBetween(old_frame: np.array, new_frame:np.array):
    """
    Compute the homogenious transformation matrix between two frames.

    ``transformBetween(old_frame, new_frame)`` computes the
    transformation from the corrdinate system with pose ``old_frame`` to
    the corrdinate system with pose ``new_frame`` where both origins are
    expressed in the same reference frame, e.g., the world's coordinate frame.
    For example, ``transformBetween(camera_frame, tool_frame)`` computes
    the transformation from a camera's coordinate system to the tool's
    coordinate system assuming the pose of both corrdinate frames is given in
    a shared world frame (or any other __shared__ frame of reference).

    Parameters
    ----------
    old_frame: np.array
        The pose of the old coordinate system's origin. This is a 6-dimensional
        vector consisting of the origin's position and the frame's orientation
        (xyz Euler Angles): [x, y, z, alpha, beta, gamma].
    new_frame: np.array
        The pose of the new coordinate system's origin. This is a 6-dimensional
        vector consisting of the origin's position and the frame's orientation
        (xyz Euler Angles): [x, y, z, alpha, beta, gamma].

    Returns
    -------
    transformation_matrix : np.ndarray
        A 4x4 matrix representing the homogenious transformation.

    Notes
    -----
    If the reference frame and ``old_frame`` are identical, use ``transform``
    instead.

    If the reference frame and ``new_frame`` are identical, use
    ``transformInverse`` instead.

    For performance reasons, it is better to sequentially apply multiple
    transformations to a vector than to first multiply a sequence of
    transformations and then apply them to a vector afterwards.

    """

    return np.matmul(transform(new_frame),inverseTransform(old_frame))


def homogenize(cartesian_vector: np.array):
    """
    Convert a vector from cartesian coordinates into homogenious coordinates.

    Parameters
    ----------
    cartesian_vector: np.array
        The vector to be converted.

    Returns
    -------
    homogenious_vector: np.array
        The vector in homogenious coordinates.

    """

    shape = cartesian_vector.shape
    homogenious_vector = np.ones((*shape[:-1], shape[-1] + 1))
    homogenious_vector[..., :-1] = cartesian_vector
    return homogenious_vector


def cartesianize(homogenious_vector: np.array):
    """
    Convert a vector from homogenious coordinates to cartesian coordinates.

    Parameters
    ----------
    homogenious_vector: np.array
        The vector in homogenious coordinates.

    Returns
    -------
    cartesian_vector: np.array
        The vector to be converted.

    """

    return homogenious_vector[..., :-1] / homogenious_vector[..., -1]

and some tests

import numpy as np
import kinematics as kine
import pytest


@pytest.mark.parametrize(
    "vector_in,frame_A,frame_B,vector_out",
    [
        (np.array((1,2,3)),np.array((0,0,0,0,0,0)),np.array((1,0,0,0,0,0)),np.array((0,2,3))),
        (np.array((1,0,0)),np.array((0,0,0,0,0,0)),np.array((0,0,0,0,0,np.pi/2)),np.array((0,1,0))),
        (np.array((0,1,0)),np.array((0,0,0,0,0,0)),np.array((0,0,0,np.pi/2,0,np.pi/2)),np.array((0,0,1))),
        (np.array((0,np.sqrt(2),0)),np.array((4.5,1,0,0,0,-np.pi/4)),np.array((0,0,0,0,0,0)),np.array((3.5,2,0))),
    ]
)
def test_transform_between(vector_in, frame_A, frame_B, vector_out):
    vector_A = kine.homogenize(vector_in)
    vector_B = np.matmul(kine.transformBetween(frame_A, frame_B), vector_A)
    vector_B = kine.cartesianize(vector_B)

    assert np.allclose(vector_B, vector_out)

\$\endgroup\$
2
\$\begingroup\$

You've type-hinted some of your parameters and none of your returns, as in

def transform(new_frame: np.array):

Add the missing return type hints, probably -> np.ndarray.

It's "homogeneous", not "homogenious".

It's more standard to use lower_snake_case for functions, i.e. inverse_transform instead of inverseTransform.

Otherwise this is pretty sane.

\$\endgroup\$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.