py_transforms3d

transforms3d §

• 使用transforms3d完成欧拉角、旋转矩阵、齐次矩阵、四元数之间的转换

import transforms3d as t3d
 
def quat2rotmat(quat):
  ## input format (quat) is [x, y, z, w]
  ## uses TF style ([x, y, z, w]), transforms3d uses the sequence [w, x, y, z]
  mat = t3d.quaternions.quat2mat([quat[3], quat[0], quat[1], quat[2]])
  return mat
 
def quat2ht(quat):
  ## input format (quat) is [x, y, z, w], transforms3d uses the sequence [w, x, y, z]
  ## return a 4x4 homogeneous transformation matrix
  ht = np.identity(4)
  ht[:3,:3] = quat2rotmat(quat)
  return ht
 
def mat2quat(mat):
  ## intput format is a 3x3 rotation matrix or a 4x4 homogeneous transformation matrix
  ## returns in TF style ([x, y, z, w]), transforms3d uses the sequence [w, x, y, z]
  rotmat = mat[:3, :3]
  quat = t3d.quaternions.mat2quat(rotmat)
  return ([quat[1], quat[2], quat[3], quat[0]])
 
 
def get_rotmat(data):
  ## get the 3x3 rotation matrix, input can be :
  ## 1) euler angle, 2) quaternion, 3) 3x3 rotation matrix, 4) 4x4 homogeneous transformation matrix
  ## return a 3x3 rotation matrix
  rot_mat = np.identity(3)
 
  d = np.array(data, dtype=float)
 
  shape = d.shape
  print(shape)
  
  if len(shape) == 1:
    if len(d) == 3:
      ## euler angle: [rx, ry, rz], rotate based on the new frame, unit: rad
      rx = np.array([[1.,        0.,         0.],\
                     [0., cos(d[0]), -sin(d[0])],\
                     [0., sin(d[0]),  cos(d[0])]])
 
      ry = np.array([[ cos(d[1]), 0., sin(d[1])],\
                     [        0., 1.,        0.],\
                     [-sin(d[1]), 0., cos(d[1])]])
 
      rz = np.array([[cos(d[2]), -sin(d[2]), 0.],\
                     [sin(d[2]),  cos(d[2]), 0.],\
                     [       0.,         0., 1.]])
 
      rot_mat[:3,:3] = np.dot(rx, rot_mat[:3,:3])
      rot_mat[:3,:3] = np.dot(ry, rot_mat[:3,:3])
      rot_mat[:3,:3] = np.dot(rz, rot_mat[:3,:3])
 
    elif len(d) == 4:
      ## quaternion: [x, y, z, w]
      rot_mat = quat2rotmat(d)[:3,:3]
 
    else:
      print("ERROR! Check your input data: {}".format(data))
 
  elif len(shape) == 2:
    if shape[0] == 3:
      rot_mat = d
    elif shape[0] == 4:
      rot_mat = d[:3, :3]
 
  else:
    print("ERROR! Check your input data: {}".format(data))
  
  return rot_mat