transform_utils
Utility functions of matrix and vector transformations.
NOTE: convention for quaternions is (x, y, z, w)
anorm(x, axis=None, keepdims=False)
axisangle2quat(vec)
Converts scaled axis-angle to quat.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
vec |
array
|
(ax,ay,az) axis-angle exponential coordinates |
required |
Returns:
Type | Description |
---|---|
np.array: (x,y,z,w) vec4 float angles |
Source code in omnigibson/utils/transform_utils.py
cartesian_to_polar(x, y)
check_quat_right_angle(quat, atol=0.05)
Check by making sure the quaternion is some permutation of +/- (1, 0, 0, 0), +/- (0.707, 0.707, 0, 0), or +/- (0.5, 0.5, 0.5, 0.5) Because orientations are all normalized (same L2-norm), every orientation should have a unique L1-norm So we check the L1-norm of the absolute value of the orientation as a proxy for verifying these values
Parameters:
Name | Type | Description | Default |
---|---|---|---|
quat |
4 - array
|
(x,y,z,w) quaternion orientation to check |
required |
atol |
float
|
Absolute tolerance permitted |
0.05
|
Returns:
Name | Type | Description |
---|---|---|
bool |
Whether the quaternion is a right angle or not |
Source code in omnigibson/utils/transform_utils.py
clip_rotation(quat, limit)
Limits a (delta) rotation to a specified limit
Converts rotation to axis-angle, clips, then re-converts back into quaternion
Parameters:
Name | Type | Description | Default |
---|---|---|---|
quat |
array
|
(x,y,z,w) rotation being clipped |
required |
limit |
float
|
Value to limit rotation by -- magnitude (scalar, in radians) |
required |
Returns:
Type | Description |
---|---|
2-tuple:
|
Source code in omnigibson/utils/transform_utils.py
clip_translation(dpos, limit)
Limits a translation (delta position) to a specified limit
Scales down the norm of the dpos to 'limit' if norm(dpos) > limit, else returns immediately
Parameters:
Name | Type | Description | Default |
---|---|---|---|
dpos |
n - array
|
n-dim Translation being clipped (e,g.: (x, y, z)) -- numpy array |
required |
limit |
float
|
Value to limit translation by -- magnitude (scalar, in same units as input) |
required |
Returns:
Type | Description |
---|---|
2-tuple:
|
Source code in omnigibson/utils/transform_utils.py
convert_quat(q, to='xyzw')
Converts quaternion from one convention to another. The convention to convert TO is specified as an optional argument. If to == 'xyzw', then the input is in 'wxyz' format, and vice-versa.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
q |
array
|
a 4-dim array corresponding to a quaternion |
required |
to |
str
|
either 'xyzw' or 'wxyz', determining which convention to convert to. |
'xyzw'
|
Source code in omnigibson/utils/transform_utils.py
euler2mat(euler)
Converts euler angles into rotation matrix form
Parameters:
Name | Type | Description | Default |
---|---|---|---|
euler |
array
|
(r,p,y) angles |
required |
Returns:
Type | Description |
---|---|
np.array: 3x3 rotation matrix |
Raises:
Type | Description |
---|---|
AssertionError
|
[Invalid input shape] |
Source code in omnigibson/utils/transform_utils.py
euler2quat(euler)
Converts euler angles into quaternion form
Parameters:
Name | Type | Description | Default |
---|---|---|---|
euler |
array
|
(r,p,y) angles |
required |
Returns:
Type | Description |
---|---|
np.array: (x,y,z,w) float quaternion angles |
Raises:
Type | Description |
---|---|
AssertionError
|
[Invalid input shape] |
Source code in omnigibson/utils/transform_utils.py
ewma_vectorized(data, alpha, offset=None, dtype=None, order='C', out=None)
Calculates the exponential moving average over a vector. Will fail for large inputs.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
data |
Iterable
|
Input data |
required |
alpha |
float
|
scalar in range (0,1) The alpha parameter for the moving average. |
required |
offset |
None or float
|
If specified, the offset for the moving average. None defaults to data[0]. |
None
|
dtype |
None or type
|
Data type used for calculations. If None, defaults to float64 unless data.dtype is float32, then it will use float32. |
None
|
order |
None or str
|
Order to use when flattening the data. Valid options are {'C', 'F', 'A'}. None defaults to 'C'. |
'C'
|
out |
None or array
|
If specified, the location into which the result is stored. If provided, it must have
the same shape as the input. If not provided or |
None
|
Returns:
Type | Description |
---|---|
np.array: Exponential moving average from @data |
Source code in omnigibson/utils/transform_utils.py
force_in_A_to_force_in_B(force_A, torque_A, pose_A_in_B)
Converts linear and rotational force at a point in frame A to the equivalent in frame B.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
force_A |
array
|
(fx,fy,fz) linear force in A |
required |
torque_A |
array
|
(tx,ty,tz) rotational force (moment) in A |
required |
pose_A_in_B |
array
|
4x4 matrix corresponding to the pose of A in frame B |
required |
Returns:
Type | Description |
---|---|
2-tuple:
|
Source code in omnigibson/utils/transform_utils.py
frustum(left, right, bottom, top, znear, zfar)
Create view frustum matrix.
Source code in omnigibson/utils/transform_utils.py
get_orientation_diff_in_radian(orn0, orn1)
Returns the difference between two quaternion orientations in radian
Parameters:
Name | Type | Description | Default |
---|---|---|---|
orn0 |
array
|
(x, y, z, w) |
required |
orn1 |
array
|
(x, y, z, w) |
required |
Returns:
Name | Type | Description |
---|---|---|
orn_diff |
float
|
orientation difference in radian |
Source code in omnigibson/utils/transform_utils.py
get_orientation_error(target_orn, current_orn)
Returns the difference between two quaternion orientations as a 3 DOF numpy array. For use in an impedance controller / task-space PD controller.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
target_orn |
array
|
(x, y, z, w) desired quaternion orientation |
required |
current_orn |
array
|
(x, y, z, w) current quaternion orientation |
required |
Returns:
Name | Type | Description |
---|---|---|
orn_error |
array
|
(ax,ay,az) current orientation error, corresponds to (target_orn - current_orn) |
Source code in omnigibson/utils/transform_utils.py
get_pose_error(target_pose, current_pose)
Computes the error corresponding to target pose - current pose as a 6-dim vector. The first 3 components correspond to translational error while the last 3 components correspond to the rotational error.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
target_pose |
array
|
a 4x4 homogenous matrix for the target pose |
required |
current_pose |
array
|
a 4x4 homogenous matrix for the current pose |
required |
Returns:
Type | Description |
---|---|
np.array: 6-dim pose error. |
Source code in omnigibson/utils/transform_utils.py
invert_pose_transform(pos, quat)
Inverts a pose transform
Parameters:
Name | Type | Description | Default |
---|---|---|---|
pos |
(x,y,z) position to transform |
required | |
quat |
(x,y,z,w) orientation to transform |
required |
Returns:
Type | Description |
---|---|
2-tuple: - (np.array) (x,y,z) position array in cartesian coordinates - (np.array) (x,y,z,w) orientation array in quaternion form |
Source code in omnigibson/utils/transform_utils.py
l2_distance(v1, v2)
make_pose(translation, rotation)
Makes a homogeneous pose matrix from a translation vector and a rotation matrix.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
translation |
array
|
(x,y,z) translation value |
required |
rotation |
array
|
a 3x3 matrix representing rotation |
required |
Returns:
Name | Type | Description |
---|---|---|
pose |
array
|
a 4x4 homogeneous matrix |
Source code in omnigibson/utils/transform_utils.py
mat2euler(rmat)
Converts given rotation matrix to euler angles in radian.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
rmat |
array
|
3x3 rotation matrix |
required |
Returns:
Type | Description |
---|---|
np.array: (r,p,y) converted euler angles in radian vec3 float |
Source code in omnigibson/utils/transform_utils.py
mat2pose(hmat)
Converts a homogeneous 4x4 matrix into pose.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
hmat |
array
|
a 4x4 homogeneous matrix |
required |
Returns:
Type | Description |
---|---|
2-tuple: - (np.array) (x,y,z) position array in cartesian coordinates - (np.array) (x,y,z,w) orientation array in quaternion form |
Source code in omnigibson/utils/transform_utils.py
mat2quat(rmat)
Converts given rotation matrix to quaternion.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
rmat |
array
|
(..., 3, 3) rotation matrix |
required |
Returns:
Type | Description |
---|---|
np.array: (..., 4) (x,y,z,w) float quaternion angles |
Source code in omnigibson/utils/transform_utils.py
mat4(array)
Converts an array to 4x4 matrix.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
array |
n - array
|
the array in form of vec, list, or tuple |
required |
Returns:
Type | Description |
---|---|
np.array: a 4x4 numpy matrix |
Source code in omnigibson/utils/transform_utils.py
matrix_inverse(matrix)
Helper function to have an efficient matrix inversion function.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
matrix |
array
|
2d-array representing a matrix |
required |
Returns:
Type | Description |
---|---|
np.array: 2d-array representing the matrix inverse |
Source code in omnigibson/utils/transform_utils.py
normalize(v, axis=None, eps=1e-10)
ortho(left, right, bottom, top, znear, zfar)
Create orthonormal projection matrix.
Source code in omnigibson/utils/transform_utils.py
perspective(fovy, aspect, znear, zfar)
Create perspective projection matrix.
Source code in omnigibson/utils/transform_utils.py
pose2mat(pose)
Converts pose to homogeneous matrix.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
pose |
2 - tuple
|
a (pos, orn) tuple where pos is vec3 float cartesian, and orn is vec4 float quaternion. |
required |
Returns:
Type | Description |
---|---|
np.array: 4x4 homogeneous matrix |
Source code in omnigibson/utils/transform_utils.py
pose_in_A_to_pose_in_B(pose_A, pose_A_in_B)
Converts a homogenous matrix corresponding to a point C in frame A to a homogenous matrix corresponding to the same point C in frame B.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
pose_A |
array
|
4x4 matrix corresponding to the pose of C in frame A |
required |
pose_A_in_B |
array
|
4x4 matrix corresponding to the pose of A in frame B |
required |
Returns:
Type | Description |
---|---|
np.array: 4x4 matrix corresponding to the pose of C in frame B |
Source code in omnigibson/utils/transform_utils.py
pose_inv(pose_mat)
Computes the inverse of a homogeneous matrix corresponding to the pose of some frame B in frame A. The inverse is the pose of frame A in frame B.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
pose_mat |
array
|
4x4 matrix for the pose to inverse |
required |
Returns:
Type | Description |
---|---|
np.array: 4x4 matrix for the inverse pose |
Source code in omnigibson/utils/transform_utils.py
pose_transform(pos1, quat1, pos0, quat0)
Conducts forward transform from pose (pos0, quat0) to pose (pos1, quat1):
pose1 @ pose0, NOT pose0 @ pose1
Parameters:
Name | Type | Description | Default |
---|---|---|---|
pos1 |
(x,y,z) position to transform |
required | |
quat1 |
(x,y,z,w) orientation to transform |
required | |
pos0 |
(x,y,z) initial position |
required | |
quat0 |
(x,y,z,w) initial orientation |
required |
Returns:
Type | Description |
---|---|
2-tuple: - (np.array) (x,y,z) position array in cartesian coordinates - (np.array) (x,y,z,w) orientation array in quaternion form |
Source code in omnigibson/utils/transform_utils.py
quat2axisangle(quat)
Converts quaternion to axis-angle format. Returns a unit vector direction scaled by its angle in radians.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
quat |
array
|
(x,y,z,w) vec4 float angles |
required |
Returns:
Type | Description |
---|---|
np.array: (ax,ay,az) axis-angle exponential coordinates |
Source code in omnigibson/utils/transform_utils.py
quat2euler(quat)
Converts euler angles into quaternion form
Parameters:
Name | Type | Description | Default |
---|---|---|---|
quat |
array
|
(x,y,z,w) float quaternion angles |
required |
Returns:
Type | Description |
---|---|
np.array: (r,p,y) angles |
Raises:
Type | Description |
---|---|
AssertionError
|
[Invalid input shape] |
Source code in omnigibson/utils/transform_utils.py
quat2mat(quaternion)
Converts given quaternion to matrix.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
quaternion |
array
|
(..., 4) (x,y,z,w) float quaternion angles |
required |
Returns:
Type | Description |
---|---|
np.array: (..., 3, 3) rotation matrix |
Source code in omnigibson/utils/transform_utils.py
quat_conjugate(quaternion)
Return conjugate of quaternion.
E.g.:
q0 = random_quaternion() q1 = quat_conjugate(q0) q1[3] == q0[3] and all(q1[:3] == -q0[:3]) True
Parameters:
Name | Type | Description | Default |
---|---|---|---|
quaternion |
array
|
(x,y,z,w) quaternion |
required |
Returns:
Type | Description |
---|---|
np.array: (x,y,z,w) quaternion conjugate |
Source code in omnigibson/utils/transform_utils.py
quat_distance(quaternion1, quaternion0)
Returns distance between two quaternions, such that distance * quaternion0 = quaternion1
Parameters:
Name | Type | Description | Default |
---|---|---|---|
quaternion1 |
array
|
(x,y,z,w) quaternion |
required |
quaternion0 |
array
|
(x,y,z,w) quaternion |
required |
Returns:
Type | Description |
---|---|
np.array: (x,y,z,w) quaternion distance |
Source code in omnigibson/utils/transform_utils.py
quat_inverse(quaternion)
Return inverse of quaternion.
E.g.:
q0 = random_quaternion() q1 = quat_inverse(q0) np.allclose(quat_multiply(q0, q1), [0, 0, 0, 1]) True
Parameters:
Name | Type | Description | Default |
---|---|---|---|
quaternion |
array
|
(x,y,z,w) quaternion |
required |
Returns:
Type | Description |
---|---|
np.array: (x,y,z,w) quaternion inverse |
Source code in omnigibson/utils/transform_utils.py
quat_multiply(quaternion1, quaternion0)
Return multiplication of two quaternions (q1 * q0).
E.g.:
q = quat_multiply([1, -2, 3, 4], [-5, 6, 7, 8]) np.allclose(q, [-44, -14, 48, 28]) True
Parameters:
Name | Type | Description | Default |
---|---|---|---|
quaternion1 |
array
|
(x,y,z,w) quaternion |
required |
quaternion0 |
array
|
(x,y,z,w) quaternion |
required |
Returns:
Type | Description |
---|---|
np.array: (x,y,z,w) multiplied quaternion |
Source code in omnigibson/utils/transform_utils.py
quat_slerp(quat0, quat1, fraction, shortestpath=True)
Return spherical linear interpolation between two quaternions.
E.g.:
q0 = random_quat() q1 = random_quat() q = quat_slerp(q0, q1, 0.0) np.allclose(q, q0) True
q = quat_slerp(q0, q1, 1.0) np.allclose(q, q1) True
q = quat_slerp(q0, q1, 0.5) angle = math.acos(np.dot(q0, q)) np.allclose(2.0, math.acos(np.dot(q0, q1)) / angle) or np.allclose(2.0, math.acos(-np.dot(q0, q1)) / angle) True
Parameters:
Name | Type | Description | Default |
---|---|---|---|
quat0 |
array
|
(x,y,z,w) quaternion startpoint |
required |
quat1 |
array
|
(x,y,z,w) quaternion endpoint |
required |
fraction |
float
|
fraction of interpolation to calculate |
required |
shortestpath |
bool
|
If True, will calculate the shortest path |
True
|
Returns:
Type | Description |
---|---|
np.array: (x,y,z,w) quaternion distance |
Source code in omnigibson/utils/transform_utils.py
random_axis_angle(angle_limit=None, random_state=None)
Samples an axis-angle rotation by first sampling a random axis and then sampling an angle. If @angle_limit is provided, the size of the rotation angle is constrained.
If @random_state is provided (instance of np.random.RandomState), it will be used to generate random numbers.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
angle_limit |
None or float
|
If set, determines magnitude limit of angles to generate |
None
|
random_state |
None or RandomState
|
RNG to use if specified |
None
|
Raises:
Type | Description |
---|---|
AssertionError
|
[Invalid RNG] |
Source code in omnigibson/utils/transform_utils.py
random_quat(rand=None)
Return uniform random unit quaternion.
E.g.:
q = random_quat() np.allclose(1.0, vector_norm(q)) True q = random_quat(np.random.random(3)) q.shape (4,)
Parameters:
Name | Type | Description | Default |
---|---|---|---|
rand |
3 - array or None
|
If specified, must be three independent random variables that are uniformly distributed between 0 and 1. |
None
|
Returns:
Type | Description |
---|---|
np.array: (x,y,z,w) random quaternion |
Source code in omnigibson/utils/transform_utils.py
relative_pose_transform(pos1, quat1, pos0, quat0)
Computes relative forward transform from pose (pos0, quat0) to pose (pos1, quat1), i.e.: solves:
pose1 = pose0 @ transform
Parameters:
Name | Type | Description | Default |
---|---|---|---|
pos1 |
(x,y,z) position to transform |
required | |
quat1 |
(x,y,z,w) orientation to transform |
required | |
pos0 |
(x,y,z) initial position |
required | |
quat0 |
(x,y,z,w) initial orientation |
required |
Returns:
Type | Description |
---|---|
2-tuple: - (np.array) (x,y,z) position array in cartesian coordinates - (np.array) (x,y,z,w) orientation array in quaternion form |
Source code in omnigibson/utils/transform_utils.py
rotation_matrix(angle, direction, point=None)
Returns matrix to rotate about axis defined by point and direction.
E.g.: >>> angle = (random.random() - 0.5) * (2math.pi) >>> direc = numpy.random.random(3) - 0.5 >>> point = numpy.random.random(3) - 0.5 >>> R0 = rotation_matrix(angle, direc, point) >>> R1 = rotation_matrix(angle-2math.pi, direc, point) >>> is_same_transform(R0, R1) True
>>> R0 = rotation_matrix(angle, direc, point)
>>> R1 = rotation_matrix(-angle, -direc, point)
>>> is_same_transform(R0, R1)
True
>>> I = numpy.identity(4, numpy.float32)
>>> numpy.allclose(I, rotation_matrix(math.pi*2, direc))
True
>>> numpy.allclose(2., numpy.trace(rotation_matrix(math.pi/2,
... direc, point)))
True
Parameters:
Name | Type | Description | Default |
---|---|---|---|
angle |
float
|
Magnitude of rotation |
required |
direction |
array
|
(ax,ay,az) axis about which to rotate |
required |
point |
None or array
|
If specified, is the (x,y,z) point about which the rotation will occur |
None
|
Returns:
Type | Description |
---|---|
np.array: 4x4 homogeneous matrix that includes the desired rotation |
Source code in omnigibson/utils/transform_utils.py
unit_vector(data, axis=None, out=None)
Returns ndarray normalized by length, i.e. eucledian norm, along axis.
E.g.: >>> v0 = numpy.random.random(3) >>> v1 = unit_vector(v0) >>> numpy.allclose(v1, v0 / numpy.linalg.norm(v0)) True
>>> v0 = numpy.random.rand(5, 4, 3)
>>> v1 = unit_vector(v0, axis=-1)
>>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=2)), 2)
>>> numpy.allclose(v1, v2)
True
>>> v1 = unit_vector(v0, axis=1)
>>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=1)), 1)
>>> numpy.allclose(v1, v2)
True
>>> v1 = numpy.empty((5, 4, 3), dtype=numpy.float32)
>>> unit_vector(v0, axis=1, out=v1)
>>> numpy.allclose(v1, v2)
True
>>> list(unit_vector([]))
[]
>>> list(unit_vector([1.0]))
[1.0]
Parameters:
Name | Type | Description | Default |
---|---|---|---|
data |
array
|
data to normalize |
required |
axis |
None or int
|
If specified, determines specific axis along data to normalize |
None
|
out |
None or array
|
If specified, will store computation in this variable |
None
|
Returns:
Type | Description |
---|---|
None or np.array: If @out is not specified, will return normalized vector. Otherwise, stores the output in @out |
Source code in omnigibson/utils/transform_utils.py
vec(values)
Converts value tuple into a numpy vector.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
values |
n - array
|
a tuple of numbers |
required |
Returns:
Type | Description |
---|---|
np.array: vector of given values |
vec2quat(vec, up=(0, 0, 1.0))
Converts given 3d-direction vector @vec to quaternion orientation with respect to another direction vector @up
Parameters:
Name | Type | Description | Default |
---|---|---|---|
vec |
3 - array
|
(x,y,z) direction vector (possible non-normalized) |
required |
up |
3 - array
|
(x,y,z) direction vector representing the canonical up direction (possible non-normalized) |
(0, 0, 1.0)
|
Source code in omnigibson/utils/transform_utils.py
vecs2axisangle(vec0, vec1)
Converts the angle from unnormalized 3D vectors @vec0 to @vec1 into an axis-angle representation of the angle
Parameters:
Name | Type | Description | Default |
---|---|---|---|
vec0 |
array
|
(..., 3) (x,y,z) 3D vector, possibly unnormalized |
required |
vec1 |
array
|
(..., 3) (x,y,z) 3D vector, possibly unnormalized |
required |
Source code in omnigibson/utils/transform_utils.py
vecs2quat(vec0, vec1, normalized=False)
Converts the angle from unnormalized 3D vectors @vec0 to @vec1 into a quaternion representation of the angle
Parameters:
Name | Type | Description | Default |
---|---|---|---|
vec0 |
array
|
(..., 3) (x,y,z) 3D vector, possibly unnormalized |
required |
vec1 |
array
|
(..., 3) (x,y,z) 3D vector, possibly unnormalized |
required |
normalized |
bool
|
If True, @vec0 and @vec1 are assumed to already be normalized and we will skip the normalization step (more efficient) |
False
|
Source code in omnigibson/utils/transform_utils.py
vel_in_A_to_vel_in_B(vel_A, ang_vel_A, pose_A_in_B)
Converts linear and angular velocity of a point in frame A to the equivalent in frame B.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
vel_A |
array
|
(vx,vy,vz) linear velocity in A |
required |
ang_vel_A |
array
|
(wx,wy,wz) angular velocity in A |
required |
pose_A_in_B |
array
|
4x4 matrix corresponding to the pose of A in frame B |
required |
Returns:
Type | Description |
---|---|
2-tuple:
|
Source code in omnigibson/utils/transform_utils.py
z_angle_from_quat(quat)
Get the angle around the Z axis produced by the quaternion.
z_rotation_from_quat(quat)
Get the quaternion for the rotation around the Z axis produced by the quaternion.