Draw a frame in PyBullet.

draw_frame_pybullet.py

import pybullet as p
import numpy as np

def draw_frame(T: np.ndarray) -> None:
  """ Draw a frame represented by the 4-by-4 homogenous transformation matrix T."""
  for i in range(3):
    color = [0, 0, 0]
    color[i] = 1
    p.addUserDebugLine(
      lineFromXYZ=T[:3, 3],
      lineToXYZ=T[:3, 3] + T[:3, i],
      lineColorRGB=color,
    )