Пример #1
1
def homogeneous2translation_quaternion(homogeneous):
  """
  Translation: [x, y, z]
  Quaternion: [x, y, z, w]
  """
  translation = transformations.translation_from_matrix(homogeneous)
  quaternion = transformations.quaternion_from_matrix(homogeneous)
  return translation, quaternion
Пример #2
0
def poseFromTransform(transform):
    '''
    Returns position, quaternion
    '''
    mat = getNumpyFromTransform(transform)
    return np.array(mat[:3, 3]), transformations.quaternion_from_matrix(
        mat, isprecise=True)
Пример #3
0
def poseFromTransform(transform):
    """
    Returns position, quaternion
    mfallon: I do not trust this function, test case of a transform of rpy [0,0,140] does match other tests (esp ROS)
    """
    mat = getNumpyFromTransform(transform)
    return (
        np.array(mat[:3, 3]),
        transformations.quaternion_from_matrix(mat, isprecise=True),
    )
Пример #4
0
def serialize_transform(tform):
    return {
        "translation": list(transformations.translation_from_matrix(tform)),
        "quaternion": list(transformations.quaternion_from_matrix(tform))
    }
Пример #5
0
def poseFromTransform(transform):
    '''
    Returns position, quaternion
    '''
    mat = getNumpyFromTransform(transform)
    return np.array(mat[:3,3]), transformations.quaternion_from_matrix(mat, isprecise=True)
Пример #6
0
def serialize_transform(tform):
    return {
        "translation": list(transformations.translation_from_matrix(tform)),
        "quaternion": list(transformations.quaternion_from_matrix(tform))
    }