enum class G1Arm5JointIndex : int {
  // Left leg
  LEFT_HIP_PITCH = 0,
  LEFT_HIP_ROLL = 1,
  LEFT_HIP_YAW = 2,
  LEFT_KNEE = 3,
  LEFT_ANKLE = 4,
  LEFT_ANKLE_ROLL = 5,

  // Right leg
  RIGHT_HIP_PITCH = 6,
  RIGHT_HIP_ROLL = 7,
  RIGHT_HIP_YAW = 8,
  RIGHT_KNEE = 9,
  RIGHT_ANKLE = 10,
  RIGHT_ANKLE_ROLL = 11,

  // Waist
  WAIST_YAW = 12,
  WAIST_ROLL = 13,
  WAIST_PITCH = 14,

  // Left arm
  LEFT_SHOULDER_PITCH = 15,
  LEFT_SHOULDER_ROLL = 16,
  LEFT_SHOULDER_YAW = 17,
  LEFT_ELBOW_PITCH = 18,
  LEFT_ELBOW_ROLL = 19,

  // Right arm
  RIGHT_SHOULDER_PITCH = 22,
  RIGHT_SHOULDER_ROLL = 23,
  RIGHT_SHOULDER_YAW = 24,
  RIGHT_ELBOW_PITCH = 25,
  RIGHT_ELBOW_ROLL = 26,

  NOT_USED_JOINT = 29,
  NOT_USED_JOINT1 = 30,
  NOT_USED_JOINT2 = 31,
  NOT_USED_JOINT3 = 32,
  NOT_USED_JOINT4 = 33,
  NOT_USED_JOINT5 = 34
};

constexpr float PI_2 = 1.57079632F;

enum class G1Arm7JointIndex : int {
  // Left leg
  LEFT_HIP_PITCH,
  LEFT_HIP_ROLL,
  LEFT_HIP_YAW,
  LEFT_KNEE,
  LEFT_ANKLE,
  LEFT_ANKLE_ROLL,

  // Right leg
  RIGHT_HIP_PITCH,
  RIGHT_HIP_ROLL,
  RIGHT_HIP_YAW,
  RIGHT_KNEE,
  RIGHT_ANKLE,
  RIGHT_ANKLE_ROLL,

  WAIST_YAW,
  WAIST_ROLL,
  WAIST_PITCH,

  // Left arm
  LEFT_SHOULDER_PITCH,
  LEFT_SHOULDER_ROLL,
  LEFT_SHOULDER_YAW,
  LEFT_ELBOW,
  LEFT_WRIST_ROLL,
  LEFT_WRIST_PITCH,
  LEFT_WRIST_YAW,
  
  // Right arm
  RIGHT_SHOULDER_PITCH,
  RIGHT_SHOULDER_ROLL,
  RIGHT_SHOULDER_YAW,
  RIGHT_ELBOW,
  RIGHT_WRIST_ROLL,
  RIGHT_WRIST_PITCH,
  RIGHT_WRIST_YAW,

  NOT_USED_JOINT,
  NOT_USED_JOINT1,
  NOT_USED_JOINT2,
  NOT_USED_JOINT3,
  NOT_USED_JOINT4,
  NOT_USED_JOINT5
};