concepts.utils.rotationlib#

Tools for converting between rotation representations.

Conventions#

  • All functions accept batches as well as individual rotations.

  • All rotation conventions match respective MuJoCo defaults (e.g., quaternions use wxyz convention).

    Note that this is DIFFERENT from PyBullet (which uses xyzw).

  • All angles are in radians.

  • Matricies follow LR convention.

  • Euler Angles are all relative with ‘xyz’ axes ordering.

  • See specific representation for more information.

Representations#

Euler

There are many euler angle frames – here we will strive to use the default in MuJoCo, which is eulerseq=’xyz’.

This frame is a relative rotating frame, about x, y, and z axes in order. Relative rotating means that after we rotate about x, then we use the new (rotated) y, and the same for z.

Quaternions

These are defined in terms of rotation (angle) about a unit vector (x, y, z) We use the following <q0, q1, q2, q3> convention:

q0 = cos(angle / 2)
q1 = sin(angle / 2) * x
q2 = sin(angle / 2) * y
q3 = sin(angle / 2) * z

This is also sometimes called qw, qx, qy, qz.

Note that quaternions are ambiguous, because we can represent a rotation by angle about vector <x, y, z> and -angle about vector <-x, -y, -z>. To choose between these, we pick “first nonzero positive”, where we make the first nonzero element of the quaternion positive.

This can result in mismatches if you’re converting an quaternion that is not “first nonzero positive” to a different representation and back.

Axis Angle

Warning

(Not currently implemented) These are very straightforward. Rotation is angle about a unit vector.

XY Axes

Warning

(Not currently implemented) We are given x axis and y axis, and z axis is cross product of x and y.

Z Axis

Warning

This is NOT RECOMMENDED. Defines a unit vector for the Z axis, but rotation about this axis is not well defined.

Instead pick a fixed reference direction for another axis (e.g. X) and calculate the other (e.g. Y = Z cross-product X), then use XY Axes rotation instead.

SO3

Warning

(Not currently implemented) While not supported by MuJoCo, this representation has a lot of nice features. We expect to add support for these in the future.

TODOs/Missings
  • Rotation integration or derivatives (e.g. velocity conversions)

  • More representations (SO3, etc)

  • Random sampling (e.g. sample uniform random rotation)

  • Performance benchmarks/measurements

  • (Maybe) define everything as to/from matricies, for simplicity

Functions

as_rotation(r)

Convert a 3x3 matrix or a quaternion into a standard 3x3 matrix representation.

axisangle2quat(axis, angle)

euler2mat(euler)

Convert Euler Angles to Rotation Matrix.

euler2point_euler(euler)

euler2quat(euler)

Convert Euler Angles to Quaternions.

get_parallel_rotations()

mat2euler(mat)

Convert Rotation Matrix to Euler Angles.

mat2quat(mat)

Convert Rotation Matrix to Quaternion.

normalize_angles(angles)

Puts angles in [-pi, pi] range.

point_euler2euler(euler)

point_quat2quat(quat)

quat2axisangle(quat)

quat2euler(quat)

Convert Quaternion to Euler Angles.

quat2mat(quat)

Convert Quaternion to Euler Angles.

quat2point_quat(quat)

quat_conjugate(q)

quat_identity()

quat_mul(q0, q1)

quat_rot_vec(q, v0)

round_to_straight_angles(angles)

Returns closest angle modulo 90 degrees

subtract_euler(e1, e2)