Utilities for Coordinate Transformations (via homogeneous coordinates)

Question

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)

Answer 1

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.

Answer 2

It appears that homogenize() will accept an array of points, but cartesianize() will only accept a single point/vector.

Changing:

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

to this:

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

enables arrays of homogeneous points to be cartesianized.

Answer 3

And using h[..., -1, None] instead of h[..., [-1]], the conversion is twice as fast:

>>> h = np.random.random((4, 5, 3))
>>> %timeit h[..., :-1] / h[..., [-1]]
3.4 μs ± 44.1 ns per loop (mean ± std. dev. of 7 runs, 100,000 loops each)
>>> %timeit h[..., :-1] / h[..., -1, None]
1.43 μs ± 7.92 ns per loop (mean ± std. dev. of 7 runs, 1,000,000 loops each)