transform_utils
Utility functions of matrix and vector transformations.
NOTE: convention for quaternions is (x, y, z, w)
align_vector_sets(vec_set1, vec_set2)
Computes a single quaternion representing the rotation that best aligns vec_set1 to vec_set2.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
vec_set1 |
Tensor
|
(N, 3) tensor of N 3D vectors |
required |
vec_set2 |
Tensor
|
(N, 3) tensor of N 3D vectors |
required |
Returns:
Type | Description |
---|---|
Tensor
|
(4,) Normalized quaternion representing the overall rotation |
Source code in omnigibson/utils/transform_utils.py
anorm(x, dim=None, keepdim=False)
axisangle2quat(vec, eps=1e-06)
Converts scaled axis-angle to quat. Args: vec (tensor): (..., 3) tensor where final dim is (ax,ay,az) axis-angle exponential coordinates eps (float): Stability value below which small values will be mapped to 0
Returns:
Type | Description |
---|---|
tensor
|
(..., 4) tensor where final dim is (x,y,z,w) vec4 float quaternion |
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 |
Tensor
|
(x,y,z,w) quaternion orientation to check |
required |
atol |
float
|
Absolute tolerance permitted |
0.05
|
Returns:
Type | Description |
---|---|
Tensor
|
Boolean tensor indicating 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 |
tensor
|
(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 |
Tensor
|
a 4-dim array corresponding to a quaternion |
required |
to |
str
|
either 'xyzw' or 'wxyz', determining which convention to convert to. |
'xyzw'
|
Returns:
Type | Description |
---|---|
Tensor
|
The converted quaternion |
Source code in omnigibson/utils/transform_utils.py
dot(v1, v2, dim=-1, keepdim=False)
Computes dot product between two vectors along the provided dim, optionally keeping the dimension
Parameters:
Name | Type | Description | Default |
---|---|---|---|
v1 |
tensor
|
(..., N, ...) arbitrary vector |
required |
v2 |
tensor
|
(..., N, ...) arbitrary vector |
required |
dim |
int
|
Dimension to sum over for dot product |
-1
|
keepdim |
bool
|
Whether to keep dimension over which dot product is calculated |
False
|
Returns:
Type | Description |
---|---|
tensor
|
(..., [1,] ...) dot product of vectors, with optional dimension kept if @keepdim is True |
Source code in omnigibson/utils/transform_utils.py
euler2mat(euler)
Converts euler angles into rotation matrix form
Parameters:
Name | Type | Description | Default |
---|---|---|---|
euler |
tensor
|
(r,p,y) angles |
required |
Returns:
Type | Description |
---|---|
tensor
|
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 |
Tensor
|
(..., 3) (r,p,y) angles |
required |
Returns:
Type | Description |
---|---|
Tensor
|
(..., 4) (x,y,z,w) float quaternion angles |
Raises:
Type | Description |
---|---|
AssertionError
|
[Invalid input shape] |
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 |
tensor
|
(fx,fy,fz) linear force in A |
required |
torque_A |
tensor
|
(tx,ty,tz) rotational force (moment) in A |
required |
pose_A_in_B |
tensor
|
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
get_orientation_diff_in_radian(orn0, orn1)
Returns the difference between two quaternion orientations in radians.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
orn0 |
Tensor
|
(x, y, z, w) quaternion |
required |
orn1 |
Tensor
|
(x, y, z, w) quaternion |
required |
Returns:
Type | Description |
---|---|
orn_diff (th.Tensor
|
orientation difference in radians |
Source code in omnigibson/utils/transform_utils.py
get_orientation_error(desired, current)
This function calculates a 3-dimensional orientation error vector, where inputs are quaternions
Parameters:
Name | Type | Description | Default |
---|---|---|---|
desired |
tensor
|
(..., 4) where final dim is (x,y,z,w) quaternion |
required |
current |
tensor
|
(..., 4) where final dim is (x,y,z,w) quaternion |
required |
Returns: tensor: (..., 3) where final dim is (ax, ay, az) axis-angle representing orientation error
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 |
tensor
|
a 4x4 homogenous matrix for the target pose |
required |
current_pose |
tensor
|
a 4x4 homogenous matrix for the current pose |
required |
Returns:
Type | Description |
---|---|
tensor
|
6-dim pose error. |
Source code in omnigibson/utils/transform_utils.py
integer_spiral_coordinates(n)
A function to map integers to 2D coordinates in a spiral pattern around the origin.
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
|
|
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 |
tensor
|
(x,y,z) translation value |
required |
rotation |
tensor
|
a 3x3 matrix representing rotation |
required |
Returns:
Type | Description |
---|---|
pose (th.tensor
|
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 |
tensor
|
3x3 rotation matrix |
required |
Returns:
Type | Description |
---|---|
tensor
|
(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 |
tensor
|
a 4x4 homogeneous matrix |
required |
Returns:
Type | Description |
---|---|
2 - tuple
|
|
Source code in omnigibson/utils/transform_utils.py
mat2quat(rmat)
Converts given rotation matrix to quaternion. Args: rmat (th.Tensor): (3, 3) or (..., 3, 3) rotation matrix Returns: th.Tensor: (4,) or (..., 4) (x,y,z,w) float quaternion angles
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 |
tensor
|
2d-array representing a matrix |
required |
Returns:
Type | Description |
---|---|
tensor
|
2d-array representing the matrix inverse |
Source code in omnigibson/utils/transform_utils.py
normalize(v, dim=None, eps=1e-10)
L2 Normalize along specified axes.
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 |
tensor
|
4x4 matrix corresponding to the pose of C in frame A |
required |
pose_A_in_B |
tensor
|
4x4 matrix corresponding to the pose of A in frame B |
required |
Returns:
Type | Description |
---|---|
tensor
|
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 |
tensor
|
4x4 matrix for the pose to inverse |
required |
Returns:
Type | Description |
---|---|
tensor
|
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
|
|
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. Args: quat (tensor): (..., 4) tensor where the final dim is (x,y,z,w) quaternion Returns: tensor: (..., 3) axis-angle exponential coordinates
Source code in omnigibson/utils/transform_utils.py
quat2mat(quaternion)
Convert quaternions into rotation matrices.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
quaternion |
Tensor
|
A tensor of shape (..., 4) representing batches of quaternions (w, x, y, z). |
required |
Returns:
Type | Description |
---|---|
Tensor
|
A tensor of shape (..., 3, 3) representing batches of rotation matrices. |
Source code in omnigibson/utils/transform_utils.py
quat_apply(quat, vec)
Apply a quaternion rotation to a vector (equivalent to R.from_quat(x).apply(y)) Args: quat (th.Tensor): (4,) or (N, 4) or (N, 1, 4) quaternion in (x, y, z, w) format vec (th.Tensor): (3,) or (M, 3) or (1, M, 3) vector to rotate Returns: th.Tensor: (M, 3) or (N, M, 3) rotated vector
Source code in omnigibson/utils/transform_utils.py
quat_conjugate(quaternion)
Return conjugate of quaternion.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
quaternion |
Tensor
|
(x,y,z,w) quaternion |
required |
Returns:
Type | Description |
---|---|
Tensor
|
(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 |
tensor
|
(x,y,z,w) quaternion |
required |
quaternion0 |
tensor
|
(x,y,z,w) quaternion |
required |
Returns:
Type | Description |
---|---|
tensor
|
(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) th.allclose(quat_multiply(q0, q1), [0, 0, 0, 1]) True
Parameters:
Name | Type | Description | Default |
---|---|---|---|
quaternion |
tensor
|
(x,y,z,w) quaternion |
required |
Returns:
Type | Description |
---|---|
tensor
|
(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).
Parameters:
Name | Type | Description | Default |
---|---|---|---|
quaternion1 |
Tensor
|
(x,y,z,w) quaternion |
required |
quaternion0 |
Tensor
|
(x,y,z,w) quaternion |
required |
Returns:
Type | Description |
---|---|
Tensor
|
(x,y,z,w) multiplied quaternion |
Source code in omnigibson/utils/transform_utils.py
quat_slerp(quat0, quat1, frac, shortestpath=True, eps=1e-15)
Return spherical linear interpolation between two quaternions.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
quat0 |
tensor
|
(..., 4) tensor where the final dim is (x,y,z,w) initial quaternion |
required |
quat1 |
tensor
|
(..., 4) tensor where the final dim is (x,y,z,w) final quaternion |
required |
frac |
tensor
|
Values in [0.0, 1.0] representing fraction of interpolation |
required |
shortestpath |
bool
|
If True, will calculate shortest path |
True
|
eps |
float
|
Value to check for singularities |
1e-15
|
Returns: tensor: (..., 4) Interpolated
Source code in omnigibson/utils/transform_utils.py
quaternions_close(q1, q2, atol=0.001)
Whether two quaternions represent the same rotation, allowing for the possibility that one is the negative of the other.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
q1 |
Tensor
|
th.Tensor First quaternion |
required |
q2 |
Tensor
|
th.Tensor Second quaternion |
required |
atol |
float
|
float Absolute tolerance for comparison |
0.001
|
Returns:
Type | Description |
---|---|
bool
|
bool Whether the quaternions are close |
Source code in omnigibson/utils/transform_utils.py
random_axis_angle(angle_limit=2.0 * math.pi)
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.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
angle_limit |
float
|
Determines magnitude limit of angles to generate |
2.0 * pi
|
Raises:
Type | Description |
---|---|
AssertionError
|
[Invalid RNG] |
Source code in omnigibson/utils/transform_utils.py
random_quaternion(num_quaternions=1)
Generate random rotation quaternions, uniformly distributed over SO(3).
Parameters:
Name | Type | Description | Default |
---|---|---|---|
num_quaternions |
int
|
int, number of quaternions to generate (default: 1) |
1
|
Returns:
Type | Description |
---|---|
Tensor
|
A tensor of shape (num_quaternions, 4) containing random unit quaternions. |
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
|
|
Source code in omnigibson/utils/transform_utils.py
rotation_matrix(angle, direction)
Returns a 3x3 rotation matrix to rotate about the given axis.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
angle |
float
|
Magnitude of rotation in radians |
required |
direction |
Tensor
|
(ax,ay,az) axis about which to rotate |
required |
Returns:
Type | Description |
---|---|
Tensor
|
3x3 rotation matrix |
Source code in omnigibson/utils/transform_utils.py
transform_points(points, matrix, translate=True)
Returns points rotated by a homogeneous transformation matrix. If points are (n, 2) matrix must be (3, 3) If points are (n, 3) matrix must be (4, 4)
Parameters:
Name | Type | Description | Default |
---|---|---|---|
points |
(n, dim) torch.Tensor |
required | |
matrix |
(3, 3) or (4, 4) torch.Tensor |
required | |
translate |
bool |
True
|
Returns:
Type | Description |
---|---|
transformed
|
(n, dim) torch.Tensor |
Tensor
|
Transformed points. |
Source code in omnigibson/utils/transform_utils.py
transformation_matrix(angle, direction, point=None)
Returns a 4x4 homogeneous transformation matrix to rotate about axis defined by point and direction.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
angle |
float
|
Magnitude of rotation in radians |
required |
direction |
Tensor
|
(ax,ay,az) axis about which to rotate |
required |
point |
Optional[Tensor]
|
If specified, is the (x,y,z) point about which the rotation will occur |
None
|
Returns:
Type | Description |
---|---|
Tensor
|
4x4 homogeneous transformation matrix |
Source code in omnigibson/utils/transform_utils.py
unit_vector(data, dim=None, out=None)
Returns tensor normalized by length, i.e. Euclidean norm, along axis.
Parameters:
Name | Type | Description | Default |
---|---|---|---|
data |
Tensor
|
data to normalize |
required |
dim |
Optional[int]
|
If specified, determines specific dimension along data to normalize |
None
|
out |
Optional[Tensor]
|
If specified, will store computation in this variable |
None
|
Returns:
Type | Description |
---|---|
Tensor
|
Normalized vector |
Source code in omnigibson/utils/transform_utils.py
vec2quat(vec, up=th.tensor([0.0, 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 |
Tensor
|
(x,y,z) direction vector (possibly non-normalized) |
required |
up |
Tensor
|
(x,y,z) direction vector representing the canonical up direction (possibly non-normalized) |
tensor([0.0, 0.0, 1.0])
|
Returns:
Type | Description |
---|---|
Tensor
|
(x,y,z,w) quaternion |
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 |
tensor
|
(..., 3) (x,y,z) 3D vector, possibly unnormalized |
required |
vec1 |
tensor
|
(..., 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 Args: vec0 (th.Tensor): (..., 3) (x,y,z) 3D vector, possibly unnormalized vec1 (th.Tensor): (..., 3) (x,y,z) 3D vector, possibly unnormalized normalized (bool): If True, @vec0 and @vec1 are assumed to already be normalized and we will skip the normalization step (more efficient) Returns: th.Tensor: (..., 4) Normalized quaternion representing the rotation from vec0 to vec1
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 |
tensor
|
(vx,vy,vz) linear velocity in A |
required |
ang_vel_A |
tensor
|
(wx,wy,wz) angular velocity in A |
required |
pose_A_in_B |
tensor
|
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.