code: def quat_to_rotation_matrix(q):
      
      q = q.flatten()
      assert q.size == 4
      assert_almost_equal(np.linalg.norm(q), 1.0, err_msg="Not a unit quaternion!")
      qq = q ** 2
      R = np.array([[qq[0] + qq[1] - qq[2] - qq[3], 2*q[1]*q[2] -
  2*q[0]*q[3], 2*q[1]*q[3] + 2*q[0]*q[2]],
                  [2*q[1]*q[2] + 2*q[0]*q[3], qq[0] - qq[1] + qq[2] -
  qq[3], 2*q[2]*q[3] - 2*q[0]*q[1]],
                  [2*q[1]*q[3] - 2*q[0]*q[2], 2*q[2]*q[3] + 2*q[0]*q[1],
  qq[0] - qq[1] - qq[2] + qq[3]]])
      return R
docs: Convert unit quaternion to rotation matrix
      
      Parameters
      -------------
      q : (4,) ndarray
              Unit quaternion, scalar as first element
  
      Returns
      ----------------
      R : (3,3) ndarray
              Rotation matrix