def trans_rot_to_hmat(trans, rot):
'''
Converts a rotation and translation to a homogeneous transform.
**Args:**
**trans (np.array):** Translation (x, y, z).
**rot (np.array):** Quaternion (x, y, z, w).
**Returns:**
H (np.array): 4x4 homogenous transform matrix.
'''
H = transformations.quaternion_matrix(rot)
H[0:3, 3] = trans
return H
def trans_rot_to_hmat(trans, rot):
'''
Converts a rotation and translation to a homogeneous transform.
**Args:**
**trans (np.array):** Translation (x, y, z).
**rot (np.array):** Quaternion (x, y, z, w).
**Returns:**
H (np.array): 4x4 homogenous transform matrix.
'''
H = transformations.quaternion_matrix(rot)
H[0:3, 3] = trans
return H
def quat2mat(quat):
return transformations.quaternion_matrix(quat)[:3, :3]
def quat2mat(quat):
return transformations.quaternion_matrix(quat)[:3, :3]