Я регулярно хочу конвертировать позы (положение 3d + ориентация 3d = 6d всего) между разными системами координат. Примеры использования — это, например, игровые движки и моделирование для вычисления положения объектов относительно друг друга или, например, в робототехнике (мой вариант использования) для вычисления положения объекта относительно захвата руки робота.
Как ни странно, я еще не нашел хорошей библиотеки Python для выполнения таких преобразований. В частности, тот, который зависит от научного стека Python (numpy, scipy, matplotlib, …), так что это моя попытка написать код, который делает именно это. Пока части есть, я не добавил функциональность для анализа дерева / иерархии координатных фреймов и проработки полного преобразования (также известного как прямая кинематика), потому что это эффективно зависит от того, как представлено дерево / иерархия.
Я бы хотел проверить общее качество кода и ясность документации.
Код:
# 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]
и некоторые тесты
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)